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

 */

    95

    96

void ec_fsm_coe_map_clear_pdos(

    97

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

    98

        )

    99

{

   100

    ec_pdo_t *pdo, *next;

   101

   102

    // free all PDOs

   103

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

   104

        list_del(&pdo->list);

   105

        ec_pdo_clear(pdo);

   106

        kfree(pdo);

   107

    }

   108

}

   109

   110

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

   111

   112

/**

   113

   Starts to upload an SDO from a slave.

   114

*/

   115

   116

void ec_fsm_coe_map_start(

   117

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

   118

        ec_slave_t *slave /**< EtherCAT slave */

   119

        )

   120

{

   121

    fsm->slave = slave;

   122

    fsm->state = ec_fsm_coe_map_state_start;

   123

}

   124

   125

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

   126

   127

/**

   128

   Executes the current state of the state machine.

   129

   \return false, if state machine has terminated

   130

*/

   131

   132

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

   133

{

   134

    fsm->state(fsm);

   135

   136

    return fsm->state != ec_fsm_coe_map_state_end

   137

        && fsm->state != ec_fsm_coe_map_state_error;

   138

}

   139

   140

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

   141

   142

/**

   143

   Returns, if the state machine terminated with success.

   144

   \return non-zero if successful.

   145

*/

   146

   147

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

   148

{

   149

    return fsm->state == ec_fsm_coe_map_state_end;

   150

}

   151

   152

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

   154

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

   155

   156

/**

   157

 */

   158

   162

{

   163

    // read mapping of first sync manager

   164

    fsm->sync_index = 0;

   165

    ec_fsm_coe_map_action_next_sync(fsm);

   166

}

   167

   168

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

   169

   170

/**

   171

 */

   172

   173

void ec_fsm_coe_map_action_next_sync(

   174

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

   175

        )

   176

{

   177

    ec_slave_t *slave = fsm->slave;

   178

    ec_sdo_entry_t *entry;

   179

   180

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

   181

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

   182

            continue;

   183

   184

        if (slave->master->debug_level)

   185

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

   186

                    fsm->sync_index, slave->ring_position);

   187

   188

        ec_fsm_coe_map_clear_pdos(fsm);

   189

   190

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

   191

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

   192

                    fsm->sync_sdo->index,

   193

                    fsm->slave->ring_position);

   194

            fsm->state = ec_fsm_coe_map_state_error;

   195

            return;

   196

        }

   197

   198

        ec_sdo_request_init_read(&fsm->request, entry);

   199

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   200

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

   201

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

   202

        return;

   203

    }

   204

   205

    fsm->state = ec_fsm_coe_map_state_end;

   206

}

   207

   208

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

   209

   210

/**

   211

 */

   212

   213

void ec_fsm_coe_map_state_pdo_count(

   214

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

   215

        )

   216

{

   217

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

   218

   219

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

   220

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

   221

                fsm->slave->ring_position);

   222

        fsm->state = ec_fsm_coe_map_state_error;

   223

        return;

   224

    }

   225

   226

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

   227

   228

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

   229

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

   230

   231

    // read first PDO

   232

    fsm->sync_subindex = 1;

   233

    ec_fsm_coe_map_action_next_pdo(fsm);

   234

}

   235

   236

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

   237

   238

/**

   239

 */

   240

   241

void ec_fsm_coe_map_action_next_pdo(

   242

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

   243

        )

   244

{

   245

    ec_sdo_entry_t *entry;

   246

   247

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

   248

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

   249

                        fsm->sync_subindex))) {

   250

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

   251

                    fsm->sync_sdo->index,

   252

                    fsm->sync_subindex,

   253

                    fsm->slave->ring_position);

   254

            fsm->state = ec_fsm_coe_map_state_error;

   255

            return;

   256

        }

   257

   258

        ec_sdo_request_init_read(&fsm->request, entry);

   259

        fsm->state = ec_fsm_coe_map_state_pdo;

   260

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

   261

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

   262

        return;

   263

    }

   264

   265

    {

   266

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

   267

        ec_pdo_t *pdo;

   268

   269

        // exchange sync manager PDO mapping

   270

        ec_sync_clear_pdos(sync);

   271

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

   272

            ec_sync_add_pdo(sync, pdo);

   273

        }

   274

        ec_fsm_coe_map_clear_pdos(fsm);

   275

   276

        // next sync manager

   277

        fsm->sync_index++;

   278

        ec_fsm_coe_map_action_next_sync(fsm);

   279

    }

   280

}

   281

   282

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

   283

   284

/**

   285

 */

   286

   287

