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) && fsm->retries--)

   336

        return; // FIXME: request again?

   337

   338

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   339

        fsm->state = ec_fsm_coe_error;

   340

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   341

                " request datagram: ");

   342

        ec_datagram_print_state(datagram);

   343

        return;

   344

    }

   345

   346

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   347

        fsm->state = ec_fsm_coe_error;

   348

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

   349

        ec_datagram_print_wc_error(datagram);

   350

        return;

   351

    }

   352

   353

    fsm->jiffies_start = datagram->jiffies_sent;

   354

   355

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

   356

    fsm->retries = EC_FSM_RETRIES;

   357

    fsm->state = ec_fsm_coe_dict_check;

   358

}

   359

   360

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

   361

   362

/**

   363

   CoE state: DICT CHECK.

   364

*/

   365

   366

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

   367

{

   368

    ec_mailbox_t *mbox = fsm->mbox;

   369

    ec_datagram_t *datagram = mbox->datagram;

   370

    ec_slave_t *slave = fsm->slave;

   371

   372

    if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)

   373

        return;

   374

   375

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   376

        fsm->state = ec_fsm_coe_error;

   377

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

   378

        ec_datagram_print_state(datagram);

   379

        return;

   380

    }

   381

   382

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   383

        fsm->state = ec_fsm_coe_error;

   384

        EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"

   385

                " datagram failed: ");

   386

        ec_datagram_print_wc_error(datagram);

   387

        return;

   388

    }

   389

   390

    if (!ec_slave_mbox_check(mbox)) {

   391

        unsigned long diff_ms =

   392

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

   393

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   394

            fsm->state = ec_fsm_coe_error;

   395

            EC_SLAVE_ERR(slave, "Timeout while waiting for"

   396

                    " SDO dictionary list response.\n");

   397

            return;

   398

        }

   399

   400

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

   401

        fsm->retries = EC_FSM_RETRIES;

   402

        return;

   403

    }

   404

   405

    // Fetch response

   406

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

   407

    fsm->retries = EC_FSM_RETRIES;

   408

    fsm->state = ec_fsm_coe_dict_response;

   409

}

   410

   411

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

   412

   413

/**

   414

   CoE state: DICT RESPONSE.

   415

   \todo Timeout behavior

   416

*/

   417

   418

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

   419

{

   420

    ec_mailbox_t *mbox = fsm->mbox;

   421

    ec_datagram_t *datagram = mbox->datagram;

   422

    ec_slave_t *slave = fsm->slave;

   423

    uint8_t *data, mbox_prot;

   424

    size_t rec_size;

   425

    unsigned int sdo_count, i;

   426

    uint16_t sdo_index, fragments_left;

   427

    ec_sdo_t *sdo;

   428

    bool first_segment;

   429

    size_t index_list_offset;

   430

   431

    if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--)

   432

        return; // FIXME: request again?

   433

   434

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   435

        fsm->state = ec_fsm_coe_error;

   436

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   437

                " response datagram: ");

   438

        ec_datagram_print_state(datagram);

   439

        return;

   440

    }

   441

   442

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   443

        fsm->state = ec_fsm_coe_error;

   444

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

   445

        ec_datagram_print_wc_error(datagram);

   446

        return;

   447

    }

   448

   449

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

   450

    if (IS_ERR(data)) {

   451

        fsm->state = ec_fsm_coe_error;

   452

        return;

   453

    }

   454

   455

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

   456

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

   457

                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, mbox); // 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_SLAVE_ERR(slave, "Received corrupted SDO dictionary response"

   472

                " (size %zu).\n", 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_SLAVE_ERR(slave, "SDO information error response!\n");

   480

        if (rec_size < 10) {

   481

            EC_SLAVE_ERR(slave, "Incomplete SDO information"

   482

                    " error response:\n");

   483

            ec_print_data(data, rec_size);

   484

        } else {

   485

            ec_canopen_abort_msg(slave, 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_SLAVE_DBG(slave, 1, "Invalid SDO list response!"

   495

                    " Retrying...\n");

   496

            ec_print_data(data, rec_size);

   497

        }

   498

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

   499

        fsm->retries = EC_FSM_RETRIES;

   500

        fsm->state = ec_fsm_coe_dict_check;

   501

        return;

   502

    }

   503