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

void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    49

void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    50

void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    51

void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    52

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    53

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    54

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    55

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    56

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    57

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    58

    59

void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    60

void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    61

void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    62

void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    63

    64

void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    65

void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    66

void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    67

void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    68

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    69

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    70

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    71

    72

void ec_fsm_coe_end(ec_fsm_coe_t *);

    73

void ec_fsm_coe_error(ec_fsm_coe_t *);

    74

    75

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

    76

    77

/**

    78

   Sdo abort messages.

    79

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

    80

   which can be translated to clear text. This table does

    81

   the mapping of the codes and messages.

    82

*/

    83

    84

const ec_code_msg_t sdo_abort_messages[] = {

    85

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

    86

    {0x05040000, "Sdo protocol timeout"},

    87

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

    88

    {0x05040005, "Out of memory"},

    89

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

    90

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

    91

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

    92

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

    93

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

    94

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

    95

     " exceed the Pdo length"},

    96

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

    97

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

    98

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

    99

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

   100

     " not match"},

   101

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

   102

     " high"},

   103

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

   104

     " low"},

   105

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

   106

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

   107

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

   108

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

   109

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

   110

    {0x08000000, "General error"},

   111

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

   112

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

   113

     " because of local control"},

   114

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

   115

     " because of the present device state"},

   116

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

   117

     " dictionary is present"},

   118

    {}

   119

};

   120

   121

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

   122

   123

/**

   124

   Outputs an Sdo abort message.

   125

*/

   126

   127

void ec_canopen_abort_msg(uint32_t abort_code)

   128

{

   129

    const ec_code_msg_t *abort_msg;

   130

   131

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

   132

        if (abort_msg->code == abort_code) {

   133

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

   134

                   abort_msg->code, abort_msg->message);

   135

            return;

   136

        }

   137

    }

   138

   139

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

   140

}

   141

   142

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

   143

   144

/**

   145

   Constructor.

   146

*/

   147

   148

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

   149

                     ec_datagram_t *datagram /**< datagram */

   150

                     )

   151

{

   152

    fsm->state = NULL;

   153

    fsm->datagram = datagram;

   154

}

   155

   156

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

   157

   158

/**

   159

   Destructor.

   160

*/

   161

   162

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

   163

{

   164

}

   165

   166

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

   167

   168

/**

   169

   Starts reading a slaves' Sdo dictionary.

   170

*/

   171

   172

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

   173

                           ec_slave_t *slave /**< EtherCAT slave */

   174

                           )

   175

{

   176

    fsm->slave = slave;

   177

    fsm->state = ec_fsm_coe_dict_start;

   178

}

   179

   180

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

   181

   182

/**

   183

   Starts to transfer an Sdo to/from a slave.

   184

*/

   185

   186

void ec_fsm_coe_transfer(

   187

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

   188

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

   189

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

   190

        )

   191

{

   192

    fsm->slave = slave;

   193

    fsm->request = request;

   194

    if (request->dir == EC_DIR_OUTPUT)

   195

        fsm->state = ec_fsm_coe_down_start;

   196

    else

   197

        fsm->state = ec_fsm_coe_up_start;

   198

}

   199

   200

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

   201

   202

/**

   203

   Executes the current state of the state machine.

   204

   \return false, if state machine has terminated

   205

*/

   206

   207

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

   208

{

   209

    fsm->state(fsm);

   210

   211

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

   212

}

   213

   214

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

   215

   216

/**

   217

   Returns, if the state machine terminated with success.

   218

   \return non-zero if successful.

   219

*/

   220

   221

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

   222

{

   223

    return fsm->state == ec_fsm_coe_end;

   224

}

   225

   226

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

   227

   228

/**

   229

 */

   230

int ec_fsm_coe_check_emergency(

   231

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

   232

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

   233

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

   234

        )

   235

