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

    ec_datagram_zero(datagram);

   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

    ec_datagram_zero(datagram);

   255

    fsm->retries = EC_FSM_RETRIES;

   256

    fsm->state = ec_fsm_slave_scan_state_base;

   257

}

   258

   259

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

   260

   261

/**

   262

   Slave scan state: BASE.

   263

*/

   264

   265

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

   266

{

   267

    ec_datagram_t *datagram = fsm->datagram;

   268

    ec_slave_t *slave = fsm->slave;

   269

   270

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

   271

        return;

   272

   273

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   274

        fsm->state = ec_fsm_slave_scan_state_error;

   275

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

   276

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

   277

               slave->ring_position, datagram->state);

   278

        return;

   279

    }

   280

   281

    if (datagram->working_counter != 1) {

   282

        fsm->slave->error_flag = 1;

   283

        fsm->state = ec_fsm_slave_scan_state_error;

   284

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

   285

               slave->ring_position);

   286

        ec_datagram_print_wc_error(datagram);

   287

        return;

   288

    }

   289

   290

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

   291

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

   292

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

   293

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

   294

   295

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   296

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

   297

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

   298

                slave->base_fmmu_count, EC_MAX_FMMUS);

   299

        slave->base_fmmu_count = EC_MAX_FMMUS;

   300

    }

   301

   302

    // read data link status

   303

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

   304

    ec_datagram_zero(datagram);

   305

    fsm->retries = EC_FSM_RETRIES;

   306

    fsm->state = ec_fsm_slave_scan_state_datalink;

   307

}

   308

   309

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

   310

   311

/**

   312

   Slave scan state: DATALINK.

   313

*/

   314

   315

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

   316

{

   317

    ec_datagram_t *datagram = fsm->datagram;

   318

    ec_slave_t *slave = fsm->slave;

   319

    uint16_t dl_status;

   320

    unsigned int i;

   321

   322

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

   323

        return;

   324

   325

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   326

        fsm->state = ec_fsm_slave_scan_state_error;

   327

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

   328

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

   329

               slave->ring_position, datagram->state);

   330

        return;

   331

    }

   332

   333

    if (datagram->working_counter != 1) {

   334

        fsm->slave->error_flag = 1;

   335

        fsm->state = ec_fsm_slave_scan_state_error;

   336

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

   337

               slave->ring_position);

   338

        ec_datagram_print_wc_error(datagram);

   339

        return;

   340

    }

   341

   342

    dl_status = EC_READ_U16(datagram->data);

   343

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

   344

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

   345

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

   346

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

   347

    }

   348

   349

    // Start fetching SII size

   350

   351

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

   352

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

   353

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   354

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   355

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

   356

}

   357

   358

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

   359

   360

/**

   361

   Slave scan state: SII SIZE.

   362

*/

   363

   364

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

   365

{

   366

    ec_slave_t *slave = fsm->slave;

   367

    uint16_t cat_type, cat_size;

   368

   369

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

   370

   371

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

   372

        fsm->slave->error_flag = 1;

   373

        fsm->state = ec_fsm_slave_scan_state_error;

   374

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

   375

               slave->ring_position);

   376

        return;

   377

    }

   378

   379

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

   380

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

   381

   382

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

   383

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

   384

        if (next_offset >= EC_MAX_SII_SIZE) {

   385

            EC_WARN("SII size of slave %u exceeds"

   386

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

   387

                    slave->ring_position, EC_MAX_SII_SIZE);

   388

            // cut off category data...

   389

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   390

            goto alloc_sii;

   391

        }

   392

        fsm->sii_offset = next_offset;

   393

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

   394

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   395

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

   396

        return;

   397

    }

   398

   399

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

   400

   401

alloc_sii:

   402

    if (slave->sii_words) {

   403

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

   404

                slave->ring_position);

   405

        kfree(slave->sii_words);

   406

    }

   407

   408

    if (!(slave->sii_words =

   409

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

   410

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

   411

               slave->sii_nwords, slave->ring_position);

   412

        slave->sii_nwords = 0;

   413

        slave->error_flag = 1;

   414

        fsm->state = ec_fsm_slave_scan_state_error;

   415

        return;

   416

    }

   417

   418

    // Start fetching SII contents

   419

   420

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   421

    fsm->sii_offset = 0x0000;

   422

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

   423

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   424

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

   425

}

   426

   427

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

   428

   429

/**

   430

   Slave scan state: SII DATA.

   431

*/

   432

   433

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

   434

{

   435

    ec_slave_t *slave = fsm->slave;

   436

    uint16_t *cat_word, cat_type, cat_size;

   437

   438

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

   439

   440

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

   441

        fsm->slave->error_flag = 1;

   442

        fsm->state = ec_fsm_slave_scan_state_error;

   443

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

   444

               slave->ring_position);

   445

        return;

   446

    }

   447

   448

    // 2 words fetched

   449

   450

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

   451

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

   452

    } else { // copy the last word

   453

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

   454

    }

   455

   456

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

   457

        // fetch the next 2 words

   458

        fsm->sii_offset += 2;

   459

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

   460

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   461

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

   462

        return;

   463

    }

   464

   465

    // Evaluate SII contents

   466

   467

    ec_slave_clear_sync_managers(slave);

   468

   469

    slave->sii.alias =

   470

        EC_READ_U16(slave->sii_words + 0x0004);

   471

    slave->sii.vendor_id =

   472

        EC_READ_U32(slave->sii_words + 0x0008);

   473

    slave->sii.product_code =

   474

        EC_READ_U32(slave->sii_words + 0x000A);

   475

    slave->sii.revision_number =

   476

        EC_READ_U32(slave->sii_words + 0x000C);

   477

    slave->sii.serial_number =

   478

        EC_READ_U32(slave->sii_words + 0x000E);

   479

    slave->sii.rx_mailbox_offset =

   480

        EC_READ_U16(slave->sii_words + 0x0018);

   481

    slave->sii.rx_mailbox_size =

   482

        EC_READ_U16(slave->sii_words + 0x0019);

   483

    slave->sii.tx_mailbox_offset =

   484

        EC_READ_U16(slave->sii_words + 0x001A);

   485

    slave->sii.tx_mailbox_size =

   486

        EC_READ_U16(slave->sii_words + 0x001B);

   487

    slave->sii.mailbox_protocols =

   488

        EC_READ_U16(slave->sii_words + 0x001C);

   489

   490

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   491

        // sii does not contain category data

   492

        fsm->state = ec_fsm_slave_scan_state_end;

   493

        return;

   494

    }

   495

   496

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

   497

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

   498

                " First category header missing.\n",

   499

                slave->ring_position);

   500

        goto end;

   501

    }

   502

   503

    // evaluate category data

   504

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   505

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

   506

   507

        // type and size words must fit

   508

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

   509

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

   510

                    " Category header incomplete.\n",

   511

                    slave->ring_position);

   512

            goto end;

   513

        }

   514

   515

        cat_type = EC_READ_U16(cat_word) & 0x7FFF;