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_validate_vendor(ec_fsm_t *);

    52

void ec_fsm_master_validate_product(ec_fsm_t *);

    53

void ec_fsm_master_rewrite_addresses(ec_fsm_t *);

    54

void ec_fsm_master_configure_slave(ec_fsm_t *);

    55

void ec_fsm_master_scan_slaves(ec_fsm_t *);

    56

void ec_fsm_master_write_eeprom(ec_fsm_t *);

    57

    58

void ec_fsm_startup_start(ec_fsm_t *);

    59

void ec_fsm_startup_broadcast(ec_fsm_t *);

    60

void ec_fsm_startup_scan(ec_fsm_t *);

    61

    62

void ec_fsm_configuration_start(ec_fsm_t *);

    63

void ec_fsm_configuration_conf(ec_fsm_t *);

    64

    65

void ec_fsm_slavescan_start(ec_fsm_t *);

    66

void ec_fsm_slavescan_address(ec_fsm_t *);

    67

void ec_fsm_slavescan_state(ec_fsm_t *);

    68

void ec_fsm_slavescan_base(ec_fsm_t *);

    69

void ec_fsm_slavescan_datalink(ec_fsm_t *);

    70

void ec_fsm_slavescan_eeprom_size(ec_fsm_t *);

    71

void ec_fsm_slavescan_eeprom_data(ec_fsm_t *);

    72

    73

void ec_fsm_slaveconf_init(ec_fsm_t *);

    74

void ec_fsm_slaveconf_sync(ec_fsm_t *);

    75

void ec_fsm_slaveconf_preop(ec_fsm_t *);

    76

void ec_fsm_slaveconf_fmmu(ec_fsm_t *);

    77

void ec_fsm_slaveconf_sdoconf(ec_fsm_t *);

    78

void ec_fsm_slaveconf_saveop(ec_fsm_t *);

    79

void ec_fsm_slaveconf_op(ec_fsm_t *);

    80

    81

void ec_fsm_sii_start_reading(ec_fsm_t *);

    82

void ec_fsm_sii_read_check(ec_fsm_t *);

    83

void ec_fsm_sii_read_fetch(ec_fsm_t *);

    84

void ec_fsm_sii_start_writing(ec_fsm_t *);

    85

void ec_fsm_sii_write_check(ec_fsm_t *);

    86

void ec_fsm_sii_write_check2(ec_fsm_t *);

    87

    88

void ec_fsm_change_start(ec_fsm_t *);

    89

void ec_fsm_change_check(ec_fsm_t *);

    90

void ec_fsm_change_status(ec_fsm_t *);

    91

void ec_fsm_change_code(ec_fsm_t *);

    92

void ec_fsm_change_ack(ec_fsm_t *);

    93

void ec_fsm_change_check_ack(ec_fsm_t *);

    94

    95

void ec_fsm_coe_down_start(ec_fsm_t *);

    96

void ec_fsm_coe_down_request(ec_fsm_t *);

    97

void ec_fsm_coe_down_check(ec_fsm_t *);

    98

void ec_fsm_coe_down_response(ec_fsm_t *);

    99

   100

void ec_fsm_end(ec_fsm_t *);

   101

void ec_fsm_error(ec_fsm_t *);

   102

   103

void ec_canopen_abort_msg(uint32_t);

   104

   105

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

   106

   107

/**

   108

   Constructor.

   109

*/

   110

   111

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

   112

                ec_master_t *master /**< EtherCAT master */

   113

                )

   114

{

   115

    fsm->master = master;

   116

    fsm->master_state = ec_fsm_master_start;

   117

    fsm->master_slaves_responding = 0;

   118

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   119

    fsm->master_validation = 0;

   120

   121

    ec_datagram_init(&fsm->datagram);

   122

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

   123

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

   124

        return -1;

   125

    }

   126

   127

    return 0;

   128

}

   129

   130

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

   131

   132

/**

   133

   Destructor.

   134

*/

   135

   136

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

   137

{

   138

    ec_datagram_clear(&fsm->datagram);

   139

}

   140

   141

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

   142

   143

/**

   144

   Resets the state machine.

   145

*/

   146

   147

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

   148

{

   149

    fsm->master_state = ec_fsm_master_start;

   150

    fsm->master_slaves_responding = 0;

   151

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   152

}

   153

   154

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

   155

   156

/**

   157

   Executes the current state of the state machine.

   158

*/

   159

   160

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

   161

{

   162

    fsm->master_state(fsm);

   163

}

   164

   165

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

   166

   167

