1

/******************************************************************************

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

 *****************************************************************************/

    26

    27

/** \file

    28

 * EtherCAT PDO configuration state machine.

    29

 */

    30

    31

/*****************************************************************************/

    32

    33

#include "globals.h"

    34

#include "master.h"

    35

#include "mailbox.h"

    36

#include "slave_config.h"

    37

    38

#include "fsm_pdo.h"

    39

    40

/*****************************************************************************/

    41

    42

void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);

    43

void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);

    44

void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);

    45

void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);

    46

    47

void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);

    48

void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);

    49

    50

void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);

    51

void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);

    52

void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);

    53

void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);

    54

void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);

    55

void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);

    56

    57

void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);

    58

void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);

    59

void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);

    60

void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);

    61

void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);

    62

void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);

    63

    64

void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);

    65

void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);

    66

    67

/*****************************************************************************/

    68

    69

/** Constructor.

    70

 */

    71

void ec_fsm_pdo_init(

    72

        ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */

    73

        ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */

    74

        )

    75

{

    76

    fsm->fsm_coe = fsm_coe;

    77

    ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);

    78

    ec_pdo_list_init(&fsm->pdos);

    79

    ec_sdo_request_init(&fsm->request);

    80

    ec_pdo_init(&fsm->slave_pdo);

    81

}

    82

    83

/*****************************************************************************/

    84

    85

/** Destructor.

    86

 */

    87

void ec_fsm_pdo_clear(

    88

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

    89

        )

    90

{

    91

    ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);

    92

    ec_pdo_list_clear(&fsm->pdos);

    93

    ec_sdo_request_clear(&fsm->request);

    94

    ec_pdo_clear(&fsm->slave_pdo);

    95

}

    96

    97

/*****************************************************************************/

    98

    99

/** Start reading the PDO configuration.

   100

 */

   101

void ec_fsm_pdo_start_reading(

   102

        ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */

   103

        ec_slave_t *slave /**< slave to configure */

   104

        )

   105

{

   106

    fsm->slave = slave;

   107

    fsm->state = ec_fsm_pdo_read_state_start;

   108

}

   109

   110

/*****************************************************************************/

   111

   112

/** Start writing the PDO configuration.

   113

 */

   114

void ec_fsm_pdo_start_configuration(

   115

        ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */

   116

        ec_slave_t *slave /**< slave to configure */

   117

        )

   118

{

   119

    fsm->slave = slave;

   120

    fsm->state = ec_fsm_pdo_conf_state_start;

   121

}

   122

   123

/*****************************************************************************/

   124

   125

/** Get running state.

   126

 *

   127

 * \return false, if state machine has terminated

   128

 */

   129

int ec_fsm_pdo_running(

   130

        const ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   131

        )

   132

{

   133

    return fsm->state != ec_fsm_pdo_state_end

   134

        && fsm->state != ec_fsm_pdo_state_error;

   135

}

   136

   137

/*****************************************************************************/

   138

   139

/** Executes the current state of the state machine.

   140

 *

   141

 * If the state machine's datagram is not sent or received yet, the execution

   142

 * of the state machine is delayed to the next cycle.

   143

 *

   144

 * \return false, if state machine has terminated

   145

 */

   146

int ec_fsm_pdo_exec(

   147

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   148

        )

   149

{

   150

    fsm->state(fsm);

   151

    return ec_fsm_pdo_running(fsm);

   152

}

   153

   154

/*****************************************************************************/

   155

   156

/** Get execution result.

   157

 *

   158

 * \return true, if the state machine terminated gracefully

   159

 */

   160

int ec_fsm_pdo_success(

   161

        const ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   162

        )

   163

{

   164

    return fsm->state == ec_fsm_pdo_state_end;

   165

}

   166

   167

/******************************************************************************

   168

 * Reading state funtions.

   169

 *****************************************************************************/

   170

   171

/** Start reading PDO assignment.

   172

 */

   173