void ec_fsm_coe_map_state_pdo(

   288

        ec_fsm_coe_map_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 mapped PDO index from slave %u.\n",

   295

                fsm->slave->ring_position);

   296

        fsm->state = ec_fsm_coe_map_state_error;

   297

        return;

   298

    }

   299

   300

    {

   301

        ec_sdo_entry_t *entry;

   302

   303

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

   304

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

   305

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

   306

            fsm->state = ec_fsm_coe_map_state_error;

   307

            return;

   308

        }

   309

   310

        ec_pdo_init(fsm->pdo);

   311

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

   312

        fsm->pdo->type =

   313

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

   314

   315

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

   316

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

   317

   318

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

   319

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

   320

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

   321

            ec_pdo_clear(fsm->pdo);

   322

            kfree(fsm->pdo);

   323

            fsm->state = ec_fsm_coe_map_state_error;

   324

            return;

   325

        }

   326

   327

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

   328

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

   329

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

   330

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

   331

                ec_pdo_clear(fsm->pdo);

   332

                kfree(fsm->pdo);

   333

                fsm->state = ec_fsm_coe_map_state_error;

   334

                return;

   335

            }

   336

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

   337

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

   338

        } else {

   339

            fsm->pdo->name = NULL;

   340

        }

   341

   342

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

   343

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

   344

                    fsm->pdo_sdo->index,

   345

                    fsm->slave->ring_position);

   346

            ec_pdo_clear(fsm->pdo);

   347

            kfree(fsm->pdo);

   348

            fsm->state = ec_fsm_coe_map_state_error;

   349

            return;

   350

        }

   351

   352

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

   353

   354

        ec_sdo_request_init_read(&fsm->request, entry);

   355

        fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   356

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

   357

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

   358

        return;

   359

    }

   360

}

   361

   362

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

   363

   364

/**

   365

 */

   366

   367

void ec_fsm_coe_map_state_pdo_entry_count(

   368

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

   369

        )

   370

{

   371

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

   372

   373

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

   374

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

   375

                fsm->slave->ring_position);

   376

        fsm->state = ec_fsm_coe_map_state_error;

   377

        return;

   378

    }

   379

   380

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

   381

   382

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

   383

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

   384

   385

    // read first PDO entry

   386

    fsm->pdo_subindex = 1;

   387

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   388

}

   389

   390

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

   391

   392

/**

   393

 */

   394

   395

void ec_fsm_coe_map_action_next_pdo_entry(

   396

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

   397

        )

   398

{

   399

    ec_sdo_entry_t *entry;

   400

   401

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

   402

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

   403

                        fsm->pdo_subindex))) {

   404

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

   405

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

   406

                    fsm->slave->ring_position);

   407

            fsm->state = ec_fsm_coe_map_state_error;

   408

            return;

   409

        }

   410

   411

        ec_sdo_request_init_read(&fsm->request, entry);

   412

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   413

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

   414

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

   415

        return;

   416

    }

   417

   418

    // next PDO

   419

    fsm->sync_subindex++;

   420

    ec_fsm_coe_map_action_next_pdo(fsm);

   421

}

   422

   423

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

   424

   425

/**

   426

 */

   427

   428

void ec_fsm_coe_map_state_pdo_entry(

   429

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

   430

        )

   431

{

   432

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

   433

   434

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

   435

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

   436

                fsm->slave->ring_position);

   437

        fsm->state = ec_fsm_coe_map_state_error;

   438

        return;

   439

    }

   440

   441

    {

   442

        uint32_t pdo_entry_info;

   443

        ec_sdo_t *sdo;

   444

        ec_sdo_entry_t *entry;

   445

        ec_pdo_entry_t *pdo_entry;

   446

   447

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

   448

   449

        if (!(pdo_entry = (ec_pdo_entry_t *)

   450

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

   451

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

   452

            fsm->state = ec_fsm_coe_map_state_error;

   453

            return;

   454

        }

   455

   456

        pdo_entry->index = pdo_entry_info >> 16;

   457

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

   458

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   459

   460

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

   461

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

   462

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

   463

            kfree(pdo_entry);

   464

            fsm->state = ec_fsm_coe_map_state_error;

   465

            return;

   466

        }

   467

   468

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

   469

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

   470

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

   471

                    pdo_entry->subindex);

   472

            kfree(pdo_entry);

   473

            fsm->state = ec_fsm_coe_map_state_error;

   474

            return;

   475

        }

   476

   477

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

   478

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

   479

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

   480

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

   481

                kfree(pdo_entry);

   482

                fsm->state = ec_fsm_coe_map_state_error;

   483

                return;

   484

            }

   485

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

   486

        } else {

   487

            pdo_entry->name = NULL;

   488

        }

   489

   490

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

   491

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

   492

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

   493

                    pdo_entry->bit_length);

   494

        }

   495

   496

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

   497

   498

        // next PDO entry

   499

        fsm->pdo_subindex++;

   500

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   501

    }

   502

}

   503

   504

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

   505

   506

/**

   507

   State: ERROR.

   508

*/

   509

   510

void ec_fsm_coe_map_state_error(

   511

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

   512

        )

   513

{

   514

}

   515

   516

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

   517

   518

/**

   519