/**

   168

   Initializes the master startup state machine.

   169

*/

   170

   171

void ec_fsm_startup(ec_fsm_t *fsm)

   172

{

   173

    fsm->master_state = ec_fsm_startup_start;

   174

}

   175

   176

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

   177

   178

/**

   179

   Returns the running state of the master startup state machine.

   180

   \return non-zero if not terminated yet.

   181

*/

   182

   183

int ec_fsm_startup_running(ec_fsm_t *fsm /**< Finite state machine */)

   184

{

   185

    return fsm->master_state != ec_fsm_end &&

   186

        fsm->master_state != ec_fsm_error;

   187

}

   188

   189

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

   190

   191

/**

   192

   Returns, if the master startup state machine terminated with success.

   193

   \return non-zero if successful.

   194

*/

   195

   196

int ec_fsm_startup_success(ec_fsm_t *fsm /**< Finite state machine */)

   197

{

   198

    return fsm->master_state == ec_fsm_end;

   199

}

   200

   201

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

   202

   203

/**

   204

   Initializes the master configuration state machine.

   205

*/

   206

   207

void ec_fsm_configuration(ec_fsm_t *fsm)

   208

{

   209

    fsm->master_state = ec_fsm_configuration_start;

   210

}

   211

   212

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

   213

   214

/**

   215

   Returns the running state of the master configuration state machine.

   216

   \return non-zero if not terminated yet.

   217

*/

   218

   219

int ec_fsm_configuration_running(ec_fsm_t *fsm /**< Finite state machine */)

   220

{

   221

    return fsm->master_state != ec_fsm_end &&

   222

        fsm->master_state != ec_fsm_error;

   223

}

   224

   225

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

   226

   227

/**

   228

   Returns, if the master confuguration state machine terminated with success.

   229

   \return non-zero if successful.

   230

*/

   231

   232

int ec_fsm_configuration_success(ec_fsm_t *fsm /**< Finite state machine */)

   233

{

   234

    return fsm->master_state == ec_fsm_end;

   235

}

   236

   237

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

   238

 *  master startup state machine

   239

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

   240

   241

/**

   242

   Master state: START.

   243

   Starts with getting slave count and slave states.

   244

*/

   245

   246

void ec_fsm_startup_start(ec_fsm_t *fsm)

   247

{

   248

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

   249

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

   250

    fsm->master_state = ec_fsm_startup_broadcast;

   251

}

   252

   253

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

   254

   255

/**

   256

   Master state: BROADCAST.

   257

   Processes the broadcast read slave count and slaves states.

   258

*/

   259

   260

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

   261

{

   262

    ec_datagram_t *datagram = &fsm->datagram;

   263

    unsigned int i;

   264

    ec_slave_t *slave;

   265

    ec_master_t *master = fsm->master;

   266

   267

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   268

        EC_ERR("Failed to receive broadcast datagram.\n");

   269

        fsm->master_state = ec_fsm_error;

   270

        return;

   271

    }

   272

   273

    EC_INFO("Scanning bus.\n");

   274

   275

    ec_master_clear_slaves(master);

   276

   277

    master->slave_count = datagram->working_counter;

   278

   279

    if (!master->slave_count) {

   280

        // no slaves present -> finish state machine.

   281

        fsm->master_state = ec_fsm_end;

   282

        return;

   283

    }

   284

   285

    // init slaves

   286

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

   287

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

   288

                                             GFP_KERNEL))) {

   289

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

   290

            fsm->master_state = ec_fsm_error;

   291

            return;

   292

        }

   293

   294

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

   295

            fsm->master_state = ec_fsm_error;

   296

            return;

   297

        }

   298

   299

        if (kobject_add(&slave->kobj)) {

   300

            EC_ERR("Failed to add kobject.\n");

   301

            kobject_put(&slave->kobj); // free

   302

            fsm->master_state = ec_fsm_error;

   303

            return;

   304

        }

   305

   306

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

   307

    }

   308

   309

    // begin scanning of slaves

   310

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

   311

    fsm->slave_state = ec_fsm_slavescan_start;

   312

    fsm->master_state = ec_fsm_startup_scan;

   313

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

   314

    return;

   315

}

   316

   317

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

   318

   319

/**

   320

   Master state: SCAN.

   321

   Executes the sub-statemachine for the scanning of a slave.

   322

*/

   323

   324

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

   325

