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

   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;

   268

   269

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

   270

        return;

   271

   272

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   273

        fsm->state = ec_fsm_slave_scan_state_error;

   274

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

   275

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

   276

               slave->ring_position, datagram->state);

   277

        return;

   278

    }

   279

   280

    if (datagram->working_counter != 1) {

   281

        fsm->slave->error_flag = 1;

   282

        fsm->state = ec_fsm_slave_scan_state_error;

   283

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

   284

               slave->ring_position);

   285

        ec_datagram_print_wc_error(datagram);

   286

        return;

   287

    }

   288

   289

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

   290

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

   291

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

   293

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

   294

    if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   295

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

   296

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

   297

                slave->base_fmmu_count, EC_MAX_FMMUS);

   298

        slave->base_fmmu_count = EC_MAX_FMMUS;

   299

    }

   300

   319

    // read data link status

   320

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

   321

    ec_datagram_zero(datagram);

   322

    fsm->retries = EC_FSM_RETRIES;

   323

    fsm->state = ec_fsm_slave_scan_state_datalink;

   324

}

   325

   326

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

   327

   328

/**

   329

   Slave scan state: DATALINK.

   330

*/

   331

   332

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

   333

{

   334

    ec_datagram_t *datagram = fsm->datagram;

   335

    ec_slave_t *slave = fsm->slave;

   336

    uint16_t dl_status;

   337

    unsigned int i;

   338

   339

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

   340

        return;

   341

   342

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   343

        fsm->state = ec_fsm_slave_scan_state_error;

   344

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

   345

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

   346

               slave->ring_position, datagram->state);

   347

        return;

   348

    }

   349

   350

    if (datagram->working_counter != 1) {

   351

        fsm->slave->error_flag = 1;

   352

        fsm->state = ec_fsm_slave_scan_state_error;

   353

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

   354

               slave->ring_position);

   355

        ec_datagram_print_wc_error(datagram);

   356

        return;

   357

    }

   358

   359

    dl_status = EC_READ_U16(datagram->data);

   360

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

   361

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

   362

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

   363

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

   364

    }

   365

   366

    // Start fetching SII size

   367

   368

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

   369

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

   370

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   371

    fsm->state = ec_fsm_slave_scan_state_sii_size;

   372

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

   373

}

   374

   375

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

   376

   377

/**

   378

   Slave scan state: SII SIZE.

   379

*/

   380

   381

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

   382

{

   383

    ec_slave_t *slave = fsm->slave;

   384

    uint16_t cat_type, cat_size;

   385

   386

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

   387

   388

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

   389

        fsm->slave->error_flag = 1;

   390

        fsm->state = ec_fsm_slave_scan_state_error;

   391

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

   392

               slave->ring_position);

   393

        return;

   394

    }

   395

   396

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

   397

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

   398

   399

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

   400

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

   401

        if (next_offset >= EC_MAX_SII_SIZE) {

   402

            EC_WARN("SII size of slave %u exceeds"

   403

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

   404

                    slave->ring_position, EC_MAX_SII_SIZE);

   405

            // cut off category data...

   406

            slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;

   407

            goto alloc_sii;

   408

        }

   409

        fsm->sii_offset = next_offset;

   410

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

   411

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   412

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

   413

        return;

   414

    }

   415

   416

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

   417

   418

alloc_sii:

   419

    if (slave->sii_words) {

   420

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

   421

                slave->ring_position);

   422

        kfree(slave->sii_words);

   423

    }

   424

   425

    if (!(slave->sii_words =

   426

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

   427

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

   428

               slave->sii_nwords, slave->ring_position);

   429

        slave->sii_nwords = 0;

   430

        slave->error_flag = 1;

   431

        fsm->state = ec_fsm_slave_scan_state_error;

   432

        return;

   433

    }

   434

   435

    // Start fetching SII contents

   436

   437

    fsm->state = ec_fsm_slave_scan_state_sii_data;

   438

    fsm->sii_offset = 0x0000;

   439

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

   440

            EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   441

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

   442

}

   443

   444

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

   445

   446

/**

   447

   Slave scan state: SII DATA.

   448

*/

   449

   450

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

   451

{

   452

    ec_slave_t *slave = fsm->slave;

   453

    uint16_t *cat_word, cat_type, cat_size;

   454

   455

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

   456

   457

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

   458

        fsm->slave->error_flag = 1;

   459

        fsm->state = ec_fsm_slave_scan_state_error;

   460

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

   461

               slave->ring_position);

   462

        return;

   463

    }

   464

   465

    // 2 words fetched

   466

   467

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

   468

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

   469

    } else { // copy the last word

   470

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

   471

    }

   472

   473

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

   474

        // fetch the next 2 words

   475

        fsm->sii_offset += 2;

   476

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

   477

                        EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   478

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

   479

        return;

   480

    }

   481

   482

    // Evaluate SII contents

   483

   484

    ec_slave_clear_sync_managers(slave);

   485

   486

    slave->sii.alias =

   487

        EC_READ_U16(slave->sii_words + 0x0004);

   488

    slave->sii.vendor_id =

   489

        EC_READ_U32(slave->sii_words + 0x0008);

   490

    slave->sii.product_code =

   491

        EC_READ_U32(slave->sii_words + 0x000A);

   492

    slave->sii.revision_number =

   493

        EC_READ_U32(slave->sii_words + 0x000C);

   494

    slave->sii.serial_number =

   495

        EC_READ_U32(slave->sii_words + 0x000E);

   496

    slave->sii.boot_rx_mailbox_offset =

   497

        EC_READ_U16(slave->sii_words + 0x0014);

   498

    slave->sii.boot_rx_mailbox_size =

   499

        EC_READ_U16(slave->sii_words + 0x0015);

   500

    slave->sii.boot_tx_mailbox_offset =

   501

        EC_READ_U16(slave->sii_words + 0x0016);

   502

    slave->sii.boot_tx_mailbox_size =

   503

        EC_READ_U16(slave->sii_words + 0x0017);

   504

    slave->sii.std_rx_mailbox_offset =

   505

        EC_READ_U16(slave->sii_words + 0x0018);

   506

    slave->sii.std_rx_mailbox_size =

   507

        EC_READ_U16(slave->sii_words + 0x0019);

   508

    slave->sii.std_tx_mailbox_offset =

   509

        EC_READ_U16(slave->sii_words + 0x001A);

   510

    slave->sii.std_tx_mailbox_size =

   511

        EC_READ_U16(slave->sii_words + 0x001B);

   512

    slave->sii.mailbox_protocols =