{

   236

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

   237

        return 0;

   238

   239

    if (size < 10) {

   240

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

   241

                fsm->slave->ring_position);

   242

        ec_print_data(data, size);

   243

        return 1;

   244

    }

   245

   246

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

   247

            fsm->slave->ring_position);

   248

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

   249

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

   250

    ec_print_data(data + 5, 5);

   251

    return 1;

   252

}

   253

   254

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

   255

 *  CoE dictionary state machine

   256

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

   257

   258

/**

   259

   CoE state: DICT START.

   260

*/

   261

   262

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

   263

{

   264

    ec_datagram_t *datagram = fsm->datagram;

   265

    ec_slave_t *slave = fsm->slave;

   266

    uint8_t *data;

   267

   268

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

   269

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

   270

        fsm->state = ec_fsm_coe_error;

   271

        return;

   272

    }

   273

   274

    if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {

   275

        fsm->state = ec_fsm_coe_error;

   276

        return;

   277

    }

   278

   279

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

   280

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

   281

    EC_WRITE_U8 (data + 3, 0x00);

   282

    EC_WRITE_U16(data + 4, 0x0000);

   283

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

   284

   285

    fsm->retries = EC_FSM_RETRIES;

   286

    fsm->state = ec_fsm_coe_dict_request;

   287

}

   288

   289

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

   290

   291

/**

   292

   CoE state: DICT REQUEST.

   293

   \todo Timeout behavior

   294

*/

   295

   296

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

   297

{

   298

    ec_datagram_t *datagram = fsm->datagram;

   299

    ec_slave_t *slave = fsm->slave;

   300

   301

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

   302

        return; // FIXME: request again?

   303

   304

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   305

        fsm->state = ec_fsm_coe_error;

   306

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

   307

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

   308

               slave->ring_position, datagram->state);

   309

        return;

   310

    }

   311

   312

    if (datagram->working_counter != 1) {

   313

        fsm->state = ec_fsm_coe_error;

   314

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

   315

                slave->ring_position);

   316

        ec_datagram_print_wc_error(datagram);

   317

        return;

   318

    }

   319

   320

    fsm->cycles_start = datagram->cycles_sent;

   321

   322

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

   323

    fsm->retries = EC_FSM_RETRIES;

   324

    fsm->state = ec_fsm_coe_dict_check;

   325

}

   326

   327

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

   328

   329

/**

   330

   CoE state: DICT CHECK.

   331

*/

   332

   333

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

   334

{

   335

    ec_datagram_t *datagram = fsm->datagram;

   336

    ec_slave_t *slave = fsm->slave;

   337

   338

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

   339

        return;

   340

   341

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   342

        fsm->state = ec_fsm_coe_error;

   343

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

   344

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

   345

               slave->ring_position, datagram->state);

   346

        return;

   347

    }

   348

   349

    if (datagram->working_counter != 1) {

   350

        fsm->state = ec_fsm_coe_error;

   351

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

   352

               slave->ring_position);

   353

        ec_datagram_print_wc_error(datagram);

   354

        return;

   355

    }

   356

   357

    if (!ec_slave_mbox_check(datagram)) {

   358

        if (datagram->cycles_received

   359

            - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {

   360

            fsm->state = ec_fsm_coe_error;

   361

            EC_ERR("Timeout while checking Sdo dictionary on slave %u.\n",

   362

                   slave->ring_position);

   363

            return;

   364

        }

   365

   366

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

   367

        fsm->retries = EC_FSM_RETRIES;

   368

        return;

   369

    }

   370

   371

    // Fetch response

   372

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

   373

    fsm->retries = EC_FSM_RETRIES;

   374

    fsm->state = ec_fsm_coe_dict_response;

   375

}

   376

   377

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

   378

   379

/**

   380

   CoE state: DICT RESPONSE.

   381

   \todo Timeout behavior

   382

*/

   383

   384

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

   385

