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;

   197

}

   198

   199

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

   200

   201

/**

   202

   Master state: BROADCAST.

   203

   Processes the broadcast read slave count and slaves states.

   204

*/

   205

   206

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

   207

{

   208

    ec_datagram_t *datagram = &fsm->datagram;

   209

    unsigned int topology_change, states_change, i;

   210

    ec_slave_t *slave;

   211

    ec_master_t *master = fsm->master;

   212

   213

    if (datagram->state == EC_DATAGRAM_TIMED_OUT) {

   214

        // always retry

   215

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

   216

        return;

   217

    }

   218

   219

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

   220

        // link is down

   221

        fsm->master_slaves_responding = 0;

   222

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

   223

            slave->online = 0;

   224

        }

   225

        fsm->master_state = ec_fsm_master_error;

   226

        return;

   227

    }

   228

   229

    topology_change = (datagram->working_counter !=

   230

                       fsm->master_slaves_responding);

   231

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

   232

   233

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

   234

    fsm->master_slaves_responding = datagram->working_counter;

   235

   236

    if (topology_change) {

   237

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

   238

                fsm->master_slaves_responding,

   239

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

   240

   241

        if (master->mode == EC_MASTER_MODE_OPERATION) {

   242

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

   243

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

   244

            }

   245

            else {

   246

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

   247

            }

   248

        }

   249

    }

   250

   251

    if (states_change) {

   252

        char states[EC_STATE_STRING_SIZE];

   253

        ec_state_string(fsm->master_slave_states, states);

   254

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

   255

    }

   256

   257

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

   258

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

   259

   260

        ec_master_eoe_stop(master);

   261

        ec_master_destroy_slaves(master);

   262

   263

        master->slave_count = datagram->working_counter;

   264

   265

        if (!master->slave_count) {

   266

            // no slaves present -> finish state machine.

   267

            fsm->master_state = ec_fsm_master_end;

   268

            return;

   269

        }

   270

   271

        // init slaves

   272

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

   273

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

   274

                                                 GFP_ATOMIC))) {

   275

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

   276

                ec_master_destroy_slaves(master);

   277

                fsm->master_state = ec_fsm_master_error;

   278

                return;

   279

            }

   280

   281

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

   282

                // freeing of "slave" already done

   283

                ec_master_destroy_slaves(master);

   284

                fsm->master_state = ec_fsm_master_error;

   285

                return;

   286

            }

   287

   288

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

   289

        }

   290

   291

        EC_INFO("Scanning bus.\n");

   292

   293

        // begin scanning of slaves

   294

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

   295

        fsm->slave_state = ec_fsm_slavescan_start;

   296

        fsm->master_state = ec_fsm_master_scan_slaves;

   297

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

   298

        return;

   299

    }

   300

   301

    // fetch state from each slave

   302

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

   303

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

   304

    ec_master_queue_datagram(master, &fsm->datagram);

   305

    fsm->retries = EC_FSM_RETRIES;

   306

    fsm->master_state = ec_fsm_master_read_states;

   307

}

   308

   309

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

   310

   311

/**

   312

   Master action: PROC_STATES.

   313

   Processes the slave states.

   314

*/

   315

   316

void ec_fsm_master_action_process_states(ec_fsm_t *fsm

   317

                                         /**< finite state machine */

   318

                                         )

   319

