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 1000

    47

    48

/** CoE download request header size.

    49

 */

    50

#define EC_COE_DOWN_REQ_HEADER_SIZE 10

    51

    52

/** CoE download segment request header size.

    53

 */

    54

#define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3

    55

    56

/** Minimum size of download segment.

    57

 */

    58

#define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7

    59

    60

/** Enable debug output for CoE retries.

    61

 */

    62

#define DEBUG_RETRIES 0

    63

    64

/** Enable warning output if transfers take too long.

    65

 */

    66

#define DEBUG_LONG 0

    67

    68

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

    69

    70

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    71

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    72

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    73

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    74

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    75

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    76

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    77

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    78

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    79

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    80

    81

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    82

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    83

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    84

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    85

void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);

    86

void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);

    87

    88

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    89

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    90

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    91

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    92

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    93

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    94

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    95

    96

void ec_fsm_coe_end(ec_fsm_coe_t *);

    97

void ec_fsm_coe_error(ec_fsm_coe_t *);

    98

    99

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

   100

   101

/**

   102

   SDO abort messages.

   103

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

   104

   which can be translated to clear text. This table does

   105

   the mapping of the codes and messages.

   106

*/

   107

   108

const ec_code_msg_t sdo_abort_messages[] = {

   109

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

   110

    {0x05040000, "SDO protocol timeout"},

   111

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

   112

    {0x05040005, "Out of memory"},

   113

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

   114

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

   115

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

   116

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

   117

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

   118

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

   119

     " exceed the PDO length"},

   120

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

   121

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

   122

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

   123

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

   124

     " not match"},

   125

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

   126

     " high"},

   127

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

   128

     " low"},

   129

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

   130

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

   131

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

   132

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

   133

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

   134

    {0x08000000, "General error"},

   135

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

   136

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

   137

     " because of local control"},

   138

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

   139

     " because of the present device state"},

   140

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

   141

     " dictionary is present"},

   142

    {}

   143

};

   144

   145

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

   146

   147

/**

   148

   Outputs an SDO abort message.

   149

*/

   150

   151

void ec_canopen_abort_msg(uint32_t abort_code)

   152

{

   153

    const ec_code_msg_t *abort_msg;

   154

   155

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

   156

        if (abort_msg->code == abort_code) {

   157

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

   158

                   abort_msg->code, abort_msg->message);

   159

            return;

   160

        }

   161

    }

   162

   163

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

   164

}

   165

   166

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

   167

   168

/**

   169

   Constructor.

   170

*/

   171

   172

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

   173

                     ec_datagram_t *datagram /**< datagram */

   174

                     )

   175

{

   176

    fsm->state = NULL;

   177

    fsm->datagram = datagram;

   178

}

   179

   180

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

   181

   182

/**

   183

   Destructor.

   184

*/

   185

   186

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

   187

{

   188

}

   189

   190

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

   191

   192

/**

   193

   Starts reading a slaves' SDO dictionary.

   194

*/

   195

   196

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

   197

                           ec_slave_t *slave /**< EtherCAT slave */

   198

                           )

   199

{

   200

    fsm->slave = slave;

   201

    fsm->state = ec_fsm_coe_dict_start;

   202

}

   203

   204

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

   205

   206

/**

   207

   Starts to transfer an SDO to/from a slave.

   208

*/

   209

   210

void ec_fsm_coe_transfer(

   211

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

   212

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

   213

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

   214

        )

   215

{

   216

    fsm->slave = slave;

   217

    fsm->request = request;

   218

    if (request->dir == EC_DIR_OUTPUT)

   219

        fsm->state = ec_fsm_coe_down_start;

   220

    else

   221

        fsm->state = ec_fsm_coe_up_start;

   222

}

   223

   224

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

   225

   226

/**

   227

   Executes the current state of the state machine.

   228

   \return false, if state machine has terminated

   229

*/

   230

   231

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

   232

{

   233

    fsm->state(fsm);

   234

   235

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

   236

}

   237

   238

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

   239

   240

/**

   241

   Returns, if the state machine terminated with success.

   242

   \return non-zero if successful.

   243

*/

   244

   245

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

   246

{

   247

    return fsm->state == ec_fsm_coe_end;

   248

}

   249

   250

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

   251

   252

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

   253

 *

   254

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

   255

 *

   256

 * \return The data were an emergency request.

   257

 */

   258

int ec_fsm_coe_check_emergency(

   259

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

   260

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

   261

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

   262

        )

   263

{

   264

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

   265

        return 0;

   266

   267

    if (size < 10) {

   268

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

   269

                fsm->slave->ring_position);

   270

        ec_print_data(data, size);

   271

        return 1;

   272

    }

   273

   274

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

   275

            fsm->slave->ring_position);

   276

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

   277

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

   278

    ec_print_data(data + 5, 5);

   279

    return 1;

   280

}

   281

   282

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

   283

 *  CoE dictionary state machine

   284

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

   285

   286

/**

   287

   CoE state: DICT START.

   288

*/

   289

   290

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

   291

