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 *);

    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

    ec_datagram_zero(datagram);

   201

    fsm->retries = EC_FSM_RETRIES;

   202

    fsm->state = ec_fsm_slave_scan_state_state;

   203

}

   204

   205

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

   206

   207

/**

   208

   Slave scan state: STATE.

   209

*/

   210

   211

void ec_fsm_slave_scan_state_state(

   212

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

   213

        )

   214

{

   215

    ec_datagram_t *datagram = fsm->datagram;

   216

    ec_slave_t *slave = fsm->slave;

   217

   218

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

   219

        return;

   220

   221

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   222

        fsm->state = ec_fsm_slave_scan_state_error;

   223

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

   224

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

   225

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

   226

        return;

   227

    }

   228

   229

    if (datagram->working_counter != 1) {

   230

        fsm->slave->error_flag = 1;

   231

        fsm->state = ec_fsm_slave_scan_state_error;

   232

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

   233

               fsm->slave->ring_position);

   234

        ec_datagram_print_wc_error(datagram);

   235

        return;

   236

    }

   237

   238

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

   239

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   240

        char state_str[EC_STATE_STRING_SIZE];

   241

        ec_state_string(slave->current_state, state_str, 0);

   242

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

   243

                slave->ring_position, state_str);

   244

    }

   245

   246

    // read base data

   247

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

   248

    ec_datagram_zero(datagram);

   249

    fsm->retries = EC_FSM_RETRIES;

   250

    fsm->state = ec_fsm_slave_scan_state_base;

   251

}

   252

   253

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

   254

   255

/**

   256

   Slave scan state: BASE.

   257

*/

   258

   259

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

   260

{

   261

    ec_datagram_t *datagram = fsm->datagram;

   262

    ec_slave_t *slave = fsm->slave;

   263

   264

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

   265

        return;

   266

   267

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   268

        fsm->state = ec_fsm_slave_scan_state_error;

   269

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

   270

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

   271

               slave->ring_position, datagram->state);

   272

        return;

   273

    }

   274

   275

    if (datagram->working_counter != 1) {

   276

        fsm->slave->error_flag = 1;

   277

        fsm->state = ec_fsm_slave_scan_state_error;

   278

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

   279

               slave->ring_position);

   280

        ec_datagram_print_wc_error(datagram);

   281

        return;

   282

    }

   283

   284

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

   285

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

   286

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

   287

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

   288

   289

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   290

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

   291

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

   292

                slave->base_fmmu_count, EC_MAX_FMMUS);

   293

        slave->base_fmmu_count = EC_MAX_FMMUS;

   294

    }

   295

   296

    // read data link status

   297

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

   298

    ec_datagram_zero(datagram);

   299

    fsm->retries = EC_FSM_RETRIES;

   300

    fsm->state = ec_fsm_slave_scan_state_datalink;

   301

}

   302

   303

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

   304

   305

/**

   306

   Slave scan state: DATALINK.

   307

*/

   308

   309

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

   310

{

   311

    ec_datagram_t *datagram = fsm->datagram;

   312

    ec_slave_t *slave = fsm->slave;

   313

    uint16_t dl_status;

   314

    unsigned int i;

   315

   316

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

   317

        return;

   318

   319

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   320

        fsm->state = ec_fsm_slave_scan_state_error;

   321

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

   322

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

   323

               slave->ring_position, datagram->state);

   324

        return;

   325

    }

   326

   327

    if (datagram->working_counter != 1) {

   328

        fsm->slave->error_flag = 1;

   329

        fsm->state = ec_fsm_slave_scan_state_error;

   330

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

   331

               slave->ring_position);

   332

        ec_datagram_print_wc_error(datagram);

   333

        return;

   334

    }

   335

   336

    dl_status = EC_READ_U16(datagram->data);

   337

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

   338

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

   339

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

   340

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

   341

    }

   342

   343

    // Start fetching SII size

   344

   345

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

   346

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

   347

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   348

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   349

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

   350

}

   351

   352

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

   353

   354

/**

   355

   Slave scan state: SII SIZE.

   356

*/

   357

   358

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

   359

