1

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

     2

 *

     3

 *  $Id$

     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

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

    63

    64

/**

    65

   Constructor.

    66

*/

    67

    68

void ec_fsm_coe_map_init(

    69

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

    70

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

    71

        )

    72

{

    73

    fsm->state = NULL;

    74

    fsm->fsm_coe = fsm_coe;

    75

    ec_pdo_mapping_init(&fsm->mapping);

    76

}

    77

    78

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

    79

    80

/**

    81

   Destructor.

    82

*/

    83

    84

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

    85

{

    86

    ec_pdo_mapping_clear(&fsm->mapping);

    87

}

    88

    89

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

    90

    91

/**

    92

   Starts to upload an SDO from a slave.

    93

*/

    94

    95

void ec_fsm_coe_map_start(

    96

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

    97

        ec_slave_t *slave /**< EtherCAT slave */

    98

        )

    99

{

   100

    fsm->slave = slave;

   101

    fsm->state = ec_fsm_coe_map_state_start;

   102

}

   103

   104

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

   105

   106

/**

   107

   Executes the current state of the state machine.

   108

   \return false, if state machine has terminated

   109

*/

   110

   111

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

   112

{

   113

    fsm->state(fsm);

   114

   115

    return fsm->state != ec_fsm_coe_map_state_end

   116

        && fsm->state != ec_fsm_coe_map_state_error;

   117

}

   118

   119

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

   120

   121

/**

   122

   Returns, if the state machine terminated with success.

   123

   \return non-zero if successful.

   124

*/

   125

   126

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

   127

{

   128

    return fsm->state == ec_fsm_coe_map_state_end;

   129

}

   130

   131

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

   132

 *  state functions

   133

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

   134

   135

/**

   136

 * Start reading mapping.

   137

 */

   138

   139

void ec_fsm_coe_map_state_start(

   140

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

   141

        )

   142

{

   143

    // read mapping of first sync manager

   144

    fsm->sync_index = 0;

   145

    ec_fsm_coe_map_action_next_sync(fsm);

   146

}

   147

   148

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

   149

   150

/**

   151

 * Read mapping of next sync manager.

   152

 */

   153

   154

void ec_fsm_coe_map_action_next_sync(

   155

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

   156

        )

   157

{

   158

    ec_slave_t *slave = fsm->slave;

   159

    ec_sdo_entry_t *entry;

   160

   161

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

   162

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

   163

            continue;

   164

   165

        if (slave->master->debug_level)

   166

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

   167

                    fsm->sync_index, slave->ring_position);

   168

   169

        ec_pdo_mapping_clear_pdos(&fsm->mapping);

   170

   171

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

   172

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

   173

                    fsm->sync_sdo->index,

   174

                    fsm->slave->ring_position);

   175

            fsm->state = ec_fsm_coe_map_state_error;

   176

            return;

   177

        }

   178

   179

        ec_sdo_request_init_read(&fsm->request, entry);

   180

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   181

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

   182

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

   183

        return;

   184

    }

   185

   186

    fsm->state = ec_fsm_coe_map_state_end;

   187

}

   188

   189

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

   190

   191

/**

   192

 * Count mapped PDOs.

   193

 */

   194

   195

void ec_fsm_coe_map_state_pdo_count(

   196

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

   197

        )

   198

{

   199

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

   200

   201

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

   202

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

   203

                fsm->slave->ring_position);

   204

        fsm->state = ec_fsm_coe_map_state_error;

   205

        return;

   206

    }

   207

   208

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

   209

   210

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

   211

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

   212

   213

    // read first PDO

   214

    fsm->sync_subindex = 1;

   215

    ec_fsm_coe_map_action_next_pdo(fsm);

   216

}

   217

   218

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

   219

   220

/**

   221

 * Read next PDO.

   222

 */

   223

   224

void ec_fsm_coe_map_action_next_pdo(

   225

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

   226

        )

   227

{

   228

    ec_sdo_entry_t *entry;

   229

   230

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

   231

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

   232

                        fsm->sync_subindex))) {

   233

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

   234

                    fsm->sync_sdo->index,

   235

                    fsm->sync_subindex,

   236

                    fsm->slave->ring_position);

   237

            fsm->state = ec_fsm_coe_map_state_error;

   238

            return;

   239

        }

   240

   241

        ec_sdo_request_init_read(&fsm->request, entry);

   242

        fsm->state = ec_fsm_coe_map_state_pdo;

   243

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

   244

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

   245

        return;

   246

    }

   247

   248

    {

   249

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

   250

   251

        if (ec_pdo_mapping_copy(&sync->mapping, &fsm->mapping)) {

   252

            fsm->state = ec_fsm_coe_map_state_error;

   253

            return;

   254

        }

   255

   256

        sync->mapping_source = EC_SYNC_MAPPING_COE;

   257

        ec_pdo_mapping_clear_pdos(&fsm->mapping);

   258

   259

        // next sync manager

   260

        fsm->sync_index++;

   261

        ec_fsm_coe_map_action_next_sync(fsm);

   262

    }

   263

}

   264

   265

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

   266

   267

/**

   268

 * Fetch PDO information.

   269

 */

   270

   271

void ec_fsm_coe_map_state_pdo(

   272

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

   273

        )

   274

