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

/** \file

    35

 * EtherCAT master state machine.

    36

 */

    37

    38

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

    39

    40

#include "globals.h"

    41

#include "master.h"

    42

#include "mailbox.h"

    43

#include "slave_config.h"

    44

#ifdef EC_EOE

    45

#include "ethernet.h"

    46

#endif

    47

    48

#include "fsm_master.h"

    49

    50

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

    51

    52

void ec_fsm_master_state_start(ec_fsm_master_t *);

    53

void ec_fsm_master_state_broadcast(ec_fsm_master_t *);

    54

void ec_fsm_master_state_read_state(ec_fsm_master_t *);

    55

void ec_fsm_master_state_acknowledge(ec_fsm_master_t *);

    56

void ec_fsm_master_state_configure_slave(ec_fsm_master_t *);

    57

void ec_fsm_master_state_clear_addresses(ec_fsm_master_t *);

    58

void ec_fsm_master_state_scan_slave(ec_fsm_master_t *);

    59

void ec_fsm_master_state_write_sii(ec_fsm_master_t *);

    60

void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);

    61

void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);

    62

    63

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

    64

    65

/** Constructor.

    66

 */

    67

void ec_fsm_master_init(

    68

        ec_fsm_master_t *fsm, /**< Master state machine. */

    69

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

    70

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

    71

        )

    72

{

    73

    fsm->master = master;

    74

    fsm->datagram = datagram;

    75

    fsm->state = ec_fsm_master_state_start;

    76

    fsm->idle = 0;

    77

    fsm->slaves_responding = 0;

    78

    fsm->topology_change_pending = 0;

    79

    fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;

    80

    81

    // init sub-state-machines

    82

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

    83

    ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe);

    84

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

    85

    ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram,

    86

            &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_pdo);

    87

    ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram,

    88

            &fsm->fsm_slave_config, &fsm->fsm_pdo);

    89

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

    90

}

    91

    92

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

    93

    94

/** Destructor.

    95

 */

    96

void ec_fsm_master_clear(

    97

        ec_fsm_master_t *fsm /**< Master state machine. */

    98

        )

    99

{

   100

    // clear sub-state machines

   101

    ec_fsm_coe_clear(&fsm->fsm_coe);

   102

    ec_fsm_pdo_clear(&fsm->fsm_pdo);

   103

    ec_fsm_change_clear(&fsm->fsm_change);

   104

    ec_fsm_slave_config_clear(&fsm->fsm_slave_config);

   105

    ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan);

   106

    ec_fsm_sii_clear(&fsm->fsm_sii);

   107

}

   108

   109

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

   110

   111

/** Executes the current state of the state machine.

   112

 *

   113

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

   114

 * of the state machine is delayed to the next cycle.

   115

 */

   116

void ec_fsm_master_exec(

   117

        ec_fsm_master_t *fsm /**< Master state machine. */

   118

        )

   119

{

   120

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

   121

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

   122

        // datagram was not sent or received yet.

   123

        return;

   124

    }

   125

   126

    fsm->state(fsm);

   127

}

   128

   129

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

   130

   131

/**

   132

 * \return true, if the state machine is in an idle phase

   133

 */

   134

int ec_fsm_master_idle(

   135

        const ec_fsm_master_t *fsm /**< Master state machine. */

   136

        )

   137

{

   138

    return fsm->idle;

   139

}

   140

   141

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

   142

   143

/** Restarts the master state machine.

   144

 */

   145

void ec_fsm_master_restart(

   146

        ec_fsm_master_t *fsm /**< Master state machine. */

   147

        )

   148

{

   149

    fsm->state = ec_fsm_master_state_start;

   150

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

   151

}

   152

   153

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

   154

 * Master state machine

   155

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

   156

   157

/** Master state: START.

   158

 *

   159

 * Starts with getting slave count and slave states.

   160

 */

   161

