1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

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

    21

 *

    22

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    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

    fsm->retries = EC_FSM_RETRIES;

   200

    fsm->state = ec_fsm_slave_scan_state_state;

   201

}

   202

   203

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

   204

   205

/**

   206

   Slave scan state: STATE.

   207

*/

   208

   209

void ec_fsm_slave_scan_state_state(

   210

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

   211

        )

   212

{

   213

    ec_datagram_t *datagram = fsm->datagram;

   214

    ec_slave_t *slave = fsm->slave;

   215

   216

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

   217

        return;

   218

   219

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   220

        fsm->state = ec_fsm_slave_scan_state_error;

   221

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

   222

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

   223

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

   224

        return;

   225

    }

   226

   227

    if (datagram->working_counter != 1) {

   228

        fsm->slave->error_flag = 1;

   229

        fsm->state = ec_fsm_slave_scan_state_error;

   230

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

   231

               fsm->slave->ring_position);

   232

        ec_datagram_print_wc_error(datagram);

   233

        return;

   234

    }

   235

   236

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

   237

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   238

        char state_str[EC_STATE_STRING_SIZE];

   239

        ec_state_string(slave->current_state, state_str);

   240

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

   241

                slave->ring_position, state_str);

   242

    }

   243

   244

    // read base data

   245

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

   246

    fsm->retries = EC_FSM_RETRIES;

   247

    fsm->state = ec_fsm_slave_scan_state_base;

   248

}

   249

   250

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

   251

   252

/**

   253

   Slave scan state: BASE.

   254

*/

   255

   256

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

   257

{

   258

    ec_datagram_t *datagram = fsm->datagram;

   259

    ec_slave_t *slave = fsm->slave;

   260

   261

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

   262

        return;

   263

   264

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   265

        fsm->state = ec_fsm_slave_scan_state_error;

   266

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

   267

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

   268

               slave->ring_position, datagram->state);

   269

        return;

   270

    }

   271

   272

    if (datagram->working_counter != 1) {

   273

        fsm->slave->error_flag = 1;

   274

        fsm->state = ec_fsm_slave_scan_state_error;

   275

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

   276

               slave->ring_position);

   277

        ec_datagram_print_wc_error(datagram);

   278

        return;

   279

    }

   280

   281

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

   282

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

   283

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

   284

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

   285

   286

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   287

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

   288

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

   289

                slave->base_fmmu_count, EC_MAX_FMMUS);

   290

        slave->base_fmmu_count = EC_MAX_FMMUS;

   291

    }

   292

   293

    // read data link status

   294

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

   295

    fsm->retries = EC_FSM_RETRIES;

   296

    fsm->state = ec_fsm_slave_scan_state_datalink;

   297

}

   298

   299

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

   300

   301

/**

   302

   Slave scan state: DATALINK.

   303

*/

   304

   305

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

   306

{

   307

    ec_datagram_t *datagram = fsm->datagram;

   308

    ec_slave_t *slave = fsm->slave;

   309

    uint16_t dl_status;

   310

    unsigned int i;

   311

   312

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

   313

        return;

   314

   315

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   316

        fsm->state = ec_fsm_slave_scan_state_error;

   317

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

   318

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

   319

               slave->ring_position, datagram->state);

   320

        return;

   321

    }

   322

   323

    if (datagram->working_counter != 1) {

   324

        fsm->slave->error_flag = 1;

   325

        fsm->state = ec_fsm_slave_scan_state_error;

   326

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

   327

               slave->ring_position);

   328

        ec_datagram_print_wc_error(datagram);

   329

        return;

   330

    }

   331

   332

    dl_status = EC_READ_U16(datagram->data);

   333

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

   334

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

   335

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

   336

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

   337

    }

   338

   339

    // Start fetching SII size

   340

   341

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

   342

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

   343

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   344

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   345

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

   346

}

   347

   348

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

   349

   350

/**

   351

   Slave scan state: SII SIZE.

   352

*/

   353

   354

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

   355

