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 slave state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "slave_config.h"

    45

    46

#include "fsm_slave_scan.h"

    47

    48

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

    49

    50

void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *);

    51

void ec_fsm_slave_scan_state_address(ec_fsm_slave_scan_t *);

    52

void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *);

    53

void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *);

    54

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);

    55

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);

    56

void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);

    57

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    58

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    59

    60

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    61

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    62

    63

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    64

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    65

    66

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

    67

    68

/** Constructor.

    69

 */

    70

void ec_fsm_slave_scan_init(

    71

        ec_fsm_slave_scan_t *fsm, /**< Slave scanning state machine. */

    72

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

    73

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    74

                                                  state machine to use. */

    75

        ec_fsm_coe_map_t *fsm_coe_map /**< Pdo mapping state machine to use.

    76

                                       */

    77

        )

    78

{

    79

    fsm->datagram = datagram;

    80

    fsm->fsm_slave_config = fsm_slave_config;

    81

    fsm->fsm_coe_map = fsm_coe_map;

    82

    83

    // init sub state machines

    84

    ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);

    85

}

    86

    87

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

    88

    89

/** Destructor.

    90

 */

    91

void ec_fsm_slave_scan_clear(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

    92

{

    93

    // clear sub state machines

    94

    ec_fsm_sii_clear(&fsm->fsm_sii);

    95

}

    96

    97

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

    98

    99

/**

   100

 * Start slave scan state machine.

   101

 */

   102

   103

void ec_fsm_slave_scan_start(

   104

        ec_fsm_slave_scan_t *fsm, /**< slave state machine */

   105

        ec_slave_t *slave /**< slave to configure */

   106

        )

   107

{

   108

    fsm->slave = slave;

   109

    fsm->state = ec_fsm_slave_scan_state_start;

   110

}

   111

   112

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

   113

   114

/**

   115

   \return false, if state machine has terminated

   116

*/

   117

   118

int ec_fsm_slave_scan_running(const ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   119

{

   120

    return fsm->state != ec_fsm_slave_scan_state_end

   121

        && fsm->state != ec_fsm_slave_scan_state_error;

   122

}

   123

   124

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

   125

   126

/**

   127

   Executes the current state of the state machine.

   128

   If the state machine's datagram is not sent or received yet, the execution

   129

   of the state machine is delayed to the next cycle.

   130

   \return false, if state machine has terminated

   131

*/

   132

   133

int ec_fsm_slave_scan_exec(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   134

{

   135

    if (fsm->datagram->state == EC_DATAGRAM_SENT

   136

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

   137

        // datagram was not sent or received yet.

   138

        return ec_fsm_slave_scan_running(fsm);

   139

    }

   140

   141

    fsm->state(fsm);

   142

    return ec_fsm_slave_scan_running(fsm);

   143

}

   144

   145

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

   146

   147

/**

   148

   \return true, if the state machine terminated gracefully

   149

*/

   150

   151

int ec_fsm_slave_scan_success(const ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   152

{

   153

    return fsm->state == ec_fsm_slave_scan_state_end;

   154

}

   155

   156

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

   157

 *  slave scan state machine

   158

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

   159

   160

/**

   161

   Slave scan state: START.

   162

   First state of the slave state machine. Writes the station address to the

   163

   slave, according to its ring position.

   164

*/

   165

   166

void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   167

{

   168

    // write station address

   169

    ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2);

   170

    EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address);

   171

    fsm->retries = EC_FSM_RETRIES;

   172

    fsm->state = ec_fsm_slave_scan_state_address;

   173

}

   174

   175

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

   176

   177

/**

   178

   Slave scan state: ADDRESS.

   179

*/

   180

   181

void ec_fsm_slave_scan_state_address(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   182

{

   183

    ec_datagram_t *datagram = fsm->datagram;

   184

   185

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

   186

        return;

   187

   188

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   189

        fsm->state = ec_fsm_slave_scan_state_error;

   190

        EC_ERR("Failed to receive station address datagram for slave %u"

   191

                " (datagram state %u)\n",

   192

                fsm->slave->ring_position, datagram->state);

   193

        return;

   194

    }

   195

   196

    if (datagram->working_counter != 1) {

   197

        fsm->slave->error_flag = 1;

   198

        fsm->state = ec_fsm_slave_scan_state_error;

   199

        EC_ERR("Failed to write station address on slave %u: ",

   200

               fsm->slave->ring_position);

   201

        ec_datagram_print_wc_error(datagram);

   202

        return;

   203

    }

   204

   205

    // Read AL state

   206

    ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0130, 2);

   207

    fsm->retries = EC_FSM_RETRIES;

   208

    fsm->state = ec_fsm_slave_scan_state_state;

   209

}

   210

   211

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

   212

   213

/**

   214

   Slave scan state: STATE.

   215

*/

   216

   217

void ec_fsm_slave_scan_state_state(

   218

        ec_fsm_slave_scan_t *fsm /**< slave state machine */

   219

        )

   220

{

   221

    ec_datagram_t *datagram = fsm->datagram;

   222

    ec_slave_t *slave = fsm->slave;

   223

   224

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

   225

        return;

   226

   227

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   228

        fsm->state = ec_fsm_slave_scan_state_error;

   229

        EC_ERR("Failed to receive AL state datagram from slave %u"

   230

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

   231

               fsm->slave->ring_position, datagram->state);

   232

        return;

   233

    }

   234

   235

    if (datagram->working_counter != 1) {

   236

        fsm->slave->error_flag = 1;

   237

        fsm->state = ec_fsm_slave_scan_state_error;

   238

        EC_ERR("Failed to read AL state of slave %u: ",

   239

               fsm->slave->ring_position);

   240

        ec_datagram_print_wc_error(datagram);

   241

        return;

   242

    }

   243

   244

    slave->current_state = EC_READ_U8(datagram->data);

   245

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   246

        char state_str[EC_STATE_STRING_SIZE];

   247

        ec_state_string(slave->current_state, state_str);

   248

        EC_WARN("Slave %u has state error bit set (%s)!\n",

   249

                slave->ring_position, state_str);

   250

    }

   251

   252

    // read base data

   253

    ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0000, 6);

   254

    fsm->retries = EC_FSM_RETRIES;

   255

    fsm->state = ec_fsm_slave_scan_state_base;

   256

}

   257

   258

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

   259

   260