void ec_fsm_master_state_start(

   162

        ec_fsm_master_t *fsm /**< Master state machine. */

   163

        )

   164

{

   165

    fsm->idle = 1;

   166

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

   167

    fsm->state = ec_fsm_master_state_broadcast;

   168

}

   169

   170

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

   171

   172

/** Master state: BROADCAST.

   173

 *

   174

 * Processes the broadcast read slave count and slaves states.

   175

 */

   176

void ec_fsm_master_state_broadcast(

   177

        ec_fsm_master_t *fsm /**< Master state machine. */

   178

        )

   179

{

   180

    ec_datagram_t *datagram = fsm->datagram;

   181

    unsigned int i, size;

   182

    ec_slave_t *slave;

   183

    ec_master_t *master = fsm->master;

   184

   185

    if (datagram->state == EC_DATAGRAM_TIMED_OUT)

   186

        return; // always retry

   187

   188

    // bus topology change?

   189

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

   190

        fsm->topology_change_pending = 1;

   191

        fsm->slaves_responding = datagram->working_counter;

   192

        EC_INFO("%u slave(s) responding.\n", fsm->slaves_responding);

   193

    }

   194

   195

    if (datagram->state != EC_DATAGRAM_RECEIVED) { // link is down

   196

        ec_fsm_master_restart(fsm);

   197

        return;

   198

    }

   199

   200

    if (fsm->slaves_responding) {

   201

        uint8_t states = EC_READ_U8(datagram->data);

   202

        if (states != fsm->slave_states) { // slave states changed?

   203

            char state_str[EC_STATE_STRING_SIZE];

   204

            fsm->slave_states = states;

   205

            ec_state_string(fsm->slave_states, state_str);

   206

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

   207

        }

   208

    } else {

   209

        fsm->slave_states = 0x00;

   210

    }

   211

   212

    if (fsm->topology_change_pending) {

   213

        down(&master->scan_sem);

   214

        if (!master->allow_scan) {

   215

            up(&master->scan_sem);

   216

        } else {

   217

            master->scan_busy = 1;

   218

            up(&master->scan_sem);

   219

   220

            // topology change when scan is allowed:

   221

            // clear all slaves and scan the bus

   222

            fsm->topology_change_pending = 0;

   223

            fsm->idle = 0;

   224

            fsm->scan_jiffies = jiffies;

   225

   226

#ifdef EC_EOE

   227

            ec_master_eoe_stop(master);

   228

            ec_master_clear_eoe_handlers(master);

   229

#endif

   230

            ec_master_clear_slaves(master);

   231

   232

            master->slave_count = fsm->slaves_responding;

   233

   234

            if (!master->slave_count) {

   235

                // no slaves present -> finish state machine.

   236

                master->scan_busy = 0;

   237

                wake_up_interruptible(&master->scan_queue);

   238

                ec_fsm_master_restart(fsm);

   239

                return;

   240

            }

   241

   242

            size = sizeof(ec_slave_t) * master->slave_count;

   243

            if (!(master->slaves = (ec_slave_t *) kmalloc(size, GFP_KERNEL))) {

   244

                EC_ERR("Failed to allocate %u bytes of slave memory!\n",

   245

                        size);

   246

                master->slave_count = 0; // TODO avoid retrying scan!

   247

                master->scan_busy = 0;

   248

                wake_up_interruptible(&master->scan_queue);

   249

                ec_fsm_master_restart(fsm);

   250

                return;

   251

            }

   252

   253

            // init slaves

   254

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

   255

                slave = master->slaves + i;

   256

                ec_slave_init(slave, master, i, i + 1);

   257

   258

                // do not force reconfiguration in operation phase to avoid

   259

                // unnecesssary process data interruptions

   260

                if (master->phase != EC_OPERATION)

   261

                    slave->force_config = 1;

   262

            }

   263

   264

            // broadcast clear all station addresses

   265

            ec_datagram_bwr(datagram, 0x0010, 2);

   266

            EC_WRITE_U16(datagram->data, 0x0000);

   267

            fsm->retries = EC_FSM_RETRIES;

   268

            fsm->state = ec_fsm_master_state_clear_addresses;

   269

            return;

   270

        }

   271

    }

   272

   273

    if (master->slave_count) {

   274

        // fetch state from first slave

   275

        fsm->slave = master->slaves;

   276

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

   277

                0x0130, 2);

   278

        fsm->retries = EC_FSM_RETRIES;

   279

        fsm->state = ec_fsm_master_state_read_state;

   280

    } else {

   281

        ec_fsm_master_restart(fsm);

   282

    }

   283

}

   284

   285

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

   286

   287

