1

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

     2

 *

     3

 *  $Id: fsm_coe.c 920 2007-09-12 10:07:55Z fp $

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT CoE mapping state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "canopen.h"

    45

#include "fsm_coe_map.h"

    46

    47

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

    48

    49

void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);

    50

void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);

    51

void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);

    52

void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *);

    53

void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);

    54

    55

void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);

    56

void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);

    57

    58

void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *);

    59

void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);

    60

void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);

    61

    62

void ec_fsm_coe_map_clear_pdos(ec_fsm_coe_map_t *);

    63

    64

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

    65

    66

/**

    67

   Constructor.

    68

*/

    69

    70

void ec_fsm_coe_map_init(

    71

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

    72

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

    73

        )

    74

{

    75

    fsm->state = NULL;

    76

    fsm->fsm_coe = fsm_coe;

    77

    INIT_LIST_HEAD(&fsm->pdos);

    78

}

    79

    80

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

    81

    82

/**

    83

   Destructor.

    84

*/

    85

    86

void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)

    87

{

    88

    ec_fsm_coe_map_clear_pdos(fsm);

    89

}

    90

    91

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

    92

    93

/**

    94

 * Clear FSM PDOs.

    95

 */

    96

    97

void ec_fsm_coe_map_clear_pdos(

    98

        ec_fsm_coe_map_t *fsm /**< finite state machine */

    99

        )

   100

{

   101

    ec_pdo_t *pdo, *next;

   102

   103

    // free all PDOs

   104

    list_for_each_entry_safe(pdo, next, &fsm->pdos, list) {

   105

        list_del(&pdo->list);

   106

        ec_pdo_clear(pdo);

   107

        kfree(pdo);

   108

    }

   109

}

   110

   111

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

   112

   113

/**

   114

   Starts to upload an SDO from a slave.

   115

*/

   116

   117

void ec_fsm_coe_map_start(

   118

        ec_fsm_coe_map_t *fsm, /**< finite state machine */

   119

        ec_slave_t *slave /**< EtherCAT slave */

   120

        )

   121

{

   122

    fsm->slave = slave;

   123

    fsm->state = ec_fsm_coe_map_state_start;

   124

}

   125

   126

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

   127

   128

/**

   129

   Executes the current state of the state machine.

   130

   \return false, if state machine has terminated

   131

*/

   132

   133

int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   134

{

   135

    fsm->state(fsm);

   136

   137

    return fsm->state != ec_fsm_coe_map_state_end

   138

        && fsm->state != ec_fsm_coe_map_state_error;

   139

}

   140

   141

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

   142

   143

/**

   144

   Returns, if the state machine terminated with success.

   145

   \return non-zero if successful.

   146

*/

   147

   148

int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */)

   149

{

   150

    return fsm->state == ec_fsm_coe_map_state_end;

   151

}

   152

   153

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

   154

 *  state functions

   155

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

   156

   157

/**

   158

 * Start reading mapping.

   159

 */

   160

   161

void ec_fsm_coe_map_state_start(

   162

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   163

        )

   164

{

   165

    // read mapping of first sync manager

   166

    fsm->sync_index = 0;

   167

    ec_fsm_coe_map_action_next_sync(fsm);

   168

}

   169

   170

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

   171

   172

/**

   173

 * Read mapping of next sync manager.

   174

 */

   175

   176

void ec_fsm_coe_map_action_next_sync(

   177

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   178

        )

   179

{

   180

    ec_slave_t *slave = fsm->slave;

   181

    ec_sdo_entry_t *entry;

   182

   183

    for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) {

   184

        if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index)))

   185

            continue;

   186

   187

        if (slave->master->debug_level)

   188

            EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n",

   189

                    fsm->sync_index, slave->ring_position);

   190

   191

        ec_fsm_coe_map_clear_pdos(fsm);

   192

   193

        if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) {

   194

            EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n",

   195

                    fsm->sync_sdo->index,

   196

                    fsm->slave->ring_position);

   197

            fsm->state = ec_fsm_coe_map_state_error;

   198

            return;

   199

        }

   200

   201

        ec_sdo_request_init_read(&fsm->request, entry);

   202

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   203

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

   204

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

   205

        return;

   206

    }

   207

   208

    fsm->state = ec_fsm_coe_map_state_end;

   209

}

   210

   211

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

   212

   213

/**

   214

 * Count mapped PDOs.

   215

 */

   216

   217

void ec_fsm_coe_map_state_pdo_count(

   218

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   219

        )

   220

