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

        sync->mapping_source = EC_SYNC_MAPPING_COE;

   280

        ec_fsm_coe_map_clear_pdos(fsm);

   281

   282

        // next sync manager

   283

        fsm->sync_index++;

   284

        ec_fsm_coe_map_action_next_sync(fsm);

   285

    }

   286

}

   287

   288

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

   289

   290

/**

   291

 * Fetch PDO information.

   292

 */

   293

   294

void ec_fsm_coe_map_state_pdo(

   295

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

   296

        )

   297

{

   298

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

   299

   300

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

   301

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

   302

                fsm->slave->ring_position);

   303

        fsm->state = ec_fsm_coe_map_state_error;

   304

        return;

   305

    }

   306

   307

    {

   308

        ec_sdo_entry_t *entry;

   309

   310

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

   311

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

   312

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

   313

            fsm->state = ec_fsm_coe_map_state_error;

   314

            return;

   315

        }

   316

   317

        ec_pdo_init(fsm->pdo);

   318

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

   319

        fsm->pdo->type =

   320

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

   321

   322

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

   323

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

   324

   325

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

   326

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

   327

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

   328

            ec_pdo_clear(fsm->pdo);

   329

            kfree(fsm->pdo);

   330

            fsm->state = ec_fsm_coe_map_state_error;

   331

            return;

   332

        }

   333

   334

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

   335

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

   336

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

   337

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

   338

                ec_pdo_clear(fsm->pdo);

   339

                kfree(fsm->pdo);

   340

                fsm->state = ec_fsm_coe_map_state_error;

   341

                return;

   342

            }

   343

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

   344

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

   345

        } else {

   346

            fsm->pdo->name = NULL;

   347

        }

   348

   349

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

   350

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

   351

                    fsm->pdo_sdo->index,

   352

                    fsm->slave->ring_position);

   353

            ec_pdo_clear(fsm->pdo);

   354

            kfree(fsm->pdo);

   355

            fsm->state = ec_fsm_coe_map_state_error;

   356

            return;

   357

        }

   358

   359

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

   360

   361

        ec_sdo_request_init_read(&fsm->request, entry);

   362

        fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   363

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

   364

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

   365

        return;

   366

    }

   367

}

   368

   369

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

   370

   371

/**

   372

 * Read number of PDO entries.

   373

 */

   374

   375

void ec_fsm_coe_map_state_pdo_entry_count(

   376

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

   377

        )

   378

{

   379

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

   380

   381

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

   382

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

   383

                fsm->slave->ring_position);

   384

        fsm->state = ec_fsm_coe_map_state_error;

   385

        return;

   386

    }

   387

   388

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

   389

   390

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

   391

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

   392

   393

    // read first PDO entry

   394

    fsm->pdo_subindex = 1;

   395

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   396

}

   397

   398

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

   399

   400

/**

   401

 * Read next PDO entry.

   402

 */

   403

   404

void ec_fsm_coe_map_action_next_pdo_entry(

   405

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

   406

        )

   407

{

   408

    ec_sdo_entry_t *entry;

   409

   410

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

   411

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

   412

                        fsm->pdo_subindex))) {

   413

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

   414

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

   415

                    fsm->slave->ring_position);

   416

            fsm->state = ec_fsm_coe_map_state_error;

   417

            return;

   418

        }

   419

   420

        ec_sdo_request_init_read(&fsm->request, entry);

   421

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   422

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

   423

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

   424

        return;

   425

    }

   426

   427

    // next PDO

   428

    fsm->sync_subindex++;

   429

    ec_fsm_coe_map_action_next_pdo(fsm);

   430

}

   431

   432

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

   433

   434

/**

   435

 * Read PDO entry information.

   436

 */

   437

   438

void ec_fsm_coe_map_state_pdo_entry(

   439

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

   440

        )

   441

{

   442

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

   443

   444

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

   445

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

   446

                fsm->slave->ring_position);

   447

        fsm->state = ec_fsm_coe_map_state_error;

   448

        return;

   449

    }

   450

   451

    {

   452

        uint32_t pdo_entry_info;

   453

        ec_sdo_t *sdo;

   454

        ec_sdo_entry_t *entry;

   455

        ec_pdo_entry_t *pdo_entry;

   456

   457

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

   458

   459

        if (!(pdo_entry = (ec_pdo_entry_t *)

   460

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

   461

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

   462

            fsm->state = ec_fsm_coe_map_state_error;

   463

            return;

   464

        }

   465

   466

        pdo_entry->index = pdo_entry_info >> 16;

   467

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

   468

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   469

   494

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

   495

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

   496

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

   497

            kfree(pdo_entry);

   498

            fsm->state = ec_fsm_coe_map_state_error;

   499

            return;

   500

        }

   501

   502

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

   503

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

   504

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

   505

                    pdo_entry->subindex);

   506

            kfree(pdo_entry);

   507

            fsm->state = ec_fsm_coe_map_state_error;

   508

            return;

   509

        }

   510

   511

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

   512

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

   513

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

   514

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

   515

                kfree(pdo_entry);