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

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

    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

}

    76

    77

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

    78

    79

/**

    80

   Destructor.

    81

*/

    82

    83

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

    84

{

    85

}

    86

    87

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

    88

    89

/**

    90

   Starts to upload an SDO from a slave.

    91

*/

    92

    93

void ec_fsm_coe_map_start(

    94

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

    95

        ec_slave_t *slave /**< EtherCAT slave */

    96

        )

    97

{

    98

    fsm->slave = slave;

    99

    fsm->state = ec_fsm_coe_map_state_start;

   100

}

   101

   102

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

   103

   104

/**

   105

   Executes the current state of the state machine.

   106

   \return false, if state machine has terminated

   107

*/

   108

   109

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

   110

{

   111

    fsm->state(fsm);

   112

   113

    return fsm->state != ec_fsm_coe_map_state_end

   114

        && fsm->state != ec_fsm_coe_map_state_error;

   115

}

   116

   117

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

   118

   119

/**

   120

   Returns, if the state machine terminated with success.

   121

   \return non-zero if successful.

   122

*/

   123

   124

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

   125

{

   126

    return fsm->state == ec_fsm_coe_map_state_end;

   127

}

   128

   129

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

   130

 *  CoE dictionary state machine

   131

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

   132

   133

/**

   134

 */

   135

   136

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

   137

{

   138

    // read mapping of first sync manager

   139

    fsm->sync_index = 0;

   140

    ec_fsm_coe_map_action_next_sync(fsm);

   141

}

   142

   143

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

   144

   145

/**

   146

 */

   147

   148

void ec_fsm_coe_map_action_next_sync(

   149

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

   150

        )

   151

{

   152

    ec_slave_t *slave = fsm->slave;

   153

    ec_sdo_entry_t *entry;

   154

   155

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

   156

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

   157

            continue;

   158

   159

        if (slave->master->debug_level)

   160

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

   161

                    fsm->sync_index, slave->ring_position);

   162

   163

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

   164

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

   165

                    fsm->sync_sdo->index,

   166

                    fsm->slave->ring_position);

   167

            fsm->state = ec_fsm_coe_map_state_error;

   168

            return;

   169

        }

   170

   171

        ec_sdo_request_init_read(&fsm->request, entry);

   172

        fsm->state = ec_fsm_coe_map_state_pdo_count;

   173

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

   174

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

   175

        return;

   176

    }

   177

   178

    fsm->state = ec_fsm_coe_map_state_end;

   179

}

   180

   181

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

   182

   183

/**

   184

 */

   185

   186

void ec_fsm_coe_map_state_pdo_count(

   187

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

   188

        )

   189

{

   190

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

   191

   192

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

   193

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

   194

                fsm->slave->ring_position);

   195

        fsm->state = ec_fsm_coe_map_state_error;

   196

        return;

   197

    }

   198

   199

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

   200

   201

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

   202

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

   203

   204

    // read first PDO

   205

    fsm->sync_subindex = 1;

   206

    ec_fsm_coe_map_action_next_pdo(fsm);

   207

}

   208

   209

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

   210

   211

/**

   212

 */

   213

   214

void ec_fsm_coe_map_action_next_pdo(

   215

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

   216

        )

   217

{

   218

    ec_sdo_entry_t *entry;

   219

   220

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

   221

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

   222

                        fsm->sync_subindex))) {

   223

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

   224

                    fsm->sync_sdo->index,

   225

                    fsm->sync_subindex,

   226

                    fsm->slave->ring_position);

   227

            fsm->state = ec_fsm_coe_map_state_error;

   228

            return;

   229

        }

   230

   231

        ec_sdo_request_init_read(&fsm->request, entry);

   232

        fsm->state = ec_fsm_coe_map_state_pdo;

   233

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

   234

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

   235

        return;

   236

    }

   237

   238

    // next sync manager

   239

    fsm->sync_index++;

   240

    ec_fsm_coe_map_action_next_sync(fsm);

   241

}

   242

   243

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

   244

   245

/**

   246

 */

   247

   248

void ec_fsm_coe_map_state_pdo(

   249

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

   250

        )

   251