/**

   261

   Slave scan state: BASE.

   262

*/

   263

   264

void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   265

{

   266

    ec_datagram_t *datagram = fsm->datagram;

   267

    ec_slave_t *slave = fsm->slave;

   268

   269

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

   270

        return;

   271

   272

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   273

        fsm->state = ec_fsm_slave_scan_state_error;

   274

        EC_ERR("Failed to receive base data datagram for slave %u"

   275

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

   276

               slave->ring_position, datagram->state);

   277

        return;

   278

    }

   279

   280

    if (datagram->working_counter != 1) {

   281

        fsm->slave->error_flag = 1;

   282

        fsm->state = ec_fsm_slave_scan_state_error;

   283

        EC_ERR("Failed to read base data from slave %u: ",

   284

               slave->ring_position);

   285

        ec_datagram_print_wc_error(datagram);

   286

        return;

   287

    }

   288

   289

    slave->base_type       = EC_READ_U8 (datagram->data);

   290

    slave->base_revision   = EC_READ_U8 (datagram->data + 1);

   291

    slave->base_build      = EC_READ_U16(datagram->data + 2);

   292

    slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4);

   293

   294

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   295

        EC_WARN("Slave %u has more FMMUs (%u) than the master can"

   296

                " handle (%u).\n", slave->ring_position,

   297

                slave->base_fmmu_count, EC_MAX_FMMUS);

   298

        slave->base_fmmu_count = EC_MAX_FMMUS;

   299

    }

   300

   301

    // read data link status

   302

    ec_datagram_fprd(datagram, slave->station_address, 0x0110, 2);

   303

    fsm->retries = EC_FSM_RETRIES;

   304

    fsm->state = ec_fsm_slave_scan_state_datalink;

   305

}

   306

   307

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

   308

   309

