1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    15

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU General Public License

    20

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT CoE state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "fsm_coe.h"

    45

    46

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

    47

    48

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

    49

 */

    50

#define EC_FSM_COE_DICT_TIMEOUT 3000

    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

    70

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    71

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    72

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    73

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    74

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    75

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    76

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    77

    78

void ec_fsm_coe_end(ec_fsm_coe_t *);

    79

void ec_fsm_coe_error(ec_fsm_coe_t *);

    80

    81

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

    82

    83

/**

    84

   Sdo abort messages.

    85

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

    86

   which can be translated to clear text. This table does

    87

   the mapping of the codes and messages.

    88

*/

    89

    90

const ec_code_msg_t sdo_abort_messages[] = {

    91

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

    92

    {0x05040000, "Sdo protocol timeout"},

    93

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

    94

    {0x05040005, "Out of memory"},

    95

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

    96

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

    97

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

    98

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

    99

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

   100

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

   101

     " exceed the Pdo length"},

   102

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

   103

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

   104

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

   105

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

   106

     " not match"},

   107

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

   108

     " high"},

   109

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

   110

     " low"},

   111

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

   112

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

   113

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

   114

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

   115

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

   116

    {0x08000000, "General error"},

   117

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

   118

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

   119

     " because of local control"},

   120

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

   121

     " because of the present device state"},

   122

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

   123

     " dictionary is present"},

   124

    {}

   125

};

   126

   127

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

   128

   129

/**

   130

   Outputs an Sdo abort message.

   131

*/

   132

   133

void ec_canopen_abort_msg(uint32_t abort_code)

   134

{

   135

    const ec_code_msg_t *abort_msg;

   136

   137

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

   138

        if (abort_msg->code == abort_code) {

   139

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

   140

                   abort_msg->code, abort_msg->message);

   141

            return;

   142

        }

   143

    }

   144

   145

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

   146

}

   147

   148

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

   149

   150

/**

   151

   Constructor.

   152

*/

   153

   154

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

   155

                     ec_datagram_t *datagram /**< datagram */

   156

                     )

   157

{

   158

    fsm->state = NULL;

   159

    fsm->datagram = datagram;

   160

}

   161

   162

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

   163

   164

/**

   165

   Destructor.

   166

*/

   167

   168

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

   169

{

   170

}

   171

   172

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

   173

   174

/**

   175

   Starts reading a slaves' Sdo dictionary.

   176

*/

   177

   178

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

   179

                           ec_slave_t *slave /**< EtherCAT slave */

   180

                           )

   181

{

   182

    fsm->slave = slave;

   183

    fsm->state = ec_fsm_coe_dict_start;

   184

}

   185

   186

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

   187

   188

/**

   189

   Starts to transfer an Sdo to/from a slave.

   190

*/

   191

   192

void ec_fsm_coe_transfer(

   193

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

   194

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

   195

        ec_sdo_request_t *request /**< Sdo request. */

   196

        )

   197

{

   198

    fsm->slave = slave;

   199

    fsm->request = request;

   200

    if (request->dir == EC_DIR_OUTPUT)

   201

        fsm->state = ec_fsm_coe_down_start;

   202

    else

   203

        fsm->state = ec_fsm_coe_up_start;

   204

}

   205

   206

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

   207

   208

/**

   209

   Executes the current state of the state machine.

   210

   \return false, if state machine has terminated

   211

*/

   212

   213

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

   214

{

   215

    fsm->state(fsm);

   216

   217

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

   218

}

   219

   220

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

   221

   222

/**

   223

   Returns, if the state machine terminated with success.

   224

   \return non-zero if successful.

   225

*/

   226

   227

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

   228

{

   229

    return fsm->state == ec_fsm_coe_end;

   230

}

   231

   232

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

   233

   234

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

   235

 *

   236

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

   237

 *

   238

 * \return The data were an emergency request.

   239

 */

   240

int ec_fsm_coe_check_emergency(

   241

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

   242

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

   243

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

   244

        )

   245

{

   246

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

   247

        return 0;

   248

   249

    if (size < 10) {

   250

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

   251

                fsm->slave->ring_position);

   252

        ec_print_data(data, size);

   253

        return 1;

   254

    }

   255

   256

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

   257

            fsm->slave->ring_position);

   258

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

   259

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

   260

    ec_print_data(data + 5, 5);

   261

    return 1;

   262

}

   263

   264

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

   265

 *  CoE dictionary state machine

   266

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

   267

   268

/**

   269

   CoE state: DICT START.

   270

*/

   271

   272

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

   273

{

   274

    ec_datagram_t *datagram = fsm->datagram;

   275

    ec_slave_t *slave = fsm->slave;

   276

    uint8_t *data;

   277

   278

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

   279

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

   280

        fsm->state = ec_fsm_coe_error;

   281

        return;

   282

    }

   283

   284

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

   285

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

   286

                slave->ring_position);

   287

        fsm->state = ec_fsm_coe_error;

   288

        return;

   289

    }

   290

   291

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

   292

    if (IS_ERR(data)) {

   293

        fsm->state = ec_fsm_coe_error;

   294

        return;

   295

    }

   296

   297

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

   298

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

   299

    EC_WRITE_U8 (data + 3, 0x00);

   300

    EC_WRITE_U16(data + 4, 0x0000);

   301

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

   302

   303

    fsm->retries = EC_FSM_RETRIES;

   304

    fsm->state = ec_fsm_coe_dict_request;

   305

}

   306

   307

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

   308

   309