{

   221

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

   222

   223

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

   224

        EC_ERR("Failed to read number of mapped PDOs from slave %u.\n",

   225

                fsm->slave->ring_position);

   226

        fsm->state = ec_fsm_coe_map_state_error;

   227

        return;

   228

    }

   229

   230

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

   231

   232

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

   233

        EC_DBG("  %u PDOs mapped.\n", fsm->sync_subindices);

   234

   235

    // read first PDO

   236

    fsm->sync_subindex = 1;

   237

    ec_fsm_coe_map_action_next_pdo(fsm);

   238

}

   239

   240

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

   241

   242

/**

   243

 * Read next PDO.

   244

 */

   245

   246

void ec_fsm_coe_map_action_next_pdo(

   247

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   248

        )

   249

{

   250

    ec_sdo_entry_t *entry;

   251

   252

    if (fsm->sync_subindex <= fsm->sync_subindices) {

   253

        if (!(entry = ec_sdo_get_entry(fsm->sync_sdo,

   254

                        fsm->sync_subindex))) {

   255

            EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n",

   256

                    fsm->sync_sdo->index,

   257

                    fsm->sync_subindex,

   258

                    fsm->slave->ring_position);

   259

            fsm->state = ec_fsm_coe_map_state_error;

   260

            return;

   261

        }

   262

   263

        ec_sdo_request_init_read(&fsm->request, entry);

   264

        fsm->state = ec_fsm_coe_map_state_pdo;

   265

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

   266

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

   267

        return;

   268

    }

   269

   270

    {

   271

        ec_sync_t *sync = fsm->slave->sii_syncs + fsm->sync_index;

   272

        ec_pdo_t *pdo;

   273

   274

        // exchange sync manager PDO mapping

   275

        ec_sync_clear_pdos(sync);

   276

        list_for_each_entry(pdo, &fsm->pdos, list) {

   277

            ec_sync_add_pdo(sync, pdo);

   278

        }

   279

        ec_fsm_coe_map_clear_pdos(fsm);

   280

   281

        // next sync manager

   282

        fsm->sync_index++;

   283

        ec_fsm_coe_map_action_next_sync(fsm);

   284

    }

   285

}

   286

   287

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

   288

   289

/**

   290

 * Fetch PDO information.

   291

 */

   292

   293

void ec_fsm_coe_map_state_pdo(

   294

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   295

        )

   296

{

   297

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

   298

   299

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

   300

        EC_ERR("Failed to read mapped PDO index from slave %u.\n",

   301

                fsm->slave->ring_position);

   302

        fsm->state = ec_fsm_coe_map_state_error;

   303

        return;

   304

    }

   305

   306

    {

   307

        ec_sdo_entry_t *entry;

   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_coe_map_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->type =

   319

            ec_sync_get_pdo_type(fsm->slave->sii_syncs + fsm->sync_index);

   320

   321

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

   322

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

   323

   324

        if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) {

   325

            EC_ERR("Slave %u has no SDO 0x%04X.\n",

   326

                    fsm->slave->ring_position, fsm->pdo->index);

   327

            ec_pdo_clear(fsm->pdo);

   328

            kfree(fsm->pdo);

   329

            fsm->state = ec_fsm_coe_map_state_error;

   330

            return;

   331

        }

   332

   333

        if (fsm->pdo_sdo->name && strlen(fsm->pdo_sdo->name)) {

   334

            if (!(fsm->pdo->name = (char *)

   335

                        kmalloc(strlen(fsm->pdo_sdo->name) + 1, GFP_KERNEL))) {

   336

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

   337

                ec_pdo_clear(fsm->pdo);

   338

                kfree(fsm->pdo);

   339

                fsm->state = ec_fsm_coe_map_state_error;

   340

                return;

   341

            }

   342

            memcpy(fsm->pdo->name, fsm->pdo_sdo->name,

   343

                    strlen(fsm->pdo_sdo->name) + 1);

   344

        } else {

   345

            fsm->pdo->name = NULL;

   346

        }

   347

   348

        if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) {

   349

            EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n",

   350

                    fsm->pdo_sdo->index,

   351

                    fsm->slave->ring_position);

   352

            ec_pdo_clear(fsm->pdo);

   353

            kfree(fsm->pdo);

   354

            fsm->state = ec_fsm_coe_map_state_error;

   355

            return;

   356

        }

   357

   358

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

   359

   360

        ec_sdo_request_init_read(&fsm->request, entry);

   361

        fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   362

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

   363

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

   364

        return;

   365

    }

   366

}

   367

   368

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

   369

   370

/**

   371

 * Read number of PDO entries.

   372

 */

   373

   374

