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

/** Outputs an SDO abort message.

   148

 */

   149

void ec_canopen_abort_msg(const ec_slave_t *slave, uint32_t abort_code)

   150

{

   151

    const ec_code_msg_t *abort_msg;

   152

   153

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

   154

        if (abort_msg->code == abort_code) {

   155

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

   156

                   abort_msg->code, abort_msg->message);

   157

            return;

   158

        }

   159

    }

   160

   161

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

   162

}

   163

   164

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

   165

   166

/**

   167

   Constructor.

   168

*/

   169

   170

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

   171

                     ec_mailbox_t *mbox /**< mailbox */

   172

                     )

   173

{

   174

    fsm->state = NULL;

   175

    fsm->mbox = mbox;

   176

}

   177

   178

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

   179

   180

/**

   181

   Destructor.

   182

*/

   183

   184

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

   185

{

   186

}

   187

   188

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

   189

   190

/**

   191

   Starts reading a slaves' SDO dictionary.

   192

*/

   193

   194

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

   195

                           ec_slave_t *slave /**< EtherCAT slave */

   196

                           )

   197

{

   198

    fsm->slave = slave;

   199

    fsm->state = ec_fsm_coe_dict_start;

   200

}

   201

   202

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

   203

   204

/**

   205

   Starts to transfer an SDO to/from a slave.

   206

*/

   207

   208

void ec_fsm_coe_transfer(

   209

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

   210

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

   211

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

   212

        )

   213

{

   214

    fsm->slave = slave;

   215

    fsm->request = request;

   216

    if (request->dir == EC_DIR_OUTPUT)

   217

        fsm->state = ec_fsm_coe_down_start;

   218

    else

   219

        fsm->state = ec_fsm_coe_up_start;

   220

}

   221

   222

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

   223

   224

/**

   225

   Executes the current state of the state machine.

   226

   \return false, if state machine has terminated

   227

*/

   228

   229

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

   230

{

   231

    fsm->state(fsm);

   232

   233

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

   234

}

   235

   236

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

   237

   238

/**

   239

   Returns, if the state machine terminated with success.

   240

   \return non-zero if successful.

   241

*/

   242

   243

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

   244

{

   245

    return fsm->state == ec_fsm_coe_end;

   246

}

   247

   248

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

   249

   250

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

   251

 *

   252

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

   253

 *

   254

 * \return The data were an emergency request.

   255

 */

   256

int ec_fsm_coe_check_emergency(

   257

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

   258

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

   259

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

   260

        )

   261

{

   262

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

   263

        return 0;

   264

   265

    if (size < 10) {

   266

        EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency"

   267

                " request:\n");

   268

        ec_print_data(data, size);

   269

        return 1;

   270

    }

   271

   272

    EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n"

   273

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

   274

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

   275

    ec_print_data(data + 5, 5);

   276

    return 1;

   277

}

   278

   279

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

   280

 *  CoE dictionary state machine

   281

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

   282

   283

/**

   284

   CoE state: DICT START.

   285

*/

   286

   287

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

   288

{

   289

    ec_mailbox_t *mbox = fsm->mbox;

   290

    ec_slave_t *slave = fsm->slave;

   291

    uint8_t *data;

   292

   293

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

   294

        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");

   295

        fsm->state = ec_fsm_coe_error;

   296

        return;

   297

    }

   298

   299

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

   300

        EC_SLAVE_ERR(slave, "Slave does not support"

   301

                " SDO information service!\n");

   302

        fsm->state = ec_fsm_coe_error;

   303

        return;

   304

    }

   305

   306

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

   307

    if (IS_ERR(data)) {

   308

        fsm->state = ec_fsm_coe_error;

   309

        return;

   310

    }

   311

   312

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

   313

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

   314

    EC_WRITE_U8 (data + 3, 0x00);

   315

    EC_WRITE_U16(data + 4, 0x0000);

   316

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

   317

   318

    fsm->retries = EC_FSM_RETRIES;

   319

    fsm->state = ec_fsm_coe_dict_request;

   320

}

   321

   322

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

   323

   324

/**

   325

   CoE state: DICT REQUEST.

   326

   \todo Timeout behavior

   327

*/

   328

   329

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

   330

{

   331

    ec_mailbox_t *mbox = fsm->mbox;

   332

    ec_datagram_t *datagram = mbox->datagram;

   333

    ec_slave_t *slave = fsm->slave;

   334

   335

    if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT)

   336

            && fsm->retries--) {

   337

        return; // FIXME: request again?

   338

    }

   339

   340

    if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {

   341

        fsm->state = ec_fsm_coe_error;

   342

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   343

                " request datagram: ");

   344

        ec_datagram_print_state(datagram);

   345

        return;

   346

    }

   347

   348

    if (!ec_mbox_is_datagram_wc(mbox, 1)) {

   349

        fsm->state = ec_fsm_coe_error;

   350

        EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");

   351

        ec_datagram_print_wc_error(datagram);

   352

        return;

   353

    }

   354

   355

    fsm->jiffies_start = datagram->jiffies_sent;

   356

   357

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

   358

    fsm->retries = EC_FSM_RETRIES;

   359

    fsm->state = ec_fsm_coe_dict_check;

   360

}

   361

   362

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

   363

   364

