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

   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"

   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 - slave %i did"

   293

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

   294

        return;

   295

    }

   296

   297

    fsm->cycles_start = datagram->cycles_sent;

   298

   299

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

   300

    fsm->retries = EC_FSM_RETRIES;

   301

    fsm->state = ec_fsm_coe_dict_check;

   302

}

   303

   304

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

   305

   306

/**

   307

   CoE state: DICT CHECK.

   308

*/

   309

   310

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

   311

{

   312

    ec_datagram_t *datagram = fsm->datagram;

   313

    ec_slave_t *slave = fsm->slave;

   314

   323

        return;

   324

    }

   325

   326

    if (datagram->working_counter != 1) {

   327

        fsm->state = ec_fsm_coe_error;

   328

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

   329

               " not respond.\n",

   330

               slave->ring_position);

   331

        return;

   332

    }

   333

   334

    if (!ec_slave_mbox_check(datagram)) {

   335

        if (datagram->cycles_received

   336

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

   337

            fsm->state = ec_fsm_coe_error;

   338

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

   339

                   slave->ring_position);

   340

            return;

   341

        }

   342

   343

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

   344

        fsm->retries = EC_FSM_RETRIES;

   345

        return;

   346

    }

   347

   348

    // Fetch response

   349

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

   350

    fsm->retries = EC_FSM_RETRIES;

   351

    fsm->state = ec_fsm_coe_dict_response;

   352

}

   353

   354

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

   355

   356

/**

   357

   CoE state: DICT RESPONSE.

   358

   \todo Timeout behavior

   359

*/

   360

   361

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

   362

{

   363

    ec_datagram_t *datagram = fsm->datagram;

   364

    ec_slave_t *slave = fsm->slave;

   365

    uint8_t *data, mbox_prot;

   366

    size_t rec_size;

   367

    unsigned int sdo_count, i;

   368

    uint16_t sdo_index;

   369

    ec_sdo_t *sdo;

   370

   373

   374

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   375

        fsm->state = ec_fsm_coe_error;

   376

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

   379

        return;

   380

    }

   381

   382

    if (datagram->working_counter != 1) {

   383

        fsm->state = ec_fsm_coe_error;

   384

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

   385

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

   386

        return;

   387

    }

   388

   389

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

   390

				     &mbox_prot, &rec_size))) {

   391

        fsm->state = ec_fsm_coe_error;

   392

        return;

   393

    }

   394

   395

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

   396

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

   397

        fsm->state = ec_fsm_coe_error;

   398

	return;

   399

    }

   400

   401

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

   402

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

   403

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

   404

               slave->ring_position);

   405

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   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) != 0x02) { // Get OD List response

   412

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

   413

               slave->ring_position);

   414

        ec_print_data(data, rec_size);

   415

        fsm->state = ec_fsm_coe_error;

   416

	return;

   417

    }

   418

   419

    if (rec_size < 8) {

   420

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

   421

        ec_print_data(data, rec_size);

   422

        fsm->state = ec_fsm_coe_error;

   423

	return;

   424

    }

   425

   426

    sdo_count = (rec_size - 8) / 2;

   427

   428

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

   429

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

   430

        if (!sdo_index) {

   431

            if (slave->master->debug_level)

   432

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

   433

                        slave->ring_position);

   434

            continue;

   435

        }

   436

   437

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

   438

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

   439

            fsm->state = ec_fsm_coe_error;

   440

            return;

   441

        }

   442

   443

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

   444

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

   445

            fsm->state = ec_fsm_coe_error;

   446

            return;

   447

        }

   448

   449

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

   450

    }

   451

   452

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

   453

        fsm->cycles_start = datagram->cycles_sent;

   454

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

   455

        fsm->retries = EC_FSM_RETRIES;

   456

        fsm->state = ec_fsm_coe_dict_check;

   457

        return;

   458

    }

   459

   460

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

   461

        // no SDOs in dictionary. finished.

   462

        fsm->state = ec_fsm_coe_end; // success

   463

        return;

   464

    }

   465

   466

    // fetch SDO descriptions

   467

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

   468

   469

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

   470

        fsm->state = ec_fsm_coe_error;

   471

        return;

   472

    }

   473

   474

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

   475

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

   476

    EC_WRITE_U8 (data + 3, 0x00);

   477

    EC_WRITE_U16(data + 4, 0x0000);

   478

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

   479

   480

    fsm->retries = EC_FSM_RETRIES;

   481

    fsm->state = ec_fsm_coe_dict_desc_request;

   482

}

   483

   484

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

   485

   486

/**

   487

   CoE state: DICT DESC REQUEST.

   488

   \todo Timeout behavior

   489

*/

   490

   491

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

   492

{

   493

    ec_datagram_t *datagram = fsm->datagram;

   494

    ec_slave_t *slave = fsm->slave;

   495

   498

   499

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   500

        fsm->state = ec_fsm_coe_error;

   501

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

   504

        return;

   505

    }

   506

   507

    if (datagram->working_counter != 1) {

   508

        fsm->state = ec_fsm_coe_error;

   509

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

   510

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

   511

        return;

   512

    }

   513

   514

    fsm->cycles_start = datagram->cycles_sent;

   515

   516

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

   517

    fsm->retries = EC_FSM_RETRIES;

   518

    fsm->state = ec_fsm_coe_dict_desc_check;

   519

}

   520

   521

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

   522