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 "fsm.h"

    43

#include "master.h"

    44

#include "mailbox.h"

    45

    46

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

    47

    48

void ec_fsm_master_start(ec_fsm_t *);

    49

void ec_fsm_master_broadcast(ec_fsm_t *);

    50

void ec_fsm_master_read_states(ec_fsm_t *);

    51

void ec_fsm_master_acknowledge(ec_fsm_t *);

    52

void ec_fsm_master_validate_vendor(ec_fsm_t *);

    53

void ec_fsm_master_validate_product(ec_fsm_t *);

    54

void ec_fsm_master_rewrite_addresses(ec_fsm_t *);

    55

void ec_fsm_master_configure_slave(ec_fsm_t *);

    56

void ec_fsm_master_scan_slaves(ec_fsm_t *);

    57

void ec_fsm_master_write_eeprom(ec_fsm_t *);

    58

void ec_fsm_master_sdodict(ec_fsm_t *);

    59

void ec_fsm_master_sdo_request(ec_fsm_t *);

    60

void ec_fsm_master_end(ec_fsm_t *);

    61

void ec_fsm_master_error(ec_fsm_t *);

    62

    63

void ec_fsm_slavescan_start(ec_fsm_t *);

    64

void ec_fsm_slavescan_address(ec_fsm_t *);

    65

void ec_fsm_slavescan_state(ec_fsm_t *);

    66

void ec_fsm_slavescan_base(ec_fsm_t *);

    67

void ec_fsm_slavescan_datalink(ec_fsm_t *);

    68

void ec_fsm_slavescan_eeprom_size(ec_fsm_t *);

    69

void ec_fsm_slavescan_eeprom_data(ec_fsm_t *);

    70

    71

void ec_fsm_slaveconf_state_start(ec_fsm_t *);

    72

void ec_fsm_slaveconf_state_init(ec_fsm_t *);

    73

void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *);

    74

void ec_fsm_slaveconf_state_sync(ec_fsm_t *);

    75

void ec_fsm_slaveconf_state_preop(ec_fsm_t *);

    76

void ec_fsm_slaveconf_state_sync2(ec_fsm_t *);

    77

void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *);

    78

void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *);

    79

void ec_fsm_slaveconf_state_saveop(ec_fsm_t *);

    80

void ec_fsm_slaveconf_state_op(ec_fsm_t *);

    81

    82

void ec_fsm_slaveconf_enter_sync(ec_fsm_t *);

    83

void ec_fsm_slaveconf_enter_preop(ec_fsm_t *);

    84

void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *);

    85

void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *);

    86

void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *);

    87

void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *);

    88

    89

void ec_fsm_slave_state_end(ec_fsm_t *);

    90

void ec_fsm_slave_state_error(ec_fsm_t *);

    91

    92

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

    93

    94

/**

    95

   Constructor.

    96

*/

    97

    98

int ec_fsm_init(ec_fsm_t *fsm, /**< finite state machine */

    99

                ec_master_t *master /**< EtherCAT master */

   100

                )

   101

{

   102

    fsm->master = master;

   103

    fsm->master_state = ec_fsm_master_start;

   104

    fsm->master_slaves_responding = 0;

   105

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   106

    fsm->master_validation = 0;

   107

   108

    ec_datagram_init(&fsm->datagram);

   109

    if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) {

   110

        EC_ERR("Failed to allocate FSM datagram.\n");

   111

        return -1;

   112

    }

   113

   114

    // init sub-state-machines

   115

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

   116

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

   117

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

   118

   119

    return 0;

   120

}

   121

   122

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

   123

   124

/**

   125

   Destructor.

   126

*/

   127

   128

void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)

   129

{

   130

    // clear sub-state machines

   131

    ec_fsm_sii_clear(&fsm->fsm_sii);

   132

    ec_fsm_change_clear(&fsm->fsm_change);

   133

    ec_fsm_coe_clear(&fsm->fsm_coe);

   134

   135

    ec_datagram_clear(&fsm->datagram);

   136

}

   137

   138

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

   139

   140

/**

   141

   Executes the current state of the state machine.

   142

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

   143

   of the state machine is delayed to the next cycle.

   144

   \return false, if state machine has terminated

   145

*/

   146

   147

int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)

   148

{

   149

    if (fsm->datagram.state == EC_DATAGRAM_SENT

   150

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

   151

        // datagram was not sent or received yet.

   152

        return ec_fsm_running(fsm);

   153

    }

   154

   155

    fsm->master_state(fsm);

   156

    return ec_fsm_running(fsm);

   157

}

   158

   159

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

   160

   161

/**

   162

   \return false, if state machine has terminated

   163

*/

   164

   165

