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

    45

#ifdef EC_EOE

    46

#include "ethernet.h"

    47

#endif

    48

    49

#include "fsm_master.h"

    50

    51

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

    52

    53

void ec_fsm_master_state_start(ec_fsm_master_t *);

    54

void ec_fsm_master_state_broadcast(ec_fsm_master_t *);

    55

void ec_fsm_master_state_read_states(ec_fsm_master_t *);

    56

void ec_fsm_master_state_acknowledge(ec_fsm_master_t *);

    57

void ec_fsm_master_state_validate_vendor(ec_fsm_master_t *);

    58

void ec_fsm_master_state_validate_product(ec_fsm_master_t *);

    59

void ec_fsm_master_state_rewrite_addresses(ec_fsm_master_t *);

    60

void ec_fsm_master_state_configure_slave(ec_fsm_master_t *);

    61

void ec_fsm_master_state_clear_addresses(ec_fsm_master_t *);

    62

void ec_fsm_master_state_scan_slaves(ec_fsm_master_t *);

    63

void ec_fsm_master_state_write_sii(ec_fsm_master_t *);

    64

void ec_fsm_master_state_sdodict(ec_fsm_master_t *);

    65

void ec_fsm_master_state_sdo_request(ec_fsm_master_t *);

    66

void ec_fsm_master_state_end(ec_fsm_master_t *);

    67

void ec_fsm_master_state_error(ec_fsm_master_t *);

    68

    69

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

    70

    71

/**

    72

   Constructor.

    73

*/

    74

    75

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

    76

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

    77

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

    78

        )

    79

{

    80

    fsm->master = master;

    81

    fsm->datagram = datagram;

    82

    fsm->state = ec_fsm_master_state_start;

    83

    fsm->idle = 0;

    84

    fsm->slaves_responding = 0;

    85

    fsm->topology_change_pending = 0;

    86

    fsm->slave_states = EC_SLAVE_STATE_UNKNOWN;

    87

    fsm->validate = 0;

    88

    fsm->tainted = 0;

    89

    90

    // init sub-state-machines

    91

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

    92

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

    93

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

    94

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

    95

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

    96

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

    97

    ec_fsm_coe_map_init(&fsm->fsm_coe_map, &fsm->fsm_coe);

    98

}

    99

   100

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

   101

   102

/**

   103

   Destructor.

   104

*/

   105

   106

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

   107

{

   108

    // clear sub-state machines

   109

    ec_fsm_slave_config_clear(&fsm->fsm_slave_config);

   110

    ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan);

   111

    ec_fsm_sii_clear(&fsm->fsm_sii);

   112

    ec_fsm_change_clear(&fsm->fsm_change);

   113

    ec_fsm_coe_clear(&fsm->fsm_coe);

   114

    ec_fsm_coe_map_clear(&fsm->fsm_coe_map);

   115

}

   116

   117

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

   118

   119

/**

   120

   Executes the current state of the state machine.

   121

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

   122

   of the state machine is delayed to the next cycle.

   123

   \return false, if state machine has terminated

   124

*/

   125

   126

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

   127

{

   128

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

   129

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

   130

        // datagram was not sent or received yet.

   131

        return ec_fsm_master_running(fsm);

   132

    }

   133

   134

    fsm->state(fsm);

   135

    return ec_fsm_master_running(fsm);

   136

}

   137

   138

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

   139

   140

/**

   141

 * \return false, if state machine has terminated

   142

 */

   143

   144

int ec_fsm_master_running(

   145

        const ec_fsm_master_t *fsm /**< master state machine */

   146

        )

   147

{

   148

    return fsm->state != ec_fsm_master_state_end

   149

        && fsm->state != ec_fsm_master_state_error;

   150

}

   151

   152

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

   153

   154

/**

   155

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

   156

 */

   157

   158

int ec_fsm_master_idle(

   159

        const ec_fsm_master_t *fsm /**< master state machine */

   160

        )

   161

{

   162

    return fsm->idle;

   163

}

   164

   165

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

   166

 *  master state machine

   167

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

   168

   169

/**

   170

   Master state: START.

   171

   Starts with getting slave count and slave states.

   172

*/

   173

   174

void ec_fsm_master_state_start(ec_fsm_master_t *fsm)

   175

{

   176

    fsm->idle = 1;

   177

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

   178

    fsm->state = ec_fsm_master_state_broadcast;

   179

}

   180

   181

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

   182

   183

/**

   184

   Master state: BROADCAST.

   185

   Processes the broadcast read slave count and slaves states.

   186

*/

   187

   188

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

   189