void ec_fsm_coe_map_state_pdo_entry_count(

   375

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   376

        )

   377

{

   378

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

   379

   380

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

   381

        EC_ERR("Failed to read number of mapped PDO entries from slave %u.\n",

   382

                fsm->slave->ring_position);

   383

        fsm->state = ec_fsm_coe_map_state_error;

   384

        return;

   385

    }

   386

   387

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

   388

   389

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

   390

        EC_DBG("    %u PDO entries mapped.\n", fsm->pdo_subindices);

   391

   392

    // read first PDO entry

   393

    fsm->pdo_subindex = 1;

   394

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   395

}

   396

   397

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

   398

   399

/**

   400

 * Read next PDO entry.

   401

 */

   402

   403

void ec_fsm_coe_map_action_next_pdo_entry(

   404

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   405

        )

   406

{

   407

    ec_sdo_entry_t *entry;

   408

   409

    if (fsm->pdo_subindex <= fsm->pdo_subindices) {

   410

        if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo,

   411

                        fsm->pdo_subindex))) {

   412

            EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n",

   413

                    fsm->pdo_sdo->index, fsm->pdo_subindex,

   414

                    fsm->slave->ring_position);

   415

            fsm->state = ec_fsm_coe_map_state_error;

   416

            return;

   417

        }

   418

   419

        ec_sdo_request_init_read(&fsm->request, entry);

   420

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   421

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

   422

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

   423

        return;

   424

    }

   425

   426

    // next PDO

   427

    fsm->sync_subindex++;

   428

    ec_fsm_coe_map_action_next_pdo(fsm);

   429

}

   430

   431

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

   432

   433

/**

   434

 * Read PDO entry information.

   435

 */

   436

   437

void ec_fsm_coe_map_state_pdo_entry(

   438

        ec_fsm_coe_map_t *fsm /**< finite state machine */

   439

        )

   440

{

   441

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

   442

   443

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

   444

        EC_ERR("Failed to read index of mapped PDO entry from slave %u.\n",

   445

                fsm->slave->ring_position);

   446

        fsm->state = ec_fsm_coe_map_state_error;

   447

        return;

   448

    }

   449

   450

    {

   451

        uint32_t pdo_entry_info;

   452

        ec_sdo_t *sdo;

   453

        ec_sdo_entry_t *entry;

   454

        ec_pdo_entry_t *pdo_entry;

   455

   456

        pdo_entry_info = EC_READ_U32(fsm->request.data);

   457

   458

        if (!(pdo_entry = (ec_pdo_entry_t *)

   459

                    kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {

   460

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

   461

            fsm->state = ec_fsm_coe_map_state_error;

   462

            return;

   463

        }

   464

   465

        pdo_entry->index = pdo_entry_info >> 16;

   466

        pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;

   467

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   468

   469

        if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) {

   470

            EC_ERR("Slave %u has no SDO 0x%04X.\n",

   471

                    fsm->slave->ring_position, pdo_entry->index);

   472

            kfree(pdo_entry);

   473

            fsm->state = ec_fsm_coe_map_state_error;

   474

            return;

   475

        }

   476

   477

        if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) {

   478

            EC_ERR("Slave %u has no SDO entry 0x%04X:%u.\n",

   479

                    fsm->slave->ring_position, pdo_entry->index,

   480

                    pdo_entry->subindex);

   481

            kfree(pdo_entry);

   482

            fsm->state = ec_fsm_coe_map_state_error;

   483

            return;

   484

        }

   485

   486

        if (entry->description && strlen(entry->description)) {

   487

            if (!(pdo_entry->name = (char *)

   488

                        kmalloc(strlen(entry->description) + 1, GFP_KERNEL))) {

   489

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

   490

                kfree(pdo_entry);

   491

                fsm->state = ec_fsm_coe_map_state_error;

   492

                return;

   493

            }

   494

            memcpy(pdo_entry->name, entry->description, strlen(entry->description) + 1);

   495

        } else {

   496

            pdo_entry->name = NULL;

   497

        }

   498

   499

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

   500

            EC_DBG("    PDO entry 0x%04X \"%s\" (%u bit).\n",

   501

                    pdo_entry->index, pdo_entry->name ? pdo_entry->name : "???",

   502

                    pdo_entry->bit_length);

   503

        }

   504

   505

        list_add_tail(&pdo_entry->list, &fsm->pdo->entries);

   506

   507

        // next PDO entry

   508

        fsm->pdo_subindex++;

   509

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   510

    }

   511

}

   512

   513

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

   514

   515

/**

   516

   State: ERROR.

   517