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_pdo_t *fsm_pdo /**< Pdo configuration machine to use. */

    76

        )

    77

{

    78

    fsm->datagram = datagram;

    79

    fsm->fsm_slave_config = fsm_slave_config;

    80

    fsm->fsm_pdo = fsm_pdo;

    81

    82

    // init sub state machines

    83

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

    84

}

    85

    86

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

    87

    88

/** Destructor.

    89

 */

    90

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

    91

{

    92

    // clear sub state machines

    93

    ec_fsm_sii_clear(&fsm->fsm_sii);

    94

}

    95

    96

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

    97

    98

/**

    99

 * Start slave scan state machine.

   100

 */

   101

   102

void ec_fsm_slave_scan_start(

   103

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

   104

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

   105

        )

   106

{

   107

    fsm->slave = slave;

   108

    fsm->state = ec_fsm_slave_scan_state_start;

   109

}

   110

   111

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

   112

   113

/**

   114

   \return false, if state machine has terminated

   115

*/

   116

   117

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

   118

{

   119

    return fsm->state != ec_fsm_slave_scan_state_end

   120

        && fsm->state != ec_fsm_slave_scan_state_error;

   121

}

   122

   123

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

   124

   125

/**

   126

   Executes the current state of the state machine.

   127

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

   128

   of the state machine is delayed to the next cycle.

   129

   \return false, if state machine has terminated

   130

*/

   131

   132

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

   133

{

   134

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

   135

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

   136

        // datagram was not sent or received yet.

   137

        return ec_fsm_slave_scan_running(fsm);

   138

    }

   139

   140

    fsm->state(fsm);

   141

    return ec_fsm_slave_scan_running(fsm);

   142

}

   143

   144

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

   145

   146

/**

   147

   \return true, if the state machine terminated gracefully

   148

*/

   149

   150

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

   151

{

   152

    return fsm->state == ec_fsm_slave_scan_state_end;

   153

}

   154

   155

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

   156

 *  slave scan state machine

   157

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

   158

   159

/**

   160

   Slave scan state: START.

   161

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

   162

   slave, according to its ring position.

   163

*/

   164

   165

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

   166

{

   167

    // write station address

   168

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

   169

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

   170

    fsm->retries = EC_FSM_RETRIES;

   171

    fsm->state = ec_fsm_slave_scan_state_address;

   172

}

   173

   174

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

   175

   176

/**

   177

   Slave scan state: ADDRESS.

   178

*/

   179

   180

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

   181

{

   182

    ec_datagram_t *datagram = fsm->datagram;

   183

   184

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

   185

        return;

   186

   187

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   188

        fsm->state = ec_fsm_slave_scan_state_error;

   189

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

   190

                " (datagram state %u)\n",

   191

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

   192

        return;

   193

    }

   194

   195

    if (datagram->working_counter != 1) {

   196

        fsm->slave->error_flag = 1;

   197

        fsm->state = ec_fsm_slave_scan_state_error;

   198

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

   199

               fsm->slave->ring_position);

   200

        ec_datagram_print_wc_error(datagram);

   201

        return;

   202

    }

   203

   204

    // Read AL state

   205

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

   206

    fsm->retries = EC_FSM_RETRIES;

   207

    fsm->state = ec_fsm_slave_scan_state_state;

   208

}

   209

   210

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

   211

   212

/**

   213

   Slave scan state: STATE.

   214

*/

   215

   216

void ec_fsm_slave_scan_state_state(

   217

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

   218

        )

   219

{

   220

    ec_datagram_t *datagram = fsm->datagram;

   221

    ec_slave_t *slave = fsm->slave;

   222

   223

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

   224

        return;

   225

   226

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   227

        fsm->state = ec_fsm_slave_scan_state_error;

   228

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

   229

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

   230

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

   231

        return;

   232

    }

   233

   234

    if (datagram->working_counter != 1) {

   235

        fsm->slave->error_flag = 1;

   236

        fsm->state = ec_fsm_slave_scan_state_error;

   237

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

   238

               fsm->slave->ring_position);

   239

        ec_datagram_print_wc_error(datagram);

   240

        return;

   241

    }

   242

   243

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

   244

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   245

        char state_str[EC_STATE_STRING_SIZE];

   246

        ec_state_string(slave->current_state, state_str);

   247

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

   248

                slave->ring_position, state_str);

   249

    }

   250

   251

    // read base data

   252

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

   253

    fsm->retries = EC_FSM_RETRIES;

   254

    fsm->state = ec_fsm_slave_scan_state_base;

   255

}

   256

   257

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

   258

   259

/**

   260

   Slave scan state: BASE.

   261

*/

   262

   263

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

   264

{

   265

    ec_datagram_t *datagram = fsm->datagram;

   266

    ec_slave_t *slave = fsm->slave;

   267

   268

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

   269

        return;

   270

   271

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   272

        fsm->state = ec_fsm_slave_scan_state_error;

   273

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

   274

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

   275

               slave->ring_position, datagram->state);

   276

        return;

   277

    }

   278

   279

    if (datagram->working_counter != 1) {

   280

        fsm->slave->error_flag = 1;

   281

        fsm->state = ec_fsm_slave_scan_state_error;

   282

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

   283

               slave->ring_position);

   284

        ec_datagram_print_wc_error(datagram);

   285

        return;

   286

    }

   287

   288

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

   289

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

   290

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

   291

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

   292

   293

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   294

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

   295

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

   296

                slave->base_fmmu_count, EC_MAX_FMMUS);

   297

        slave->base_fmmu_count = EC_MAX_FMMUS;

   298

    }

   299

   300

    // read data link status

   301

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

   302

    fsm->retries = EC_FSM_RETRIES;

   303

    fsm->state = ec_fsm_slave_scan_state_datalink;

   304

}

   305

   306

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

   307

   308

