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 state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "fsm_coe.h"

    45

    46

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

    47

    54

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    55

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    56

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    57

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    58

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    59

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    60

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    61

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    62

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    63

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    64

    65

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    66

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    67

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    68

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    69

    70

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    71

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    72

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    73

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    74

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    75

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    76

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    77

    78

void ec_fsm_coe_end(ec_fsm_coe_t *);

    79

void ec_fsm_coe_error(ec_fsm_coe_t *);

    80

    81

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

    82

    83

/**

    84

   Sdo abort messages.

    85

   The "abort Sdo transfer request" supplies an abort code,

    86

   which can be translated to clear text. This table does

    87

   the mapping of the codes and messages.

    88

*/

    89

    90

const ec_code_msg_t sdo_abort_messages[] = {

    91

    {0x05030000, "Toggle bit not changed"},

    92

    {0x05040000, "Sdo protocol timeout"},

    93

    {0x05040001, "Client/Server command specifier not valid or unknown"},

    94

    {0x05040005, "Out of memory"},

    95

    {0x06010000, "Unsupported access to an object"},

    96

    {0x06010001, "Attempt to read a write-only object"},

    97

    {0x06010002, "Attempt to write a read-only object"},

    98

    {0x06020000, "This object does not exist in the object directory"},

    99

    {0x06040041, "The object cannot be mapped into the Pdo"},

   100

    {0x06040042, "The number and length of the objects to be mapped would"

   101

     " exceed the Pdo length"},

   102

    {0x06040043, "General parameter incompatibility reason"},

   103

    {0x06040047, "Gerneral internal incompatibility in device"},

   104

    {0x06060000, "Access failure due to a hardware error"},

   105

    {0x06070010, "Data type does not match, length of service parameter does"

   106

     " not match"},

   107

    {0x06070012, "Data type does not match, length of service parameter too"

   108

     " high"},

   109

    {0x06070013, "Data type does not match, length of service parameter too"

   110

     " low"},

   111

    {0x06090011, "Subindex does not exist"},

   112

    {0x06090030, "Value range of parameter exceeded"},

   113

    {0x06090031, "Value of parameter written too high"},

   114

    {0x06090032, "Value of parameter written too low"},

   115

    {0x06090036, "Maximum value is less than minimum value"},

   116

    {0x08000000, "General error"},

   117

    {0x08000020, "Data cannot be transferred or stored to the application"},

   118

    {0x08000021, "Data cannot be transferred or stored to the application"

   119

     " because of local control"},

   120

    {0x08000022, "Data cannot be transferred or stored to the application"

   121

     " because of the present device state"},

   122

    {0x08000023, "Object dictionary dynamic generation fails or no object"

   123

     " dictionary is present"},

   124

    {}

   125

};

   126

   127

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

   128

   129

/**

   130

   Outputs an Sdo abort message.

   131

*/

   132

   133

void ec_canopen_abort_msg(uint32_t abort_code)

   134

{

   135

    const ec_code_msg_t *abort_msg;

   136

   137

    for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {

   138

        if (abort_msg->code == abort_code) {

   139

            EC_ERR("Sdo abort message 0x%08X: \"%s\".\n",

   140

                   abort_msg->code, abort_msg->message);

   141

            return;

   142

        }

   143

    }

   144

   145

    EC_ERR("Unknown Sdo abort code 0x%08X.\n", abort_code);

   146

}

   147

   148

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

   149

   150

/**

   151

   Constructor.

   152

*/

   153

   154

void ec_fsm_coe_init(ec_fsm_coe_t *fsm, /**< finite state machine */

   155

                     ec_datagram_t *datagram /**< datagram */

   156

                     )

   157

{

   158

    fsm->state = NULL;

   159

    fsm->datagram = datagram;

   160

}

   161

   162

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

   163

   164

/**

   165

   Destructor.

   166

*/

   167

   168

void ec_fsm_coe_clear(ec_fsm_coe_t *fsm /**< finite state machine */)

   169

{

   170

}

   171

   172

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

   173

   174

/**

   175

   Starts reading a slaves' Sdo dictionary.

   176

*/

   177

   178

void ec_fsm_coe_dictionary(ec_fsm_coe_t *fsm, /**< finite state machine */

   179

                           ec_slave_t *slave /**< EtherCAT slave */

   180

                           )

   181

{

   182

    fsm->slave = slave;

   183

    fsm->state = ec_fsm_coe_dict_start;

   184

}

   185

   186

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

   187

   188

/**

   189

   Starts to transfer an Sdo to/from a slave.

   190

*/

   191

   192

void ec_fsm_coe_transfer(

   193

        ec_fsm_coe_t *fsm, /**< State machine. */

   194

        ec_slave_t *slave, /**< EtherCAT slave. */

   195

        ec_sdo_request_t *request /**< Sdo request. */

   196

        )

   197

