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

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

    53

    54

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    55

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    56

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    57

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    58

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    59

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    60

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    61

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    62

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    63

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    64

    65

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    66

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    67

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    68

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    69

void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *);

    70

void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *);

    71

    72

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    73

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    74

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    75

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    76

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    77

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    78

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    79

    80

void ec_fsm_coe_end(ec_fsm_coe_t *);

    81

void ec_fsm_coe_error(ec_fsm_coe_t *);

    82

    83

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

    84

    85

/**

    86

   SDO abort messages.

    87

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

    88

   which can be translated to clear text. This table does

    89

   the mapping of the codes and messages.

    90

*/

    91

    92

const ec_code_msg_t sdo_abort_messages[] = {

    93

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

    94

    {0x05040000, "SDO protocol timeout"},

    95

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

    96

    {0x05040005, "Out of memory"},

    97

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

    98

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

    99

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

   100

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

   101

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

   102

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

   103

     " exceed the PDO length"},

   104

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

   105

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

   106

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

   107

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

   108

     " not match"},

   109

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

   110

     " high"},

   111

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

   112

     " low"},

   113

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

   114

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

   115

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

   116

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

   117

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

   118

    {0x08000000, "General error"},

   119

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

   120

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

   121

     " because of local control"},

   122

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

   123

     " because of the present device state"},

   124

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

   125

     " dictionary is present"},

   126

    {}

   127

};

   128

   129

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

   130

   131

/**

   132

   Outputs an SDO abort message.

   133

*/

   134

   135

void ec_canopen_abort_msg(uint32_t abort_code)

   136

{

   137

    const ec_code_msg_t *abort_msg;

   138

   139

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

   140

        if (abort_msg->code == abort_code) {

   141

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

   142

                   abort_msg->code, abort_msg->message);

   143

            return;

   144

        }

   145

    }

   146

   147

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

   148

}

   149

   150

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

   151

   152

/**

   153

   Constructor.

   154

*/

   155

   156

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

   157

                     ec_datagram_t *datagram /**< datagram */

   158

                     )

   159

{

   160

    fsm->state = NULL;

   161

    fsm->datagram = datagram;

   162

}

   163

   164

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

   165

   166

/**

   167

   Destructor.

   168

*/

   169

   170

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

   171

{

   172

}

   173

   174

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

   175

   176

/**

   177

   Starts reading a slaves' SDO dictionary.

   178

*/

   179

   180

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

   181

                           ec_slave_t *slave /**< EtherCAT slave */

   182

                           )

   183

{

   184

    fsm->slave = slave;

   185

    fsm->state = ec_fsm_coe_dict_start;

   186

}

   187

   188

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

   189

   190

/**

   191

   Starts to transfer an SDO to/from a slave.

   192

*/

   193

   194

void ec_fsm_coe_transfer(

   195

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

   196

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

   197

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

   198

        )

   199

{

   200

    fsm->slave = slave;

   201

    fsm->request = request;

   202

    if (request->dir == EC_DIR_OUTPUT)

   203

        fsm->state = ec_fsm_coe_down_start;

   204

    else

   205

        fsm->state = ec_fsm_coe_up_start;

   206

}

   207

   208

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

   209

   210

/**

   211

   Executes the current state of the state machine.

   212

   \return false, if state machine has terminated

   213

*/

   214

   215

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

   216

{

   217

    fsm->state(fsm);

   218

   219

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

   220

}

   221

   222

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

   223

   224

/**

   225

   Returns, if the state machine terminated with success.

   226

   \return non-zero if successful.

   227

*/

   228

   229

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

   230

{

   231

    return fsm->state == ec_fsm_coe_end;

   232

}

   233

   234

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

   235

   236

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

   237

 *

   238

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

   239

 *

   240

 * \return The data were an emergency request.

   241

 */

   242

int ec_fsm_coe_check_emergency(

   243

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

   244

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

   245

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

   246

        )

   247

{

   248

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

   249

        return 0;

   250

   251

    if (size < 10) {

   252

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

   253

                fsm->slave->ring_position);

   254

        ec_print_data(data, size);

   255

        return 1;

   256

    }

   257

   258

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

   259

            fsm->slave->ring_position);

   260

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

   261

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

   262

    ec_print_data(data + 5, 5);

   263

    return 1;

   264

}

   265

   266

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

   267

 *  CoE dictionary state machine

   268

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

   269

   270

/**

   271

   CoE state: DICT START.

   272

*/

   273

   274

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

   275

{

   276

    ec_datagram_t *datagram = fsm->datagram;

   277

    ec_slave_t *slave = fsm->slave;

   278

    uint8_t *data;

   279

   280

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

   281

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

   282

        fsm->state = ec_fsm_coe_error;

   283

        return;

   284

    }

   285

   286

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

   287

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

   288

                slave->ring_position);

   289

        fsm->state = ec_fsm_coe_error;

   290

        return;

   291

    }

   292

   293

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

   294

    if (IS_ERR(data)) {

   295

        fsm->state = ec_fsm_coe_error;

   296

        return;

   297

    }

   298

   299

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

   300

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

   301

    EC_WRITE_U8 (data + 3, 0x00);

   302

    EC_WRITE_U16(data + 4, 0x0000);

   303

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

   304

   305

    fsm->retries = EC_FSM_RETRIES;

   306

    fsm->state = ec_fsm_coe_dict_request;

   307

}

   308

   309

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

   310

   311