void ec_fsm_pdo_read_state_start(

   174

        ec_fsm_pdo_t *fsm /**< finite state machine */

   175

        )

   176

{

   177

    // read PDO assignment for first sync manager not reserved for mailbox

   178

    fsm->sync_index = 1; // next is 2

   179

    ec_fsm_pdo_read_action_next_sync(fsm);

   180

}

   181

   182

/*****************************************************************************/

   183

   184

/** Read PDO assignment of next sync manager.

   185

 */

   186

void ec_fsm_pdo_read_action_next_sync(

   187

        ec_fsm_pdo_t *fsm /**< Finite state machine */

   188

        )

   189

{

   190

    ec_slave_t *slave = fsm->slave;

   191

   192

    fsm->sync_index++;

   193

   194

    for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {

   195

        if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))

   196

            continue;

   197

   198

        if (slave->master->debug_level)

   199

            EC_DBG("Reading PDO assignment of SM%u.\n", fsm->sync_index);

   200

   201

        ec_pdo_list_clear_pdos(&fsm->pdos);

   202

   203

        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);

   204

        ecrt_sdo_request_read(&fsm->request);

   205

        fsm->state = ec_fsm_pdo_read_state_pdo_count;

   206

        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   207

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   208

        return;

   209

    }

   210

   211

    if (slave->master->debug_level)

   212

        EC_DBG("Reading of PDO configuration finished.\n");

   213

   214

    fsm->state = ec_fsm_pdo_state_end;

   215

}

   216

   217

/*****************************************************************************/

   218

   219

/** Count assigned PDOs.

   220

 */

   221

void ec_fsm_pdo_read_state_pdo_count(

   222

        ec_fsm_pdo_t *fsm /**< finite state machine */

   223

        )

   224

{

   225

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   226

   227

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   228

        EC_ERR("Failed to read number of assigned PDOs for SM%u.\n",

   229

                fsm->sync_index);

   230

        fsm->state = ec_fsm_pdo_state_error;

   231

        return;

   232

    }

   233

   234

    if (fsm->request.data_size != sizeof(uint8_t)) {

   235

        EC_ERR("Invalid data size %u returned when uploading SDO 0x%04X:%02X "

   236

                "from slave %u.\n", fsm->request.data_size,

   237

                fsm->request.index, fsm->request.subindex,

   238

                fsm->slave->ring_position);

   239

        fsm->state = ec_fsm_pdo_state_error;

   240

        return;

   241

    }

   242

    fsm->pdo_count = EC_READ_U8(fsm->request.data);

   243

   244

    if (fsm->slave->master->debug_level)

   245

        EC_DBG("%u PDOs assigned.\n", fsm->pdo_count);

   246

   247

    // read first PDO

   248

    fsm->pdo_pos = 1;

   249

    ec_fsm_pdo_read_action_next_pdo(fsm);

   250

}

   251

   252

/*****************************************************************************/

   253

   254

/** Read next PDO.

   255

 */

   256

void ec_fsm_pdo_read_action_next_pdo(

   257

        ec_fsm_pdo_t *fsm /**< finite state machine */

   258

        )

   259

{

   260

    if (fsm->pdo_pos <= fsm->pdo_count) {

   261

        ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index,

   262

                fsm->pdo_pos);

   263

        ecrt_sdo_request_read(&fsm->request);

   264

        fsm->state = ec_fsm_pdo_read_state_pdo;

   265

        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

   266

        ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   267

        return;

   268

    }

   269

   270

    // finished reading PDO configuration

   271

   272

    if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {

   273

        fsm->state = ec_fsm_pdo_state_error;

   274

        return;

   275

    }

   276

   277

    ec_pdo_list_clear_pdos(&fsm->pdos);

   278

   279

    // next sync manager

   280

    ec_fsm_pdo_read_action_next_sync(fsm);

   281

}

   282

   283

/*****************************************************************************/

   284

   285

