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->state = ec_fsm_coe_dict_request;

   265

}

   266

   267

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

   268

   269

/**

   270

   CoE state: DICT REQUEST.

   271

*/

   272

   273

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

   274

{

   275

    ec_datagram_t *datagram = fsm->datagram;

   276

    ec_slave_t *slave = fsm->slave;

   277

   278

    if (datagram->state != EC_DATAGRAM_RECEIVED

   279

        || datagram->working_counter != 1) {

   280

        fsm->state = ec_fsm_coe_error;

   281

        EC_ERR("Reception of CoE dictionary request failed on slave %i.\n",

   282

               slave->ring_position);

   283

        return;

   284

    }

   285

   286

    fsm->cycles_start = datagram->cycles_sent;

   287

   288

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

   289

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

   290

    fsm->state = ec_fsm_coe_dict_check;

   291

}

   292

   293

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

   294

   295

/**

   296

   CoE state: DICT CHECK.

   297

*/

   298

   299

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

   300

{

   301

    ec_datagram_t *datagram = fsm->datagram;

   302

    ec_slave_t *slave = fsm->slave;

   303

   304

    if (datagram->state != EC_DATAGRAM_RECEIVED

   305

        || datagram->working_counter != 1) {

   306

        fsm->state = ec_fsm_coe_error;

   307

        EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",

   308

               slave->ring_position);

   309

        return;

   310

    }

   311

   312

    if (!ec_slave_mbox_check(datagram)) {

   313

        if (datagram->cycles_received

   314

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

   315

            fsm->state = ec_fsm_coe_error;

   316

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

   317

                   slave->ring_position);

   318

            return;

   319

        }

   320

   321

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

   322

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

   323

        return;

   324

    }

   325

   326

    // Fetch response

   327

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

   328

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

   329

    fsm->state = ec_fsm_coe_dict_response;

   330

}

   331

   332

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

   333

   334

/**

   335

   CoE state: DICT RESPONSE.

   336

*/

   337

   338

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

   339

{

   340

    ec_datagram_t *datagram = fsm->datagram;

   341

    ec_slave_t *slave = fsm->slave;

   342

    uint8_t *data, mbox_prot;

   343

    size_t rec_size;

   344

    unsigned int sdo_count, i;

   345

    uint16_t sdo_index;

   346

    ec_sdo_t *sdo;

   347

   348

    if (datagram->state != EC_DATAGRAM_RECEIVED

   349

        || datagram->working_counter != 1) {

   350

        fsm->state = ec_fsm_coe_error;

   351

        EC_ERR("Reception of CoE dictionary response failed on slave %i.\n",

   352

               slave->ring_position);

   353

        return;

   354

    }

   355

   356

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

   357

				     &mbox_prot, &rec_size))) {

   358

        fsm->state = ec_fsm_coe_error;

   359

        return;

   360

    }

   361

   362

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

   363

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

   364

        fsm->state = ec_fsm_coe_error;

   365

	return;

   366

    }

   367

   368

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

   369

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

   370

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

   371

               slave->ring_position);

   372

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   373

        fsm->state = ec_fsm_coe_error;

   374

	return;

   375

    }

   376

   377

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

   378

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

   379

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

   380

               slave->ring_position);

   381

        ec_print_data(data, rec_size);

   382

        fsm->state = ec_fsm_coe_error;

   383

	return;

   384

    }

   385

   386

    if (rec_size < 8) {

   387

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

   388

        ec_print_data(data, rec_size);

   389

        fsm->state = ec_fsm_coe_error;

   390

	return;

   391

    }

   392

   393

    sdo_count = (rec_size - 8) / 2;

   394

   395

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

   396

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

   397

        if (!sdo_index) {

   398

            if (slave->master->debug_level)

   399

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

   400

                        slave->ring_position);

   401

            continue;

   402

        }

   403

   404

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

   405

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

   406

            fsm->state = ec_fsm_coe_error;

   407

            return;

   408

        }

   409

   410

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

   411

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

   412

            fsm->state = ec_fsm_coe_error;

   413

            return;

   414

        }

   415

   416

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

   417

    }

   418

   419

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

   420

        fsm->cycles_start = datagram->cycles_sent;

   421

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

   422

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

   423

        fsm->state = ec_fsm_coe_dict_check;

   424

        return;

   425

    }

   426

   427

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

   428

        // no SDOs in dictionary. finished.

   429

        fsm->state = ec_fsm_coe_end; // success

   430

        return;

   431

    }

   432

   433

    // fetch SDO descriptions

   434

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

   435

   436

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

   437

        fsm->state = ec_fsm_coe_error;

   438

        return;

   439

    }

   440

   441

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

   442

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

   443

    EC_WRITE_U8 (data + 3, 0x00);

   444

    EC_WRITE_U16(data + 4, 0x0000);

   445

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

   446

   447

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

   448

    fsm->state = ec_fsm_coe_dict_desc_request;

   449

}

   450

   451

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

   452

   453

/**

   454

   CoE state: DICT DESC REQUEST.

   455

*/

   456

   457

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

   458

{

   459

    ec_datagram_t *datagram = fsm->datagram;

   460

    ec_slave_t *slave = fsm->slave;

   461

   462

    if (datagram->state != EC_DATAGRAM_RECEIVED

   463

        || datagram->working_counter != 1) {

   464

        fsm->state = ec_fsm_coe_error;

   465

        EC_ERR("Reception of CoE SDO description"

   466

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

   467

        return;

   468

    }

   469

   470

    fsm->cycles_start = datagram->cycles_sent;

   471

   472

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

   473

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

   474

    fsm->state = ec_fsm_coe_dict_desc_check;

   475

}

   476

   477

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

   478

   479

/**

   480

   CoE state: DICT DESC CHECK.

   481

*/

   482

   483

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

   484

{

   485

    ec_datagram_t *datagram = fsm->datagram;

   486

    ec_slave_t *slave = fsm->slave;

   487

   488

    if (datagram->state != EC_DATAGRAM_RECEIVED

   489

        || datagram->working_counter != 1) {

   490

        fsm->state = ec_fsm_coe_error;

   491

        EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",

   492

               slave->ring_position);

   493

        return;

   494

    }

   495

   496

    if (!ec_slave_mbox_check(datagram)) {

   497

        if (datagram->cycles_received

   498

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

   499

            fsm->state = ec_fsm_coe_error;

   500

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

   501

                   slave->ring_position);

   502

            return;

   503

        }

   504

   505

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

   506

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

   507

        return;

   508

    }

   509

   510

    // Fetch response

   511

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

   512

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

   513

    fsm->state = ec_fsm_coe_dict_desc_response;

   514

}

   515

   516

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

   517

   518

/**

   519

   CoE state: DICT DESC RESPONSE.

   520

*/

   521

   522

void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm

   523

                                   /**< finite state machine */)

   524

{

   525

    ec_datagram_t *datagram = fsm->datagram;

   526

    ec_slave_t *slave = fsm->slave;

   527

    ec_sdo_t *sdo = fsm->sdo;

   528

    uint8_t *data, mbox_prot;

   529

    size_t rec_size, name_size;