{

   190

    ec_datagram_t *datagram = fsm->datagram;

   191

    unsigned int i;

   192

    ec_slave_t *slave;

   193

    ec_master_t *master = fsm->master;

   194

   195

    if (datagram->state == EC_DATAGRAM_TIMED_OUT)

   196

        return; // always retry

   197

   198

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

   199

        fsm->slaves_responding = 0;

   200

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

   201

            ec_slave_set_online_state(slave, EC_SLAVE_OFFLINE);

   202

        }

   203

        fsm->state = ec_fsm_master_state_error;

   204

        return;

   205

    }

   206

   207

    // bus topology change?

   208

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

   209

        fsm->topology_change_pending = 1;

   210

        fsm->slaves_responding = datagram->working_counter;

   211

   212

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

   213

                fsm->slaves_responding,

   214

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

   215

   216

        if (master->mode == EC_MASTER_MODE_OPERATION) {

   217

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

   218

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

   219

            }

   220

            else {

   221

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

   222

                fsm->tainted = 1;

   223

            }

   224

        }

   225

    }

   226

   227

    // slave states changed?

   228

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

   229

        char states[EC_STATE_STRING_SIZE];

   230

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

   231

        ec_state_string(fsm->slave_states, states);

   232

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

   233

    }

   234

   235

    if (fsm->topology_change_pending) {

   236

        down(&master->scan_sem);

   237

        if (!master->allow_scan) {

   238

            up(&master->scan_sem);

   239

        }

   240

        else {

   241

            master->scan_state = EC_REQUEST_BUSY;

   242

            up(&master->scan_sem);

   243

   244

            // topology change when scan is allowed:

   245

            // clear all slaves and scan the bus

   246

            fsm->topology_change_pending = 0;

   247

            fsm->tainted = 0;

   248

            fsm->idle = 0;

   249

            fsm->scan_jiffies = jiffies;

   250

   251

#ifdef EC_EOE

   252

            ec_master_eoe_stop(master);

   253

            ec_master_clear_eoe_handlers(master);

   254

#endif

   255

            ec_master_destroy_slaves(master);

   256

   257

            master->slave_count = datagram->working_counter;

   258

   259

            if (!master->slave_count) {

   260

                // no slaves present -> finish state machine.

   261

                master->scan_state = EC_REQUEST_SUCCESS;

   262

                wake_up_interruptible(&master->scan_queue);

   263

                fsm->state = ec_fsm_master_state_end;

   264

                return;

   265

            }

   266

   267

            // init slaves

   268

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

   269

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

   270

                                GFP_ATOMIC))) {

   271

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

   272

                    ec_master_destroy_slaves(master);

   273

                    master->scan_state = EC_REQUEST_FAILURE;

   274

                    wake_up_interruptible(&master->scan_queue);

   275

                    fsm->state = ec_fsm_master_state_error;

   276

                    return;

   277

                }

   278

   279

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

   280

                    // freeing of "slave" already done

   281

                    ec_master_destroy_slaves(master);

   282

                    master->scan_state = EC_REQUEST_FAILURE;

   283

                    wake_up_interruptible(&master->scan_queue);

   284

                    fsm->state = ec_fsm_master_state_error;

   285

                    return;

   286

                }

   287

   288

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

   289

            }

   290

   291

            if (master->debug_level)

   292

                EC_DBG("Clearing station addresses...\n");

   293

   294

            ec_datagram_bwr(datagram, 0x0010, 2);

   295

            EC_WRITE_U16(datagram->data, 0x0000);

   296

            fsm->retries = EC_FSM_RETRIES;

   297

            fsm->state = ec_fsm_master_state_clear_addresses;

   298

            return;

   299

        }

   300

    }

   301

   302

    if (list_empty(&master->slaves)) {

   303

        fsm->state = ec_fsm_master_state_end;

   304

    } else {

   305

        // fetch state from each slave

   306

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

   307

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

   308

        fsm->retries = EC_FSM_RETRIES;

   309

        fsm->state = ec_fsm_master_state_read_states;

   310

    }

   311

}

   312

   313

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

   314

   315

/**

   316

 * Check for pending SII write requests and process one.

   317

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

   318

 */

   319

   320

int ec_fsm_master_action_process_sii(

   321

        ec_fsm_master_t *fsm /**< master state machine */

   322

        )

   323