/**

   310

   Slave scan state: DATALINK.

   311

*/

   312

   313

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   314

{

   315

    ec_datagram_t *datagram = fsm->datagram;

   316

    ec_slave_t *slave = fsm->slave;

   317

    uint16_t dl_status;

   318

    unsigned int i;

   319

   320

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

   321

        return;

   322

   323

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   324

        fsm->state = ec_fsm_slave_scan_state_error;

   325

        EC_ERR("Failed to receive DL status datagram from slave %u"

   326

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

   327

               slave->ring_position, datagram->state);

   328

        return;

   329

    }

   330

   331

    if (datagram->working_counter != 1) {

   332

        fsm->slave->error_flag = 1;

   333

        fsm->state = ec_fsm_slave_scan_state_error;

   334

        EC_ERR("Failed to read DL status from slave %u: ",

   335

               slave->ring_position);

   336

        ec_datagram_print_wc_error(datagram);

   337

        return;

   338

    }

   339

   340

    dl_status = EC_READ_U16(datagram->data);

   341

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

   342

        slave->ports[i].dl_link = dl_status & (1 << (4 + i)) ? 1 : 0;

   343

        slave->ports[i].dl_loop = dl_status & (1 << (8 + i * 2)) ? 1 : 0;

   344

        slave->ports[i].dl_signal = dl_status & (1 << (9 + i * 2)) ? 1 : 0;

   345

    }

   346

   347

    // Start fetching SII size

   348

   349

    fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header

   350

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   351

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   352

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   353

    fsm->state(fsm); // execute state immediately

   354

}

   355

   356

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

   357

   358

/**

   359

   Slave scan state: SII SIZE.

   360

*/

   361

   362

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   363

{

   364

    ec_slave_t *slave = fsm->slave;

   365

    uint16_t cat_type, cat_size;

   366

   367

    if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   368

   369

    if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   370

        fsm->slave->error_flag = 1;

   371

        fsm->state = ec_fsm_slave_scan_state_error;

   372

        EC_ERR("Failed to read SII size of slave %u.\n",

   373

               slave->ring_position);

   374

        return;

   375

    }

   376

   377

    cat_type = EC_READ_U16(fsm->fsm_sii.value);

   378

    cat_size = EC_READ_U16(fsm->fsm_sii.value + 2);

   379

   380

    if (cat_type != 0xFFFF) { // not the last category

   381

        off_t next_offset = 2UL + fsm->sii_offset + cat_size;

   382

        if (next_offset >= EC_MAX_SII_SIZE) {

   383

            EC_WARN("SII size of slave %u exceeds"

   384

                    " %u words (0xffff limiter missing?).\n",

   385

                    slave->ring_position, EC_MAX_SII_SIZE);

   386

            // cut off category data...

   387

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   388

            goto alloc_sii;

   389

        }

   390

        fsm->sii_offset = next_offset;

   391

        ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   392

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   393

        ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   394

        return;

   395

    }

   396

   397

    slave->sii_nwords = fsm->sii_offset + 1;

   398

   399

alloc_sii:

   400

    if (slave->sii_words) {

   401

        EC_WARN("Freeing old SII data on slave %u...\n",

   402

                slave->ring_position);

   403

        kfree(slave->sii_words);

   404

    }

   405

   406

    if (!(slave->sii_words =

   407

                (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_KERNEL))) {

   408

        EC_ERR("Failed to allocate %u words of SII data for slave %u.\n",

   409

               slave->sii_nwords, slave->ring_position);

   410

        slave->sii_nwords = 0;

   411

        slave->error_flag = 1;

   412

        fsm->state = ec_fsm_slave_scan_state_error;

   413

        return;

   414

    }

   415

   416

    // Start fetching SII contents

   417

   418

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   419

    fsm->sii_offset = 0x0000;

   420

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   421

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   422

    ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   423

}

   424

   425

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

   426

   427