/** Check for pending SII write requests and process one.

   288

 * 

   289

 * \return non-zero, if an SII write request is processed.

   290

 */

   291

int ec_fsm_master_action_process_sii(

   292

        ec_fsm_master_t *fsm /**< Master state machine. */

   293

        )

   294

{

   295

    ec_master_t *master = fsm->master;

   296

    ec_sii_write_request_t *request;

   297

   298

    // search the first request to be processed

   299

    while (1) {

   300

        if (list_empty(&master->sii_requests))

   301

            break;

   302

   303

        // get first request

   304

        request = list_entry(master->sii_requests.next,

   305

                ec_sii_write_request_t, list);

   306

        list_del_init(&request->list); // dequeue

   307

        request->state = EC_REQUEST_BUSY;

   308

   309

        // found pending SII write operation. execute it!

   310

        if (master->debug_level)

   311

            EC_DBG("Writing SII data to slave %u...\n",

   312

                    request->slave->ring_position);

   313

        fsm->sii_request = request;

   314

        fsm->sii_index = 0;

   315

        ec_fsm_sii_write(&fsm->fsm_sii, request->slave, request->offset,

   316

                request->words, EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   317

        fsm->state = ec_fsm_master_state_write_sii;

   318

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

   319

        return 1;

   320

    }

   321

   322

    return 0;

   323

}

   324

   325

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

   326

   327

/** Check for pending Sdo requests and process one.

   328

 * 

   329

 * \return non-zero, if an Sdo request is processed.

   330

 */

   331

int ec_fsm_master_action_process_sdo(

   332

        ec_fsm_master_t *fsm /**< Master state machine. */

   333

        )

   334

{

   335

    ec_master_t *master = fsm->master;

   336

    ec_slave_t *slave;

   337

    ec_sdo_request_t *req;

   338

    ec_master_sdo_request_t *request;

   339

   340

    // search for internal requests to be processed

   341

    for (slave = master->slaves;

   342

            slave < master->slaves + master->slave_count;

   343

            slave++) {

   344

        if (!slave->config)

   345

            continue;

   346

        list_for_each_entry(req, &slave->config->sdo_requests, list) {

   347

            if (req->state == EC_REQUEST_QUEUED) {

   348

   349

                if (ec_sdo_request_timed_out(req)) {

   350

                    req->state = EC_REQUEST_FAILURE;

   351

                    if (master->debug_level)

   352

                        EC_DBG("Sdo request for slave %u timed out...\n",

   353

                                slave->ring_position);

   354

                    continue;

   355

                }

   356

   357

                if (slave->current_state == EC_SLAVE_STATE_INIT) {

   358

                    req->state = EC_REQUEST_FAILURE;

   359

                    continue;

   360

                }

   361

   362

                req->state = EC_REQUEST_BUSY;

   363

                if (master->debug_level)

   364

                    EC_DBG("Processing Sdo request for slave %u...\n",

   365

                            slave->ring_position);

   366

   367

                fsm->idle = 0;

   368

                fsm->sdo_request = req;

   369

                fsm->slave = slave;

   370

                fsm->state = ec_fsm_master_state_sdo_request;

   371

                ec_fsm_coe_transfer(&fsm->fsm_coe, slave, req);

   372

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

   373

                return 1;

   374

            }

   375

        }

   376

    }

   377

   378

    // search the first external request to be processed

   379

    while (1) {

   380

        if (list_empty(&master->slave_sdo_requests))

   381

            break;

   382

   383

        // get first request

   384

        request = list_entry(master->slave_sdo_requests.next,

   385

                ec_master_sdo_request_t, list);

   386

        list_del_init(&request->list); // dequeue

   387

        request->req.state = EC_REQUEST_BUSY;

   388

   389

        slave = request->slave;

   390

        if (slave->current_state == EC_SLAVE_STATE_INIT) {

   391

            EC_ERR("Discarding Sdo request, slave %u is in INIT.\n",

   392

                    slave->ring_position);

   393

            request->req.state = EC_REQUEST_FAILURE;

   394

            wake_up(&master->sdo_queue);

   395

            continue;

   396

        }

   397

   398

        // Found pending Sdo request. Execute it!

   399

        if (master->debug_level)

   400

            EC_DBG("Processing Sdo request for slave %u...\n",

   401

                    slave->ring_position);

   402

   403

        // Start uploading Sdo

   404

        fsm->idle = 0;

   405

        fsm->sdo_request = &request->req;

   406

        fsm->slave = slave;

   407

        fsm->state = ec_fsm_master_state_sdo_request;

   408

        ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);

   409

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

   410

        return 1;

   411

    }

   412

   413

    return 0;

   414

}

   415

   416

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

   417

   418

