1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT slave state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "fsm_slave.h"

    45

    46

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

    47

    48

void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);

    49

void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);

    50

void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *);

    51

void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *);

    52

void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *);

    53

void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *);

    54

void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *);

    55

    56

void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *);

    57

void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *);

    58

void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *);

    59

void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *);

    60

void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);

    61

void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *);

    62

void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);

    63

void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *);

    64

void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *);

    65

void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);

    66

    67

void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *);

    68

void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);

    69

void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *);

    70

void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    71

void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *);

    72

void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *);

    73

    74

void ec_fsm_slave_state_end(ec_fsm_slave_t *);

    75

void ec_fsm_slave_state_error(ec_fsm_slave_t *);

    76

    77

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

    78

    79

/**

    80

   Constructor.

    81

*/

    82

    83

void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */

    84

        ec_datagram_t *datagram /**< datagram structure to use */

    85

        )

    86

{

    87

    fsm->datagram = datagram;

    88

    89

    // init sub state machines

    90

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

    91

    ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);

    92

    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);

    93

}

    94

    95

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

    96

    97

/**

    98

   Destructor.

    99

*/

   100

   101

void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */)

   102

{

   103

    // clear sub state machines

   104

    ec_fsm_sii_clear(&fsm->fsm_sii);

   105

    ec_fsm_change_clear(&fsm->fsm_change);

   106

    ec_fsm_coe_clear(&fsm->fsm_coe);

   107

}

   108

   109

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

   110

   111

/**

   112

 * Start slave scan state machine.

   113

 */

   114

   115

void ec_fsm_slave_start_scan(ec_fsm_slave_t *fsm, /**< slave state machine */

   116

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

   117

        )

   118

{

   119

    fsm->slave = slave;

   120

    fsm->state = ec_fsm_slave_scan_state_start;

   121

}

   122

   123

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

   124

   125

/**

   126

 * Start slave configuration state machine.

   127

 */

   128

   129

void ec_fsm_slave_start_conf(ec_fsm_slave_t *fsm, /**< slave state machine */

   130

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

   131

        )

   132

{

   133

    fsm->slave = slave;

   134

    fsm->state = ec_fsm_slave_conf_state_start;

   135

}

   136

   137

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

   138

   139

/**

   140

   \return false, if state machine has terminated

   141

*/

   142

   143

int ec_fsm_slave_running(const ec_fsm_slave_t *fsm /**< slave state machine */)

   144

{

   145

    return fsm->state != ec_fsm_slave_state_end

   146

        && fsm->state != ec_fsm_slave_state_error;

   147

}

   148

   149

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

   150

   151

/**

   152

   Executes the current state of the state machine.

   153

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

   154

   of the state machine is delayed to the next cycle.

   155

   \return false, if state machine has terminated

   156

*/

   157

   158

int ec_fsm_slave_exec(ec_fsm_slave_t *fsm /**< slave state machine */)

   159

{

   160

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

   161

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

   162

        // datagram was not sent or received yet.

   163

        return ec_fsm_slave_running(fsm);

   164

    }

   165

   166

    fsm->state(fsm);

   167

    return ec_fsm_slave_running(fsm);

   168

}

   169

   170

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

   171

   172

/**

   173

   \return true, if the state machine terminated gracefully

   174

*/

   175

   176

int ec_fsm_slave_success(const ec_fsm_slave_t *fsm /**< slave state machine */)

   177

{

   178

    return fsm->state == ec_fsm_slave_state_end;

   179

}

   180

   181

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

   182

 *  slave scan state machine

   183

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

   184

   185

/**

   186

   Slave scan state: START.

   187

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

   188

   slave, according to its ring position.

   189

*/

   190

   191

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

   192

{

   193

    // write station address

   194

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

   195

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

   196

    ec_master_queue_datagram(fsm->slave->master, fsm->datagram);

   197

    fsm->retries = EC_FSM_RETRIES;

   198

    fsm->state = ec_fsm_slave_scan_state_address;

   199

}

   200

   201

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

   202

   203

/**

   204

   Slave scan state: ADDRESS.

   205

*/

   206

   207

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

   208

{

   209

    ec_datagram_t *datagram = fsm->datagram;

   210

   211

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

   212

        ec_master_queue_datagram(fsm->slave->master, fsm->datagram);

   213

        return;

   214

    }

   215

   216

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   217

        fsm->state = ec_fsm_slave_state_error;

   218

        EC_ERR("Failed to receive station address datagram for slave %i.\n",

   219

               fsm->slave->ring_position);

   220

        return;

   221

    }

   222

   223

    if (datagram->working_counter != 1) {

   224

        fsm->slave->error_flag = 1;

   225

        fsm->state = ec_fsm_slave_state_error;

   226

        EC_ERR("Failed to write station address - slave %i did not respond.\n",

   227

               fsm->slave->ring_position);

   228

        return;

   229

    }

   230

   231

    // Read AL state

   232

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

   233

    ec_master_queue_datagram(fsm->slave->master, datagram);

   234

    fsm->retries = EC_FSM_RETRIES;

   235

    fsm->state = ec_fsm_slave_scan_state_state;

   236

}

   237

   238

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

   239

   240