{

   275

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

   276

   277

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

   278

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

   279

                fsm->slave->ring_position);

   280

        fsm->state = ec_fsm_coe_map_state_error;

   281

        return;

   282

    }

   283

   284

    {

   285

        ec_sdo_entry_t *entry;

   286

   287

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

   288

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

   289

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

   290

            fsm->state = ec_fsm_coe_map_state_error;

   291

            return;

   292

        }

   293

   294

        ec_pdo_init(fsm->pdo);

   295

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

   296

        fsm->pdo->dir =

   297

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

   298

   299

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

   300

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

   301

   302

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

   303

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

   304

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

   305

            ec_pdo_clear(fsm->pdo);

   306

            kfree(fsm->pdo);

   307

            fsm->state = ec_fsm_coe_map_state_error;

   308

            return;

   309

        }

   310

   311

        if (ec_pdo_set_name(fsm->pdo, fsm->pdo_sdo->name)) {

   312

            ec_pdo_clear(fsm->pdo);

   313

            kfree(fsm->pdo);

   314

            fsm->state = ec_fsm_coe_map_state_error;

   315

            return;

   316

        }

   317

   318

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

   319

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

   320

                    fsm->pdo_sdo->index,

   321

                    fsm->slave->ring_position);

   322

            ec_pdo_clear(fsm->pdo);

   323

            kfree(fsm->pdo);

   324

            fsm->state = ec_fsm_coe_map_state_error;

   325

            return;

   326

        }

   327

   328

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

   329

   330

        ec_sdo_request_init_read(&fsm->request, entry);

   331

        fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   332

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

   333

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

   334

        return;

   335

    }

   336

}

   337

   338

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

   339

   340

/**

   341

 * Read number of PDO entries.

   342

 */

   343

   344

void ec_fsm_coe_map_state_pdo_entry_count(

   345

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

   346

        )

   347

{

   348

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

   349

   350

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

   351

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

   352

                fsm->slave->ring_position);

   353

        fsm->state = ec_fsm_coe_map_state_error;

   354

        return;

   355

    }

   356

   357

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

   358

   359

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

   360

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

   361

   362

    // read first PDO entry

   363

    fsm->pdo_subindex = 1;

   364

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   365

}

   366

   367

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

   368

   369

/**

   370

 * Read next PDO entry.

   371

 */

   372

   373

void ec_fsm_coe_map_action_next_pdo_entry(

   374

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

   375

        )

   376

{

   377

    ec_sdo_entry_t *entry;

   378

   379

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

   380

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

   381

                        fsm->pdo_subindex))) {

   382

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

   383

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

   384

                    fsm->slave->ring_position);

   385

            fsm->state = ec_fsm_coe_map_state_error;

   386

            return;

   387

        }

   388

   389

        ec_sdo_request_init_read(&fsm->request, entry);

   390

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   391

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

   392

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

   393

        return;

   394

    }

   395

   396

    // next PDO

   397

    fsm->sync_subindex++;

   398

    ec_fsm_coe_map_action_next_pdo(fsm);

   399

}

   400

   401

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

   402

   403

/**

   404

 * Read PDO entry information.

   405

 */

   406

   407

void ec_fsm_coe_map_state_pdo_entry(

   408

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

   409

        )

   410

{

   411

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

   412

   413

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

   414

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

   415

                fsm->slave->ring_position);

   416

        fsm->state = ec_fsm_coe_map_state_error;

   417

        return;

   418

    }

   419

   420

    {

   421

        uint32_t pdo_entry_info;

   422

        ec_sdo_t *sdo;

   423

        ec_sdo_entry_t *entry;

   424

        ec_pdo_entry_t *pdo_entry;

   425

   426

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

   427

   428

        if (!(pdo_entry = (ec_pdo_entry_t *)

   429

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

   430

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

   431

            fsm->state = ec_fsm_coe_map_state_error;

   432

            return;

   433

        }

   434

   435

        ec_pdo_entry_init(pdo_entry);

   436

        pdo_entry->index = pdo_entry_info >> 16;

   437

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

   438

        pdo_entry->bit_length = pdo_entry_info & 0xFF;

   439

   440

        if (!pdo_entry->index && !pdo_entry->subindex) {

   441

            // we have a gap in the PDO, next PDO entry

   442

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

   443

                EC_DBG("    PDO entry gap: %u bit.\n",

   444

                        pdo_entry->bit_length);

   445

            }

   446

   447

            if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {

   448

                ec_pdo_entry_clear(pdo_entry);

   449

                kfree(pdo_entry);

   450

                fsm->state = ec_fsm_coe_map_state_error;

   451

                return;

   452

            }

   453

   454

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

   455

            fsm->pdo_subindex++;

   456

            ec_fsm_coe_map_action_next_pdo_entry(fsm);

   457

            return;

   458

        }

   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

            ec_pdo_entry_clear(pdo_entry);

   464

            kfree(pdo_entry);

   465

            fsm->state = ec_fsm_coe_map_state_error;

   466

            return;

   467

        }

   468

   469

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

   470

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

   471

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

   472

                    pdo_entry->subindex);

   473

            ec_pdo_entry_clear(pdo_entry);

   474

            kfree(pdo_entry);

   475

            fsm->state = ec_fsm_coe_map_state_error;

   476

            return;

   477

        }

   478

   479

        if (ec_pdo_entry_set_name(pdo_entry, entry->description)) {

   480

            ec_pdo_entry_clear(pdo_entry);

   481

            kfree(pdo_entry);

   482

            fsm->state = ec_fsm_coe_map_state_error;

   483

            return;

   484

        }

   485

   486

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

   487

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

   488

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

   489

                    pdo_entry->bit_length);

   490

        }

   491

   492

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

   493

   494

        // next PDO entry

   495

        fsm->pdo_subindex++;

   496

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   497

    }

   498

}

   499

   500

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

   501

   502

/**

   503

   State: ERROR.

   504

*/

   505

   506

void ec_fsm_coe_map_state_error(

   507

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

   508

        )

   509

{

   510

}

   511

   512

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

   513

   514

/**

   515

   State: END.

   516

*/