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 download an SDO to a slave.

   184

*/

   185

   186

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

   187

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

   188

                         ec_sdo_data_t *sdodata /**< SDO data object */

   189

                         )

   190

{

   191

    fsm->slave = slave;

   192

    fsm->sdodata = sdodata;

   193

    fsm->state = ec_fsm_coe_down_start;

   194

}

   195

   196

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

   197

   198

/**

   199

   Starts to upload an SDO from a slave.

   200

*/

   201

   202

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

   203

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

   204

                       ec_sdo_request_t *request /**< SDO request */

   205

                       )

   206

{

   207

    fsm->slave = slave;

   208

    fsm->request = request;

   209

    fsm->state = ec_fsm_coe_up_start;

   210

}

   211

   212

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

   213

   214

/**

   215

   Executes the current state of the state machine.

   216

   \return false, if state machine has terminated

   217

*/

   218

   219

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

   220

{

   221

    fsm->state(fsm);

   222

   223

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

   224

}

   225

   226

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

   227

   228

/**

   229

   Returns, if the state machine terminated with success.

   230

   \return non-zero if successful.

   231

*/

   232

   233

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

   234

{

   235

    return fsm->state == ec_fsm_coe_end;

   236

}

   237

   238

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

   239

 *  CoE dictionary state machine

   240

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

   241

   242

/**

   243

   CoE state: DICT START.

   244

*/

   245

   246

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

   247

{

   248

    ec_datagram_t *datagram = fsm->datagram;

   249

    ec_slave_t *slave = fsm->slave;

   250

    uint8_t *data;

   251

   252

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

   253

        fsm->state = ec_fsm_coe_error;

   254

        return;

   255

    }

   256

   257

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

   258

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

   259

    EC_WRITE_U8 (data + 3, 0x00);

   260

    EC_WRITE_U16(data + 4, 0x0000);

   261

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

   262

   263

    ec_master_queue_datagram(fsm->slave->master, datagram);

   264

    fsm->retries = EC_FSM_RETRIES;

   265

    fsm->state = ec_fsm_coe_dict_request;

   266

}

   267

   268

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

   269

   270

/**

   271

   CoE state: DICT REQUEST.

   272

   \todo Timeout behavior

   273

*/

   274

   275

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

   276

{

   277

    ec_datagram_t *datagram = fsm->datagram;

   278

    ec_slave_t *slave = fsm->slave;

   279

   280

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

   281

        // FIXME: request again?

   282

        ec_master_queue_datagram(fsm->slave->master, datagram);

   283

        return;

   284

    }

   285

   286

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   287

        fsm->state = ec_fsm_coe_error;

   288

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

   289

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

   290

        return;

   291

    }

   292

   293

    if (datagram->working_counter != 1) {

   294

        fsm->state = ec_fsm_coe_error;

   295

        EC_ERR("Reception of CoE dictionary request failed - slave %i did"

   296

               " not respond.\n", slave->ring_position);

   297

        return;

   298

    }

   299

   300

    fsm->cycles_start = datagram->cycles_sent;

   301

   302

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

   303

    ec_master_queue_datagram(fsm->slave->master, datagram);

   304

    fsm->retries = EC_FSM_RETRIES;

   305

    fsm->state = ec_fsm_coe_dict_check;

   306

}

   307

   308

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

   309

   310

/**

   311

   CoE state: DICT CHECK.

   312

*/

   313

   314

void ec_fsm_coe_dict_check(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

        ec_master_queue_datagram(fsm->slave->master, datagram);

   321

        return;

   322

    }

   323

   324

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   325

        fsm->state = ec_fsm_coe_error;

   326

        EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n",

   327

               slave->ring_position);

   328

        return;

   329

    }

   330

   331

    if (datagram->working_counter != 1) {

   332

        fsm->state = ec_fsm_coe_error;

   333

        EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did"

   334

               " not respond.\n",

   335

               slave->ring_position);

   336

        return;

   337

    }

   338

   339

    if (!ec_slave_mbox_check(datagram)) {

   340

        if (datagram->cycles_received

   341

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

   342

            fsm->state = ec_fsm_coe_error;

   343

            EC_ERR("Timeout while checking SDO dictionary on slave %i.\n",

   344

                   slave->ring_position);

   345

            return;

   346

        }

   347

   348

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

   349

        ec_master_queue_datagram(fsm->slave->master, datagram);

   350

        fsm->retries = EC_FSM_RETRIES;

   351

        return;

   352

    }

   353

   354

    // Fetch response

   355

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

   356

    ec_master_queue_datagram(fsm->slave->master, datagram);

   357

    fsm->retries = EC_FSM_RETRIES;

   358

    fsm->state = ec_fsm_coe_dict_response;

   359

}

   360

   361

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

   362

   363