int ec_fsm_running(ec_fsm_t *fsm /**< finite state machine */)

   166

{

   167

    return fsm->master_state != ec_fsm_master_end

   168

        && fsm->master_state != ec_fsm_master_error;

   169

}

   170

   171

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

   172

   173

/**

   174

   \return true, if the master state machine terminated gracefully

   175

*/

   176

   177

int ec_fsm_success(ec_fsm_t *fsm /**< finite state machine */)

   178

{

   179

    return fsm->master_state == ec_fsm_master_end;

   180

}

   181

   182

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

   183

 *  operation/idle state machine

   184

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

   185

   186

/**

   187

   Master state: START.

   188

   Starts with getting slave count and slave states.

   189

*/

   190

   191

void ec_fsm_master_start(ec_fsm_t *fsm)

   192

{

   193

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

   194

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

   195

    fsm->master_state = ec_fsm_master_broadcast;

   196

}

   197

   198

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

   199

   200

/**

   201

   Master state: BROADCAST.

   202

   Processes the broadcast read slave count and slaves states.

   203

*/

   204

   205

void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */)

   206

{

   207

    ec_datagram_t *datagram = &fsm->datagram;

   208

    unsigned int topology_change, states_change, i;

   209

    ec_slave_t *slave;

   210

    ec_master_t *master = fsm->master;

   211

   212

    if (datagram->state == EC_DATAGRAM_TIMED_OUT) {

   213

        // always retry

   214

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

   215

        return;

   216

    }

   217

   218

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

   219

        // link is down

   220

        fsm->master_slaves_responding = 0;

   221

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

   222

            slave->online = 0;

   223

        }

   224

        fsm->master_state = ec_fsm_master_error;

   225

        return;

   226

    }

   227

   228

    topology_change = (datagram->working_counter !=

   229

                       fsm->master_slaves_responding);

   230

    states_change = (EC_READ_U8(datagram->data) != fsm->master_slave_states);

   231

   232

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

   233

    fsm->master_slaves_responding = datagram->working_counter;

   234

   235

    if (topology_change) {

   236

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

   237

                fsm->master_slaves_responding,

   238

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

   239

   240

        if (master->mode == EC_MASTER_MODE_OPERATION) {

   241

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

   242

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

   243

            }

   244

            else {

   245

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

   246

            }

   247

        }

   248

    }

   249

   250

    if (states_change) {

   251

        char states[EC_STATE_STRING_SIZE];

   252

        ec_state_string(fsm->master_slave_states, states);

   253

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

   254

    }

   255

   256

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

   257

    if (topology_change && master->mode == EC_MASTER_MODE_IDLE) {

   258

   259

        ec_master_eoe_stop(master);

   260

        ec_master_destroy_slaves(master);

   261

   262

        master->slave_count = datagram->working_counter;

   263

   264

        if (!master->slave_count) {

   265

            // no slaves present -> finish state machine.

   266

            fsm->master_state = ec_fsm_master_end;

   267

            return;

   268

        }

   269

   270

        // init slaves

   271

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

   272

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

   273

                                                 GFP_ATOMIC))) {

   274

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

   275

                ec_master_destroy_slaves(master);

   276

                fsm->master_state = ec_fsm_master_error;

   277

                return;

   278

            }

   279

   280

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

   281

                // freeing of "slave" already done

   282

                ec_master_destroy_slaves(master);

   283

                fsm->master_state = ec_fsm_master_error;

   284

                return;

   285

            }

   286

   287

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

   288

        }

   289

   290

        EC_INFO("Scanning bus.\n");

   291

   292

        // begin scanning of slaves

   293

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

   294

        fsm->slave_state = ec_fsm_slavescan_start;

   295

        fsm->master_state = ec_fsm_master_scan_slaves;

   296

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

   297

        return;

   298

    }

   299

   300

    // fetch state from each slave

   301

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

   302

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

   303

    ec_master_queue_datagram(master, &fsm->datagram);

   304

    fsm->retries = EC_FSM_RETRIES;

   305

    fsm->master_state = ec_fsm_master_read_states;

   306

}

   307

   308

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

   309

   310

/**

   311

   Master action: PROC_STATES.

   312

   Processes the slave states.

   313

*/

   314

   315

void ec_fsm_master_action_process_states(ec_fsm_t *fsm

   316

                                         /**< finite state machine */

   317

                                         )

   318

