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

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

    49

    50

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    51

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    52

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    53

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    54

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    55

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    56

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    57

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    58

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    59

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    60

    61

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    62

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    63

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    64

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    65

    66

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    67

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    68

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    69

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    70

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    71

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    72

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    73

    74

void ec_fsm_coe_end(ec_fsm_coe_t *);

    75

void ec_fsm_coe_error(ec_fsm_coe_t *);

    76

    77

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

    78

    79

/**

    80

   SDO abort messages.

    81

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

    82

   which can be translated to clear text. This table does

    83

   the mapping of the codes and messages.

    84

*/

    85

    86

const ec_code_msg_t sdo_abort_messages[] = {

    87

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

    88

    {0x05040000, "SDO protocol timeout"},

    89

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

    90

    {0x05040005, "Out of memory"},

    91

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

    92

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

    93

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

    94

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

    95

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

    96

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

    97

     " exceed the PDO length"},

    98

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

    99

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

   100

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

   101

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

   102

     " not match"},

   103

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

   104

     " high"},

   105

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

   106

     " low"},

   107

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

   108

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

   109

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

   110

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

   111

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

   112

    {0x08000000, "General error"},

   113

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

   114

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

   115

     " because of local control"},

   116

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

   117

     " because of the present device state"},

   118

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

   119

     " dictionary is present"},

   120

    {}

   121

};

   122

   123

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

   124

   125

/**

   126

   Outputs an SDO abort message.

   127

*/

   128

   129

void ec_canopen_abort_msg(uint32_t abort_code)

   130

{

   131

    const ec_code_msg_t *abort_msg;

   132

   133

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

   134

        if (abort_msg->code == abort_code) {

   135

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

   136

                   abort_msg->code, abort_msg->message);

   137

            return;

   138

        }

   139

    }

   140

   141

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

   142

}

   143

   144

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

   145

   146

/**

   147

   Constructor.

   148

*/

   149

   150

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

   151

                     ec_datagram_t *datagram /**< datagram */

   152

                     )

   153

{

   154

    fsm->state = NULL;

   155

    fsm->datagram = datagram;

   156

}

   157

   158

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

   159

   160

/**

   161

   Destructor.

   162

*/

   163

   164

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

   165

{

   166

}

   167

   168

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

   169

   170

/**

   171

   Starts reading a slaves' SDO dictionary.

   172

*/

   173

   174

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

   175

                           ec_slave_t *slave /**< EtherCAT slave */

   176

                           )

   177

{

   178

    fsm->slave = slave;

   179

    fsm->state = ec_fsm_coe_dict_start;

   180

}

   181

   182

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

   183

   184

/**

   185

   Starts to transfer an SDO to/from a slave.

   186

*/

   187

   188

void ec_fsm_coe_transfer(

   189

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

   190

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

   191

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

   192

        )

   193

{

   194

    fsm->slave = slave;

   195

    fsm->request = request;

   196

    if (request->dir == EC_DIR_OUTPUT)

   197

        fsm->state = ec_fsm_coe_down_start;

   198

    else

   199

        fsm->state = ec_fsm_coe_up_start;

   200

}

   201

   202

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

   203

   204

/**

   205

   Executes the current state of the state machine.

   206

   \return false, if state machine has terminated

   207

*/

   208

   209

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

   210

{

   211

    fsm->state(fsm);

   212

   213

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

   214

}

   215

   216

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

   217

   218

/**

   219

   Returns, if the state machine terminated with success.

   220

   \return non-zero if successful.

   221

*/

   222

   223

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

   224

{

   225

    return fsm->state == ec_fsm_coe_end;

   226

}

   227

   228

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

   229

   230

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

   231

 *

   232

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

   233

 *

   234

 * \return The data were an emergency request.

   235

 */

   236

int ec_fsm_coe_check_emergency(

   237

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

   238

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

   239

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

   240

        )

   241

{

   242

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

   243

        return 0;

   244

   245

    if (size < 10) {

   246

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

   247

                fsm->slave->ring_position);

   248

        ec_print_data(data, size);

   249

        return 1;

   250

    }

   251

   252

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

   253

            fsm->slave->ring_position);

   254

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

   255

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

   256

    ec_print_data(data + 5, 5);

   257

    return 1;

   258

}

   259

   260

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

   261

 *  CoE dictionary state machine

   262

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

   263

   264

/**

   265

   CoE state: DICT START.

   266

*/

   267

   268

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

   269

{

   270

    ec_datagram_t *datagram = fsm->datagram;

   271

    ec_slave_t *slave = fsm->slave;

   272

    uint8_t *data;

   273

   274

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

   275

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

   276

        fsm->state = ec_fsm_coe_error;

   277

        return;

   278

    }

   279

   280

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

   281

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

   282

                slave->ring_position);

   283

        fsm->state = ec_fsm_coe_error;

   284

        return;

   285

    }

   286

   287

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

   288

    if (IS_ERR(data)) {

   289

        fsm->state = ec_fsm_coe_error;

   290

        return;

   291

    }

   292

   293

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

   294

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

   295

    EC_WRITE_U8 (data + 3, 0x00);

   296

    EC_WRITE_U16(data + 4, 0x0000);

   297

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

   298

   299

    fsm->retries = EC_FSM_RETRIES;

   300

    fsm->state = ec_fsm_coe_dict_request;

   301

}

   302

   303

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

   304

   305