{

   386

    ec_datagram_t *datagram = fsm->datagram;

   387

    ec_slave_t *slave = fsm->slave;

   388

    uint8_t *data, mbox_prot;

   389

    size_t rec_size;

   390

    unsigned int sdo_count, i;

   391

    uint16_t sdo_index, fragments_left;

   392

    ec_sdo_t *sdo;

   393

   394

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

   395

        return; // FIXME: request again?

   396

   397

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   398

        fsm->state = ec_fsm_coe_error;

   399

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

   400

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

   401

               slave->ring_position, datagram->state);

   402

        return;

   403

    }

   404

   405

    if (datagram->working_counter != 1) {

   406

        fsm->state = ec_fsm_coe_error;

   407

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

   408

                slave->ring_position);

   409

        ec_datagram_print_wc_error(datagram);

   410

        return;

   411

    }

   412

   413

    if (!(data = ec_slave_mbox_fetch(slave, datagram,

   414

				     &mbox_prot, &rec_size))) {

   415

        fsm->state = ec_fsm_coe_error;

   416

        return;

   417

    }

   418

   419

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

   420

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

   421

        fsm->state = ec_fsm_coe_error;

   422

        return;

   423

    }

   424

   425

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

   426

        // check for CoE response again

   427

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

   428

        fsm->retries = EC_FSM_RETRIES;

   429

        fsm->state = ec_fsm_coe_dict_check;

   430

        return;

   431

    }

   432

   433

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

   434

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

   435

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

   436

               slave->ring_position);

   437

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   438

        fsm->state = ec_fsm_coe_error;

   439

        return;

   440

    }

   441

   442

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

   443

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

   444

        EC_ERR("Invalid Sdo list response at slave %u!\n",

   445

               slave->ring_position);

   446

        ec_print_data(data, rec_size);

   447

        fsm->state = ec_fsm_coe_error;

   448

        return;

   449

    }

   450

   451

    if (rec_size < 8) {

   452

        EC_ERR("Invalid data size!\n");

   453

        ec_print_data(data, rec_size);

   454

        fsm->state = ec_fsm_coe_error;

   455

        return;

   456

    }

   457

   458

    sdo_count = (rec_size - 8) / 2;

   459

   460

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

   461

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

   462

        if (!sdo_index) {

   463

            if (slave->master->debug_level)

   464

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

   465

                        slave->ring_position);

   466

            continue;

   467

        }

   468

   469

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

   470

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

   471

            fsm->state = ec_fsm_coe_error;

   472

            return;

   473

        }

   474

   475

        ec_sdo_init(sdo, slave, sdo_index);

   476

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

   477

    }

   478

   479

    fragments_left = EC_READ_U16(data + 4);

   480

    if (slave->master->debug_level && fragments_left) {

   481

        EC_DBG("Sdo list fragments left: %u\n", fragments_left);

   482

    }

   483

   484

    if (EC_READ_U8(data + 2) & 0x80 || fragments_left) { // more messages waiting. check again.

   485

        fsm->cycles_start = datagram->cycles_sent;

   486

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

   487

        fsm->retries = EC_FSM_RETRIES;

   488

        fsm->state = ec_fsm_coe_dict_check;

   489

        return;

   490

    }

   491

   492

    if (list_empty(&slave->sdo_dictionary)) {

   493

        // no Sdos in dictionary. finished.

   494

        fsm->state = ec_fsm_coe_end; // success

   495

        return;

   496

    }

   497

   498

    // fetch Sdo descriptions

   499

    fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);

   500

   501

    if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {

   502

        fsm->state = ec_fsm_coe_error;

   503

        return;

   504

    }

   505

   506

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

   507

    EC_WRITE_U8 (data + 2, 0x03); // Get object description request

   508

    EC_WRITE_U8 (data + 3, 0x00);

   509

    EC_WRITE_U16(data + 4, 0x0000);

   510

    EC_WRITE_U16(data + 6, fsm->sdo->index); // Sdo index

   511

   512

    fsm->retries = EC_FSM_RETRIES;