{

   319

    ec_master_t *master = fsm->master;

   320

    ec_slave_t *slave;

   321

    char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];

   322

   323

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

   324

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

   325

        if (slave->error_flag

   326

            || !slave->online

   327

            || slave->requested_state == EC_SLAVE_STATE_UNKNOWN

   328

            || slave->current_state == slave->requested_state) continue;

   329

   330

        if (master->debug_level) {

   331

            ec_state_string(slave->current_state, old_state);

   332

            ec_state_string(slave->requested_state, new_state);

   333

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

   334

                   slave->ring_position, old_state, new_state);

   335

        }

   336

   337

        fsm->master_state = ec_fsm_master_configure_slave;

   338

        fsm->slave = slave;

   339

        fsm->slave_state = ec_fsm_slaveconf_state_start;

   340

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

   341

        return;

   342

    }

   343

   344

    // Check, if EoE processing has to be started

   345

    ec_master_eoe_start(master);

   346

   347

    if (master->mode == EC_MASTER_MODE_IDLE) {

   348

   349

        // Check for a pending SDO request

   350

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

   351

            if (master->debug_level)

   352

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

   353

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

   354

            if (slave->current_state == EC_SLAVE_STATE_INIT

   355

                || !slave->online) {

   356

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

   357

                       slave->ring_position);

   358

                master->sdo_request->return_code = -1;

   359

                master->sdo_seq_master++;

   360

            }

   361

            else {

   362

                // start uploading SDO

   363

                fsm->slave = slave;

   364

                fsm->master_state = ec_fsm_master_sdo_request;

   365

                fsm->sdo_request = master->sdo_request;

   366

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

   367

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

   368

                return;

   369

            }

   370

        }

   371

   372

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

   373

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

   374

            if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)

   375

                || slave->sdo_dictionary_fetched

   376

                || slave->current_state == EC_SLAVE_STATE_INIT

   377

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

   378

                || !slave->online

   379

                || slave->error_flag) continue;

   380

   381

            if (master->debug_level) {

   382

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

   383

                       slave->ring_position);

   384

            }

   385

   386

            slave->sdo_dictionary_fetched = 1;

   387

   388

            // start fetching SDO dictionary

   389

            fsm->slave = slave;

   390

            fsm->master_state = ec_fsm_master_sdodict;

   391

            ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);

   392

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

   393

            return;

   394

        }

   395

   396

        // check for pending EEPROM write operations.

   397

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

   398

            if (!slave->new_eeprom_data) continue;

   399

   400

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

   401

                kfree(slave->new_eeprom_data);

   402

                slave->new_eeprom_data = NULL;

   403

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

   404

                       slave->ring_position);

   405

                continue;

   406

            }

   407

   408

            // found pending EEPROM write operation. execute it!

   409

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

   410

            fsm->slave = slave;

   411

            fsm->sii_offset = 0x0000;

   412

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

   413

                             slave->new_eeprom_data, EC_FSM_SII_NODE);

   414

            fsm->master_state = ec_fsm_master_write_eeprom;

   415

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

   416

            return;

   417

        }

   418

    }

   419

   420

    fsm->master_state = ec_fsm_master_end;

   421

}

   422

   423

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

   424

   425

/**

   426

   Master action: Get state of next slave.

   427

*/

   428

   429

void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm

   430

                                           /**< finite state machine */)

   431

{

   432

    ec_master_t *master = fsm->master;

   433

    ec_slave_t *slave = fsm->slave;

   434

   435

    // is there another slave to query?

   436

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

   437

        // process next slave

   438

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

   439

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

   440

                         0x0130, 2);

   441

        ec_master_queue_datagram(master, &fsm->datagram);

   442

        fsm->retries = EC_FSM_RETRIES;

   443

        fsm->master_state = ec_fsm_master_read_states;

   444

        return;

   445

    }

   446

   447

    // all slave states read

   448

   449

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

   450

    if (fsm->master_validation) {

   451

        fsm->master_validation = 0;

   452

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

   453

            if (slave->online) continue;

   454

   455

            // At least one slave is offline. validate!

   456

            EC_INFO("Validating bus.\n");

   457

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

   458

            fsm->master_state = ec_fsm_master_validate_vendor;

   459

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

   460

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

   461

            return;

   462

        }

   463

    }

   464

   465

    ec_fsm_master_action_process_states(fsm);

   466

}

   467

   468

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

   469

   470

/**

   471

   Master state: READ STATES.

   472

   Fetches the AL- and online state of a slave.

   473

*/

   474

   475

void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */)

   476

{

   477

    ec_slave_t *slave = fsm->slave;

   478

    ec_datagram_t *datagram = &fsm->datagram;

   479

    uint8_t new_state;

   480

   481

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

   482

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

   483

        return;

   484

    }

   485

   486

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   487

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

   488

               slave->ring_position);

   489

        fsm->master_state = ec_fsm_master_error;

   490

        return;

   491

    }