1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

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

    14

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

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

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

    19

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

    20

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

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/** \file EtherCAT CoE state machines.

    31

 */

    32

    33

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

    34

    35

#include "globals.h"

    36

#include "master.h"

    37

#include "mailbox.h"

    38

#include "fsm_coe.h"

    39

#include "slave_config.h"

    40

    41

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

    42

    43

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

    44

 */

    45

#define EC_FSM_COE_DICT_TIMEOUT 1000

    46

    47

/** CoE download request header size.

    48

 */

    49

#define EC_COE_DOWN_REQ_HEADER_SIZE 10

    50

    51

/** CoE download segment request header size.

    52

 */

    53

#define EC_COE_DOWN_SEG_REQ_HEADER_SIZE 3

    54

    55

/** Minimum size of download segment.

    56

 */

    57

#define EC_COE_DOWN_SEG_MIN_DATA_SIZE 7

    58

    59

/** Enable debug output for CoE retries.

    60

 */

    61

#define DEBUG_RETRIES 0

    62

    63

/** Enable warning output if transfers take too long.

    64

 */

    65

#define DEBUG_LONG 0

    66

    67

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

    68

    69

void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *);

    70

void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *);

    71

void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *);

    72

void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *);

    73

void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *);

    74

void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *);

    75

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *);

    76

void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *);

    77

void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *);

    78

void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *);

    79

    80

void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *);

    81

void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *);

    82

void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *);

    83

void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *);

    84

void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *);

    85

void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *);

    86

    87

void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *);

    88

void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *);

    89

void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *);

    90

void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *);

    91

void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *);

    92

void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *);

    93

void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *);

    94

    95

void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *);

    96

void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *);

    97

    98

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

    99

   100

/** SDO abort messages.

   101

 *

   102

 * The "abort SDO transfer request" supplies an abort code, which can be

   103

 * translated to clear text. This table does the mapping of the codes and

   104

 * messages.

   105

 */

   106

const ec_code_msg_t sdo_abort_messages[] = {

   107

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

   108

    {0x05040000, "SDO protocol timeout"},

   109

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

   110

    {0x05040005, "Out of memory"},

   111

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

   112

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

   113

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

   114

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

   115

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

   116

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

   117

     " exceed the PDO length"},

   118

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

   119

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

   120

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

   121

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

   122

     " not match"},

   123

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

   124

     " high"},

   125

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

   126

     " low"},

   127

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

   128

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

   129

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

   130

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

   131

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

   132

    {0x08000000, "General error"},

   133

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

   134

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

   135

     " because of local control"},

   136

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

   137

     " because of the present device state"},

   138

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

   139

     " dictionary is present"},

   140

    {}

   141

};

   142

   143

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

   144

   145

/** Outputs an SDO abort message.

   146

 */

   147

void ec_canopen_abort_msg(

   148

        const ec_slave_t *slave, /**< Slave. */

   149

        uint32_t abort_code /**< Abort code to search for. */

   150

        )

   151

{

   152

    const ec_code_msg_t *abort_msg;

   153

   154

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

   155

        if (abort_msg->code == abort_code) {

   156

            EC_SLAVE_ERR(slave, "SDO abort message 0x%08X: \"%s\".\n",

   157

                   abort_msg->code, abort_msg->message);

   158

            return;

   159

        }

   160

    }

   161

   162

    EC_SLAVE_ERR(slave, "Unknown SDO abort code 0x%08X.\n", abort_code);

   163

}

   164

   165

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

   166

   167

/** Constructor.

   168

 */

   169

void ec_fsm_coe_init(

   170

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

   171

        )

   172

{

   173

    fsm->state = NULL;

   174

    fsm->datagram = NULL;

   175

}

   176

   177

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

   178

   179

/** Destructor.

   180

 */

   181

void ec_fsm_coe_clear(

   182

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

   183

        )

   184

{

   185

}

   186

   187

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

   188

   189

/** Starts reading a slaves' SDO dictionary.

   190

 */

   191

void ec_fsm_coe_dictionary(

   192

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

   193

        ec_slave_t *slave /**< EtherCAT slave */

   194

        )

   195

{

   196

    fsm->slave = slave;

   197

    fsm->state = ec_fsm_coe_dict_start;

   198

}

   199

   200

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

   201

   202

/** Starts to transfer an SDO to/from a slave.

   203

 */

   204

