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 *);

    67

    68

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    69

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    70

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    71

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    72

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    73

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    74

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    75

    76

void ec_fsm_coe_end(ec_fsm_coe_t *);

    77

void ec_fsm_coe_error(ec_fsm_coe_t *);

    78

    79

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

    80

    81

/**

    82

   SDO abort messages.

    83

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

    84

   which can be translated to clear text. This table does

    85

   the mapping of the codes and messages.

    86

*/

    87

    88

const ec_code_msg_t sdo_abort_messages[] = {

    89

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

    90

    {0x05040000, "SDO protocol timeout"},

    91

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

    92

    {0x05040005, "Out of memory"},

    93

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

    94

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

    95

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

    96

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

    97

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

    98

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

    99

     " exceed the PDO length"},

   100

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

   101

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

   102

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

   103

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

   104

     " not match"},

   105

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

   106

     " high"},

   107

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

   108

     " low"},

   109

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

   110

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

   111

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

   112

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

   113

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

   114

    {0x08000000, "General error"},

   115

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

   116

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

   117

     " because of local control"},

   118

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

   119

     " because of the present device state"},

   120

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

   121

     " dictionary is present"},

   122

    {}

   123

};

   124

   125

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

   126

   127

/**

   128

   Outputs an SDO abort message.

   129

*/

   130

   131

void ec_canopen_abort_msg(uint32_t abort_code)

   132

{

   133

    const ec_code_msg_t *abort_msg;

   134

   135

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

   136

        if (abort_msg->code == abort_code) {

   137

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

   138

                   abort_msg->code, abort_msg->message);

   139

            return;

   140

        }

   141

    }

   142

   143

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

   144

}

   145

   146

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

   147

   148

/**

   149

   Constructor.

   150

*/

   151

   152

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

   153

                     ec_datagram_t *datagram /**< datagram */

   154

                     )

   155

{

   156

    fsm->state = NULL;

   157

    fsm->datagram = datagram;

   158

}

   159

   160

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

   161

   162

/**

   163

   Destructor.

   164

*/

   165

   166

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

   167

{

   168

}

   169

   170

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

   171

   172

/**

   173

   Starts reading a slaves' SDO dictionary.

   174

*/

   175

   176

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

   177

                           ec_slave_t *slave /**< EtherCAT slave */

   178

                           )

   179

{

   180

    fsm->slave = slave;

   181

    fsm->state = ec_fsm_coe_dict_start;

   182

}

   183

   184

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

   185

   186

/**

   187

   Starts to transfer an SDO to/from a slave.

   188

*/

   189

   190

void ec_fsm_coe_transfer(

   191

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

   192

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

   193

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

   194

        )

   195

{

   196

    fsm->slave = slave;

   197

    fsm->request = request;

   198

    if (request->dir == EC_DIR_OUTPUT)

   199

        fsm->state = ec_fsm_coe_down_start;

   200

    else

   201

        fsm->state = ec_fsm_coe_up_start;

   202

}

   203

   204

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

   205

   206

/**

   207

   Executes the current state of the state machine.

   208

   \return false, if state machine has terminated

   209

*/

   210

   211

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

   212

{

   213

    fsm->state(fsm);

   214

   215

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

   216

}

   217

   218

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

   219

   220

/**

   221

   Returns, if the state machine terminated with success.

   222

   \return non-zero if successful.

   223

*/

   224

   225

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

   226

{

   227

    return fsm->state == ec_fsm_coe_end;

   228

}

   229

   230

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

   231

   232

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

   233

 *

   234

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

   235

 *

   236

 * \return The data were an emergency request.

   237

 */

   238

int ec_fsm_coe_check_emergency(

   239

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

   240

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

   241

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

   242

        )

   243

{

   244

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

   245

        return 0;

   246

   247

    if (size < 10) {

   248

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

   249

                fsm->slave->ring_position);

   250

        ec_print_data(data, size);

   251

        return 1;

   252

    }

   253

   254

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

   255

            fsm->slave->ring_position);

   256

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

   257

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

   258

    ec_print_data(data + 5, 5);

   259

    return 1;

   260

}

   261

   262

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

   263

 *  CoE dictionary state machine

   264

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

   265

   266

/**

   267

   CoE state: DICT START.

   268

*/

   269

   270

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

   271

{

   272

    ec_datagram_t *datagram = fsm->datagram;

   273

    ec_slave_t *slave = fsm->slave;

   274

    uint8_t *data;

   275

   276

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

   277

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

   278

        fsm->state = ec_fsm_coe_error;

   279

        return;

   280

    }

   281

   282

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

   283

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

   284

                slave->ring_position);

   285

        fsm->state = ec_fsm_coe_error;

   286

        return;

   287

    }

   288

   289

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

   290

    if (IS_ERR(data)) {

   291

        fsm->state = ec_fsm_coe_error;

   292

        return;

   293

    }

   294

   295

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

   296

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

   297

    EC_WRITE_U8 (data + 3, 0x00);

   298

    EC_WRITE_U16(data + 4, 0x0000);

   299

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

   300

   301

    fsm->retries = EC_FSM_RETRIES;

   302

    fsm->state = ec_fsm_coe_dict_request;

   303

}

   304

   305

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

   306

   307

