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_sync(ec_fsm_slave_scan_t *);

    52

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    53

    54

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    55

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    56

    57

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    58

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    59

    60

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

    61

    62

/** Constructor.

    63

 */

    64

void ec_fsm_slave_scan_init(

    65

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

    66

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

    67

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    68

                                                  state machine to use. */

    69

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

    70

        )

    71

{

    72

    fsm->datagram = datagram;

    73

    fsm->fsm_slave_config = fsm_slave_config;

    74

    fsm->fsm_pdo = fsm_pdo;

    75

    76

    // init sub state machines

    77

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

    78

}

    79

    80

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

    81

    82

/** Destructor.

    83

 */

    84

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

    85

{

    86

    // clear sub state machines

    87

    ec_fsm_sii_clear(&fsm->fsm_sii);

    88

}

    89

    90

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

    91

    92

/**

    93

 * Start slave scan state machine.

    94

 */

    95

    96

void ec_fsm_slave_scan_start(

    97

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

    98

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

    99

        )

   100

{

   101

    fsm->slave = slave;

   102

    fsm->state = ec_fsm_slave_scan_state_start;

   103

}

   104

   105

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

   106

   107

/**

   108

   \return false, if state machine has terminated

   109

*/

   110

   111

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

   112

{

   113

    return fsm->state != ec_fsm_slave_scan_state_end

   114

        && fsm->state != ec_fsm_slave_scan_state_error;

   115

}

   116

   117

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

   118

   119

/**

   120

   Executes the current state of the state machine.

   121

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

   122

   of the state machine is delayed to the next cycle.

   123

   \return false, if state machine has terminated

   124

*/

   125

   126

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

   127

{

   128

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

   129

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

   130

        // datagram was not sent or received yet.

   131

        return ec_fsm_slave_scan_running(fsm);

   132

    }

   133

   134

    fsm->state(fsm);

   135

    return ec_fsm_slave_scan_running(fsm);

   136

}

   137

   138

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

   139

   140

/**

   141

   \return true, if the state machine terminated gracefully

   142

*/

   143

   144

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

   145

{

   146

    return fsm->state == ec_fsm_slave_scan_state_end;

   147

}

   148

   149

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

   150

 *  slave scan state machine

   151

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

   152

   153

/**

   154

   Slave scan state: START.

   155

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

   156

   slave, according to its ring position.

   157

*/

   158

   159

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

   160

{

   161

    // write station address

   162

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

   163

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

   164

    fsm->retries = EC_FSM_RETRIES;

   165

    fsm->state = ec_fsm_slave_scan_state_address;

   166

}

   167

   168

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

   169

   170

/**

   171

   Slave scan state: ADDRESS.

   172

*/

   173

   174

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

   175

{

   176

    ec_datagram_t *datagram = fsm->datagram;

   177

   178

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

   179

        return;

   180

   181

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   182

        fsm->state = ec_fsm_slave_scan_state_error;

   183

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

   184

                " (datagram state %u)\n",

   185

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

   186

        return;

   187

    }

   188

   189

    if (datagram->working_counter != 1) {

   190

        fsm->slave->error_flag = 1;

   191

        fsm->state = ec_fsm_slave_scan_state_error;

   192

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

   193

               fsm->slave->ring_position);

   194

        ec_datagram_print_wc_error(datagram);

   195

        return;

   196

    }

   197

   198

    // Read AL state

   199

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

   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, 0);

   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

    fsm->retries = EC_FSM_RETRIES;

   248

    fsm->state = ec_fsm_slave_scan_state_base;

   249

}

   250

   251

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

   252

   253

/**

   254

   Slave scan state: BASE.

   255

*/

   256

   257

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

   258

{

   259

    ec_datagram_t *datagram = fsm->datagram;

   260

    ec_slave_t *slave = fsm->slave;

   261

   262

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

   263

        return;

   264

   265

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   266

        fsm->state = ec_fsm_slave_scan_state_error;

   267

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

   268

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

   269

               slave->ring_position, datagram->state);

   270

        return;

   271

    }

   272

   273

    if (datagram->working_counter != 1) {

   274

        fsm->slave->error_flag = 1;

   275

        fsm->state = ec_fsm_slave_scan_state_error;

   276

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

   277

               slave->ring_position);

   278

        ec_datagram_print_wc_error(datagram);

   279

        return;

   280

    }

   281

   282

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

   283

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

   284

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

   285

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

   286

   287

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   288

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

   289

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

   290

                slave->base_fmmu_count, EC_MAX_FMMUS);

   291

        slave->base_fmmu_count = EC_MAX_FMMUS;

   292

    }

   293

   294

    // read data link status

   295

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

   296

    fsm->retries = EC_FSM_RETRIES;

   297

    fsm->state = ec_fsm_slave_scan_state_datalink;

   298

}

   299

   300

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

   301

   302

/**

   303

   Slave scan state: DATALINK.

   304

*/

   305

   306

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

   307

