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 finite state machines.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "mailbox.h"

    44

#include "fsm_master.h"

    45

    46

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

    47

    48

void ec_fsm_master_state_start(ec_fsm_master_t *);

    49

void ec_fsm_master_state_broadcast(ec_fsm_master_t *);

    50

void ec_fsm_master_state_read_states(ec_fsm_master_t *);

    51

void ec_fsm_master_state_acknowledge(ec_fsm_master_t *);

    52

void ec_fsm_master_state_validate_vendor(ec_fsm_master_t *);

    53

void ec_fsm_master_state_validate_product(ec_fsm_master_t *);

    54

void ec_fsm_master_state_rewrite_addresses(ec_fsm_master_t *);

    55

void ec_fsm_master_state_configure_slave(ec_fsm_master_t *);

    56

void ec_fsm_master_state_scan_slaves(ec_fsm_master_t *);

    57

void ec_fsm_master_state_write_eeprom(ec_fsm_master_t *);

    58

void ec_fsm_master_state_sdodict(ec_fsm_master_t *);

    59

void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);

    60

void ec_fsm_master_state_end(ec_fsm_master_t *);

    61

void ec_fsm_master_state_error(ec_fsm_master_t *);

    62

    63

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

    64

    65

/**

    66

   Constructor.

    67

*/

    68

    69

void ec_fsm_master_init(ec_fsm_master_t *fsm, /**< master state machine */

    70

        ec_master_t *master, /**< EtherCAT master */

    71

        ec_datagram_t *datagram /**< datagram object to use */

    72

        )

    73

{

    74

    fsm->master = master;

    75

    fsm->datagram = datagram;

    76

    fsm->state = ec_fsm_master_state_start;

    77

    fsm->slaves_responding = 0;

    78

    fsm->topology_change_pending = 0;

    79

    fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;

    80

    fsm->validate = 0;

    81

    82

    // init sub-state-machines

    83

    ec_fsm_slave_init(&fsm->fsm_slave, fsm->datagram);

    84

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

    85

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

    86

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

    87

}

    88

    89

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

    90

    91

/**

    92

   Destructor.

    93

*/

    94

    95

void ec_fsm_master_clear(ec_fsm_master_t *fsm /**< master state machine */)

    96

{

    97

    // clear sub-state machines

    98

    ec_fsm_slave_clear(&fsm->fsm_slave);

    99

    ec_fsm_sii_clear(&fsm->fsm_sii);

   100

    ec_fsm_change_clear(&fsm->fsm_change);

   101

    ec_fsm_coe_clear(&fsm->fsm_coe);

   102

}

   103

   104

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

   105

   106

/**

   107

   Executes the current state of the state machine.

   108

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

   109

   of the state machine is delayed to the next cycle.

   110

   \return false, if state machine has terminated

   111

*/

   112

   113

int ec_fsm_master_exec(ec_fsm_master_t *fsm /**< master state machine */)

   114

{

   115

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

   116

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

   117

        // datagram was not sent or received yet.

   118

        return ec_fsm_master_running(fsm);

   119

    }

   120

   121

    fsm->state(fsm);

   122

    return ec_fsm_master_running(fsm);

   123

}

   124

   125

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

   126

   127

/**

   128

   \return false, if state machine has terminated

   129

*/

   130

   131

int ec_fsm_master_running(ec_fsm_master_t *fsm /**< master state machine */)

   132

{

   133

    return fsm->state != ec_fsm_master_state_end

   134

        && fsm->state != ec_fsm_master_state_error;

   135

}

   136

   137

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

   138

   139

/**

   140

   \return true, if the master state machine terminated gracefully

   141

*/

   142

   143

int ec_fsm_master_success(ec_fsm_master_t *fsm /**< master state machine */)

   144

{

   145

    return fsm->state == ec_fsm_master_state_end;

   146

}

   147

   148

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

   149

 *  operation/idle state machine

   150

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

   151

   152

/**

   153

   Master state: START.

   154

   Starts with getting slave count and slave states.

   155

*/

   156

   157

void ec_fsm_master_state_start(ec_fsm_master_t *fsm)

   158

{

   159

    ec_datagram_brd(fsm->datagram, 0x0130, 2);

   160

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

   161

    fsm->state = ec_fsm_master_state_broadcast;

   162

}

   163

   164

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

   165

   166

/**

   167

   Master state: BROADCAST.

   168

   Processes the broadcast read slave count and slaves states.

   169

*/

   170

   171

