1

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

     2

 *

     3

 *  $Id$

     4

 *

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

    20

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

    21

 *

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   EtherCAT slave state machines.

    30

*/

    31

    32

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

    33

    34

#include "globals.h"

    35

#include "master.h"

    36

#include "mailbox.h"

    37

#include "slave_config.h"

    38

    39

#include "fsm_slave_scan.h"

    40

    41

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

    42

    43

void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *);

    44

void ec_fsm_slave_scan_state_address(ec_fsm_slave_scan_t *);

    45

void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *);

    46

void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *);

    47

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);

    48

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);

    49

void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);

    50

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    51

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    52

    53

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    54

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    55

    56

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    57

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    58

    59

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

    60

    61

/** Constructor.

    62

 */

    63

void ec_fsm_slave_scan_init(

    64

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

    65

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

    66

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    67

                                                  state machine to use. */

    68

        ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration machine to use. */

    69

        )

    70

{

    71

    fsm->datagram = datagram;

    72

    fsm->fsm_slave_config = fsm_slave_config;

    73

    fsm->fsm_pdo = fsm_pdo;

    74

    75

    // init sub state machines

    76

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

    77

}

    78

    79

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

    80

    81

/** Destructor.

    82

 */

    83

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

    84

{

    85

    // clear sub state machines

    86

    ec_fsm_sii_clear(&fsm->fsm_sii);

    87

}

    88

    89

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

    90

    91

/**

    92

 * Start slave scan state machine.

    93

 */

    94

    95

void ec_fsm_slave_scan_start(

    96

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

    97

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

    98

        )

    99

{

   100

    fsm->slave = slave;

   101

    fsm->state = ec_fsm_slave_scan_state_start;

   102

}

   103

   104

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

   105

   106

/**

   107

   \return false, if state machine has terminated

   108

*/

   109

   110

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

   111

{

   112

    return fsm->state != ec_fsm_slave_scan_state_end

   113

        && fsm->state != ec_fsm_slave_scan_state_error;

   114

}

   115

   116

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

   117

   118

/**

   119

   Executes the current state of the state machine.

   120

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

   121

   of the state machine is delayed to the next cycle.

   122

   \return false, if state machine has terminated

   123

*/

   124

   125

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

   126

{

   127

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

   128

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

   129

        // datagram was not sent or received yet.

   130

        return ec_fsm_slave_scan_running(fsm);

   131

    }

   132

   133

    fsm->state(fsm);

   134

    return ec_fsm_slave_scan_running(fsm);

   135

}

   136

   137

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

   138

   139

/**

   140

   \return true, if the state machine terminated gracefully

   141

*/

   142

   143

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

   144

{

   145

    return fsm->state == ec_fsm_slave_scan_state_end;

   146

}

   147

   148

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

   149

 *  slave scan state machine

   150

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

   151

   152

/**

   153

   Slave scan state: START.

   154

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

   155

   slave, according to its ring position.

   156

*/

   157

   158

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

   159

{

   160

    // write station address

   161

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

   162

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

   163

    fsm->retries = EC_FSM_RETRIES;

   164

    fsm->state = ec_fsm_slave_scan_state_address;

   165

}

   166

   167

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

   168

   169

/**

   170

   Slave scan state: ADDRESS.

   171

*/

   172

   173

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

   174

{

   175

    ec_datagram_t *datagram = fsm->datagram;

   176

   177

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

   178

        return;

   179

   180

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   181

        fsm->state = ec_fsm_slave_scan_state_error;

   182

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

   183

                " (datagram state %u)\n",

   184

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

   185

        return;

   186

    }

   187

   188

    if (datagram->working_counter != 1) {

   189

        fsm->slave->error_flag = 1;

   190

        fsm->state = ec_fsm_slave_scan_state_error;

   191

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

   192

               fsm->slave->ring_position);

   193

        ec_datagram_print_wc_error(datagram);

   194

        return;

   195

    }

   196

   197

    // Read AL state

   198

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

   199

    ec_datagram_zero(datagram);

   200

    fsm->retries = EC_FSM_RETRIES;

   201

    fsm->state = ec_fsm_slave_scan_state_state;

   202

}

   203

   204

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

   205

   206

