1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   EtherCAT CoE state machines.

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "fsm_coe.h"

    41

    42

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

    43

    44

/** Maximum time in ms to wait for responses when reading out the dictionary.

    45

 */

    46

#define EC_FSM_COE_DICT_TIMEOUT 3000

    47

    48

#define EC_COE_DOWN_REQ_HEADER_SIZE      10

    49

#define EC_COE_DOWN_SEG_REQ_HEADER_SIZE  3

    50

#define EC_COE_DOWN_SEG_MIN_DATA_SIZE    7

    51

    52

#define DEBUG_RETRIES 0

    54

    55

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

    56

    57

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    58

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    59

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    60

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    61

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    62

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    63

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    64

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    65

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    66

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    67

    68

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    69

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    70

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    71

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    72

void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);

    73

void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);

    74

    75

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    76

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    77

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    78

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    79

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    80

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    81

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    82

    83

void ec_fsm_coe_end(ec_fsm_coe_t *);

    84

void ec_fsm_coe_error(ec_fsm_coe_t *);

    85

    86

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

    87

    88

/**

    89

   SDO abort messages.

    90

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

    91

   which can be translated to clear text. This table does

    92

   the mapping of the codes and messages.

    93

*/

    94

    95

const ec_code_msg_t sdo_abort_messages[] = {

    96

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

    97

    {0x05040000, "SDO protocol timeout"},

    98

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

    99

    {0x05040005, "Out of memory"},

   100

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

   101

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

   102

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

   103

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

   104

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

   105

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

   106

     " exceed the PDO length"},

   107

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

   108

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

   109

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

   110

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

   111

     " not match"},

   112

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

   113

     " high"},

   114

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

   115

     " low"},

   116

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

   117

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

   118

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

   119

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

   120

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

   121

    {0x08000000, "General error"},

   122

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

   123

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

   124

     " because of local control"},

   125

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

   126

     " because of the present device state"},

   127

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

   128

     " dictionary is present"},

   129

    {}

   130

};

   131

   132

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

   133

   134

/**

   135

   Outputs an SDO abort message.

   136

*/

   137

   138

void ec_canopen_abort_msg(uint32_t abort_code)

   139

{

   140

    const ec_code_msg_t *abort_msg;

   141

   142

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

   143

        if (abort_msg->code == abort_code) {

   144

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

   145

                   abort_msg->code, abort_msg->message);

   146

            return;

   147

        }

   148

    }

   149

   150

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

   151

}

   152

   153

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

   154

   155

/**

   156

   Constructor.

   157

*/

   158

   159

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

   160

                     ec_datagram_t *datagram /**< datagram */

   161

                     )

   162

{

   163

    fsm->state = NULL;

   164

    fsm->datagram = datagram;

   165

}

   166

   167

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

   168

   169

/**

   170

   Destructor.

   171

*/

   172

   173

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

   174

{

   175

}

   176

   177

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

   178

   179

/**

   180

   Starts reading a slaves' SDO dictionary.

   181

*/

   182

   183

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

   184

                           ec_slave_t *slave /**< EtherCAT slave */

   185

                           )

   186

{

   187

    fsm->slave = slave;

   188

    fsm->state = ec_fsm_coe_dict_start;

   189

}

   190

   191

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

   192

   193

/**

   194

   Starts to transfer an SDO to/from a slave.

   195

*/

   196

   197

void ec_fsm_coe_transfer(

   198

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

   199

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

   200

        ec_sdo_request_t *request /**< SDO request. */

   201

        )

   202

{

   203

    fsm->slave = slave;

   204

    fsm->request = request;

   205

    if (request->dir == EC_DIR_OUTPUT)

   206

        fsm->state = ec_fsm_coe_down_start;

   207

    else

   208

        fsm->state = ec_fsm_coe_up_start;

   209

}

   210

   211

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

   212

   213

/**

   214

   Executes the current state of the state machine.

   215

   \return false, if state machine has terminated

   216

*/

   217

   218

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

   219

{

   220

    fsm->state(fsm);

   221

   222

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

   223

}

   224

   225

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

   226

   227

/**

   228

   Returns, if the state machine terminated with success.

   229

   \return non-zero if successful.

   230

*/

   231

   232

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

   233

{

   234

    return fsm->state == ec_fsm_coe_end;

   235

}

   236

   237

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

   238

   239

/** Check if the received data are a CoE emergency request.

   240

 *

   241

 * If the check is positive, the emergency request is output.

   242

 *

   243

 * \return The data were an emergency request.

   244

 */

   245

int ec_fsm_coe_check_emergency(

   246

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

   247

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

   248

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

   249

        )

   250

{

   251

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

   252

        return 0;

   253

   254

    if (size < 10) {

   255

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

   256

                fsm->slave->ring_position);

   257

        ec_print_data(data, size);

   258

        return 1;

   259

    }

   260

   261

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

   262

            fsm->slave->ring_position);

   263

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

   264

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

   265

    ec_print_data(data + 5, 5);

   266

    return 1;

   267

}

   268

   269

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

   270

 *  CoE dictionary state machine

   271

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

   272

   273

/**

   274

   CoE state: DICT START.

   275

*/

   276

   277

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

   278

{

   279

    ec_datagram_t *datagram = fsm->datagram;

   280

    ec_slave_t *slave = fsm->slave;

   281

    uint8_t *data;

   282

   283

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

   284

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

   285

        fsm->state = ec_fsm_coe_error;

   286

        return;

   287

    }

   288

   289

    if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) {

   290

        EC_ERR("Slave %u does not support SDO information service!\n",

   291

                slave->ring_position);

   292

        fsm->state = ec_fsm_coe_error;

   293

        return;

   294

    }

   295

   296

    data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8);

   297

    if (IS_ERR(data)) {

   298

        fsm->state = ec_fsm_coe_error;

   299

        return;

   300

    }

   301

   302

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

   303

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

   304

    EC_WRITE_U8 (data + 3, 0x00);

   305

    EC_WRITE_U16(data + 4, 0x0000);

   306

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

   307

   308

    fsm->retries = EC_FSM_RETRIES;

   309

    fsm->state = ec_fsm_coe_dict_request;

   310

}

   311

   312

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

   313

   314