/**

   364

   CoE state: DICT RESPONSE.

   365

   \todo Timeout behavior

   366

*/

   367

   368

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

   369

{

   370

    ec_datagram_t *datagram = fsm->datagram;

   371

    ec_slave_t *slave = fsm->slave;

   372

    uint8_t *data, mbox_prot;

   373

    size_t rec_size;

   374

    unsigned int sdo_count, i;

   375

    uint16_t sdo_index;

   376

    ec_sdo_t *sdo;

   377

   378

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

   379

        // FIXME: request again?

   380

        ec_master_queue_datagram(fsm->slave->master, datagram);

   381

        return;

   382

    }

   383

   384

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   385

        fsm->state = ec_fsm_coe_error;

   386

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

   387

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

   388

        return;

   389

    }

   390

   391

    if (datagram->working_counter != 1) {

   392

        fsm->state = ec_fsm_coe_error;

   393

        EC_ERR("Reception of CoE dictionary response failed - slave %i did"

   394

               " not respond.\n", slave->ring_position);

   395

        return;

   396

    }

   397

   398

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

   399

				     &mbox_prot, &rec_size))) {

   400

        fsm->state = ec_fsm_coe_error;

   401

        return;

   402

    }

   403

   404

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

   405

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

   406

        fsm->state = ec_fsm_coe_error;

   407

	return;

   408

    }

   409

   410

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

   411

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

   412

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

   413

               slave->ring_position);

   414

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   415

        fsm->state = ec_fsm_coe_error;

   416

	return;

   417

    }

   418

   419

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

   420

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

   421

        EC_ERR("Invalid SDO list response at slave %i!\n",

   422

               slave->ring_position);

   423

        ec_print_data(data, rec_size);

   424

        fsm->state = ec_fsm_coe_error;

   425

	return;

   426

    }

   427

   428

    if (rec_size < 8) {

   429

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

   430

        ec_print_data(data, rec_size);

   431

        fsm->state = ec_fsm_coe_error;

   432

	return;

   433

    }

   434

   435

    sdo_count = (rec_size - 8) / 2;

   436

   437

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

   438

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

   439

        if (!sdo_index) {

   440

            if (slave->master->debug_level)

   441

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

   442

                        slave->ring_position);

   443

            continue;

   444

        }

   445

   446

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

   447

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

   448

            fsm->state = ec_fsm_coe_error;

   449

            return;

   450

        }

   451

   452

        if (ec_sdo_init(sdo, sdo_index, slave)) {

   453

            EC_ERR("Failed to init SDO!\n");

   454

            fsm->state = ec_fsm_coe_error;

   455

            return;

   456

        }

   457

   458

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

   459

    }

   460

   461

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

   462

        fsm->cycles_start = datagram->cycles_sent;

   463

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

   464

        ec_master_queue_datagram(fsm->slave->master, datagram);

   465

        fsm->retries = EC_FSM_RETRIES;

   466

        fsm->state = ec_fsm_coe_dict_check;

   467

        return;

   468

    }

   469

   470

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

   471

        // no SDOs in dictionary. finished.

   472

        fsm->state = ec_fsm_coe_end; // success

   473

        return;

   474

    }

   475

   476

    // fetch SDO descriptions

   477

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

   478

   479

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

   480

        fsm->state = ec_fsm_coe_error;

   481

        return;

   482

    }

   483

   484

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

   485

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

   486

    EC_WRITE_U8 (data + 3, 0x00);

   487

    EC_WRITE_U16(data + 4, 0x0000);

   488

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

   489

   490

    ec_master_queue_datagram(fsm->slave->master, datagram);

   491

    fsm->retries = EC_FSM_RETRIES;

   492

    fsm->state = ec_fsm_coe_dict_desc_request;

   493

}

   494

   495

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

   496

   497

/**

   498

   CoE state: DICT DESC REQUEST.

   499

   \todo Timeout behavior

   500

*/

   501

   502

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

   503

{

   504

    ec_datagram_t *datagram = fsm->datagram;

   505

    ec_slave_t *slave = fsm->slave;

   506

   507

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

   508

        // FIXME: check for response first?

   509

        ec_master_queue_datagram(fsm->slave->master, datagram);

   510

        return;

   511

    }

   512

   513

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   514

        fsm->state = ec_fsm_coe_error;

   515

        EC_ERR("Failed to receive CoE SDO description request datagram for"

   516

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

   517

        return;

   518

    }

   519

   520

    if (datagram->working_counter != 1) {

   521

        fsm->state = ec_fsm_coe_error;

   522

        EC_ERR("Reception of CoE SDO description request failed - slave %i did"

   523