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

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   EtherCAT CoE state machines.

    30

*/

    31

    32

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

    33

    34

#include "globals.h"

    35

#include "master.h"

    36

#include "mailbox.h"

    37

#include "fsm_coe.h"

    38

    39

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

    40

    41

/** Maximum time in ms to wait for responses when reading out the dictionary.

    42

 */

    43

#define EC_FSM_COE_DICT_TIMEOUT 3000

    44

    45

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

    46

    47

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    48

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    49

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    50

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    51

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    52

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    53

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    54

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    55

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    56

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    57

    58

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    59

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    60

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    61

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    62

    63

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    64

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    65

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    66

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    67

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    68

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    69

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    70

    71

void ec_fsm_coe_end(ec_fsm_coe_t *);

    72

void ec_fsm_coe_error(ec_fsm_coe_t *);

    73

    74

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

    75

    76

/**

    77

   SDO abort messages.

    78

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

    79

   which can be translated to clear text. This table does

    80

   the mapping of the codes and messages.

    81

*/

    82

    83

const ec_code_msg_t sdo_abort_messages[] = {

    84

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

    85

    {0x05040000, "SDO protocol timeout"},

    86

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

    87

    {0x05040005, "Out of memory"},

    88

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

    89

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

    90

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

    91

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

    92

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

    93

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

    94

     " exceed the PDO length"},

    95

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

    96

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

    97

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

    98

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

    99

     " not match"},

   100

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

   101

     " high"},

   102

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

   103

     " low"},

   104

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

   105

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

   106

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

   107

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

   108

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

   109

    {0x08000000, "General error"},

   110

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

   111

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

   112

     " because of local control"},

   113

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

   114

     " because of the present device state"},

   115

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

   116

     " dictionary is present"},

   117

    {}

   118

};

   119

   120

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

   121

   122

/**

   123

   Outputs an SDO abort message.

   124

*/

   125

   126

void ec_canopen_abort_msg(uint32_t abort_code)

   127

{

   128

    const ec_code_msg_t *abort_msg;

   129

   130

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

   131

        if (abort_msg->code == abort_code) {

   132

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

   133

                   abort_msg->code, abort_msg->message);

   134

            return;

   135

        }

   136

    }

   137

   138

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

   139

}

   140

   141

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

   142

   143

/**

   144

   Constructor.

   145

*/

   146

   147

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

   148

                     ec_datagram_t *datagram /**< datagram */

   149

                     )

   150

{

   151

    fsm->state = NULL;

   152

    fsm->datagram = datagram;

   153

}

   154

   155

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

   156

   157

/**

   158

   Destructor.

   159

*/

   160

   161

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

   162

{

   163

}

   164

   165

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

   166

   167

/**

   168

   Starts reading a slaves' SDO dictionary.

   169

*/

   170

   171

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

   172

                           ec_slave_t *slave /**< EtherCAT slave */

   173

                           )

   174

{

   175

    fsm->slave = slave;

   176

    fsm->state = ec_fsm_coe_dict_start;

   177

}

   178

   179

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

   180

   181

/**

   182

   Starts to transfer an SDO to/from a slave.

   183

*/

   184

   185

void ec_fsm_coe_transfer(

   186

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

   187

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

   188

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

   189

        )

   190

{

   191

    fsm->slave = slave;

   192

    fsm->request = request;

   193

    if (request->dir == EC_DIR_OUTPUT)

   194

        fsm->state = ec_fsm_coe_down_start;

   195

    else

   196

        fsm->state = ec_fsm_coe_up_start;

   197

}

   198

   199

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

   200

   201

/**

   202

   Executes the current state of the state machine.

   203

   \return false, if state machine has terminated

   204

*/

   205

   206

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

   207

{

   208

    fsm->state(fsm);

   209

   210

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

   211

}

   212

   213

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

   214

   215

/**

   216

   Returns, if the state machine terminated with success.

   217

   \return non-zero if successful.

   218

*/

   219

   220

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

   221

{

   222

    return fsm->state == ec_fsm_coe_end;

   223

}

   224

   225

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

   226

   227

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

   228

 *

   229

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

   230

 *

   231

 * \return The data were an emergency request.

   232

 */

   233

int ec_fsm_coe_check_emergency(

   234

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

   235

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

   236

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

   237

        )

   238

{

   239

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

   240

        return 0;

   241

   242

    if (size < 10) {

   243

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

   244

                fsm->slave->ring_position);

   245

        ec_print_data(data, size);

   246

        return 1;

   247

    }

   248

   249

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

   250

            fsm->slave->ring_position);

   251

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

   252

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

   253

    ec_print_data(data + 5, 5);

   254

    return 1;

   255

}

   256

   257

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

   258

 *  CoE dictionary state machine

   259

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

   260

   261

/**

   262

   CoE state: DICT START.

   263

*/

   264

   265

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

   266

{

   267

    ec_datagram_t *datagram = fsm->datagram;

   268

    ec_slave_t *slave = fsm->slave;

   269

    uint8_t *data;

   270

   271

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

   272

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

   273

        fsm->state = ec_fsm_coe_error;

   274

        return;

   275

    }

   276

   277

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

   278

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

   279

                slave->ring_position);

   280

        fsm->state = ec_fsm_coe_error;

   281

        return;

   282

    }

   283

   284

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

   285

    if (IS_ERR(data)) {

   286

        fsm->state = ec_fsm_coe_error;

   287

        return;

   288

    }

   289

   290

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

   291

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

   292

    EC_WRITE_U8 (data + 3, 0x00);

   293

    EC_WRITE_U16(data + 4, 0x0000);

   294

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

   295

   296

    fsm->retries = EC_FSM_RETRIES;

   297

    fsm->state = ec_fsm_coe_dict_request;

   298

}

   299

   300

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

   301

   302