/**

   241

   Slave scan state: STATE.

   242

*/

   243

   244

void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *fsm /**< slave state machine */)

   245

{

   246

    ec_datagram_t *datagram = fsm->datagram;

   247

    ec_slave_t *slave = fsm->slave;

   248

   249

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

   250

        ec_master_queue_datagram(fsm->slave->master, datagram);

   251

        return;

   252

    }

   253

   254

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   255

        fsm->state = ec_fsm_slave_state_error;

   256

        EC_ERR("Failed to receive AL state datagram from slave %i.\n",

   257

               fsm->slave->ring_position);

   258

        return;

   259

    }

   260

   261

    if (datagram->working_counter != 1) {

   262

        fsm->slave->error_flag = 1;

   263

        fsm->state = ec_fsm_slave_state_error;

   264

        EC_ERR("Failed to read AL state - slave %i did not respond.\n",

   265

               fsm->slave->ring_position);

   266

        return;

   267

    }

   268

   269

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

   270

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   271

        char state_str[EC_STATE_STRING_SIZE];

   272

        ec_state_string(slave->current_state, state_str);

   273

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

   274

                slave->ring_position, state_str);

   275

    }

   276

   277

    // read base data

   278

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

   279

    ec_master_queue_datagram(fsm->slave->master, datagram);

   280

    fsm->retries = EC_FSM_RETRIES;

   281

    fsm->state = ec_fsm_slave_scan_state_base;

   282

}

   283

   284

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

   285

   286

/**

   287

   Slave scan state: BASE.

   288

*/

   289

   290

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

   291

{

   292

    ec_datagram_t *datagram = fsm->datagram;

   293

    ec_slave_t *slave = fsm->slave;

   294

   295

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

   296

        ec_master_queue_datagram(fsm->slave->master, datagram);

   297

        return;

   298

    }

   299

   300

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   301

        fsm->state = ec_fsm_slave_state_error;

   302

        EC_ERR("Failed to receive base data datagram for slave %i.\n",

   303

               slave->ring_position);

   304

        return;

   305

    }

   306

   307

    if (datagram->working_counter != 1) {

   308

        fsm->slave->error_flag = 1;

   309

        fsm->state = ec_fsm_slave_state_error;

   310

        EC_ERR("Failed to read base data - slave %i did not respond.\n",

   311

               slave->ring_position);

   312

        return;

   313

    }

   314

   315

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

   316

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

   317

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

   318

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

   319

    slave->base_sync_count = EC_READ_U8 (datagram->data + 5);

   320

   321

    if (slave->base_fmmu_count > EC_MAX_FMMUS)

   322

        slave->base_fmmu_count = EC_MAX_FMMUS;

   323

   324

    // read data link status

   325

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

   326

    ec_master_queue_datagram(slave->master, datagram);

   327

    fsm->retries = EC_FSM_RETRIES;

   328

    fsm->state = ec_fsm_slave_scan_state_datalink;

   329

}

   330

   331

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

   332

   333

/**

   334

   Slave scan state: DATALINK.

   335

*/

   336

   337

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

   338

{

   339

    ec_datagram_t *datagram = fsm->datagram;

   340

    ec_slave_t *slave = fsm->slave;

   341

    uint16_t dl_status;

   342

    unsigned int i;

   343

   344

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

   345

        ec_master_queue_datagram(fsm->slave->master, datagram);

   346

        return;

   347

    }

   348

   349

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   350

        fsm->state = ec_fsm_slave_state_error;

   351

        EC_ERR("Failed to receive DL status datagram from slave %i.\n",

   352

               slave->ring_position);

   353

        return;

   354

    }

   355

   356

    if (datagram->working_counter != 1) {

   357

        fsm->slave->error_flag = 1;

   358

        fsm->state = ec_fsm_slave_state_error;

   359

        EC_ERR("Failed to read DL status - slave %i did not respond.\n",

   360

               slave->ring_position);

   361

        return;

   362

    }

   363

   364

    dl_status = EC_READ_U16(datagram->data);

   365

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

   366

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

   367

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

   368

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

   369

    }

   370

   371

    // Start fetching EEPROM size

   372

   373

    fsm->sii_offset = 0x0040; // first category header

   374

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

   375

    fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   376

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

   377

}

   378

   379

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

   380

   381

/**

   382

   Slave scan state: EEPROM SIZE.

   383

*/

   384

   385

void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *fsm /**< slave state machine */)

   386