/**

   309

   Slave scan state: DATALINK.

   310

*/

   311

   312

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

   313

{

   314

    ec_datagram_t *datagram = fsm->datagram;

   315

    ec_slave_t *slave = fsm->slave;

   316

    uint16_t dl_status;

   317

    unsigned int i;

   318

   319

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

   320

        return;

   321

   322

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   323

        fsm->state = ec_fsm_slave_scan_state_error;

   324

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

   325

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

   326

               slave->ring_position, datagram->state);

   327

        return;

   328

    }

   329

   330

    if (datagram->working_counter != 1) {

   331

        fsm->slave->error_flag = 1;

   332

        fsm->state = ec_fsm_slave_scan_state_error;

   333

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

   334

               slave->ring_position);

   335

        ec_datagram_print_wc_error(datagram);

   336

        return;

   337

    }

   338

   339

    dl_status = EC_READ_U16(datagram->data);

   340

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

   341

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

   342

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

   343

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

   344

    }

   345

   346

    // Start fetching SII size

   347

   348

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

   349

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

   350

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   351

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   352

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

   353

}

   354

   355

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

   356

   357

/**

   358

   Slave scan state: SII SIZE.

   359

*/

   360

   361

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

   362

{

   363

    ec_slave_t *slave = fsm->slave;

   364

    uint16_t cat_type, cat_size;

   365

   366

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

   367

   368

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

   369

        fsm->slave->error_flag = 1;

   370

        fsm->state = ec_fsm_slave_scan_state_error;

   371

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

   372

               slave->ring_position);

   373

        return;

   374

    }

   375

   376

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

   377

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

   378

   379

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

   380

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

   381

        if (next_offset >= EC_MAX_SII_SIZE) {

   382

            EC_WARN("SII size of slave %u exceeds"

   383

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

   384

                    slave->ring_position, EC_MAX_SII_SIZE);

   385

            // cut off category data...

   386

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   387

            goto alloc_sii;

   388

        }

   389

        fsm->sii_offset = next_offset;

   390

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

   391

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   392

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

   393

        return;

   394

    }

   395

   396

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

   397

   398

alloc_sii:

   399

    if (slave->sii_words) {

   400

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

   401

                slave->ring_position);

   402

        kfree(slave->sii_words);

   403

    }

   404

   405

    if (!(slave->sii_words =

   406

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

   407

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

   408

               slave->sii_nwords, slave->ring_position);

   409

        slave->sii_nwords = 0;

   410

        slave->error_flag = 1;

   411

        fsm->state = ec_fsm_slave_scan_state_error;

   412

        return;

   413

    }

   414

   415

    // Start fetching SII contents

   416

   417

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   418

    fsm->sii_offset = 0x0000;

   419

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

   420

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   421

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

   422

}

   423

   424

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

   425

   426

/**

   427

   Slave scan state: SII DATA.

   428

*/

   429

   430

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

   431

{

   432

    ec_slave_t *slave = fsm->slave;

   433

    uint16_t *cat_word, cat_type, cat_size;

   434

   435

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

   436

   437

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

   438

        fsm->slave->error_flag = 1;

   439

        fsm->state = ec_fsm_slave_scan_state_error;

   440

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

   441

               slave->ring_position);

   442

        return;

   443

    }

   444

   445

    // 2 words fetched

   446

   447

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

   448

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

   449

    } else { // copy the last word

   450

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

   451

    }

   452

   453

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

   454

        // fetch the next 2 words

   455

        fsm->sii_offset += 2;

   456

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

   457

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   458

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

   459

        return;

   460

    }

   461

   462

    // Evaluate SII contents

   463

   464

    ec_slave_clear_sync_managers(slave);

   465

   466

    slave->sii.alias =

   467

        EC_READ_U16(slave->sii_words + 0x0004);

   468

    slave->sii.vendor_id =

   469

        EC_READ_U32(slave->sii_words + 0x0008);

   470

    slave->sii.product_code =

   471

        EC_READ_U32(slave->sii_words + 0x000A);

   472

    slave->sii.revision_number =

   473

        EC_READ_U32(slave->sii_words + 0x000C);

   474

    slave->sii.serial_number =

   475

        EC_READ_U32(slave->sii_words + 0x000E);

   476

    slave->sii.rx_mailbox_offset =

   477

        EC_READ_U16(slave->sii_words + 0x0018);

   478

    slave->sii.rx_mailbox_size =

   479

        EC_READ_U16(slave->sii_words + 0x0019);

   480

    slave->sii.tx_mailbox_offset =

   481

        EC_READ_U16(slave->sii_words + 0x001A);

   482

    slave->sii.tx_mailbox_size =

   483

        EC_READ_U16(slave->sii_words + 0x001B);

   484

    slave->sii.mailbox_protocols =

   485

        EC_READ_U16(slave->sii_words + 0x001C);

   486

   487

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   488

        // sii does not contain category data

   489

        fsm->state = ec_fsm_slave_scan_state_end;

   490

        return;

   491

    }

   492

   493

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

   494

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

   495

                " First category header missing.\n",

   496

                slave->ring_position);

   497

        goto end;

   498

    }

   499

   500

    // evaluate category data

   501

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   502

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

   503

   504

        // type and size words must fit

   505

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

   506

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

   507

                    " Category header incomplete.\n",

   508

                    slave->ring_position);

   509

            goto end;

   510

        }

   511

   512

        cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   513

        cat_size = EC_READ_U16(cat_word + 1);

   514

        cat_word += 2;

   515