/**

   306

   CoE state: DICT REQUEST.

   307

   \todo Timeout behavior

   308

*/

   309

   310

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

   311

{

   312

    ec_datagram_t *datagram = fsm->datagram;

   313

    ec_slave_t *slave = fsm->slave;

   314

   315

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

   316

        return; // FIXME: request again?

   317

   318

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   319

        fsm->state = ec_fsm_coe_error;

   320

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

   321

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

   322

               slave->ring_position, datagram->state);

   323

        return;

   324

    }

   325

   326

    if (datagram->working_counter != 1) {

   327

        fsm->state = ec_fsm_coe_error;

   328

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

   329

                slave->ring_position);

   330

        ec_datagram_print_wc_error(datagram);

   331

        return;

   332

    }

   333

   334

    fsm->jiffies_start = datagram->jiffies_sent;

   335

   336

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

   337

    fsm->retries = EC_FSM_RETRIES;

   338

    fsm->state = ec_fsm_coe_dict_check;

   339

}

   340

   341

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

   342

   343

/**

   344

   CoE state: DICT CHECK.

   345

*/

   346

   347

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

   348

{

   349

    ec_datagram_t *datagram = fsm->datagram;

   350

    ec_slave_t *slave = fsm->slave;

   351

   352

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

   353

        return;

   354

   355

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   356

        fsm->state = ec_fsm_coe_error;

   357

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

   358

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

   359

               slave->ring_position, datagram->state);

   360

        return;

   361

    }

   362

   363

    if (datagram->working_counter != 1) {

   364

        fsm->state = ec_fsm_coe_error;

   365

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

   366

               slave->ring_position);

   367

        ec_datagram_print_wc_error(datagram);

   368

        return;

   369

    }

   370

   371

    if (!ec_slave_mbox_check(datagram)) {

   372

        unsigned long diff_ms =

   373

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

   374

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   375

            fsm->state = ec_fsm_coe_error;

   376

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

   377

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

   378

            return;

   379

        }

   380

   381

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

   382

        fsm->retries = EC_FSM_RETRIES;

   383

        return;

   384

    }

   385

   386

    // Fetch response

   387

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

   388

    fsm->retries = EC_FSM_RETRIES;

   389

    fsm->state = ec_fsm_coe_dict_response;

   390

}

   391

   392

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

   393

   394

/**

   395

   CoE state: DICT RESPONSE.

   396

   \todo Timeout behavior

   397

*/

   398

   399

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

   400

{

   401

    ec_datagram_t *datagram = fsm->datagram;

   402

    ec_slave_t *slave = fsm->slave;

   403

    uint8_t *data, mbox_prot;

   404

    size_t rec_size;

   405

    unsigned int sdo_count, i;

   406

    uint16_t sdo_index, fragments_left;

   407

    ec_sdo_t *sdo;

   408

   409

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

   410

        return; // FIXME: request again?

   411

   412

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   413

        fsm->state = ec_fsm_coe_error;

   414

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

   415

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

   416

               slave->ring_position, datagram->state);

   417

        return;

   418

    }

   419

   420

    if (datagram->working_counter != 1) {

   421

        fsm->state = ec_fsm_coe_error;

   422

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

   423

                slave->ring_position);

   424

        ec_datagram_print_wc_error(datagram);

   425

        return;

   426

    }

   427

   428

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

   429

    if (IS_ERR(data)) {

   430

        fsm->state = ec_fsm_coe_error;

   431

        return;

   432

    }

   433

   434

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

   435

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

   436

        fsm->state = ec_fsm_coe_error;

   437

        return;

   438

    }

   439

   440

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

   441

        // check for CoE response again

   442

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

   443

        fsm->retries = EC_FSM_RETRIES;

   444

        fsm->state = ec_fsm_coe_dict_check;

   445

        return;

   446

    }

   447

   448

    if (rec_size < 3) {

   449

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

   450

                rec_size);

   451

        fsm->state = ec_fsm_coe_error;

   452

        return;

   453

    }

   454

   455

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

   456

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

   457

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

   458

               slave->ring_position);

   459

        if (rec_size < 10) {

   460

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

   461

            ec_print_data(data, rec_size);

   462

        } else {

   463

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   464

        }

   465

        fsm->state = ec_fsm_coe_error;

   466

        return;

   467

    }

   468

   469

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

   470

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

   471

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

   472

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

   473

                    slave->ring_position);

   474

            ec_print_data(data, rec_size);

   475

        }

   476

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

   477

        fsm->retries = EC_FSM_RETRIES;

   478

        fsm->state = ec_fsm_coe_dict_check;

   479

        return;

   480

    }

   481

   482

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

   483

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

   484

        ec_print_data(data, rec_size);

   485

        fsm->state = ec_fsm_coe_error;

   486

        return;

   487

    }

   488

   489

    sdo_count = (rec_size - 8) / 2;

   490

   491

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

   492

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

   493

        if (!sdo_index) {

   494

            if (slave->master->debug_level)

   495

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

   496

                        slave->ring_position);

   497

            continue;

   498

        }

   499

   500

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

   501

            EC_ERR("Failed to allocate memory for SDO!\n");

   502

            fsm->state = ec_fsm_coe_error;

   503

            return;

   504

        }

   505

   506

        ec_sdo_init(sdo, slave, sdo_index);

   507

        list_add_tail(&sdo->list, &slave->sdo_dictionary);