{

   198

    fsm->slave = slave;

   199

    fsm->request = request;

   200

    if (request->dir == EC_DIR_OUTPUT)

   201

        fsm->state = ec_fsm_coe_down_start;

   202

    else

   203

        fsm->state = ec_fsm_coe_up_start;

   204

}

   205

   206

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

   207

   208

/**

   209

   Executes the current state of the state machine.

   210

   \return false, if state machine has terminated

   211

*/

   212

   213

int ec_fsm_coe_exec(ec_fsm_coe_t *fsm /**< finite state machine */)

   214

{

   215

    fsm->state(fsm);

   216

   217

    return fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;

   218

}

   219

   220

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

   221

   222

/**

   223

   Returns, if the state machine terminated with success.

   224

   \return non-zero if successful.

   225

*/

   226

   227

int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */)

   228

{

   229

    return fsm->state == ec_fsm_coe_end;

   230

}

   231

   232

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

   233

   234

/**

   235

 */

   236

int ec_fsm_coe_check_emergency(

   237

        ec_fsm_coe_t *fsm, /**< Finite state machine */

   238

        const uint8_t *data, /**< CoE mailbox data. */

   239

        size_t size /**< CoE mailbox data size. */

   240

        )

   241

{

   242

    if (size < 2 || ((EC_READ_U16(data) >> 12) & 0x0F) != 0x01)

   243

        return 0;

   244

   245

    if (size < 10) {

   246

        EC_WARN("Received incomplete CoE Emergency request from slave %u:\n",

   247

                fsm->slave->ring_position);

   248

        ec_print_data(data, size);

   249

        return 1;

   250

    }

   251

   252

    EC_INFO("CoE Emergency Request received from slave %u:\n",

   253

            fsm->slave->ring_position);

   254

    EC_INFO("Error code 0x%04X, Error register 0x%02X, data:\n",

   255

            EC_READ_U16(data + 2), EC_READ_U8(data + 4));

   256

    ec_print_data(data + 5, 5);

   257

    return 1;

   258

}

   259

   260

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

   261

 *  CoE dictionary state machine

   262

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

   263

   264

/**

   265

   CoE state: DICT START.

   266

*/

   267

   268

void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)

   269

{

   270

    ec_datagram_t *datagram = fsm->datagram;

   271

    ec_slave_t *slave = fsm->slave;

   272

    uint8_t *data;

   273

   274

    if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {

   275

        EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);

   276

        fsm->state = ec_fsm_coe_error;

   277

        return;

   278

    }

   279

   280

    if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {

   281

        fsm->state = ec_fsm_coe_error;

   282

        return;

   283

    }

   284

   285

    EC_WRITE_U16(data, 0x8 << 12); // Sdo information

   286

    EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request

   287

    EC_WRITE_U8 (data + 3, 0x00);

   288

    EC_WRITE_U16(data + 4, 0x0000);

   289

    EC_WRITE_U16(data + 6, 0x0001); // deliver all Sdos!

   290

   291

    fsm->retries = EC_FSM_RETRIES;

   292

    fsm->state = ec_fsm_coe_dict_request;

   293

}

   294

   295

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

   296

   297

/**

   298

   CoE state: DICT REQUEST.

   299

   \todo Timeout behavior

   300

*/

   301

   302

void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)

   303

{

   304

    ec_datagram_t *datagram = fsm->datagram;

   305

    ec_slave_t *slave = fsm->slave;

   306

   307

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   308

        return; // FIXME: request again?

   309

   310

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   311

        fsm->state = ec_fsm_coe_error;

   312

        EC_ERR("Failed to receive CoE dictionary request datagram for"

   313

               " slave %u (datagram state %u).\n",

   314

               slave->ring_position, datagram->state);

   315

        return;

   316

    }

   317

   318

    if (datagram->working_counter != 1) {

   319

        fsm->state = ec_fsm_coe_error;

   320

        EC_ERR("Reception of CoE dictionary request failed on slave %u: ",

   321

                slave->ring_position);

   322

        ec_datagram_print_wc_error(datagram);

   323

        return;

   324

    }

   325

   326

    fsm->jiffies_start = datagram->jiffies_sent;

   327

   328

    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   329

    fsm->retries = EC_FSM_RETRIES;

   330

    fsm->state = ec_fsm_coe_dict_check;

   331

}

   332

   333

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

   334

   335

/**

   336

   CoE state: DICT CHECK.

   337

*/

   338

   339

void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)

   340