/**

   315

   CoE state: DICT REQUEST.

   316

   \todo Timeout behavior

   317

*/

   318

   319

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

   320

{

   321

    ec_datagram_t *datagram = fsm->datagram;

   322

    ec_slave_t *slave = fsm->slave;

   323

   324

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

   325

        return; // FIXME: request again?

   326

   327

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   328

        fsm->state = ec_fsm_coe_error;

   329

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

   330

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

   331

               slave->ring_position, datagram->state);

   332

        return;

   333

    }

   334

   335

    if (datagram->working_counter != 1) {

   336

        fsm->state = ec_fsm_coe_error;

   337

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

   338

                slave->ring_position);

   339

        ec_datagram_print_wc_error(datagram);

   340

        return;

   341

    }

   342

   343

    fsm->jiffies_start = datagram->jiffies_sent;

   344

   345

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

   346

    fsm->retries = EC_FSM_RETRIES;

   347

    fsm->state = ec_fsm_coe_dict_check;

   348

}

   349

   350

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

   351

   352

/**

   353

   CoE state: DICT CHECK.

   354

*/

   355

   356

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

   357

{

   358

    ec_datagram_t *datagram = fsm->datagram;

   359

    ec_slave_t *slave = fsm->slave;

   360

   361

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

   362

        return;

   363

   364

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   365

        fsm->state = ec_fsm_coe_error;

   366

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

   367

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

   368

               slave->ring_position, datagram->state);

   369

        return;

   370

    }

   371

   372

    if (datagram->working_counter != 1) {

   373

        fsm->state = ec_fsm_coe_error;

   374

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

   375

               slave->ring_position);

   376

        ec_datagram_print_wc_error(datagram);

   377

        return;

   378

    }

   379

   380

    if (!ec_slave_mbox_check(datagram)) {

   381

        unsigned long diff_ms =

   382

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

   383

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   384

            fsm->state = ec_fsm_coe_error;

   385

            EC_ERR("Timeout while waiting for SDO dictionary list response "

   386

                    "on slave %u.\n", slave->ring_position);

   387

            return;

   388

        }

   389

   390

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

   391

        fsm->retries = EC_FSM_RETRIES;

   392

        return;

   393

    }

   394

   395

    // Fetch response

   396

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

   397

    fsm->retries = EC_FSM_RETRIES;

   398

    fsm->state = ec_fsm_coe_dict_response;

   399

}

   400

   401

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

   402

   403

/**

   404

   CoE state: DICT RESPONSE.

   405

   \todo Timeout behavior

   406

*/

   407

   408

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

   409

{

   410

    ec_datagram_t *datagram = fsm->datagram;

   411

    ec_slave_t *slave = fsm->slave;

   412

    uint8_t *data, mbox_prot;

   413

    size_t rec_size;

   414

    unsigned int sdo_count, i;

   415

    uint16_t sdo_index, fragments_left;

   416

    ec_sdo_t *sdo;

   417

   418

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

   419

        return; // FIXME: request again?

   420

   421

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   422

        fsm->state = ec_fsm_coe_error;

   423

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

   424

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

   425

               slave->ring_position, datagram->state);

   426

        return;

   427

    }

   428

   429

    if (datagram->working_counter != 1) {

   430

        fsm->state = ec_fsm_coe_error;

   431

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

   432

                slave->ring_position);

   433

        ec_datagram_print_wc_error(datagram);

   434

        return;

   435

    }

   436

   437

    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);

   438

    if (IS_ERR(data)) {

   439

        fsm->state = ec_fsm_coe_error;

   440

        return;

   441

    }

   442

   443

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

   444

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

   445

        fsm->state = ec_fsm_coe_error;

   446

        return;

   447

    }

   448

   449

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

   450

        // check for CoE response again

   451

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

   452

        fsm->retries = EC_FSM_RETRIES;

   453

        fsm->state = ec_fsm_coe_dict_check;

   454

        return;

   455

    }

   456

   457

    if (rec_size < 3) {

   458

        EC_ERR("Received corrupted SDO dictionary response (size %zu).\n",

   459

                rec_size);

   460

        fsm->state = ec_fsm_coe_error;

   461

        return;

   462

    }

   463

   464

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

   465

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

   466

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

   467

               slave->ring_position);

   468

        if (rec_size < 10) {

   469

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

   470

            ec_print_data(data, rec_size);

   471

        } else {

   472

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   473

        }

   474

        fsm->state = ec_fsm_coe_error;

   475

        return;

   476

    }

   477

   478

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

   479

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

   480

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

   481

            EC_DBG("Invalid SDO list response at slave %u! Retrying...\n",

   482

                    slave->ring_position);

   483

            ec_print_data(data, rec_size);

   484

        }

   485

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

   486

        fsm->retries = EC_FSM_RETRIES;

   487

        fsm->state = ec_fsm_coe_dict_check;

   488

        return;

   489

    }

   490

   491

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

   492

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

   493

        ec_print_data(data, rec_size);

   494

        fsm->state = ec_fsm_coe_error;

   495

        return;

   496

    }

   497

   498

    sdo_count = (rec_size - 8) / 2;

   499

   500

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

   501

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

   502

        if (!sdo_index) {

   503

            if (slave->master->debug_level)

   504

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

   505

                        slave->ring_position);

   506

            continue;