{

   308

    ec_datagram_t *datagram = fsm->datagram;

   309

    ec_slave_t *slave = fsm->slave;

   310

    uint16_t dl_status;

   311

    unsigned int i;

   312

   313

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

   314

        return;

   315

   316

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   317

        fsm->state = ec_fsm_slave_scan_state_error;

   318

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

   319

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

   320

               slave->ring_position, datagram->state);

   321

        return;

   322

    }

   323

   324

    if (datagram->working_counter != 1) {

   325

        fsm->slave->error_flag = 1;

   326

        fsm->state = ec_fsm_slave_scan_state_error;

   327

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

   328

               slave->ring_position);

   329

        ec_datagram_print_wc_error(datagram);

   330

        return;

   331

    }

   332

   333

    dl_status = EC_READ_U16(datagram->data);

   334

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

   335

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

   336

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

   337

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

   338

    }

   339

   340

    // Start fetching SII size

   341

   342

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

   343

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

   344

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   345

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   346

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

   347

}

   348

   349

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

   350

   351

/**

   352

   Slave scan state: SII SIZE.

   353

*/

   354

   355

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

   356

{

   357

    ec_slave_t *slave = fsm->slave;

   358

    uint16_t cat_type, cat_size;

   359

   360

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

   361

   362

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

   363

        fsm->slave->error_flag = 1;

   364

        fsm->state = ec_fsm_slave_scan_state_error;

   365

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

   366

               slave->ring_position);

   367

        return;

   368

    }

   369

   370

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

   371

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

   372

   373

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

   374

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

   375

        if (next_offset >= EC_MAX_SII_SIZE) {

   376

            EC_WARN("SII size of slave %u exceeds"

   377

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

   378

                    slave->ring_position, EC_MAX_SII_SIZE);

   379

            // cut off category data...

   380

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   381

            goto alloc_sii;

   382

        }

   383

        fsm->sii_offset = next_offset;

   384

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

   385

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   386

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

   387

        return;

   388

    }

   389

   390

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

   391

   392

alloc_sii:

   393

    if (slave->sii_words) {

   394

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

   395

                slave->ring_position);

   396

        kfree(slave->sii_words);

   397

    }

   398

   399

    if (!(slave->sii_words =

   400

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

   401

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

   402

               slave->sii_nwords, slave->ring_position);

   403

        slave->sii_nwords = 0;

   404

        slave->error_flag = 1;

   405

        fsm->state = ec_fsm_slave_scan_state_error;

   406

        return;

   407

    }

   408

   409

    // Start fetching SII contents

   410

   411

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   412

    fsm->sii_offset = 0x0000;

   413

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

   414

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   415

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

   416

}

   417

   418

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

   419

   420

/**

   421

   Slave scan state: SII DATA.

   422

*/

   423

   424

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

   425

{

   426

    ec_slave_t *slave = fsm->slave;

   427

    uint16_t *cat_word, cat_type, cat_size;

   428

   429

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

   430

   431

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

   432

        fsm->slave->error_flag = 1;

   433

        fsm->state = ec_fsm_slave_scan_state_error;

   434

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

   435

               slave->ring_position);

   436

        return;

   437

    }

   438

   439

    // 2 words fetched

   440

   441

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

   442

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

   443

    } else { // copy the last word

   444

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

   445

    }

   446

   447

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

   448

        // fetch the next 2 words

   449

        fsm->sii_offset += 2;

   450

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

   451

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   452

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

   453

        return;

   454

    }

   455

   456

    // Evaluate SII contents

   457

   458

    ec_slave_clear_sync_managers(slave);

   459

   460

    slave->sii.alias =

   461

        EC_READ_U16(slave->sii_words + 0x0004);

   462

    slave->sii.vendor_id =

   463

        EC_READ_U32(slave->sii_words + 0x0008);

   464

    slave->sii.product_code =

   465

        EC_READ_U32(slave->sii_words + 0x000A);

   466

    slave->sii.revision_number =

   467

        EC_READ_U32(slave->sii_words + 0x000C);

   468

    slave->sii.serial_number =

   469

        EC_READ_U32(slave->sii_words + 0x000E);

   470

    slave->sii.boot_rx_mailbox_offset =

   471

        EC_READ_U16(slave->sii_words + 0x0014);

   472

    slave->sii.boot_rx_mailbox_size =

   473

        EC_READ_U16(slave->sii_words + 0x0015);

   474

    slave->sii.boot_tx_mailbox_offset =

   475

        EC_READ_U16(slave->sii_words + 0x0016);

   476

    slave->sii.boot_tx_mailbox_size =

   477

        EC_READ_U16(slave->sii_words + 0x0017);

   478

    slave->sii.std_rx_mailbox_offset =

   479

        EC_READ_U16(slave->sii_words + 0x0018);

   480

    slave->sii.std_rx_mailbox_size =

   481

        EC_READ_U16(slave->sii_words + 0x0019);

   482

    slave->sii.std_tx_mailbox_offset =

   483

        EC_READ_U16(slave->sii_words + 0x001A);

   484

    slave->sii.std_tx_mailbox_size =

   485

        EC_READ_U16(slave->sii_words + 0x001B);

   486

    slave->sii.mailbox_protocols =

   487

        EC_READ_U16(slave->sii_words + 0x001C);

   488

   489

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   490

        // sii does not contain category data

   491

        fsm->state = ec_fsm_slave_scan_state_end;

   492

        return;

   493

    }

   494

   495

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

   496

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

   497

                " First category header missing.\n",

   498

                slave->ring_position);

   499

        goto end;

   500

    }

   501

   502

    // evaluate category data

   503

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   504

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

   505

   506

        // type and size words must fit

   507

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

   508

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

   509

                    " Category header incomplete.\n",

   510

                    slave->ring_position);

   511

            goto end;

   512

        }

   513

   514