{

   252

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

   253

   254

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

   255

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

   256

                fsm->slave->ring_position);

   257

        fsm->state = ec_fsm_coe_map_state_error;

   258

        return;

   259

    }

   260

   261

    {

   262

        uint16_t pdo_index = EC_READ_U16(fsm->request.data);

   263

        ec_sdo_entry_t *entry;

   264

   265

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

   266

            EC_DBG("  PDO 0x%04X.\n", pdo_index);

   267

   268

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

   269

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

   270

                    fsm->slave->ring_position, pdo_index);

   271

            fsm->state = ec_fsm_coe_map_state_error;

   272

            return;

   273

        }

   274

   275

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

   276

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

   277

                    fsm->pdo_sdo->index,

   278

                    fsm->slave->ring_position);

   279

            fsm->state = ec_fsm_coe_map_state_error;

   280

            return;

   281

        }

   282

   283

        ec_sdo_request_init_read(&fsm->request, entry);

   284

        fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   285

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

   286

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

   287

        return;

   288

    }

   289

}

   290

   291

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

   292

   293

/**

   294

 */

   295

   296

void ec_fsm_coe_map_state_pdo_entry_count(

   297

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

   298

        )

   299

{

   300

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

   301

   302

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

   303

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

   304

                fsm->slave->ring_position);

   305

        fsm->state = ec_fsm_coe_map_state_error;

   306

        return;

   307

    }

   308

   309

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

   310

   311

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

   312

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

   313

   314

    // read first PDO entry

   315

    fsm->pdo_subindex = 1;

   316

    ec_fsm_coe_map_action_next_pdo_entry(fsm);

   317

}

   318

   319

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

   320

   321

/**

   322

 */

   323

   324

void ec_fsm_coe_map_action_next_pdo_entry(

   325

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

   326

        )

   327

{

   328

    ec_sdo_entry_t *entry;

   329

   330

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

   331

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

   332

                        fsm->pdo_subindex))) {

   333

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

   334

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

   335

                    fsm->slave->ring_position);

   336

            fsm->state = ec_fsm_coe_map_state_error;

   337

            return;

   338

        }

   339

   340

        ec_sdo_request_init_read(&fsm->request, entry);

   341

        fsm->state = ec_fsm_coe_map_state_pdo_entry;

   342

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

   343

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

   344

        return;

   345

    }

   346

   347

    // next PDO

   348

    fsm->sync_subindex++;

   349

    ec_fsm_coe_map_action_next_pdo(fsm);

   350

}

   351

   352

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

   353

   354

/**

   355

 */

   356

   357

void ec_fsm_coe_map_state_pdo_entry(

   358

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

   359

        )

   360

{

   361

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

   362

   363

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

   364

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

   365

                fsm->slave->ring_position);

   366

        fsm->state = ec_fsm_coe_map_state_error;

   367

        return;

   368

    }

   369

   370

    {

   371

        uint32_t pdo_entry_info;

   372

        uint16_t pdo_entry_index;

   373

        ec_sdo_t *sdo;

   374

   375

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

   376

        pdo_entry_index = pdo_entry_info >> 16;

   377

   378

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

   379

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

   380

                    fsm->slave->ring_position, pdo_entry_index);

   381

            fsm->state = ec_fsm_coe_map_state_error;

   382

            return;

   383

        }

   384

   385

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

   386

            size_t bitsize = pdo_entry_info & 0xFFFF;

   387

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

   388

                    pdo_entry_index, sdo->name, bitsize);

   389

        }

   390

   391

        // next PDO entry

   392

        fsm->pdo_subindex++;

   393

        ec_fsm_coe_map_action_next_pdo_entry(fsm);

   394

    }

   395

}

   396

   397

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

   398

   399

/**

   400

   State: ERROR.

   401

*/

   402

   403

void ec_fsm_coe_map_state_error(

   404

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

   405

        )

   406

{

   407

}

   408

   409

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

   410

   411

/**

   412

   State: END.

   413

*/

   414

   415

void ec_fsm_coe_map_state_end(

   416

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

   417

        )

   418

{

   419

}

   420

   421

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