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

   329

                && slave->self_configured)) continue;

   330

   331

        if (master->debug_level) {

   332

            ec_state_string(slave->current_state, old_state);

   333

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

   334

                ec_state_string(slave->requested_state, new_state);

   335

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

   336

                       slave->ring_position, old_state, new_state);

   337

            }

   338

            else if (!slave->self_configured) {

   339

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

   340

                       slave->ring_position, old_state);

   341

            }

   342

        }

   343

   344

        fsm->master_state = ec_fsm_master_configure_slave;

   345

        fsm->slave = slave;

   346

        fsm->slave_state = ec_fsm_slaveconf_state_start;

   347

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

   348

        return;

   349

    }

   350

   351

    // Check, if EoE processing has to be started

   352

    ec_master_eoe_start(master);

   353

   354

    if (master->mode == EC_MASTER_MODE_IDLE) {

   355

   356

        // Check for a pending SDO request

   357

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

   358

            if (master->debug_level)

   359

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

   360

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

   361

            if (slave->current_state == EC_SLAVE_STATE_INIT

   362

                || !slave->online) {

   363

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

   364

                       slave->ring_position);

   365

                master->sdo_request->return_code = -1;

   366

                master->sdo_seq_master++;

   367

            }

   368

            else {

   369

                // start uploading SDO

   370

                fsm->slave = slave;

   371

                fsm->master_state = ec_fsm_master_sdo_request;

   372

                fsm->sdo_request = master->sdo_request;

   373

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

   374

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

   375

                return;

   376

            }

   377

        }

   378

   379

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

   380

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

   381

            if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)

   382

                || slave->sdo_dictionary_fetched

   383

                || slave->current_state == EC_SLAVE_STATE_INIT

   384

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

   385

                || !slave->online

   386

                || slave->error_flag) continue;

   387

   388

            if (master->debug_level) {

   389

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

   390

                       slave->ring_position);

   391

            }

   392

   393

            slave->sdo_dictionary_fetched = 1;

   394

   395

            // start fetching SDO dictionary

   396

            fsm->slave = slave;

   397

            fsm->master_state = ec_fsm_master_sdodict;

   398

            ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);

   399

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

   400

            return;

   401

        }

   402

   403

        // check for pending EEPROM write operations.

   404

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

   405

            if (!slave->new_eeprom_data) continue;

   406

   407

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

   408

                kfree(slave->new_eeprom_data);

   409

                slave->new_eeprom_data = NULL;

   410

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

   411

                       slave->ring_position);

   412

                continue;

   413

            }

   414

   415

            // found pending EEPROM write operation. execute it!

   416

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

   417

            fsm->slave = slave;

   418

            fsm->sii_offset = 0x0000;

   419

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

   420

                             slave->new_eeprom_data, EC_FSM_SII_NODE);

   421

            fsm->master_state = ec_fsm_master_write_eeprom;

   422

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

   423

            return;

   424

        }

   425

    }

   426

   427

    fsm->master_state = ec_fsm_master_end;

   428

}

   429

   430

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

   431

   432

/**

   433

   Master action: Get state of next slave.

   434

*/

   435

   436

void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm

   437

                                           /**< finite state machine */)

   438

{

   439

    ec_master_t *master = fsm->master;

   440

    ec_slave_t *slave = fsm->slave;

   441

   442

    // is there another slave to query?

   443

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

   444

        // process next slave

   445

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

   446

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

   447

                         0x0130, 2);

   448

        ec_master_queue_datagram(master, &fsm->datagram);

   449

        fsm->retries = EC_FSM_RETRIES;

   450

        fsm->master_state = ec_fsm_master_read_states;

   451

        return;

   452

    }

   453

   454

    // all slave states read

   455

   456

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

   457

    if (fsm->master_validation) {

   458

        fsm->master_validation = 0;

   459

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

   460

            if (slave->online) continue;

   461

   462

            // At least one slave is offline. validate!

   463

            EC_INFO("Validating bus.\n");

   464

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

   465

            fsm->master_state = ec_fsm_master_validate_vendor;

   466

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

   467

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

   468

            return;

   469

        }

   470

    }

   471

   472

    ec_fsm_master_action_process_states(fsm);

   473

}

   474

   475

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

   476

   477

/**

   478

   Master state: READ STATES.

   479

   Fetches the AL- and online state of a slave.

   480

*/

   481

   482

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

   483

{

   484

    ec_slave_t *slave = fsm->slave;

   485

    ec_datagram_t *datagram = &fsm->datagram;

   486

    uint8_t new_state;

   487

   488

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

   489

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

   490

        return;

   491

    }