/**

   303

   CoE state: DICT REQUEST.

   304

   \todo Timeout behavior

   305

*/

   306

   307

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

   308

{

   309

    ec_datagram_t *datagram = fsm->datagram;

   310

    ec_slave_t *slave = fsm->slave;

   311

   312

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

   313

        return; // FIXME: request again?

   314

   315

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   316

        fsm->state = ec_fsm_coe_error;

   317

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

   318

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

   319

               slave->ring_position, datagram->state);

   320

        return;

   321

    }

   322

   323

    if (datagram->working_counter != 1) {

   324

        fsm->state = ec_fsm_coe_error;

   325

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

   326

                slave->ring_position);

   327

        ec_datagram_print_wc_error(datagram);

   328

        return;

   329

    }

   330

   331

    fsm->jiffies_start = datagram->jiffies_sent;

   332

   333

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

   334

    fsm->retries = EC_FSM_RETRIES;

   335

    fsm->state = ec_fsm_coe_dict_check;

   336

}

   337

   338

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

   339

   340

/**

   341

   CoE state: DICT CHECK.

   342

*/

   343

   344

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

   345

{

   346

    ec_datagram_t *datagram = fsm->datagram;

   347

    ec_slave_t *slave = fsm->slave;

   348

   349

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

   350

        return;

   351

   352

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   353

        fsm->state = ec_fsm_coe_error;

   354

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

   355

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

   356

               slave->ring_position, datagram->state);

   357

        return;

   358

    }

   359

   360

    if (datagram->working_counter != 1) {

   361

        fsm->state = ec_fsm_coe_error;

   362

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

   363

               slave->ring_position);

   364

        ec_datagram_print_wc_error(datagram);

   365

        return;

   366

    }

   367

   368

    if (!ec_slave_mbox_check(datagram)) {

   369

        unsigned long diff_ms =

   370

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

   371

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   372

            fsm->state = ec_fsm_coe_error;

   373

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

   374

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

   375

            return;

   376

        }

   377

   378

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

   379

        fsm->retries = EC_FSM_RETRIES;

   380

        return;

   381

    }

   382

   383

    // Fetch response

   384

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

   385

    fsm->retries = EC_FSM_RETRIES;

   386

    fsm->state = ec_fsm_coe_dict_response;

   387

}

   388

   389

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

   390

   391

/**

   392

   CoE state: DICT RESPONSE.

   393

   \todo Timeout behavior

   394

*/

   395

   396

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

   397

{

   398

    ec_datagram_t *datagram = fsm->datagram;

   399

    ec_slave_t *slave = fsm->slave;

   400

    uint8_t *data, mbox_prot;

   401

    size_t rec_size;

   402

    unsigned int sdo_count, i;

   403

    uint16_t sdo_index, fragments_left;

   404

    ec_sdo_t *sdo;

   405

   406

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

   407

        return; // FIXME: request again?

   408

   409

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   410

        fsm->state = ec_fsm_coe_error;

   411

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

   412

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

   413

               slave->ring_position, datagram->state);

   414

        return;

   415

    }

   416

   417

    if (datagram->working_counter != 1) {

   418

        fsm->state = ec_fsm_coe_error;

   419

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

   420

                slave->ring_position);

   421

        ec_datagram_print_wc_error(datagram);

   422

        return;

   423

    }

   424

   425

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

   426

    if (IS_ERR(data)) {

   427

        fsm->state = ec_fsm_coe_error;

   428

        return;

   429

    }

   430

   431

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

   432

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

   433

        fsm->state = ec_fsm_coe_error;

   434

        return;

   435

    }

   436

   437

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

   438

        // check for CoE response again

   439

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

   440

        fsm->retries = EC_FSM_RETRIES;

   441

        fsm->state = ec_fsm_coe_dict_check;

   442

        return;

   443

    }

   444

   445

    if (rec_size < 3) {

   446

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

   447

                rec_size);

   448

        fsm->state = ec_fsm_coe_error;

   449

        return;

   450

    }

   451

   452

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

   453

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

   454

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

   455

               slave->ring_position);

   456

        if (rec_size < 10) {

   457

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

   458

            ec_print_data(data, rec_size);

   459

        } else {

   460

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   461

        }

   462

        fsm->state = ec_fsm_coe_error;

   463

        return;

   464

    }

   465

   466

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

   467

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

   468

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

   469

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

   470

                    slave->ring_position);

   471

            ec_print_data(data, rec_size);

   472

        }

   473

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

   474

        fsm->retries = EC_FSM_RETRIES;

   475

        fsm->state = ec_fsm_coe_dict_check;

   476

        return;

   477

    }

   478

   479

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

   480

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

   481

        ec_print_data(data, rec_size);

   482

        fsm->state = ec_fsm_coe_error;

   483

        return;

   484

    }

   485

   486

    sdo_count = (rec_size - 8) / 2;

   487

   488

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

   489

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

   490

        if (!sdo_index) {

   491

            if (slave->master->debug_level)

   492

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

   493

                        slave->ring_position);

   494

            continue;

   495

        }

   496

   497

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

   498

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

   499

            fsm->state = ec_fsm_coe_error;

   500

            return;

   501

        }

   502

   503

        ec_sdo_init(sdo, slave, sdo_index);

   504

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

   505

    }

   506

   507

    fragments_left = EC_READ_U16(data + 4);