{

   324

    ec_master_t *master = fsm->master;

   325

    ec_sii_write_request_t *request;

   326

    ec_slave_t *slave;

   327

   328

    // search the first request to be processed

   329

    while (1) {

   330

        down(&master->sii_sem);

   331

        if (list_empty(&master->sii_requests)) {

   332

            up(&master->sii_sem);

   333

            break;

   334

        }

   335

        // get first request

   336

        request = list_entry(master->sii_requests.next,

   337

                ec_sii_write_request_t, list);

   338

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

   339

        request->state = EC_REQUEST_BUSY;

   340

        up(&master->sii_sem);

   341

   342

        slave = request->slave;

   343

        if (slave->online_state == EC_SLAVE_OFFLINE) {

   344

            EC_ERR("Discarding SII data, slave %i offline.\n",

   345

                    slave->ring_position);

   346

            request->state = EC_REQUEST_FAILURE;

   347

            wake_up(&master->sii_queue);

   348

            continue;

   349

        }

   350

   351

        // found pending SII write operation. execute it!

   352

        if (master->debug_level)

   353

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

   354

                    slave->ring_position);

   355

        fsm->sii_request = request;

   356

        fsm->sii_index = 0;

   357

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

   358

                request->data, EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   359

        fsm->state = ec_fsm_master_state_write_sii;

   360

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

   361

        return 1;

   362

    }

   363

   364

    return 0;

   365

}

   366

   367

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

   368

   369

/**

   370

 * Check for pending Sdo requests and process one.

   371

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

   372

 */

   373

   374

int ec_fsm_master_action_process_sdo(

   375

        ec_fsm_master_t *fsm /**< master state machine */

   376

        )

   377

{

   378

    ec_master_t *master = fsm->master;

   379

    ec_master_sdo_request_t *request;

   380

    ec_sdo_request_t *req;

   381

    ec_slave_t *slave;

   382

   383

    // search for internal requests to be processed

   384

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

   385

        if (!slave->config)

   386

            continue;

   387

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

   388

            if (req->state == EC_REQUEST_QUEUED) {

   397

   398

                if (slave->current_state == EC_SLAVE_STATE_INIT ||

   399

                        slave->error_flag) {

   400

                    req->state = EC_REQUEST_FAILURE;

   401

                    continue;

   402

                }

   403

   405

                if (master->debug_level)

   406

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

   407

                            slave->ring_position);

   408

   409

                fsm->idle = 0;

   410

                fsm->sdo_request = req;

   411

                fsm->slave = slave;

   412

                fsm->state = ec_fsm_master_state_sdo_request;

   413

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

   414

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

   415

                return 1;

   416

            }

   417

        }

   418

    }

   419

   420

    // search the first external request to be processed

   421

    while (1) {

   422

        down(&master->sdo_sem);

   423

        if (list_empty(&master->slave_sdo_requests)) {

   424

            up(&master->sdo_sem);

   425

            break;

   426

        }

   427

        // get first request

   428

        request = list_entry(master->slave_sdo_requests.next,

   429

                ec_master_sdo_request_t, list);

   430

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

   431

        request->req.state = EC_REQUEST_BUSY;

   432

        up(&master->sdo_sem);

   433

   434

        slave = request->slave;

   435

        if (slave->current_state == EC_SLAVE_STATE_INIT ||

   436

                slave->online_state == EC_SLAVE_OFFLINE ||

   437

                slave->error_flag) {

   438

            EC_ERR("Discarding Sdo request, slave %u not ready.\n",

   439

                    slave->ring_position);

   440

            request->req.state = EC_REQUEST_FAILURE;

   441

            wake_up(&master->sdo_queue);

   442

            continue;

   443

        }

   444

   445

        // Found pending Sdo request. Execute it!

   446

        if (master->debug_level)

   447

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

   448

                    slave->ring_position);

   449

   450

        // Start uploading Sdo

   451

        fsm->idle = 0;

   452

        fsm->sdo_request = &request->req;

   453

        fsm->slave = slave;

   454

        fsm->state = ec_fsm_master_state_sdo_request;

   455

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

   456

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

   457

        return 1;

   458

    }

   459

   460

    return 0;

   461

}

   462

   463

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

   464

   465

/**

   466

 * Check for slaves that are not configured and configure them.

   467

 */

   468

   469

int ec_fsm_master_action_configure(

   470

        ec_fsm_master_t *fsm /**< master state machine */

   471

        )

   472

{

   473

    ec_slave_t *slave;

   474

    ec_master_t *master = fsm->master;

   475

    char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];

   476

   477

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

   478

    // FIXME do not check all slaves in every cycle...

   479

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

   480

        if (slave->error_flag

   481

                || slave->online_state == EC_SLAVE_OFFLINE

   482

                || slave->requested_state == EC_SLAVE_STATE_UNKNOWN

   483

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

   484

                    && slave->self_configured)) continue;

   485

   486

        if (master->debug_level) {

   487

            ec_state_string(slave->current_state, old_state);

   488

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