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

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

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

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   EtherCAT slave state machines.

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "slave_config.h"

    41

    42

#include "fsm_slave_scan.h"

    43

    44

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

    45

    46

void ec_fsm_slave_scan_state_start(ec_fsm_slave_scan_t *);

    47

void ec_fsm_slave_scan_state_address(ec_fsm_slave_scan_t *);

    48

void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *);

    49

void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *);

    50

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);

    51

void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);

    52

void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);

    53

void ec_fsm_slave_scan_state_preop(ec_fsm_slave_scan_t *);

    54

void ec_fsm_slave_scan_state_sync(ec_fsm_slave_scan_t *);

    55

void ec_fsm_slave_scan_state_pdos(ec_fsm_slave_scan_t *);

    56

    57

void ec_fsm_slave_scan_state_end(ec_fsm_slave_scan_t *);

    58

void ec_fsm_slave_scan_state_error(ec_fsm_slave_scan_t *);

    59

    60

void ec_fsm_slave_scan_enter_preop(ec_fsm_slave_scan_t *);

    61

void ec_fsm_slave_scan_enter_pdos(ec_fsm_slave_scan_t *);

    62

    63

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

    64

    65

/** Constructor.

    66

 */

    67

void ec_fsm_slave_scan_init(

    68

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

    69

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

    70

        ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration

    71

                                                  state machine to use. */

    72

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

    73

        )

    74

{

    75

    fsm->datagram = datagram;

    76

    fsm->fsm_slave_config = fsm_slave_config;

    77

    fsm->fsm_pdo = fsm_pdo;

    78

    79

    // init sub state machines

    80

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

    81

}

    82

    83

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

    84

    85

/** Destructor.

    86

 */

    87

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

    88

{

    89

    // clear sub state machines

    90

    ec_fsm_sii_clear(&fsm->fsm_sii);

    91

}

    92

    93

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

    94

    95

/**

    96

 * Start slave scan state machine.

    97

 */

    98

    99

void ec_fsm_slave_scan_start(

   100

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

   101

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

   102

        )

   103

{

   104

    fsm->slave = slave;

   105

    fsm->state = ec_fsm_slave_scan_state_start;

   106

}

   107

   108

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

   109

   110

/**

   111

   \return false, if state machine has terminated

   112

*/

   113

   114

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

   115

{

   116

    return fsm->state != ec_fsm_slave_scan_state_end

   117

        && fsm->state != ec_fsm_slave_scan_state_error;

   118

}

   119

   120

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

   121

   122

/**

   123

   Executes the current state of the state machine.

   124

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

   125

   of the state machine is delayed to the next cycle.

   126

   \return false, if state machine has terminated

   127

*/

   128

   129

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

   130

{

   131

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

   132

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

   133

        // datagram was not sent or received yet.

   134

        return ec_fsm_slave_scan_running(fsm);

   135

    }

   136

   137

    fsm->state(fsm);

   138

    return ec_fsm_slave_scan_running(fsm);

   139

}

   140

   141

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

   142

   143

/**

   144

   \return true, if the state machine terminated gracefully

   145

*/

   146

   147

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

   148

{

   149

    return fsm->state == ec_fsm_slave_scan_state_end;

   150

}

   151

   152

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

   153

 *  slave scan state machine

   154

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

   155

   156

/**

   157

   Slave scan state: START.

   158

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

   159

   slave, according to its ring position.

   160

*/

   161

   162

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

   163

{

   164

    // write station address

   165

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

   166

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

   167

    fsm->retries = EC_FSM_RETRIES;

   168

    fsm->state = ec_fsm_slave_scan_state_address;

   169

}

   170

   171

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

   172

   173

/**

   174

   Slave scan state: ADDRESS.

   175

*/

   176

   177

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

   178

{

   179

    ec_datagram_t *datagram = fsm->datagram;

   180

   181

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

   182

        return;

   183

   184

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   185

        fsm->state = ec_fsm_slave_scan_state_error;

   186

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

   187

                " (datagram state %u)\n",

   188

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

   189

        return;

   190

    }

   191

   192

    if (datagram->working_counter != 1) {

   193

        fsm->slave->error_flag = 1;

   194

        fsm->state = ec_fsm_slave_scan_state_error;

   195

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

   196

               fsm->slave->ring_position);

   197

        ec_datagram_print_wc_error(datagram);

   198

        return;

   199

    }

   200

   201

    // Read AL state

   202

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

   203

    ec_datagram_zero(datagram);

   204

    fsm->retries = EC_FSM_RETRIES;

   205

    fsm->state = ec_fsm_slave_scan_state_state;

   206

}

   207

   208

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

   209

   210