{

   292

    ec_datagram_t *datagram = fsm->datagram;

   293

    ec_slave_t *slave = fsm->slave;

   294

    uint8_t *data;

   295

   296

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

   297

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

   298

        fsm->state = ec_fsm_coe_error;

   299

        return;

   300

    }

   301

   302

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

   303

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

   304

                slave->ring_position);

   305

        fsm->state = ec_fsm_coe_error;

   306

        return;

   307

    }

   308

   309

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

   310

    if (IS_ERR(data)) {

   311

        fsm->state = ec_fsm_coe_error;

   312

        return;

   313

    }

   314

   315

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

   316

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

   317

    EC_WRITE_U8 (data + 3, 0x00);

   318

    EC_WRITE_U16(data + 4, 0x0000);

   319

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

   320

   321

    fsm->retries = EC_FSM_RETRIES;

   322

    fsm->state = ec_fsm_coe_dict_request;

   323

}

   324

   325

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

   326

   327

/**

   328

   CoE state: DICT REQUEST.

   329

   \todo Timeout behavior

   330

*/

   331

   332

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

   333

{

   334

    ec_datagram_t *datagram = fsm->datagram;

   335

    ec_slave_t *slave = fsm->slave;

   336

   337

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

   338

        return; // FIXME: request again?

   339

   340

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   341

        fsm->state = ec_fsm_coe_error;

   342

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

   343

               " slave %u: ", slave->ring_position);

   344

        ec_datagram_print_state(datagram);

   345

        return;

   346

    }

   347

   348

    if (datagram->working_counter != 1) {

   349

        fsm->state = ec_fsm_coe_error;

   350

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

   351

                slave->ring_position);

   352

        ec_datagram_print_wc_error(datagram);

   353

        return;

   354

    }

   355

   356

    fsm->jiffies_start = datagram->jiffies_sent;

   357

   358

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

   359

    fsm->retries = EC_FSM_RETRIES;

   360

    fsm->state = ec_fsm_coe_dict_check;

   361

}

   362

   363

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

   364

   365

/**

   366

   CoE state: DICT CHECK.

   367

*/

   368

   369

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

   370

{

   371

    ec_datagram_t *datagram = fsm->datagram;

   372

    ec_slave_t *slave = fsm->slave;

   373

   374

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

   375

        return;

   376

   377

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   378

        fsm->state = ec_fsm_coe_error;

   379

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

   380

               slave->ring_position);

   381

        ec_datagram_print_state(datagram);

   382

        return;

   383

    }

   384

   385

    if (datagram->working_counter != 1) {

   386

        fsm->state = ec_fsm_coe_error;

   387

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

   388

               slave->ring_position);

   389

        ec_datagram_print_wc_error(datagram);

   390

        return;

   391

    }

   392

   393

    if (!ec_slave_mbox_check(datagram)) {

   394

        unsigned long diff_ms =

   395

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

   396

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   397

            fsm->state = ec_fsm_coe_error;

   398

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

   399

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

   400

            return;

   401

        }

   402

   403

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

   404

        fsm->retries = EC_FSM_RETRIES;

   405

        return;

   406

    }

   407

   408

    // Fetch response

   409

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

   410

    fsm->retries = EC_FSM_RETRIES;

   411

    fsm->state = ec_fsm_coe_dict_response;

   412

}

   413

   414

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

   415

   416

/**

   417

   CoE state: DICT RESPONSE.

   418

   \todo Timeout behavior

   419

*/

   420

   421

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

   422

{

   423

    ec_datagram_t *datagram = fsm->datagram;

   424

    ec_slave_t *slave = fsm->slave;

   425

    uint8_t *data, mbox_prot;

   426

    size_t rec_size;

   427

    unsigned int sdo_count, i;

   428

    uint16_t sdo_index, fragments_left;

   429

    ec_sdo_t *sdo;

   430

   431

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

   432

        return; // FIXME: request again?

   433

   434

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   435

        fsm->state = ec_fsm_coe_error;

   436

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

   437

               " slave %u: ", slave->ring_position);

   438

        ec_datagram_print_state(datagram);

   439

        return;

   440

    }

   441

   442

    if (datagram->working_counter != 1) {

   443

        fsm->state = ec_fsm_coe_error;

   444

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

   445

                slave->ring_position);

   446

        ec_datagram_print_wc_error(datagram);

   447

        return;

   448

    }

   449

   450

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

   451

    if (IS_ERR(data)) {

   452

        fsm->state = ec_fsm_coe_error;

   453

        return;

   454

    }

   455

   456

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

   457

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

   458

        fsm->state = ec_fsm_coe_error;

   459

        return;

   460

    }

   461

   462

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

   463

        // check for CoE response again

   464

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

   465

        fsm->retries = EC_FSM_RETRIES;

   466

        fsm->state = ec_fsm_coe_dict_check;

   467

        return;

   468

    }

   469

   470

    if (rec_size < 3) {

   471

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

   472

                rec_size);

   473

        fsm->state = ec_fsm_coe_error;

   474

        return;

   475

    }

   476

   477

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

   478

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

   479

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

   480

               slave->ring_position);

   481

        if (rec_size < 10) {

   482

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

   483

            ec_print_data(data, rec_size);

   484

        } else {

   485

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   486

        }

   487

        fsm->state = ec_fsm_coe_error;

   488

        return;

   489

    }

   490

   491

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

   492

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

   493

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

   494

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

   495

                    slave->ring_position);

   496

            ec_print_data(data, rec_size);

   497

        }

   498

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

   499

        fsm->retries = EC_FSM_RETRIES;

   500

        fsm->state = ec_fsm_coe_dict_check;

   501

        return;

   502

    }

   503

   504

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

   505

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

   506

        ec_print_data(data, rec_size);