void ec_fsm_coe_transfer(

   205

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

   206

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

   207

        ec_sdo_request_t *request /**< SDO request. */

   208

        )

   209

{

   210

    fsm->slave = slave;

   211

    fsm->request = request;

   212

   213

    if (request->dir == EC_DIR_OUTPUT) {

   214

        fsm->state = ec_fsm_coe_down_start;

   215

    }

   216

    else {

   217

        fsm->state = ec_fsm_coe_up_start;

   218

    }

   219

}

   220

   221

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

   222

   223

/** Executes the current state of the state machine.

   224

 *

   225

 * \return 1 if the datagram was used, else 0.

   226

 */

   227

int ec_fsm_coe_exec(

   228

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

   229

        ec_datagram_t *datagram /**< Datagram to use. */

   230

        )

   231

{

   232

    int datagram_used = 0;

   233

   234

    if (fsm->datagram &&

   235

            (fsm->datagram->state == EC_DATAGRAM_INIT ||

   236

             fsm->datagram->state == EC_DATAGRAM_QUEUED ||

   237

             fsm->datagram->state == EC_DATAGRAM_SENT)) {

   238

        // datagram not received yet

   239

        return datagram_used;

   240

    }

   241

   242

    fsm->state(fsm, datagram);

   243

   244

    datagram_used =

   245

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

   246

   247

    if (datagram_used) {

   248

        fsm->datagram = datagram;

   249

    } else {

   250

        fsm->datagram = NULL;

   251

    }

   252

   253

    return datagram_used;

   254

}

   255

   256

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

   257

   258

/** Returns, if the state machine terminated with success.

   259

 * \return non-zero if successful.

   260

 */

   261

int ec_fsm_coe_success(

   262

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

   263

        )

   264

{

   265

    return fsm->state == ec_fsm_coe_end;

   266

}

   267

   268

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

   269

   270

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

   271

 *

   272

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

   273

 *

   274

 * \return The data were an emergency request.

   275

 */

   276

int ec_fsm_coe_check_emergency(

   277

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

   278

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

   279

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

   280

        )

   281

{

   282

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

   283

        return 0;

   284

   285

    if (size < 10) {

   286

        EC_SLAVE_WARN(fsm->slave, "Received incomplete CoE Emergency"

   287

                " request:\n");

   288

        ec_print_data(data, size);

   289

        return 1;

   290

    }

   291

   292

    {

   293

        ec_slave_config_t *sc = fsm->slave->config;

   294

        if (sc) {

   295

            ec_coe_emerg_ring_push(&sc->emerg_ring, data + 2);

   296

        }

   297

    }

   298

   299

    EC_SLAVE_WARN(fsm->slave, "CoE Emergency Request received:\n"

   300

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

   301

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

   302

    ec_print_data(data + 5, 5);

   303

    return 1;

   304

}

   305

   306

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

   307

 *  CoE dictionary state machine

   308

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

   309

   310

int ec_fsm_coe_prepare_dict(

   311

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

   312

        ec_datagram_t *datagram /**< Datagram to use. */

   313

        )

   314

{

   315

    ec_slave_t *slave = fsm->slave;

   316

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

   317

    if (IS_ERR(data)) {

   318

        return PTR_ERR(data);

   319

    }

   320

   321

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

   322

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

   323

    EC_WRITE_U8 (data + 3, 0x00);

   324

    EC_WRITE_U16(data + 4, 0x0000);

   325

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

   326

   327

    fsm->state = ec_fsm_coe_dict_request;

   328

    return 0;

   329

}

   330

   331

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

   332

   333

/** CoE state: DICT START.

   334

 */

   335

void ec_fsm_coe_dict_start(

   336

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

   337

        ec_datagram_t *datagram /**< Datagram to use. */

   338

        )

   339

{

   340

    ec_slave_t *slave = fsm->slave;

   341

   342

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

   343

        EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");

   344

        fsm->state = ec_fsm_coe_error;

   345

        return;

   346

    }

   347

   348

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

   349

        EC_SLAVE_ERR(slave, "Slave does not support"

   350

                " SDO information service!\n");

   351

        fsm->state = ec_fsm_coe_error;

   352

        return;

   353

    }

   354

   355

    fsm->retries = EC_FSM_RETRIES;

   356

   357

    if (ec_fsm_coe_prepare_dict(fsm, datagram)) {

   358

        fsm->state = ec_fsm_coe_error;

   359

    }

   360

}

   361

   362

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

   363

   364