/**

   211

   Slave scan state: STATE.

   212

*/

   213

   214

void ec_fsm_slave_scan_state_state(

   215

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

   216

        )

   217

{

   218

    ec_datagram_t *datagram = fsm->datagram;

   219

    ec_slave_t *slave = fsm->slave;

   220

   221

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

   222

        return;

   223

   224

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   225

        fsm->state = ec_fsm_slave_scan_state_error;

   226

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

   227

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

   228

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

   229

        return;

   230

    }

   231

   232

    if (datagram->working_counter != 1) {

   233

        fsm->slave->error_flag = 1;

   234

        fsm->state = ec_fsm_slave_scan_state_error;

   235

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

   236

               fsm->slave->ring_position);

   237

        ec_datagram_print_wc_error(datagram);

   238

        return;

   239

    }

   240

   241

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

   242

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   243

        char state_str[EC_STATE_STRING_SIZE];

   244

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

   245

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

   246

                slave->ring_position, state_str);

   247

    }

   248

   249

    // read base data

   250

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

   251

    ec_datagram_zero(datagram);

   252

    fsm->retries = EC_FSM_RETRIES;

   253

    fsm->state = ec_fsm_slave_scan_state_base;

   254

}

   255

   256

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

   257

   258

/**

   259

   Slave scan state: BASE.

   260

*/

   261

   262

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

   263

{

   264

    ec_datagram_t *datagram = fsm->datagram;

   265

    ec_slave_t *slave = fsm->slave;

   266

   267

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

   268

        return;

   269

   270

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   271

        fsm->state = ec_fsm_slave_scan_state_error;

   272

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

   273

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

   274

               slave->ring_position, datagram->state);

   275

        return;

   276

    }

   277

   278

    if (datagram->working_counter != 1) {

   279

        fsm->slave->error_flag = 1;

   280

        fsm->state = ec_fsm_slave_scan_state_error;

   281

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

   282

               slave->ring_position);

   283

        ec_datagram_print_wc_error(datagram);

   284

        return;

   285

    }

   286

   287

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

   288

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

   289

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

   290

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

   291

   292

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   293

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

   294

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

   295

                slave->base_fmmu_count, EC_MAX_FMMUS);

   296

        slave->base_fmmu_count = EC_MAX_FMMUS;

   297

    }

   298

   299

    // read data link status

   300

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

   301

    ec_datagram_zero(datagram);

   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.boot_rx_mailbox_offset =

   477

        EC_READ_U16(slave->sii_words + 0x0014);

   478

    slave->sii.boot_rx_mailbox_size =

   479

        EC_READ_U16(slave->sii_words + 0x0015);

   480

    slave->sii.boot_tx_mailbox_offset =

   481

        EC_READ_U16(slave->sii_words + 0x0016);

   482

    slave->sii.boot_tx_mailbox_size =

   483

        EC_READ_U16(slave->sii_words + 0x0017);

   484

    slave->sii.std_rx_mailbox_offset =

   485

        EC_READ_U16(slave->sii_words + 0x0018);

   486

    slave->sii.std_rx_mailbox_size =

   487

        EC_READ_U16(slave->sii_words + 0x0019);

   488

    slave->sii.std_tx_mailbox_offset =

   489

        EC_READ_U16(slave->sii_words + 0x001A);

   490

    slave->sii.std_tx_mailbox_size =

   491

        EC_READ_U16(slave->sii_words + 0x001B);

   492

    slave->sii.mailbox_protocols =

   493

        EC_READ_U16(slave->sii_words + 0x001C);

   494

   495

    if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {

   496

        // sii does not contain category data

   497

        fsm->state = ec_fsm_slave_scan_state_end;

   498

        return;

   499

    }

   500

   501

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

   502

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

   503

                " First category header missing.\n",

   504

                slave->ring_position);

   505

        goto end;

   506

    }

   507

   508

    // evaluate category data

   509

    cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;

   510

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

   511

   512

        // type and size words must fit

   513

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