/**

   207

   Slave scan state: STATE.

   208

*/

   209

   210

void ec_fsm_slave_scan_state_state(

   211

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

   212

        )

   213

{

   214

    ec_datagram_t *datagram = fsm->datagram;

   215

    ec_slave_t *slave = fsm->slave;

   216

   217

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

   218

        return;

   219

   220

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   221

        fsm->state = ec_fsm_slave_scan_state_error;

   222

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

   223

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

   224

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

   225

        return;

   226

    }

   227

   228

    if (datagram->working_counter != 1) {

   229

        fsm->slave->error_flag = 1;

   230

        fsm->state = ec_fsm_slave_scan_state_error;

   231

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

   232

               fsm->slave->ring_position);

   233

        ec_datagram_print_wc_error(datagram);

   234

        return;

   235

    }

   236

   237

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

   238

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   239

        char state_str[EC_STATE_STRING_SIZE];

   240

        ec_state_string(slave->current_state, state_str);

   241

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

   242

                slave->ring_position, state_str);

   243

    }

   244

   245

    // read base data

   246

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

   247

    ec_datagram_zero(datagram);

   248

    fsm->retries = EC_FSM_RETRIES;

   249

    fsm->state = ec_fsm_slave_scan_state_base;

   250

}

   251

   252

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

   253

   254

/**

   255

   Slave scan state: BASE.

   256

*/

   257

   258

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

   259

{

   260

    ec_datagram_t *datagram = fsm->datagram;

   261

    ec_slave_t *slave = fsm->slave;

   262

   263

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

   264

        return;

   265

   266

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   267

        fsm->state = ec_fsm_slave_scan_state_error;

   268

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

   269

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

   270

               slave->ring_position, datagram->state);

   271

        return;

   272

    }

   273

   274

    if (datagram->working_counter != 1) {

   275

        fsm->slave->error_flag = 1;

   276

        fsm->state = ec_fsm_slave_scan_state_error;

   277

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

   278

               slave->ring_position);

   279

        ec_datagram_print_wc_error(datagram);

   280

        return;

   281

    }

   282

   283

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

   284

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

   285

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

   286

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

   287

   288

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   289

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

   290

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

   291

                slave->base_fmmu_count, EC_MAX_FMMUS);

   292

        slave->base_fmmu_count = EC_MAX_FMMUS;

   293

    }

   294

   295

    // read data link status

   296

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

   297

    ec_datagram_zero(datagram);

   298

    fsm->retries = EC_FSM_RETRIES;

   299

    fsm->state = ec_fsm_slave_scan_state_datalink;

   300

}

   301

   302

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

   303

   304

/**

   305

   Slave scan state: DATALINK.

   306

*/

   307

   308

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

   309

{

   310

    ec_datagram_t *datagram = fsm->datagram;

   311

    ec_slave_t *slave = fsm->slave;

   312

    uint16_t dl_status;

   313

    unsigned int i;

   314

   315

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

   316

        return;

   317

   318

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   319

        fsm->state = ec_fsm_slave_scan_state_error;

   320

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

   321

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

   322

               slave->ring_position, datagram->state);

   323

        return;

   324

    }

   325

   326

    if (datagram->working_counter != 1) {

   327

        fsm->slave->error_flag = 1;

   328

        fsm->state = ec_fsm_slave_scan_state_error;

   329

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

   330

               slave->ring_position);

   331

        ec_datagram_print_wc_error(datagram);

   332

        return;

   333

    }

   334

   335

    dl_status = EC_READ_U16(datagram->data);

   336

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

   337

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

   338

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

   339

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

   340

    }

   341

   342

    // Start fetching SII size

   343

   344

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

   345

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

   346

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   347

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   348

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

   349

}

   350

   351

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

   352

   353

/**

   354

   Slave scan state: SII SIZE.

   355

*/

   356

   357

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

   358