/**

   310

   CoE state: DICT REQUEST.

   311

   \todo Timeout behavior

   312

*/

   313

   314

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

   315

{

   316

    ec_datagram_t *datagram = fsm->datagram;

   317

    ec_slave_t *slave = fsm->slave;

   318

   319

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

   320

        return; // FIXME: request again?

   321

   322

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   323

        fsm->state = ec_fsm_coe_error;

   324

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

   325

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

   326

               slave->ring_position, datagram->state);

   327

        return;

   328

    }

   329

   330

    if (datagram->working_counter != 1) {

   331

        fsm->state = ec_fsm_coe_error;

   332

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

   333

                slave->ring_position);

   334

        ec_datagram_print_wc_error(datagram);

   335

        return;

   336

    }

   337

   338

    fsm->jiffies_start = datagram->jiffies_sent;

   339

   340

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

   341

    fsm->retries = EC_FSM_RETRIES;

   342

    fsm->state = ec_fsm_coe_dict_check;

   343

}

   344

   345

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

   346

   347

/**

   348

   CoE state: DICT CHECK.

   349

*/

   350

   351

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

   352

{

   353

    ec_datagram_t *datagram = fsm->datagram;

   354

    ec_slave_t *slave = fsm->slave;

   355

   356

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

   357

        return;

   358

   359

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   360

        fsm->state = ec_fsm_coe_error;

   361

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

   362

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

   363

               slave->ring_position, datagram->state);

   364

        return;

   365

    }

   366

   367

    if (datagram->working_counter != 1) {

   368

        fsm->state = ec_fsm_coe_error;

   369

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

   370

               slave->ring_position);

   371

        ec_datagram_print_wc_error(datagram);

   372

        return;

   373

    }

   374

   375

    if (!ec_slave_mbox_check(datagram)) {

   376

        unsigned long diff_ms =

   377

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

   378

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   379

            fsm->state = ec_fsm_coe_error;

   380

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

   381

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

   382

            return;

   383

        }

   384

   385

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

   386

        fsm->retries = EC_FSM_RETRIES;

   387

        return;

   388

    }

   389

   390

    // Fetch response

   391

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

   392

    fsm->retries = EC_FSM_RETRIES;

   393

    fsm->state = ec_fsm_coe_dict_response;

   394

}

   395

   396

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

   397

   398

/**

   399

   CoE state: DICT RESPONSE.

   400

   \todo Timeout behavior

   401

*/

   402

   403

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

   404

{

   405

    ec_datagram_t *datagram = fsm->datagram;

   406

    ec_slave_t *slave = fsm->slave;

   407

    uint8_t *data, mbox_prot;

   408

    size_t rec_size;

   409

    unsigned int sdo_count, i;

   410

    uint16_t sdo_index, fragments_left;

   411

    ec_sdo_t *sdo;

   412

   413

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

   414

        return; // FIXME: request again?

   415

   416

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   417

        fsm->state = ec_fsm_coe_error;

   418

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

   419

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

   420

               slave->ring_position, datagram->state);

   421

        return;

   422

    }

   423

   424

    if (datagram->working_counter != 1) {

   425

        fsm->state = ec_fsm_coe_error;

   426

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

   427

                slave->ring_position);

   428

        ec_datagram_print_wc_error(datagram);

   429

        return;

   430

    }

   431

   432

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

   433

    if (IS_ERR(data)) {

   434

        fsm->state = ec_fsm_coe_error;

   435

        return;

   436

    }

   437

   438

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

   439

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

   440

        fsm->state = ec_fsm_coe_error;

   441

        return;

   442

    }

   443

   444

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

   445

        // check for CoE response again

   446

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

   447

        fsm->retries = EC_FSM_RETRIES;

   448

        fsm->state = ec_fsm_coe_dict_check;

   449

        return;

   450

    }

   451

   452

    if (rec_size < 3) {

   453

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

   454

                rec_size);

   455

        fsm->state = ec_fsm_coe_error;

   456

        return;

   457

    }

   458

   459

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

   460

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

   461

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

   462

               slave->ring_position);

   463

        if (rec_size < 10) {

   464

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

   465

            ec_print_data(data, rec_size);

   466

        } else {

   467

            ec_canopen_abort_msg(EC_READ_U32(data + 6));

   468

        }

   469

        fsm->state = ec_fsm_coe_error;

   470

        return;

   471

    }

   472

   473

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

   474

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

   475

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

   476

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

   477

                    slave->ring_position);

   478

            ec_print_data(data, rec_size);

   479

        }

   480

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

   481

        fsm->retries = EC_FSM_RETRIES;

   482

        fsm->state = ec_fsm_coe_dict_check;

   483

        return;

   484

    }

   485

   486

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

   487

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

   488

        ec_print_data(data, rec_size);

   489

        fsm->state = ec_fsm_coe_error;

   490

        return;

   491

    }

   492

   493

    sdo_count = (rec_size - 8) / 2;

   494

   495

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

   496

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

   497

        if (!sdo_index) {

   498

            if (slave->master->debug_level)

   499

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

   500

                        slave->ring_position);

   501

            continue;

   502

        }

   503

   504

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

   505

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

   506

            fsm->state = ec_fsm_coe_error;

   507

            return;

   508

        }

   509