/**

   365

   CoE state: DICT CHECK.

   366

*/

   367

   368

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

   369

{

   370

    ec_mailbox_t *mbox = fsm->mbox;

   371

    ec_datagram_t *datagram = mbox->datagram;

   372

    ec_slave_t *slave = fsm->slave;

   373

   374

    if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT)

   375

            && fsm->retries--) {

   376

        return;

   377

    }

   378

   379

    if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {

   380

        fsm->state = ec_fsm_coe_error;

   381

        EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   382

        ec_datagram_print_state(datagram);

   383

        return;

   384

    }

   385

   386

    if (!ec_mbox_is_datagram_wc(mbox, 1)) {

   387

        fsm->state = ec_fsm_coe_error;

   388

        EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

   389

                " datagram failed: ");

   390

        ec_datagram_print_wc_error(datagram);

   391

        return;

   392

    }

   393

   394

    if (!ec_slave_mbox_check(mbox)) {

   395

        unsigned long diff_ms =

   396

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

   397

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   398

            fsm->state = ec_fsm_coe_error;

   399

            EC_SLAVE_ERR(slave, "Timeout while waiting for"

   400

                    " SDO dictionary list response.\n");

   401

            return;

   402

        }

   403

   404

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

   405

        fsm->retries = EC_FSM_RETRIES;

   406

        return;

   407

    }

   408

   409

    // Fetch response

   410

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

   411

    fsm->retries = EC_FSM_RETRIES;

   412

    fsm->state = ec_fsm_coe_dict_response;

   413

}

   414

   415

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

   416

   417

/**

   418

   CoE state: DICT RESPONSE.

   419

   \todo Timeout behavior

   420

*/

   421

   422

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

   423

{

   424

    ec_mailbox_t *mbox = fsm->mbox;

   425

    ec_datagram_t *datagram = mbox->datagram;

   426

    ec_slave_t *slave = fsm->slave;

   427

    uint8_t *data, mbox_prot;

   428

    size_t rec_size;

   429

    unsigned int sdo_count, i;

   430

    uint16_t sdo_index, fragments_left;

   431

    ec_sdo_t *sdo;

   432

    bool first_segment;

   433

    size_t index_list_offset;

   434

   435

    if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT)

   436

            && fsm->retries--) {

   437

        return; // FIXME: request again?

   438

    }

   439

   440

    if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) {

   441

        fsm->state = ec_fsm_coe_error;

   442

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   443

                " response datagram: ");

   444

        ec_datagram_print_state(datagram);

   445

        return;

   446

    }

   447

   448

    if (!ec_mbox_is_datagram_wc(mbox, 1)) {

   449

        fsm->state = ec_fsm_coe_error;

   450

        EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");

   451

        ec_datagram_print_wc_error(datagram);

   452

        return;

   453

    }

   454

   455

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

   456

    if (IS_ERR(data)) {

   457

        fsm->state = ec_fsm_coe_error;

   458

        return;

   459

    }

   460

   461

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

   462

        EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",

   463

                mbox_prot);

   464

        fsm->state = ec_fsm_coe_error;

   465

        return;

   466

    }

   467

   468

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

   469

        // check for CoE response again

   470

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

   471

        fsm->retries = EC_FSM_RETRIES;

   472

        fsm->state = ec_fsm_coe_dict_check;

   473

        return;

   474

    }

   475

   476

    if (rec_size < 3) {

   477

        EC_SLAVE_ERR(slave, "Received corrupted SDO dictionary response"

   478

                " (size %zu).\n", rec_size);

   479

        fsm->state = ec_fsm_coe_error;

   480

        return;

   481

    }

   482

   483

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

   484

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

   485

        EC_SLAVE_ERR(slave, "SDO information error response!\n");

   486

        if (rec_size < 10) {

   487

            EC_SLAVE_ERR(slave, "Incomplete SDO information"

   488

                    " error response:\n");

   489

            ec_print_data(data, rec_size);

   490

        } else {

   491

            ec_canopen_abort_msg(slave, EC_READ_U32(data + 6));

   492

        }

   493

        fsm->state = ec_fsm_coe_error;

   494

        return;

   495

    }

   496

   497

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

   498

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

   499

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

   500

            EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!"

   501

                    " Retrying...\n");

   502

            ec_print_data(data, rec_size);

   503

        }