/** Fetch PDO information.

   286

 */

   287

void ec_fsm_pdo_read_state_pdo(

   288

        ec_fsm_pdo_t *fsm /**< finite state machine */

   289

        )

   290

{

   291

    if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   292

   293

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   294

        EC_ERR("Failed to read index of assigned PDO %u from SM%u.\n",

   295

                fsm->pdo_pos, fsm->sync_index);

   296

        fsm->state = ec_fsm_pdo_state_error;

   297

        return;

   298

    }

   299

   300

    if (fsm->request.data_size != sizeof(uint16_t)) {

   301

        EC_ERR("Invalid data size %u returned when uploading SDO 0x%04X:%02X "

   302

                "from slave %u.\n", fsm->request.data_size,

   303

                fsm->request.index, fsm->request.subindex,

   304

                fsm->slave->ring_position);

   305

        fsm->state = ec_fsm_pdo_state_error;

   306

        return;

   307

    }

   308

   309

    if (!(fsm->pdo = (ec_pdo_t *)

   310

                kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   311

        EC_ERR("Failed to allocate PDO.\n");

   312

        fsm->state = ec_fsm_pdo_state_error;

   313

        return;

   314

    }

   315

   316

    ec_pdo_init(fsm->pdo);

   317

    fsm->pdo->index = EC_READ_U16(fsm->request.data);

   318

    fsm->pdo->sync_index = fsm->sync_index;

   319

   320

    if (fsm->slave->master->debug_level)

   321

        EC_DBG("PDO 0x%04X.\n", fsm->pdo->index);

   322

   323

    list_add_tail(&fsm->pdo->list, &fsm->pdos.list);

   324

   325

    fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   326

    ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);

   327

    fsm->state(fsm); // execute immediately

   328

}

   329

   330

/*****************************************************************************/

   331

   332

/** Fetch PDO information.

   333

 */

   334

void ec_fsm_pdo_read_state_pdo_entries(

   335

        ec_fsm_pdo_t *fsm /**< finite state machine */

   336

        )

   337

{

   338

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   339

        return;

   340

   341

    if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {

   342

        EC_ERR("Failed to read mapped PDO entries for PDO 0x%04X.\n",

   343

                fsm->pdo->index);

   344

        fsm->state = ec_fsm_pdo_state_error;

   345

        return;

   346

    }

   347

   348

    // next PDO

   349

    fsm->pdo_pos++;

   350

    ec_fsm_pdo_read_action_next_pdo(fsm);

   351

}

   352

   353

/******************************************************************************

   354

 * Writing state functions.

   355

 *****************************************************************************/

   356

   357

/** Start PDO configuration.

   358

 */

   359

void ec_fsm_pdo_conf_state_start(

   360

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   361

        )

   362

{

   363

    if (!fsm->slave->config) {

   364

        fsm->state = ec_fsm_pdo_state_end;

   365

        return;

   366

    }

   367

   368

    fsm->sync_index = 0xff; // next is zero

   369

    ec_fsm_pdo_conf_action_next_sync(fsm);

   370

}

   371

   372

/*****************************************************************************/

   373

   374

/** Assign next PDO.

   375

 */

   376

ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   377

        const ec_fsm_pdo_t *fsm, /**< PDO configuration state machine. */

   378

        const struct list_head *list /**< current PDO list item */

   379

        )

   380

{

   381

    list = list->next; 

   382

    if (list == &fsm->pdos.list)

   383

        return NULL; // no next PDO

   384

    return list_entry(list, ec_pdo_t, list);

   385

}

   386

   387

/*****************************************************************************/

   388

   389

/** Get the next sync manager for a pdo configuration.

   390

 */

   391

void ec_fsm_pdo_conf_action_next_sync(

   392

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   393

        )

   394