{

   320

    ec_master_t *master = fsm->master;

   321

    ec_slave_t *slave;

   322

    char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];

   325

   326

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

   327

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

   328

        if (slave->error_flag

   329

            || !slave->online

   330

            || slave->requested_state == EC_SLAVE_STATE_UNKNOWN

   331

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

   332

   333

        if (master->debug_level) {

   334

            ec_state_string(slave->current_state, old_state);

   335

            ec_state_string(slave->requested_state, new_state);

   336

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

   337

                   slave->ring_position, old_state, new_state);

   338

        }

   339

   340

        fsm->master_state = ec_fsm_master_configure_slave;

   341

        fsm->slave = slave;

   342

        fsm->slave_state = ec_fsm_slaveconf_state_start;

   343

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

   344

        return;

   345

    }

   346

   347

    // Check, if EoE processing has to be started

   348

    ec_master_eoe_start(master);

   349

   350

    if (master->mode == EC_MASTER_MODE_IDLE) {

   351

   352

        // Check for a pending SDO request

   353

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

   354

            if (master->debug_level)

   355

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

   356

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

   357

            if (slave->current_state == EC_SLAVE_STATE_INIT

   358

                || !slave->online) {

   359

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

   360

                       slave->ring_position);

   361

                master->sdo_request->return_code = -1;

   362

                master->sdo_seq_master++;

   363

            }

   364

            else {

   365

                // start uploading SDO

   366

                fsm->slave = slave;

   367

                fsm->master_state = ec_fsm_master_sdo_request;

   368

                fsm->sdo_request = master->sdo_request;

   369

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

   370

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

   371

                return;

   372

            }

   373

        }

   374

   375

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

   376

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

   377

            if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)

   378

                || slave->sdo_dictionary_fetched

   379

                || slave->current_state == EC_SLAVE_STATE_INIT

   380

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

   381

                || !slave->online

   382

                || slave->error_flag) continue;

   383

   384

            if (master->debug_level) {

   385

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

   386

                       slave->ring_position);

   387

            }

   388

   389

            slave->sdo_dictionary_fetched = 1;

   390

   391

            // start fetching SDO dictionary

   392

            fsm->slave = slave;

   393

            fsm->master_state = ec_fsm_master_sdodict;

   394

            ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);

   395

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

   396

            return;

   397

        }

   398

   399

        // check for pending EEPROM write operations.

   400

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

   401

            if (!slave->new_eeprom_data) continue;

   402

   403

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

   404

                kfree(slave->new_eeprom_data);

   405

                slave->new_eeprom_data = NULL;

   406

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

   407

                       slave->ring_position);

   408

                continue;

   409

            }

   410

   411

            // found pending EEPROM write operation. execute it!

   412

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

   413

            fsm->slave = slave;

   414

            fsm->sii_offset = 0x0000;

   415

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

   416

                             slave->new_eeprom_data, EC_FSM_SII_NODE);

   417

            fsm->master_state = ec_fsm_master_write_eeprom;

   418

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

   419

            return;

   420

        }

   421

    }

   422

   423

    fsm->master_state = ec_fsm_master_end;

   424

}

   425

   426

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

   427

   428

/**

   429

   Master action: Get state of next slave.

   430

*/

   431

   432

void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm

   433

                                           /**< finite state machine */)

   434

{

   435

    ec_master_t *master = fsm->master;

   436

    ec_slave_t *slave = fsm->slave;

   437

   438

    // is there another slave to query?

   439

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

   440

        // process next slave

   441

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

   442

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

   443

                         0x0130, 2);

   444

        ec_master_queue_datagram(master, &fsm->datagram);

   445

        fsm->retries = EC_FSM_RETRIES;

   446

        fsm->master_state = ec_fsm_master_read_states;

   447

        return;

   448

    }

   449

   450

    // all slave states read

   451

   452

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

   453

    if (fsm->master_validation) {

   454

        fsm->master_validation = 0;

   455

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

   456

            if (slave->online) continue;

   457

   458

            // At least one slave is offline. validate!

   459

            EC_INFO("Validating bus.\n");

   460

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

   461

            fsm->master_state = ec_fsm_master_validate_vendor;

   462

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

   463

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

   464

            return;

   465

        }

   466

    }

   467

   468

    ec_fsm_master_action_process_states(fsm);

   469

}

   470

   471

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

   472

   473

/**

   474

   Master state: READ STATES.

   475

   Fetches the AL- and online state of a slave.

   476

*/

   477

   478

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

   479

{

   480

    ec_slave_t *slave = fsm->slave;

   481

    ec_datagram_t *datagram = &fsm->datagram;

   482

    uint8_t new_state;

   483

   484

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

   485

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

   486

        return;

   487

    }

   488

   489

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   490

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

   491

               slave->ring_position);