/**

   308

   CoE state: DICT REQUEST.

   309

   \todo Timeout behavior

   310

*/

   311

   312

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

   313

{

   314

    ec_datagram_t *datagram = fsm->datagram;

   315

    ec_slave_t *slave = fsm->slave;

   316

   317

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

   318

        return; // FIXME: request again?

   319

   320

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   321

        fsm->state = ec_fsm_coe_error;

   322

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

   323

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

   324

               slave->ring_position, datagram->state);

   325

        return;

   326

    }

   327

   328

    if (datagram->working_counter != 1) {

   329

        fsm->state = ec_fsm_coe_error;

   330

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

   331

                slave->ring_position);

   332

        ec_datagram_print_wc_error(datagram);

   333

        return;

   334

    }

   335

   336

    fsm->jiffies_start = datagram->jiffies_sent;

   337

   338

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

   339

    fsm->retries = EC_FSM_RETRIES;

   340

    fsm->state = ec_fsm_coe_dict_check;

   341

}

   342

   343

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

   344

   345

/**

   346

   CoE state: DICT CHECK.

   347

*/

   348

   349

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

   350

{

   351

    ec_datagram_t *datagram = fsm->datagram;

   352

    ec_slave_t *slave = fsm->slave;

   353

   354

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

   355

        return;

   356

   357

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   358

        fsm->state = ec_fsm_coe_error;

   359

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

   360

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

   361

               slave->ring_position, datagram->state);

   362

        return;

   363

    }

   364

   365

    if (datagram->working_counter != 1) {

   366

        fsm->state = ec_fsm_coe_error;

   367

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

   368

               slave->ring_position);

   369

        ec_datagram_print_wc_error(datagram);

   370

        return;

   371

    }

   372

   373

    if (!ec_slave_mbox_check(datagram)) {

   374

        unsigned long diff_ms =

   375

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

   376

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   377

            fsm->state = ec_fsm_coe_error;

   378

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

   379

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

   380

            return;

   381

        }

   382

   383

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

   384

        fsm->retries = EC_FSM_RETRIES;

   385

        return;

   386

    }

   387

   388

    // Fetch response

   389

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

   390

    fsm->retries = EC_FSM_RETRIES;

   391

    fsm->state = ec_fsm_coe_dict_response;

   392

}

   393

   394

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

   395

   396

/**

   397

   CoE state: DICT RESPONSE.

   398

   \todo Timeout behavior

   399

*/

   400

   401

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

   402

{

   403

    ec_datagram_t *datagram = fsm->datagram;

   404

    ec_slave_t *slave = fsm->slave;

   405

    uint8_t *data, mbox_prot;

   406

    size_t rec_size;

   407

    unsigned int sdo_count, i;

   408

    uint16_t sdo_index, fragments_left;

   409

    ec_sdo_t *sdo;

   410

   411

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

   412

        return; // FIXME: request again?

   413

   414

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   415

        fsm->state = ec_fsm_coe_error;

   416

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

   417

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

   418

               slave->ring_position, datagram->state);

   419

        return;

   420

    }

   421

   422

    if (datagram->working_counter != 1) {

   423

        fsm->state = ec_fsm_coe_error;

   424

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

   425

                slave->ring_position);

   426

        ec_datagram_print_wc_error(datagram);

   427

        return;

   428

    }

   429

   430

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

   431

    if (IS_ERR(data)) {

   432

        fsm->state = ec_fsm_coe_error;

   433

        return;

   434

    }

   435

   436

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

   437

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

   438

        fsm->state = ec_fsm_coe_error;

   439

        return;

   440

    }

   441

   442

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

   443

        // check for CoE response again

   444

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

   445

        fsm->retries = EC_FSM_RETRIES;

   446

        fsm->state = ec_fsm_coe_dict_check;

   447

        return;

   448

    }

   449

   450

    if (rec_size < 3) {

   451

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

   452

                rec_size);

   453

        fsm->state = ec_fsm_coe_error;

   454

        return;

   455

    }

   456

   457

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

   458

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

   459

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

   460

               slave->ring_position);

   461

        if (rec_size < 10) {

   462

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

   463

            ec_print_data(data, rec_size);

   464

        } else {

   465

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   466

        }

   467

        fsm->state = ec_fsm_coe_error;

   468

        return;

   469

    }

   470

   471

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

   472

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

   473

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

   474

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

   475

                    slave->ring_position);

   476

            ec_print_data(data, rec_size);

   477

        }

   478

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

   479

        fsm->retries = EC_FSM_RETRIES;

   480

        fsm->state = ec_fsm_coe_dict_check;

   481

        return;

   482

    }

   483

   484

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

   485

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

   486

        ec_print_data(data, rec_size);

   487

        fsm->state = ec_fsm_coe_error;

   488

        return;

   489

    }

   490

   491

    sdo_count = (rec_size - 8) / 2;

   492

   493

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

   494

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

   495

        if (!sdo_index) {

   496

            if (slave->master->debug_level)

   497

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

   498

                        slave->ring_position);

   499

            continue;

   500

        }

   501

   502

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

   503

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

   504

            fsm->state = ec_fsm_coe_error;

   505

            return;

   506

        }

   507