{

   326

    ec_master_t *master = fsm->master;

   327

    ec_slave_t *slave = fsm->slave;

   328

   329

    fsm->slave_state(fsm); // execute slave state machine

   330

   331

    if (fsm->slave_state == ec_fsm_error) {

   332

        EC_ERR("Slave scanning failed.\n");

   333

        fsm->master_state = ec_fsm_error;

   334

        return;

   335

    }

   336

   337

    if (fsm->slave_state != ec_fsm_end) return;

   338

   339

    // another slave to scan?

   340

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

   341

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

   342

        fsm->slave_state = ec_fsm_slavescan_start;

   343

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

   344

        return;

   345

    }

   346

   347

    EC_INFO("Bus scanning completed.\n");

   348

   349

    ec_master_calc_addressing(master);

   350

   351

    fsm->master_state = ec_fsm_end;

   352

}

   353

   354

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

   355

 *  master configuration state machine

   356

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

   357

   358

/**

   359

   Master configuration state machine: START.

   360

*/

   361

   362

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

   363

{

   364

    ec_master_t *master = fsm->master;

   365

   366

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

   367

        fsm->master_state = ec_fsm_end;

   368

        return;

   369

    }

   370

   371

    // begin configuring slaves

   372

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

   373

    fsm->slave_state = ec_fsm_slaveconf_init;

   374

    fsm->change_new = EC_SLAVE_STATE_INIT;

   375

    fsm->change_state = ec_fsm_change_start;

   376

    fsm->master_state = ec_fsm_configuration_conf;

   377

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

   378

}

   379

   380

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

   381

   382

/**

   383

   Master state: CONF.

   384

*/

   385

   386

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

   387

{

   388

    ec_master_t *master = fsm->master;

   389

    ec_slave_t *slave = fsm->slave;

   390

   391

    fsm->slave_state(fsm); // execute slave's state machine

   392

   393

    if (fsm->slave_state == ec_fsm_error) {

   394

        fsm->master_state = ec_fsm_error;

   395

        return;

   396

    }

   397

   398

    if (fsm->slave_state != ec_fsm_end) return;

   399

   400

    // another slave to configure?

   401

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

   402

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

   403

        fsm->slave_state = ec_fsm_slaveconf_init;

   404

        fsm->change_new = EC_SLAVE_STATE_INIT;

   405

        fsm->change_state = ec_fsm_change_start;

   406

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

   407

        return;

   408

    }

   409

   410

    fsm->master_state = ec_fsm_end;

   411

}

   412

   413

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

   414

 *  operation / idle state machine

   415

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

   416

   417

/**

   418

   Master state: START.

   419

   Starts with getting slave count and slave states.

   420

*/

   421

   422

void ec_fsm_master_start(ec_fsm_t *fsm)

   423

{

   424

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

   425

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

   426

    fsm->master_state = ec_fsm_master_broadcast;

   427

}

   428

   429

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

   430

   431

/**

   432

   Master state: BROADCAST.

   433

   Processes the broadcast read slave count and slaves states.

   434

*/

   435

   436

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

   437

{

   438

    ec_datagram_t *datagram = &fsm->datagram;

   439

    unsigned int topology_change, states_change, i;

   440

    ec_slave_t *slave;

   441

    ec_master_t *master = fsm->master;

   442

   443

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   444

        if (!master->device->link_state) {

   445

            fsm->master_slaves_responding = 0;

   446

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

   447

                slave->online = 0;

   448

            }

   449

        }

   450

        fsm->master_state = ec_fsm_master_start;

   451

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

   452

        return;

   453

    }

   454

   455

    topology_change = (datagram->working_counter !=

   456

                       fsm->master_slaves_responding);

   457

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

   458

   459

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

   460

    fsm->master_slaves_responding = datagram->working_counter;

   461

   462

    if (topology_change) {

   463

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

   464

                fsm->master_slaves_responding,

   465

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

   466

   467

        if (master->mode == EC_MASTER_MODE_OPERATION) {

   468

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

   469

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

   470

            }

   471

            else {

   472

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

   473

            }

   474

        }

   475

    }

   476

   477

    if (states_change) {

   478

        char states[EC_STATE_STRING_SIZE];

   479

        ec_state_string(fsm->master_slave_states, states);

   480

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

   481

    }

   482

   483

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

   484

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

   485

        EC_INFO("Scanning bus.\n");

   486

   487

        ec_master_eoe_stop(master);

   488

        ec_master_clear_slaves(master);

   489

   490

        master->slave_count = datagram->working_counter;

   491

   492

        if (!master->slave_count) {

   493

            // no slaves present -> finish state machine.

   494

            fsm->master_state = ec_fsm_master_start;

   495

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

   496

            return;