{

   395

    fsm->sync_index++;

   396

   397

    for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {

   398

        if (!fsm->slave->config) { // slave configuration removed in the meantime

   399

            fsm->state = ec_fsm_pdo_state_error;

   400

            return;

   401

        }

   402

   403

        if (ec_pdo_list_copy(&fsm->pdos,

   404

                    &fsm->slave->config->sync_configs[fsm->sync_index].pdos)) {

   405

            fsm->state = ec_fsm_pdo_state_error;

   406

            return;

   407

        }

   408

   409

        if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {

   410

            if (!list_empty(&fsm->pdos.list))

   411

                EC_WARN("PDOs configured for SM%u, but slave %u does not "

   412

                        "provide the sync manager information!\n",

   413

                        fsm->sync_index, fsm->slave->ring_position);

   414

            continue;

   415

        }

   416

   417

        // get first configured PDO

   418

        if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {

   419

            // no pdos configured

   420

            ec_fsm_pdo_conf_action_check_assignment(fsm);

   421

            return;

   422

        }

   423

   424

        ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   425

        return;

   426

    }

   427

   428

    fsm->state = ec_fsm_pdo_state_end;

   429

}

   430

   431

/*****************************************************************************/

   432

   433

/** Check if the mapping has to be read, otherwise start to configure it.

   434

 */

   435

void ec_fsm_pdo_conf_action_pdo_mapping(

   436

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   437

        )

   438

{

   439

    const ec_pdo_t *assigned_pdo;

   440

   441

    fsm->slave_pdo.index = fsm->pdo->index;

   442

   443

    if ((assigned_pdo = ec_slave_find_pdo(fsm->slave, fsm->pdo->index))) {

   444

        ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   445

    } else { // configured PDO is not assigned and thus unknown

   446

        ec_pdo_clear_entries(&fsm->slave_pdo);

   447

    }

   448

   449

    if (list_empty(&fsm->slave_pdo.entries)) {

   450

   451

        if (fsm->slave->master->debug_level)

   452

            EC_DBG("Reading mapping of PDO 0x%04X.\n",

   453

                    fsm->pdo->index);

   454

   455

        // pdo mapping is unknown; start loading it

   456

        ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,

   457

                &fsm->slave_pdo);

   458

        fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   459

        fsm->state(fsm); // execute immediately

   460

        return;

   461

    }

   462

   463

    // pdo mapping is known, check if it most be re-configured

   464

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   465

}

   466

   467

/*****************************************************************************/

   468

   469

/** Execute the PDO entry state machine to read the current PDO's mapping.

   470

 */

   471

void ec_fsm_pdo_conf_state_read_mapping(

   472

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   473

        )

   474

{

   475

    if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   476

        return;

   477

   478

    if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))

   479

        EC_WARN("Failed to read mapped PDO entries for PDO 0x%04X.\n",

   480

                fsm->pdo->index);

   481

   482

    // check if the mapping must be re-configured

   483

    ec_fsm_pdo_conf_action_check_mapping(fsm);

   484

}

   485

   486

/*****************************************************************************/

   487

   488

/** Check if the mapping has to be re-configured.

   489

 *

   490

 * \todo Display mapping differences.

   491

 */

   492

void ec_fsm_pdo_conf_action_check_mapping(

   493

        ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */

   494

        )

   495

{

   496

    if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {

   497

        if (fsm->slave->master->debug_level)

   498

            EC_DBG("Mapping of PDO 0x%04X is already configured correctly.\n",

   499

                    fsm->pdo->index);

   500

        ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);

   501

        return;

   502

    }

   503

   504

    if (fsm->slave->master->debug_level) {

   505

        // TODO display diff

   506

        EC_DBG("Changing mapping of PDO 0x%04X.\n", fsm->pdo->index);

   507

    }

   508

   509

    ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,

   510

            fsm->pdo);

   511

    fsm->state = ec_fsm_pdo_conf_state_mapping;

   512

    fsm->state(fsm); // execure immediately

   513

}

   514

   515

/*****************************************************************************/

   516

   517

/** Let the PDO entry state machine configure the current PDO's mapping.