/**

   312

   CoE state: DICT REQUEST.

   313

   \todo Timeout behavior

   314

*/

   315

   316

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

   317

{

   318

    ec_datagram_t *datagram = fsm->datagram;

   319

    ec_slave_t *slave = fsm->slave;

   320

   321

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

   322

        return; // FIXME: request again?

   323

   324

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   325

        fsm->state = ec_fsm_coe_error;

   326

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

   327

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

   328

               slave->ring_position, datagram->state);

   329

        return;

   330

    }

   331

   332

    if (datagram->working_counter != 1) {

   333

        fsm->state = ec_fsm_coe_error;

   334

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

   335

                slave->ring_position);

   336

        ec_datagram_print_wc_error(datagram);

   337

        return;

   338

    }

   339

   340

    fsm->jiffies_start = datagram->jiffies_sent;

   341

   342

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

   343

    fsm->retries = EC_FSM_RETRIES;

   344

    fsm->state = ec_fsm_coe_dict_check;

   345

}

   346

   347

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

   348

   349

/**

   350

   CoE state: DICT CHECK.

   351

*/

   352

   353

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

   354

{

   355

    ec_datagram_t *datagram = fsm->datagram;

   356

    ec_slave_t *slave = fsm->slave;

   357

   358

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

   359

        return;

   360

   361

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   362

        fsm->state = ec_fsm_coe_error;

   363

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

   364

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

   365

               slave->ring_position, datagram->state);

   366

        return;

   367

    }

   368

   369

    if (datagram->working_counter != 1) {

   370

        fsm->state = ec_fsm_coe_error;

   371

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

   372

               slave->ring_position);

   373

        ec_datagram_print_wc_error(datagram);

   374

        return;

   375

    }

   376

   377

    if (!ec_slave_mbox_check(datagram)) {

   378

        unsigned long diff_ms =

   379

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

   380

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   381

            fsm->state = ec_fsm_coe_error;

   382

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

   383

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

   384

            return;

   385

        }

   386

   387

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

   388

        fsm->retries = EC_FSM_RETRIES;

   389

        return;

   390

    }

   391

   392

    // Fetch response

   393

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

   394

    fsm->retries = EC_FSM_RETRIES;

   395

    fsm->state = ec_fsm_coe_dict_response;

   396

}

   397

   398

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

   399

   400

/**

   401

   CoE state: DICT RESPONSE.

   402

   \todo Timeout behavior

   403

*/

   404

   405

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

   406

{

   407

    ec_datagram_t *datagram = fsm->datagram;

   408

    ec_slave_t *slave = fsm->slave;

   409

    uint8_t *data, mbox_prot;

   410

    size_t rec_size;

   411

    unsigned int sdo_count, i;

   412

    uint16_t sdo_index, fragments_left;

   413

    ec_sdo_t *sdo;

   414

   415

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

   416

        return; // FIXME: request again?

   417

   418

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   419

        fsm->state = ec_fsm_coe_error;

   420

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

   421

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

   422

               slave->ring_position, datagram->state);

   423

        return;

   424

    }

   425

   426

    if (datagram->working_counter != 1) {

   427

        fsm->state = ec_fsm_coe_error;

   428

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

   429

                slave->ring_position);

   430

        ec_datagram_print_wc_error(datagram);

   431

        return;

   432

    }

   433

   434

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

   435

    if (IS_ERR(data)) {

   436

        fsm->state = ec_fsm_coe_error;

   437

        return;

   438

    }

   439

   440

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

   441

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

   442

        fsm->state = ec_fsm_coe_error;

   443

        return;

   444

    }

   445

   446

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

   447

        // check for CoE response again

   448

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

   449

        fsm->retries = EC_FSM_RETRIES;

   450

        fsm->state = ec_fsm_coe_dict_check;

   451

        return;

   452

    }

   453

   454

    if (rec_size < 3) {

   455

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

   456

                rec_size);

   457

        fsm->state = ec_fsm_coe_error;

   458

        return;

   459

    }

   460

   461

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

   462

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

   463

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

   464

               slave->ring_position);

   465

        if (rec_size < 10) {

   466

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

   467

            ec_print_data(data, rec_size);

   468

        } else {

   469

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   470

        }

   471

        fsm->state = ec_fsm_coe_error;

   472

        return;

   473

    }

   474

   475

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

   476

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

   477

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

   478

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

   479

                    slave->ring_position);

   480

            ec_print_data(data, rec_size);

   481

        }

   482

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

   483

        fsm->retries = EC_FSM_RETRIES;

   484

        fsm->state = ec_fsm_coe_dict_check;

   485

        return;

   486

    }

   487

   488

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

   489

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

   490

        ec_print_data(data, rec_size);

   491

        fsm->state = ec_fsm_coe_error;

   492

        return;

   493

    }

   494

   495

    sdo_count = (rec_size - 8) / 2;

   496

   497

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

   498

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

   499

        if (!sdo_index) {

   500

            if (slave->master->debug_level)

   501

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

   502

                        slave->ring_position);

   503

            continue;

   504

        }

   505

   506

        if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) {