{

   341

    ec_datagram_t *datagram = fsm->datagram;

   342

    ec_slave_t *slave = fsm->slave;

   343

   344

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   345

        return;

   346

   347

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   348

        fsm->state = ec_fsm_coe_error;

   349

        EC_ERR("Failed to receive CoE mailbox check datagram for slave %u"

   350

                " (datagram state %u).\n",

   351

               slave->ring_position, datagram->state);

   352

        return;

   353

    }

   354

   355

    if (datagram->working_counter != 1) {

   356

        fsm->state = ec_fsm_coe_error;

   357

        EC_ERR("Reception of CoE mailbox check datagram failed on slave %u: ",

   358

               slave->ring_position);

   359

        ec_datagram_print_wc_error(datagram);

   360

        return;

   361

    }

   362

   363

    if (!ec_slave_mbox_check(datagram)) {

   364

        unsigned long diff_ms =

   365

            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   367

            fsm->state = ec_fsm_coe_error;

   368

            EC_ERR("Timeout while checking Sdo dictionary on slave %u.\n",

   369

                   slave->ring_position);

   370

            return;

   371

        }

   372

   373

        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   374

        fsm->retries = EC_FSM_RETRIES;

   375

        return;

   376

    }

   377

   378

    // Fetch response

   379

    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   380

    fsm->retries = EC_FSM_RETRIES;

   381

    fsm->state = ec_fsm_coe_dict_response;

   382

}

   383

   384

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

   385

   386

/**

   387

   CoE state: DICT RESPONSE.

   388

   \todo Timeout behavior

   389

*/

   390

   391

void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)

   392

{

   393

    ec_datagram_t *datagram = fsm->datagram;

   394

    ec_slave_t *slave = fsm->slave;

   395

    uint8_t *data, mbox_prot;

   396

    size_t rec_size;

   397

    unsigned int sdo_count, i;

   398

    uint16_t sdo_index, fragments_left;

   399

    ec_sdo_t *sdo;

   400

   401

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   402

        return; // FIXME: request again?

   403

   404

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   405

        fsm->state = ec_fsm_coe_error;

   406

        EC_ERR("Failed to receive CoE dictionary response datagram for"

   407

               " slave %u (datagram state %u).\n",

   408

               slave->ring_position, datagram->state);

   409

        return;

   410

    }

   411

   412

    if (datagram->working_counter != 1) {

   413

        fsm->state = ec_fsm_coe_error;

   414

        EC_ERR("Reception of CoE dictionary response failed on slave %u: ",

   415

                slave->ring_position);

   416

        ec_datagram_print_wc_error(datagram);

   417

        return;

   418

    }

   419

   420

    if (!(data = ec_slave_mbox_fetch(slave, datagram,

   421

				     &mbox_prot, &rec_size))) {

   422

        fsm->state = ec_fsm_coe_error;

   423

        return;

   424

    }

   425

   426

    if (mbox_prot != 0x03) { // CoE

   427

        EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   428

        fsm->state = ec_fsm_coe_error;

   429

        return;

   430

    }

   431

   432

    if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   433

        // check for CoE response again

   434

        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   435

        fsm->retries = EC_FSM_RETRIES;

   436

        fsm->state = ec_fsm_coe_dict_check;

   437

        return;

   438

    }

   439

   440

    if (rec_size < 3) {

   441

        EC_ERR("Received corrupted Sdo dictionary response (size %u).\n",

   442

                rec_size);

   443

        fsm->state = ec_fsm_coe_error;

   444

        return;

   445

    }

   446

   447

    if (EC_READ_U16(data) >> 12 == 0x8 && // Sdo information

   448

        (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response

   449

        EC_ERR("Sdo information error response at slave %u!\n",

   450

               slave->ring_position);

   451

        if (rec_size < 10) {

   452

            EC_ERR("Incomplete Sdo information error response:\n");

   453

            ec_print_data(data, rec_size);

   454

        } else {

   455

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   456

        }

   457

        fsm->state = ec_fsm_coe_error;

   458

        return;

   459

    }

   460

   461

    if (EC_READ_U16(data) >> 12 != 0x8 || // Sdo information

   462

        (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response

   471

        return;

   472

    }

   473

   474

    if (rec_size < 8 || rec_size % 2) {

   475

        EC_ERR("Invalid data size %u!\n", rec_size);

   476

        ec_print_data(data, rec_size);

   477

        fsm->state = ec_fsm_coe_error;

   478

        return;

   479

    }

   480

   481

    sdo_count = (rec_size - 8) / 2;

   482

   483

    for (i = 0; i < sdo_count; i++) {

   484

        sdo_index = EC_READ_U16(data + 8 + i * 2);

   485

        if (!sdo_index) {

   486

            if (slave->master->debug_level)

   487

                EC_WARN("Sdo dictionary of slave %u contains index 0x0000.\n",

   488

                        slave->ring_position);

   489

            continue;

   490

        }

   491

   492

        if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) {

   493

            EC_ERR("Failed to allocate memory for Sdo!\n");

   494

            fsm->state = ec_fsm_coe_error;

   495

            return;

   496

        }

   497

   498

        ec_sdo_init(sdo, slave, sdo_index);

   499

        list_add_tail(&sdo->list, &slave->sdo_dictionary);

   500

    }

   501

   502

    fragments_left = EC_READ_U16(data + 4);

   503

    if (slave->master->debug_level && fragments_left) {

   504

        EC_DBG("Sdo list fragments left: %u\n", fragments_left);

   505

    }

   506

   507

    if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again.

   508

        fsm->jiffies_start = datagram->jiffies_sent;

   509

        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   510

        fsm->retries = EC_FSM_RETRIES;