void ec_fsm_master_state_broadcast(ec_fsm_master_t *fsm /**< master state machine */)

   172

{

   173

    ec_datagram_t *datagram = fsm->datagram;

   174

    unsigned int i;

   175

    ec_slave_t *slave;

   176

    ec_master_t *master = fsm->master;

   177

   178

    if (datagram->state == EC_DATAGRAM_TIMED_OUT) {

   179

        // always retry

   180

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

   181

        return;

   182

    }

   183

   184

    if (datagram->state != EC_DATAGRAM_RECEIVED) { // EC_DATAGRAM_ERROR

   185

        // link is down

   186

        fsm->slaves_responding = 0;

   187

        list_for_each_entry(slave, &master->slaves, list) {

   188

            slave->online = 0;

   189

        }

   190

        fsm->state = ec_fsm_master_state_error;

   191

        return;

   192

    }

   193

   194

    // bus topology change?

   195

    if (datagram->working_counter != fsm->slaves_responding) {

   196

        fsm->topology_change_pending = 1;

   197

        fsm->slaves_responding = datagram->working_counter;

   198

   199

        EC_INFO("%i slave%s responding.\n",

   200

                fsm->slaves_responding,

   201

                fsm->slaves_responding == 1 ? "" : "s");

   202

   203

        if (master->mode == EC_MASTER_MODE_OPERATION) {

   204

            if (fsm->slaves_responding == master->slave_count) {

   205

                fsm->validate = 1; // start validation later

   206

            }

   207

            else {

   208

                EC_WARN("Invalid slave count. Bus in tainted state.\n");

   209

            }

   210

        }

   211

    }

   212

   213

    // slave states changed?

   214

    if (EC_READ_U8(datagram->data) != fsm->slave_states) {

   215

        char states[EC_STATE_STRING_SIZE];

   216

        fsm->slave_states = EC_READ_U8(datagram->data);

   217

        ec_state_string(fsm->slave_states, states);

   218

        EC_INFO("Slave states: %s.\n", states);

   219

    }

   220

   221

    // topology change in idle mode: clear all slaves and scan the bus

   222

    if (fsm->topology_change_pending &&

   223

            master->mode == EC_MASTER_MODE_IDLE) {

   224

        fsm->topology_change_pending = 0;

   225

   226

        ec_master_eoe_stop(master);

   227

        ec_master_destroy_slaves(master);

   228

   229

        master->slave_count = datagram->working_counter;

   230

   231

        if (!master->slave_count) {

   232

            // no slaves present -> finish state machine.

   233

            fsm->state = ec_fsm_master_state_end;

   234

            return;

   235

        }

   236

   237

        // init slaves

   238

        for (i = 0; i < master->slave_count; i++) {

   239

            if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),

   240

                                                 GFP_ATOMIC))) {

   241

                EC_ERR("Failed to allocate slave %i!\n", i);

   242

                ec_master_destroy_slaves(master);

   243

                fsm->state = ec_fsm_master_state_error;

   244

                return;

   245

            }

   246

   247

            if (ec_slave_init(slave, master, i, i + 1)) {

   248

                // freeing of "slave" already done

   249

                ec_master_destroy_slaves(master);

   250

                fsm->state = ec_fsm_master_state_error;

   251

                return;

   252

            }

   253

   254

            list_add_tail(&slave->list, &master->slaves);

   255

        }

   256

   257

        EC_INFO("Scanning bus.\n");

   258

   259

        // begin scanning of slaves

   260

        fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);

   261

        ec_fsm_slave_start_scan(&fsm->fsm_slave, fsm->slave);

   262

        ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately

   263

        fsm->state = ec_fsm_master_state_scan_slaves;

   264

        return;

   265

    }

   266

   267

    // fetch state from each slave

   268

    fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);

   269

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

   270

    ec_master_queue_datagram(master, fsm->datagram);

   271

    fsm->retries = EC_FSM_RETRIES;

   272

    fsm->state = ec_fsm_master_state_read_states;

   273

}

   274

   275

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

   276

   277

/**

   278

   Master action: PROC_STATES.

   279

   Processes the slave states.

   280

*/

   281

   282

void ec_fsm_master_action_process_states(ec_fsm_master_t *fsm

   283

                                         /**< master state machine */

   284

                                         )

   285

