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

    fsm->retries = EC_FSM_RETRIES;

   264

    fsm->state = ec_fsm_coe_dict_request;

   265

}

   266

   267

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

   268

   269

/**

   270

   CoE state: DICT REQUEST.

   271

   \todo Timeout behavior

   272

*/

   273

   274

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

   275

{

   276

    ec_datagram_t *datagram = fsm->datagram;

   277

    ec_slave_t *slave = fsm->slave;

   278

   279

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

   280

        return; // FIXME: request again?

   281

   282

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   283

        fsm->state = ec_fsm_coe_error;

   284

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

   285

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

   286

               slave->ring_position, datagram->state);

   287

        return;

   288

    }

   289

   290

    if (datagram->working_counter != 1) {

   291

        fsm->state = ec_fsm_coe_error;

   292

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

   293

                slave->ring_position);

   294

        ec_datagram_print_wc_error(datagram);

   295

        return;

   296

    }

   297

   298

    fsm->cycles_start = datagram->cycles_sent;

   299

   300

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

   301

    fsm->retries = EC_FSM_RETRIES;

   302

    fsm->state = ec_fsm_coe_dict_check;

   303

}

   304

   305

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

   306

   307

/**

   308

   CoE state: DICT CHECK.

   309

*/

   310

   311

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

   312

{

   313

    ec_datagram_t *datagram = fsm->datagram;

   314

    ec_slave_t *slave = fsm->slave;

   315

   316

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

   317

        return;

   318

   319

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   320

        fsm->state = ec_fsm_coe_error;

   321

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

   322

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

   323

               slave->ring_position, datagram->state);

   324

        return;

   325

    }

   326

   327

    if (datagram->working_counter != 1) {

   328

        fsm->state = ec_fsm_coe_error;

   329

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

   330

               slave->ring_position);

   331

        ec_datagram_print_wc_error(datagram);

   332

        return;

   333

    }

   334

   335

    if (!ec_slave_mbox_check(datagram)) {

   336

        if (datagram->cycles_received

   337

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

   338

            fsm->state = ec_fsm_coe_error;

   339

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

   340

                   slave->ring_position);

   341

            return;

   342

        }

   343

   344

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

   345

        fsm->retries = EC_FSM_RETRIES;

   346

        return;

   347

    }

   348

   349

    // Fetch response

   350

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

   351

    fsm->retries = EC_FSM_RETRIES;

   352

    fsm->state = ec_fsm_coe_dict_response;

   353

}

   354

   355

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

   356

   357

/**

   358

   CoE state: DICT RESPONSE.

   359

   \todo Timeout behavior

   360

*/

   361

   362

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

   363

{

   364

    ec_datagram_t *datagram = fsm->datagram;

   365

    ec_slave_t *slave = fsm->slave;

   366

    uint8_t *data, mbox_prot;

   367

    size_t rec_size;

   368

    unsigned int sdo_count, i;

   369

    uint16_t sdo_index;

   370

    ec_sdo_t *sdo;

   371

   372

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

   373

        return; // FIXME: request again?

   374

   375

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   376

        fsm->state = ec_fsm_coe_error;

   377

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

   378

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

   379

               slave->ring_position, datagram->state);

   380

        return;

   381

    }

   382

   383

    if (datagram->working_counter != 1) {

   384

        fsm->state = ec_fsm_coe_error;

   385

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

   386

                slave->ring_position);

   387

        ec_datagram_print_wc_error(datagram);

   388

        return;

   389

    }

   390

   391

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

   392

				     &mbox_prot, &rec_size))) {

   393

        fsm->state = ec_fsm_coe_error;

   394

        return;

   395

    }

   396

   397

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

   398

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

   399

        fsm->state = ec_fsm_coe_error;

   400

	return;

   401

    }

   402

   403

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

   404

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

   405

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

   406

               slave->ring_position);

   407

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   408

        fsm->state = ec_fsm_coe_error;

   409

	return;

   410

    }

   411

   412

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

   413

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

   414

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

   415

               slave->ring_position);

   416

        ec_print_data(data, rec_size);

   417

        fsm->state = ec_fsm_coe_error;

   418

	return;

   419

    }

   420

   421

    if (rec_size < 8) {

   422

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

   423

        ec_print_data(data, rec_size);

   424

        fsm->state = ec_fsm_coe_error;

   425

	return;

   426

    }

   427

   428

    sdo_count = (rec_size - 8) / 2;

   429

   430

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

   431

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

   432

        if (!sdo_index) {

   433

            if (slave->master->debug_level)

   434

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

   435

                        slave->ring_position);

   436

            continue;

   437

        }

   438

   439

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

   440

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

   441

            fsm->state = ec_fsm_coe_error;

   442

            return;

   443

        }

   444

   445

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

   446

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

   447

            fsm->state = ec_fsm_coe_error;

   448

            return;

   449

        }

   450

   451

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

   452

    }

   453

   454

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

   455

        fsm->cycles_start = datagram->cycles_sent;

   456

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

   457

        fsm->retries = EC_FSM_RETRIES;

   458

        fsm->state = ec_fsm_coe_dict_check;

   459

        return;

   460

    }

   461

   462

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

   463

        // no SDOs in dictionary. finished.

   464

        fsm->state = ec_fsm_coe_end; // success

   465

        return;

   466

    }

   467

   468

    // fetch SDO descriptions

   469

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

   470

   471

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

   472

        fsm->state = ec_fsm_coe_error;

   473

        return;

   474

    }

   475

   476

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

   477

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

   478

    EC_WRITE_U8 (data + 3, 0x00);

   479

    EC_WRITE_U16(data + 4, 0x0000);

   480

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

   481

   482

    fsm->retries = EC_FSM_RETRIES;

   483

    fsm->state = ec_fsm_coe_dict_desc_request;

   484

}

   485

   486

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

   487

   488

/**

   489

   CoE state: DICT DESC REQUEST.

   490

   \todo Timeout behavior

   491

*/

   492

   493

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

   494

{

   495

    ec_datagram_t *datagram = fsm->datagram;

   496

    ec_slave_t *slave = fsm->slave;

   497

   498

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

   499

        return; // FIXME: check for response first?

   500

   501

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   502

        fsm->state = ec_fsm_coe_error;

   503

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

   504

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

   505

               slave->ring_position, datagram->state);

   506

        return;

   507

    }

   508

   509

    if (datagram->working_counter != 1) {

   510

        fsm->state = ec_fsm_coe_error;

   511

        EC_ERR("Reception of CoE SDO description"

   512

                " request failed on slave %i: ", slave->ring_position);

   513

        ec_datagram_print_wc_error(datagram);

   514

        return;

   515

    }

   516

   517

    fsm->cycles_start = datagram->cycles_sent;

   518

   519

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

   520

    fsm->retries = EC_FSM_RETRIES;

   521

    fsm->state = ec_fsm_coe_dict_desc_check;

   522

}