{

   356

    ec_slave_t *slave = fsm->slave;

   357

    uint16_t cat_type, cat_size;

   358

   359

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

   360

   361

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

   362

        fsm->slave->error_flag = 1;

   363

        fsm->state = ec_fsm_slave_scan_state_error;

   364

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

   365

               slave->ring_position);

   366

        return;

   367

    }

   368

   369

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

   370

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

   371

   372

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

   373

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

   374

        if (next_offset >= EC_MAX_SII_SIZE) {

   375

            EC_WARN("SII size of slave %u exceeds"

   376

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

   377

                    slave->ring_position, EC_MAX_SII_SIZE);

   378

            // cut off category data...

   379

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   380

            goto alloc_sii;

   381

        }

   382

        fsm->sii_offset = next_offset;

   383

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

   384

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   385

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

   386

        return;

   387

    }

   388

   389

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

   390

   391

alloc_sii:

   392

    if (slave->sii_words) {

   393

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

   394

                slave->ring_position);

   395

        kfree(slave->sii_words);

   396

    }

   397

   398

    if (!(slave->sii_words =

   399

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

   400

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

   401

               slave->sii_nwords, slave->ring_position);

   402

        slave->sii_nwords = 0;

   403

        slave->error_flag = 1;

   404

        fsm->state = ec_fsm_slave_scan_state_error;

   405

        return;

   406

    }

   407

   408

    // Start fetching SII contents

   409

   410

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   411

    fsm->sii_offset = 0x0000;

   412

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

   413

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   414

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

   415

}

   416

   417

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

   418

   419

/**

   420

   Slave scan state: SII DATA.

   421

*/

   422

   423

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

   424

{

   425

    ec_slave_t *slave = fsm->slave;

   426

    uint16_t *cat_word, cat_type, cat_size;

   427

   428

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

   429

   430

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

   431

        fsm->slave->error_flag = 1;

   432

        fsm->state = ec_fsm_slave_scan_state_error;

   433

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

   434

               slave->ring_position);

   435

        return;

   436

    }

   437

   438

    // 2 words fetched

   439

   440

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

   441

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

   442

    } else { // copy the last word

   443

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

   444

    }

   445

   446

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

   447

        // fetch the next 2 words

   448

        fsm->sii_offset += 2;

   449

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

   450

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   451

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

   452

        return;

   453

    }

   454

   455

    // Evaluate SII contents

   456

   457

    ec_slave_clear_sync_managers(slave);

   458

   459

    slave->sii.alias =

   460

        EC_READ_U16(slave->sii_words + 0x0004);

   461

    slave->sii.vendor_id =

   462

        EC_READ_U32(slave->sii_words + 0x0008);

   463

    slave->sii.product_code =

   464

        EC_READ_U32(slave->sii_words + 0x000A);

   465

    slave->sii.revision_number =

   466

        EC_READ_U32(slave->sii_words + 0x000C);

   467

    slave->sii.serial_number =

   468

        EC_READ_U32(slave->sii_words + 0x000E);

   469

    slave->sii.rx_mailbox_offset =

   470

        EC_READ_U16(slave->sii_words + 0x0018);

   471

    slave->sii.rx_mailbox_size =

   472

        EC_READ_U16(slave->sii_words + 0x0019);

   473

    slave->sii.tx_mailbox_offset =

   474

        EC_READ_U16(slave->sii_words + 0x001A);

   475

    slave->sii.tx_mailbox_size =

   476

        EC_READ_U16(slave->sii_words + 0x001B);

   477

    slave->sii.mailbox_protocols =

   478

        EC_READ_U16(slave->sii_words + 0x001C);

   479

   480

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   481

        // sii does not contain category data

   482

        fsm->state = ec_fsm_slave_scan_state_end;

   483

        return;

   484

    }

   485

   486

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

   487

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

   488

                " First category header missing.\n",

   489

                slave->ring_position);

   490

        goto end;

   491

    }

   492

   493

    // evaluate category data

   494

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   495

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

   496

   497

        // type and size words must fit

   498

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

   499

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

   500

                    " Category header incomplete.\n",

   501

                    slave->ring_position);

   502

            goto end;

   503

        }

   504

   505

        cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   506

        cat_size = EC_READ_U16(cat_word + 1);

   507

        cat_word += 2;

   508

   509

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

   510

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

   511

                    " Category data incomplete.\n",

   512

                    slave->ring_position);

   513

            goto end;

   514

        }