{

   286

    ec_master_t *master = fsm->master;

   287

    ec_slave_t *slave;

   288

    char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];

   289

   290

    // check if any slaves are not in the state, they're supposed to be

   291

    list_for_each_entry(slave, &master->slaves, list) {

   292

        if (slave->error_flag

   293

            || !slave->online

   294

            || slave->requested_state == EC_SLAVE_STATE_UNKNOWN

   295

            || (slave->current_state == slave->requested_state

   296

                && slave->self_configured)) continue;

   297

   298

        if (master->debug_level) {

   299

            ec_state_string(slave->current_state, old_state);

   300

            if (slave->current_state != slave->requested_state) {

   301

                ec_state_string(slave->requested_state, new_state);

   302

                EC_DBG("Changing state of slave %i (%s -> %s).\n",

   303

                       slave->ring_position, old_state, new_state);

   304

            }

   305

            else if (!slave->self_configured) {

   306

                EC_DBG("Reconfiguring slave %i (%s).\n",

   307

                       slave->ring_position, old_state);

   308

            }

   309

        }

   310

   311

        fsm->slave = slave;

   312

        ec_fsm_slave_start_conf(&fsm->fsm_slave, slave);

   313

        ec_fsm_slave_exec(&fsm->fsm_slave); // execute immediately

   314

        fsm->state = ec_fsm_master_state_configure_slave;

   315

        return;

   316

    }

   317

   318

    // Check, if EoE processing has to be started

   319

    ec_master_eoe_start(master);

   320

   321

    if (master->mode == EC_MASTER_MODE_IDLE) {

   322

   323

        // Check for a pending SDO request

   324

        if (master->sdo_seq_master != master->sdo_seq_user) {

   325

            if (master->debug_level)

   326

                EC_DBG("Processing SDO request...\n");

   327

            slave = master->sdo_request->sdo->slave;

   328

            if (slave->current_state == EC_SLAVE_STATE_INIT

   329

                || !slave->online) {

   330

                EC_ERR("Failed to process SDO request, slave %i not ready.\n",

   331

                       slave->ring_position);

   332

                master->sdo_request->return_code = -1;

   333

                master->sdo_seq_master++;

   334

            }

   335

            else {

   336

                // start uploading SDO

   337

                fsm->slave = slave;

   338

                fsm->state = ec_fsm_master_state_sdo_request;

   339

                fsm->sdo_request = master->sdo_request;

   340

                ec_fsm_coe_upload(&fsm->fsm_coe, slave, fsm->sdo_request);

   341

                ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   342

                return;

   343

            }

   344

        }

   345

   346

        // check, if slaves have an SDO dictionary to read out.

   347

        list_for_each_entry(slave, &master->slaves, list) {

   348

            if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)

   349

                || slave->sdo_dictionary_fetched

   350

                || slave->current_state == EC_SLAVE_STATE_INIT

   351

                || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ

   352

                || !slave->online

   353

                || slave->error_flag) continue;

   354

   355

            if (master->debug_level) {

   356

                EC_DBG("Fetching SDO dictionary from slave %i.\n",

   357

                       slave->ring_position);

   358

            }

   359

   360

            slave->sdo_dictionary_fetched = 1;

   361

   362

            // start fetching SDO dictionary

   363

            fsm->slave = slave;

   364

            fsm->state = ec_fsm_master_state_sdodict;

   365

            ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);

   366

            ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   367

            return;

   368

        }

   369

   370

        // check for pending EEPROM write operations.

   371

        list_for_each_entry(slave, &master->slaves, list) {

   372

            if (!slave->new_eeprom_data) continue;

   373

   374

            if (!slave->online || slave->error_flag) {

   375

                kfree(slave->new_eeprom_data);

   376

                slave->new_eeprom_data = NULL;

   377

                EC_ERR("Discarding EEPROM data, slave %i not ready.\n",

   378

                       slave->ring_position);

   379

                continue;

   380

            }

   381

   382

            // found pending EEPROM write operation. execute it!

   383

            EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position);

   384

            fsm->slave = slave;

   385

            fsm->sii_offset = 0x0000;

   386

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

   387

                             slave->new_eeprom_data, EC_FSM_SII_NODE);

   388

            fsm->state = ec_fsm_master_state_write_eeprom;

   389

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

   390

            return;

   391

        }

   392

    }

   393

   394

    fsm->state = ec_fsm_master_state_end;

   395

}

   396

   397

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

   398

   399

/**

   400

   Master action: Get state of next slave.

   401

*/

   402

   403

void ec_fsm_master_action_next_slave_state(ec_fsm_master_t *fsm

   404

                                           /**< master state machine */)

   405