/** Master action: IDLE.

   419

 *

   420

 * Does secondary work.

   421

 */

   422

void ec_fsm_master_action_idle(

   423

        ec_fsm_master_t *fsm /**< Master state machine. */

   424

        )

   425

{

   426

    ec_master_t *master = fsm->master;

   427

    ec_slave_t *slave;

   428

   429

    // Check for pending Sdo requests

   430

    if (ec_fsm_master_action_process_sdo(fsm))

   431

        return;

   432

   433

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

   434

    for (slave = master->slaves;

   435

            slave < master->slaves + master->slave_count;

   436

            slave++) {

   437

        if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)

   438

                || (slave->sii.has_general

   439

                    && !slave->sii.coe_details.enable_sdo_info)

   440

                || slave->sdo_dictionary_fetched

   441

                || slave->current_state == EC_SLAVE_STATE_INIT

   442

                || slave->current_state == EC_SLAVE_STATE_UNKNOWN

   443

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

   444

                ) continue;

   445

   446

        if (master->debug_level) {

   447

            EC_DBG("Fetching Sdo dictionary from slave %u.\n",

   448

                    slave->ring_position);

   449

        }

   450

   451

        slave->sdo_dictionary_fetched = 1;

   452

   453

        // start fetching Sdo dictionary

   454

        fsm->idle = 0;

   455

        fsm->slave = slave;

   456

        fsm->state = ec_fsm_master_state_sdo_dictionary;

   457

        ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);

   458

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

   459

        return;

   460

    }

   461

   462

    // check for pending SII write operations.

   463

    if (ec_fsm_master_action_process_sii(fsm))

   464

        return; // SII write request found

   465

   466

    ec_fsm_master_restart(fsm);

   467

}

   468

   469

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

   470

   471

/** Master action: Get state of next slave.

   472

 */

   473

void ec_fsm_master_action_next_slave_state(

   474

        ec_fsm_master_t *fsm /**< Master state machine. */

   475

        )

   476

{

   477

    ec_master_t *master = fsm->master;

   478

   479

    // is there another slave to query?

   480

    fsm->slave++;

   481

    if (fsm->slave < master->slaves + master->slave_count) {

   482

        // fetch state from next slave

   483

        fsm->idle = 1;

   484

        ec_datagram_fprd(fsm->datagram,

   485

                fsm->slave->station_address, 0x0130, 2);

   486