/** CoE state: DICT REQUEST.

   365

 * \todo Timeout behavior

   366

 */

   367

void ec_fsm_coe_dict_request(

   368

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

   369

        ec_datagram_t *datagram /**< Datagram to use. */

   370

        )

   371

{

   372

    ec_slave_t *slave = fsm->slave;

   373

   374

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

   375

        if (ec_fsm_coe_prepare_dict(fsm, datagram)) {

   376

            fsm->state = ec_fsm_coe_error;

   377

        }

   378

        return;

   379

    }

   380

   381

    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {

   382

        fsm->state = ec_fsm_coe_error;

   383

        EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   384

                " request datagram: ");

   385

        ec_datagram_print_state(fsm->datagram);

   386

        return;

   387

    }

   388

   389

    if (fsm->datagram->working_counter != 1) {

   390

        fsm->state = ec_fsm_coe_error;

   391

        EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");

   392

        ec_datagram_print_wc_error(fsm->datagram);

   393

        return;

   394

    }

   395

   396

    fsm->jiffies_start = fsm->datagram->jiffies_sent;

   397

   398

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

   399

    fsm->retries = EC_FSM_RETRIES;

   400

    fsm->state = ec_fsm_coe_dict_check;

   401

}

   402

   403

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

   404

   405

/** CoE state: DICT CHECK.

   406

 */

   407

void ec_fsm_coe_dict_check(

   408

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

   409

        ec_datagram_t *datagram /**< Datagram to use. */

   410

        )

   411

{

   412

    ec_slave_t *slave = fsm->slave;

   413

   414

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

   415

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

   416

        return;

   417

    }

   418

   419

    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {

   420

        fsm->state = ec_fsm_coe_error;

   421

        EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   422

        ec_datagram_print_state(fsm->datagram);

   423

        return;

   424

    }

   425

   426

    if (fsm->datagram->working_counter != 1) {

   427

        fsm->state = ec_fsm_coe_error;

   428

        EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"

   429

                " datagram failed: ");

   430

        ec_datagram_print_wc_error(fsm->datagram);

   431

        return;

   432

    }

   433

   434

    if (!ec_slave_mbox_check(fsm->datagram)) {

   435

        unsigned long diff_ms =

   436

            (fsm->datagram->jiffies_received - fsm->jiffies_start) *

   437

            1000 / HZ;

   438

        if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   439

            fsm->state = ec_fsm_coe_error;

   440

            EC_SLAVE_ERR(slave, "Timeout while waiting for"

   441

                    " SDO dictionary list response.\n");

   442

            return;

   443

        }

   444

   445

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

   446

        fsm->retries = EC_FSM_RETRIES;

   447

        return;

   448

    }

   449

   450

    // Fetch response

   451

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

   452

    fsm->retries = EC_FSM_RETRIES;

   453

    fsm->state = ec_fsm_coe_dict_response;

   454

}

   455

   456

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

   457

   458

int ec_fsm_coe_dict_prepare_desc(

   459

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

   460

        ec_datagram_t *datagram /**< Datagram to use. */

   461

        )

   462

{

   463

    ec_slave_t *slave = fsm->slave;

   464

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

   465

    if (IS_ERR(data)) {

   466

        return PTR_ERR(data);

   467

    }

   468

   469

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

   470

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

   471

    EC_WRITE_U8 (data + 3, 0x00);

   472

    EC_WRITE_U16(data + 4, 0x0000);

   473

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

   474

   475

    fsm->state = ec_fsm_coe_dict_desc_request;

   476

    return 0;

   477

}

   478

   479

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

   480

   481

/**

   482

   CoE state: DICT RESPONSE.

   483

   \todo Timeout behavior

   484

*/

   485

   486

void ec_fsm_coe_dict_response(

   487

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

   488

        ec_datagram_t *datagram /**< Datagram to use. */

   489

        )

   490

{

   491

    ec_slave_t *slave = fsm->slave;

   492

    uint8_t *data, mbox_prot;

   493

    size_t rec_size;

   494

    unsigned int sdo_count, i;

   495

    uint16_t sdo_index, fragments_left;

   496

    ec_sdo_t *sdo;

   497

    bool first_segment;