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_datagram_t *datagram /**< datagram */

   172

                     )

   173

{

   174

    fsm->state = NULL;

   175

    fsm->datagram = datagram;

   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_datagram_t *datagram = fsm->datagram;

   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, datagram, 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_datagram_t *datagram = fsm->datagram;

   332

    ec_slave_t *slave = fsm->slave;

   333

   334

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

   335

        return; // FIXME: request again?

   336

   337

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   338

        fsm->state = ec_fsm_coe_error;

   339

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   340

                " request datagram: ");

   341

        ec_datagram_print_state(datagram);

   342

        return;

   343

    }

   344

   345

    if (datagram->working_counter != 1) {

   346

        fsm->state = ec_fsm_coe_error;

   347

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

   348

        ec_datagram_print_wc_error(datagram);

   349

        return;

   350

    }

   351

   352

    fsm->jiffies_start = datagram->jiffies_sent;

   353

   354

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

   355

    fsm->retries = EC_FSM_RETRIES;

   356

    fsm->state = ec_fsm_coe_dict_check;

   357

}

   358

   359

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

   360

   361

/**

   362

   CoE state: DICT CHECK.

   363

*/

   364

   365

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

   366

{

   367

    ec_datagram_t *datagram = fsm->datagram;

   368

    ec_slave_t *slave = fsm->slave;

   369

   370

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

   371

        return;

   372

   373

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   374

        fsm->state = ec_fsm_coe_error;

   375

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

   376

        ec_datagram_print_state(datagram);

   377

        return;

   378

    }

   379

   380

    if (datagram->working_counter != 1) {

   381

        fsm->state = ec_fsm_coe_error;

   382

        EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"

   383

                " datagram failed: ");

   384

        ec_datagram_print_wc_error(datagram);

   385

        return;

   386

    }

   387

   388

    if (!ec_slave_mbox_check(datagram)) {

   389

        unsigned long diff_ms =

   390

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

   391

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   392

            fsm->state = ec_fsm_coe_error;

   393

            EC_SLAVE_ERR(slave, "Timeout while waiting for"

   394

                    " SDO dictionary list response.\n");

   395

            return;

   396

        }

   397

   398

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

   399

        fsm->retries = EC_FSM_RETRIES;

   400

        return;

   401

    }

   402

   403

    // Fetch response

   404

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

   405

    fsm->retries = EC_FSM_RETRIES;

   406

    fsm->state = ec_fsm_coe_dict_response;

   407

}

   408

   409

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

   410

   411

/**

   412

   CoE state: DICT RESPONSE.

   413

   \todo Timeout behavior

   414

*/

   415

   416

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

   417

{

   418

    ec_datagram_t *datagram = fsm->datagram;

   419

    ec_slave_t *slave = fsm->slave;

   420

    uint8_t *data, mbox_prot;

   421

    size_t rec_size;

   422

    unsigned int sdo_count, i;

   423

    uint16_t sdo_index, fragments_left;

   424

    ec_sdo_t *sdo;

   425

    bool first_segment;

   426

    size_t index_list_offset;

   427

   428

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

   429

        return; // FIXME: request again?

   430

   431

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   432

        fsm->state = ec_fsm_coe_error;

   433

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   434

                " response datagram: ");

   435

        ec_datagram_print_state(datagram);

   436

        return;

   437

    }

   438

   439

    if (datagram->working_counter != 1) {

   440

        fsm->state = ec_fsm_coe_error;

   441

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

   442

        ec_datagram_print_wc_error(datagram);

   443

        return;

   444

    }

   445

   446

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

   447

    if (IS_ERR(data)) {

   448

        fsm->state = ec_fsm_coe_error;

   449

        return;

   450

    }

   451

   452

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

   453

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

   454

                mbox_prot);

   455

        fsm->state = ec_fsm_coe_error;

   456

        return;

   457

    }

   458

   459

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

   460

        // check for CoE response again

   461

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

   462

        fsm->retries = EC_FSM_RETRIES;

   463

        fsm->state = ec_fsm_coe_dict_check;

   464

        return;

   465

    }

   466

   467

    if (rec_size < 3) {

   468

        EC_SLAVE_ERR(slave, "Received corrupted SDO dictionary response"

   469

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

   470

        fsm->state = ec_fsm_coe_error;

   471

        return;

   472

    }

   473

   474

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

   475

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

   476

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

   477

        if (rec_size < 10) {

   478

            EC_SLAVE_ERR(slave, "Incomplete SDO information"

   479

                    " error response:\n");

   480

            ec_print_data(data, rec_size);

   481

        } else {

   482

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

   483

        }

   484

        fsm->state = ec_fsm_coe_error;

   485

        return;

   486

    }

   487

   488

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

   489

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

   490

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

   491

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

   492

                    " Retrying...\n");

   493

            ec_print_data(data, rec_size);

   494

        }

   495

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

   496

        fsm->retries = EC_FSM_RETRIES;

   497

        fsm->state = ec_fsm_coe_dict_check;

   498

        return;

   499

    }

   500

   501

    first_segment = list_empty(&slave->sdo_dictionary) ? true : false;

   502

    index_list_offset = first_segment ? 8 : 6;

   503

   504

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

   505

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