{

   387

    ec_slave_t *slave = fsm->slave;

   388

    uint16_t cat_type, cat_size;

   389

   390

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

   391

   392

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

   393

        fsm->slave->error_flag = 1;

   394

        fsm->state = ec_fsm_slave_state_error;

   395

        EC_ERR("Failed to read EEPROM size of slave %i.\n",

   396

               slave->ring_position);

   397

        return;

   398

    }

   399

   400

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

   401

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

   402

   403

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

   404

        fsm->sii_offset += cat_size + 2;

   405

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

   406

                        EC_FSM_SII_NODE);

   407

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

   408

        return;

   409

    }

   410

   411

    slave->eeprom_size = (fsm->sii_offset + 1) * 2;

   412

   413

    if (slave->eeprom_data) {

   414

        EC_INFO("Freeing old EEPROM data on slave %i...\n",

   415

                slave->ring_position);

   416

        kfree(slave->eeprom_data);

   417

    }

   418

   419

    if (!(slave->eeprom_data =

   420

          (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) {

   421

        fsm->slave->error_flag = 1;

   422

        fsm->state = ec_fsm_slave_state_error;

   423

        EC_ERR("Failed to allocate EEPROM data on slave %i.\n",

   424

               slave->ring_position);

   425

        return;

   426

    }

   427

   428

    // Start fetching EEPROM contents

   429

   430

    fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   431

    fsm->sii_offset = 0x0000;

   432

    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

   433

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

   434

}

   435

   436

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

   437

   438

/**

   439

   Slave scan state: EEPROM DATA.

   440

*/

   441

   442

void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *fsm /**< slave state machine */)

   443

{

   444

    ec_slave_t *slave = fsm->slave;

   445

    uint16_t *cat_word, cat_type, cat_size;

   446

   447

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

   448

   449

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

   450

        fsm->slave->error_flag = 1;

   451

        fsm->state = ec_fsm_slave_state_error;

   452

        EC_ERR("Failed to fetch EEPROM contents of slave %i.\n",

   453

               slave->ring_position);

   454

        return;

   455

    }

   456

   457

    // 2 words fetched

   458

   459

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

   460

        memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   461

               fsm->fsm_sii.value, 4);

   462

    }

   463

    else { // copy the last word

   464

        memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   465

               fsm->fsm_sii.value, 2);

   466

    }

   467

   468

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

   469

        // fetch the next 2 words

   470

        fsm->sii_offset += 2;

   471

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

   472

                        EC_FSM_SII_NODE);

   473

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

   474

        return;

   475

    }

   476

   477

    // Evaluate EEPROM contents

   478

   479

    slave->sii_alias =

   480

        EC_READ_U16(slave->eeprom_data + 2 * 0x0004);

   481

    slave->sii_vendor_id =

   482

        EC_READ_U32(slave->eeprom_data + 2 * 0x0008);

   483

    slave->sii_product_code =

   484

        EC_READ_U32(slave->eeprom_data + 2 * 0x000A);

   485

    slave->sii_revision_number =

   486

        EC_READ_U32(slave->eeprom_data + 2 * 0x000C);

   487

    slave->sii_serial_number =

   488

        EC_READ_U32(slave->eeprom_data + 2 * 0x000E);

   489

    slave->sii_rx_mailbox_offset =

   490

        EC_READ_U16(slave->eeprom_data + 2 * 0x0018);

   491

    slave->sii_rx_mailbox_size =

   492

        EC_READ_U16(slave->eeprom_data + 2 * 0x0019);

   493

    slave->sii_tx_mailbox_offset =

   494

        EC_READ_U16(slave->eeprom_data + 2 * 0x001A);

   495

    slave->sii_tx_mailbox_size =

   496

        EC_READ_U16(slave->eeprom_data + 2 * 0x001B);

   497

    slave->sii_mailbox_protocols =

   498

        EC_READ_U16(slave->eeprom_data + 2 * 0x001C);

   499

   500

    // evaluate category data

   501

    cat_word = (uint16_t *) slave->eeprom_data + 0x0040;

   502

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

   503

        cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   504

        cat_size = EC_READ_U16(cat_word + 1);

   505

   506

        switch (cat_type) {

   507

            case 0x000A:

   508

                if (ec_slave_fetch_strings(slave, (uint8_t *) (cat_word + 2)))

   509

                    goto end;

   510

                break;

   511

            case 0x001E:

   512

                ec_slave_fetch_general(slave, (uint8_t *) (cat_word + 2));

   513

                break;

   514

            case 0x0028:

   515

                break;

   516

            case 0x0029:

   517

                if (ec_slave_fetch_sync(slave, (uint8_t *) (cat_word + 2),

   518

                                        cat_size))

   519

                    goto end;

   520

                break;

   521

            case 0x0032:

   522

                if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2),

   523

                                       cat_size, EC_TX_PDO))

   524

                    goto end;

   525

                break;

   526

            case 0x0033:

   527

                if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2),

   528

                                       cat_size, EC_RX_PDO))

   529

                    goto end;