/**

   428

   Slave scan state: SII DATA.

   429

*/

   430

   431

void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state machine */)

   432

{

   433

    ec_slave_t *slave = fsm->slave;

   434

    uint16_t *cat_word, cat_type, cat_size;

   435

   436

    if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   437

   438

    if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   439

        fsm->slave->error_flag = 1;

   440

        fsm->state = ec_fsm_slave_scan_state_error;

   441

        EC_ERR("Failed to fetch SII contents of slave %u.\n",

   442

               slave->ring_position);

   443

        return;

   444

    }

   445

   446

    // 2 words fetched

   447

   448

    if (fsm->sii_offset + 2 <= slave->sii_nwords) { // 2 words fit

   449

        memcpy(slave->sii_words + fsm->sii_offset, fsm->fsm_sii.value, 4);

   450

    } else { // copy the last word

   451

        memcpy(slave->sii_words + fsm->sii_offset, fsm->fsm_sii.value, 2);

   452

    }

   453

   454

    if (fsm->sii_offset + 2 < slave->sii_nwords) {

   455

        // fetch the next 2 words

   456

        fsm->sii_offset += 2;

   457

        ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   458

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   459

        ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   460

        return;

   461

    }

   462

   463

    // Evaluate SII contents

   464

   465

    ec_slave_clear_sync_managers(slave);

   466

   467

    slave->sii.alias =

   468

        EC_READ_U16(slave->sii_words + 0x0004);

   469

    slave->sii.vendor_id =

   470

        EC_READ_U32(slave->sii_words + 0x0008);

   471

    slave->sii.product_code =

   472

        EC_READ_U32(slave->sii_words + 0x000A);

   473

    slave->sii.revision_number =

   474

        EC_READ_U32(slave->sii_words + 0x000C);

   475

    slave->sii.serial_number =

   476

        EC_READ_U32(slave->sii_words + 0x000E);

   477

    slave->sii.rx_mailbox_offset =

   478

        EC_READ_U16(slave->sii_words + 0x0018);

   479

    slave->sii.rx_mailbox_size =

   480

        EC_READ_U16(slave->sii_words + 0x0019);

   481

    slave->sii.tx_mailbox_offset =

   482

        EC_READ_U16(slave->sii_words + 0x001A);

   483

    slave->sii.tx_mailbox_size =

   484

        EC_READ_U16(slave->sii_words + 0x001B);

   485

    slave->sii.mailbox_protocols =

   486

        EC_READ_U16(slave->sii_words + 0x001C);

   487

   488

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   489

        // sii does not contain category data

   490

        fsm->state = ec_fsm_slave_scan_state_end;

   491

        return;

   492

    }

   493

   494

    if (slave->sii_nwords < EC_FIRST_SII_CATEGORY_OFFSET + 1) {

   495

        EC_ERR("Unexpected end of SII data in slave %u:"

   496

                " First category header missing.\n",

   497

                slave->ring_position);

   498

        goto end;

   499

    }

   500

   501

    // evaluate category data

   502

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   503

    while (EC_READ_U16(cat_word) != 0xFFFF) {

   504

   505

        // type and size words must fit

   506

        if (cat_word + 2 - slave->sii_words > slave->sii_nwords) {

   507

            EC_ERR("Unexpected end of SII data in slave %u:"

   508

                    " Category header incomplete.\n",

   509

                    slave->ring_position);

   510

            goto end;

   511

        }

   512

   513

        cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   514

        cat_size = EC_READ_U16(cat_word + 1);

   515

        cat_word += 2;

   516