{

   360

    ec_slave_t *slave = fsm->slave;

   361

    uint16_t cat_type, cat_size;

   362

   363

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

   364

   365

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

   366

        fsm->slave->error_flag = 1;

   367

        fsm->state = ec_fsm_slave_scan_state_error;

   368

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

   369

               slave->ring_position);

   370

        return;

   371

    }

   372

   373

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

   374

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

   375

   376

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

   377

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

   378

        if (next_offset >= EC_MAX_SII_SIZE) {

   379

            EC_WARN("SII size of slave %u exceeds"

   380

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

   381

                    slave->ring_position, EC_MAX_SII_SIZE);

   382

            // cut off category data...

   383

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   384

            goto alloc_sii;

   385

        }

   386

        fsm->sii_offset = next_offset;

   387

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

   388

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   389

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

   390

        return;

   391

    }

   392

   393

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

   394

   395

alloc_sii:

   396

    if (slave->sii_words) {

   397

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

   398

                slave->ring_position);

   399

        kfree(slave->sii_words);

   400

    }

   401

   402

    if (!(slave->sii_words =

   403

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

   404

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

   405

               slave->sii_nwords, slave->ring_position);

   406

        slave->sii_nwords = 0;

   407

        slave->error_flag = 1;

   408

        fsm->state = ec_fsm_slave_scan_state_error;

   409

        return;

   410

    }

   411

   412

    // Start fetching SII contents

   413

   414

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   415

    fsm->sii_offset = 0x0000;

   416

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

   417

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   418

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

   419

}

   420

   421

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

   422

   423

/**

   424

   Slave scan state: SII DATA.

   425

*/

   426

   427

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

   428

{

   429

    ec_slave_t *slave = fsm->slave;

   430

    uint16_t *cat_word, cat_type, cat_size;

   431

   432

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

   433

   434

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

   435

        fsm->slave->error_flag = 1;

   436

        fsm->state = ec_fsm_slave_scan_state_error;

   437

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

   438

               slave->ring_position);

   439

        return;

   440

    }

   441

   442

    // 2 words fetched

   443

   444

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

   445

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

   446

    } else { // copy the last word

   447

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

   448

    }

   449

   450

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

   451

        // fetch the next 2 words

   452

        fsm->sii_offset += 2;

   453

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

   454

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   455

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

   456

        return;

   457

    }

   458

   459

    // Evaluate SII contents

   460

   461

    ec_slave_clear_sync_managers(slave);

   462

   463

    slave->sii.alias =

   464

        EC_READ_U16(slave->sii_words + 0x0004);

   465

    slave->sii.vendor_id =

   466

        EC_READ_U32(slave->sii_words + 0x0008);

   467

    slave->sii.product_code =

   468

        EC_READ_U32(slave->sii_words + 0x000A);

   469

    slave->sii.revision_number =

   470

        EC_READ_U32(slave->sii_words + 0x000C);

   471

    slave->sii.serial_number =

   472

        EC_READ_U32(slave->sii_words + 0x000E);

   473

    slave->sii.boot_rx_mailbox_offset =

   474

        EC_READ_U16(slave->sii_words + 0x0014);

   475

    slave->sii.boot_rx_mailbox_size =

   476

        EC_READ_U16(slave->sii_words + 0x0015);

   477

    slave->sii.boot_tx_mailbox_offset =

   478

        EC_READ_U16(slave->sii_words + 0x0016);

   479

    slave->sii.boot_tx_mailbox_size =

   480

        EC_READ_U16(slave->sii_words + 0x0017);

   481

    slave->sii.std_rx_mailbox_offset =

   482

        EC_READ_U16(slave->sii_words + 0x0018);

   483

    slave->sii.std_rx_mailbox_size =

   484

        EC_READ_U16(slave->sii_words + 0x0019);

   485

    slave->sii.std_tx_mailbox_offset =

   486

        EC_READ_U16(slave->sii_words + 0x001A);

   487

    slave->sii.std_tx_mailbox_size =

   488

        EC_READ_U16(slave->sii_words + 0x001B);

   489

    slave->sii.mailbox_protocols =

   490

        EC_READ_U16(slave->sii_words + 0x001C);

   491

   492

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   493

        // sii does not contain category data

   494

        fsm->state = ec_fsm_slave_scan_state_end;

   495

        return;

   496

    }

   497

   498

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

   499

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

   500

                " First category header missing.\n",

   501

                slave->ring_position);

   502

        goto end;

   503

    }

   504

   505

    // evaluate category data

   506

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   507

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

   508

   509

        // type and size words must fit

   510

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

   511

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

   512

                    " Category header incomplete.\n",

   513

                    slave->ring_position);