{

   406

    ec_master_t *master = fsm->master;

   407

    ec_slave_t *slave = fsm->slave;

   408

   409

    // is there another slave to query?

   410

    if (slave->list.next != &master->slaves) {

   411

        // process next slave

   412

        fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   413

        ec_datagram_nprd(fsm->datagram, fsm->slave->station_address,

   414

                         0x0130, 2);

   415

        ec_master_queue_datagram(master, fsm->datagram);

   416

        fsm->retries = EC_FSM_RETRIES;

   417

        fsm->state = ec_fsm_master_state_read_states;

   418

        return;

   419

    }

   420

   421

    // all slave states read

   422

   423

    // check, if a bus validation has to be done

   424

    if (fsm->validate) {

   425

        fsm->validate = 0;

   426

        list_for_each_entry(slave, &master->slaves, list) {

   427

            if (slave->online) continue;

   428

   429

            // At least one slave is offline. validate!

   430

            EC_INFO("Validating bus.\n");

   431

            fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);

   432

            fsm->state = ec_fsm_master_state_validate_vendor;

   433

            ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x0008, EC_FSM_SII_POSITION);

   434

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

   435

            return;

   436

        }

   437

    }

   438

   439

    ec_fsm_master_action_process_states(fsm);

   440

}

   441

   442

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

   443

   444

/**

   445

   Master state: READ STATES.

   446

   Fetches the AL- and online state of a slave.

   447

*/

   448

   449

void ec_fsm_master_state_read_states(ec_fsm_master_t *fsm /**< master state machine */)

   450

{

   451

    ec_slave_t *slave = fsm->slave;

   452

    ec_datagram_t *datagram = fsm->datagram;

   453

    uint8_t new_state;

   454

   455

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

   456

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

   457

        return;

   458

    }

   459

   460

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   461

        EC_ERR("Failed to receive AL state datagram for slave %i!\n",

   462

               slave->ring_position);

   463

        fsm->state = ec_fsm_master_state_error;

   464

        return;

   465

    }

   466

   467

    // did the slave not respond to its station address?

   468

    if (datagram->working_counter != 1) {

   469

        if (slave->online) {

   470

            slave->online = 0;

   471

            if (slave->master->debug_level)

   472

                EC_DBG("Slave %i: offline.\n", slave->ring_position);

   473

        }

   474

        ec_fsm_master_action_next_slave_state(fsm);

   475

        return;

   476

    }

   477

   478

    // slave responded

   479

    new_state = EC_READ_U8(datagram->data);

   480

    if (!slave->online) { // slave was offline before

   481

        slave->online = 1;

   482

        slave->error_flag = 0; // clear error flag

   483

        slave->current_state = new_state;

   484

        if (slave->master->debug_level) {

   485

            char cur_state[EC_STATE_STRING_SIZE];

   486

            ec_state_string(slave->current_state, cur_state);

   487

            EC_DBG("Slave %i: online (%s).\n",

   488

                   slave->ring_position, cur_state);

   489

        }

   490

    }

   491

    else if (new_state != slave->current_state) {

   492

        if (slave->master->debug_level) {

   493

            char old_state[EC_STATE_STRING_SIZE],

   494

                cur_state[EC_STATE_STRING_SIZE];

   495

            ec_state_string(slave->current_state, old_state);

   496

            ec_state_string(new_state, cur_state);

   497

            EC_DBG("Slave %i: %s -> %s.\n",

   498

                   slave->ring_position, old_state, cur_state);

   499

        }

   500

        slave->current_state = new_state;

   501

    }

   502

   503

    // check, if new slave state has to be acknowledged

   504

    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR && !slave->error_flag) {

   505

        ec_fsm_change_ack(&fsm->fsm_change, slave);

   506

        ec_fsm_change_exec(&fsm->fsm_change);

   507

        fsm->state = ec_fsm_master_state_acknowledge;

   508

        return;

   509

    }

   510

   511

    ec_fsm_master_action_next_slave_state(fsm);

   512

}

   513

   514

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

   515

   516

/**

   517

   Master state: ACKNOWLEDGE

   518

*/

   519

   520

void ec_fsm_master_state_acknowledge(ec_fsm_master_t *fsm /**< master state machine */)

   521

{

   522

    ec_slave_t *slave = fsm->slave;

   523

   524

    if (ec_fsm_change_exec(&fsm->fsm_change)) return;

   525

   526

    if (!ec_fsm_change_success(&fsm->fsm_change)) {

   527

        fsm->slave->error_flag = 1;

   528

        EC_ERR("Failed to acknowledge state change on slave %i.\n",

   529

               slave->ring_position);

   530