{

   359

    ec_slave_t *slave = fsm->slave;

   360

    uint16_t cat_type, cat_size;

   361

   362

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

   363

   364

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

   365

        fsm->slave->error_flag = 1;

   366

        fsm->state = ec_fsm_slave_scan_state_error;

   367

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

   368

               slave->ring_position);

   369

        return;

   370

    }

   371

   372

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

   373

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

   374

   375

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

   376

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

   377

        if (next_offset >= EC_MAX_SII_SIZE) {

   378

            EC_WARN("SII size of slave %u exceeds"

   379

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

   380

                    slave->ring_position, EC_MAX_SII_SIZE);

   381

            // cut off category data...

   382

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   383

            goto alloc_sii;

   384

        }

   385

        fsm->sii_offset = next_offset;

   386

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

   387

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   388

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

   389

        return;

   390

    }

   391

   392

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

   393

   394

alloc_sii:

   395

    if (slave->sii_words) {

   396

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

   397

                slave->ring_position);

   398

        kfree(slave->sii_words);

   399

    }

   400

   401

    if (!(slave->sii_words =

   402

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

   403

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

   404

               slave->sii_nwords, slave->ring_position);

   405

        slave->sii_nwords = 0;

   406

        slave->error_flag = 1;

   407

        fsm->state = ec_fsm_slave_scan_state_error;

   408

        return;

   409

    }

   410

   411

    // Start fetching SII contents

   412

   413

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   414

    fsm->sii_offset = 0x0000;

   415

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

   416

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   417

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

   418

}

   419

   420

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

   421

   422

/**

   423

   Slave scan state: SII DATA.

   424

*/

   425

   426

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

   427

{

   428

    ec_slave_t *slave = fsm->slave;

   429

    uint16_t *cat_word, cat_type, cat_size;

   430

   431

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

   432

   433

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

   434

        fsm->slave->error_flag = 1;

   435

        fsm->state = ec_fsm_slave_scan_state_error;

   436

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

   437

               slave->ring_position);

   438

        return;

   439

    }

   440

   441

    // 2 words fetched

   442

   443

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

   444

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

   445

    } else { // copy the last word

   446

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

   447

    }

   448

   449

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

   450

        // fetch the next 2 words

   451

        fsm->sii_offset += 2;

   452

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

   453

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   454

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

   455

        return;

   456

    }

   457

   458

    // Evaluate SII contents

   459

   460

    ec_slave_clear_sync_managers(slave);

   461

   462

    slave->sii.alias =

   463

        EC_READ_U16(slave->sii_words + 0x0004);

   464

    slave->sii.vendor_id =

   465

        EC_READ_U32(slave->sii_words + 0x0008);

   466

    slave->sii.product_code =

   467

        EC_READ_U32(slave->sii_words + 0x000A);

   468

    slave->sii.revision_number =

   469

        EC_READ_U32(slave->sii_words + 0x000C);

   470

    slave->sii.serial_number =

   471

        EC_READ_U32(slave->sii_words + 0x000E);

   472

    slave->sii.rx_mailbox_offset =

   473

        EC_READ_U16(slave->sii_words + 0x0018);

   474

    slave->sii.rx_mailbox_size =

   475

        EC_READ_U16(slave->sii_words + 0x0019);

   476

    slave->sii.tx_mailbox_offset =

   477

        EC_READ_U16(slave->sii_words + 0x001A);

   478

    slave->sii.tx_mailbox_size =

   479

        EC_READ_U16(slave->sii_words + 0x001B);

   480

    slave->sii.mailbox_protocols =

   481

        EC_READ_U16(slave->sii_words + 0x001C);

   482

   483

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   484

        // sii does not contain category data

   485

        fsm->state = ec_fsm_slave_scan_state_end;

   486

        return;

   487

    }

   488

   489

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

   490

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

   491

                " First category header missing.\n",

   492

                slave->ring_position);

   493

        goto end;

   494

    }

   495

   496

    // evaluate category data

   497

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   498

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

   499

   500

        // type and size words must fit

   501

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

   502

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

   503

                    " Category header incomplete.\n",

   504

                    slave->ring_position);

   505

            goto end;

   506

        }

   507

   508

        cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   509

        cat_size = EC_READ_U16(cat_word + 1);

   510

        cat_word += 2;

   511

   512

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

   513

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

   514