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

    77

void ec_fsm_slaveconf_sdoconf(ec_fsm_t *);

    78

void ec_fsm_slaveconf_fmmu(ec_fsm_t *);

    79

void ec_fsm_slaveconf_saveop(ec_fsm_t *);

    80

void ec_fsm_slaveconf_op(ec_fsm_t *);

    81

    82

void ec_fsm_sii_start_reading(ec_fsm_t *);

    83

void ec_fsm_sii_read_check(ec_fsm_t *);

    84

void ec_fsm_sii_read_fetch(ec_fsm_t *);

    85

void ec_fsm_sii_start_writing(ec_fsm_t *);

    86

void ec_fsm_sii_write_check(ec_fsm_t *);

    87

void ec_fsm_sii_write_check2(ec_fsm_t *);

    88

    89

void ec_fsm_change_start(ec_fsm_t *);

    90

void ec_fsm_change_check(ec_fsm_t *);

    91

void ec_fsm_change_status(ec_fsm_t *);

    92

void ec_fsm_change_code(ec_fsm_t *);

    93

void ec_fsm_change_ack(ec_fsm_t *);

    94

void ec_fsm_change_check_ack(ec_fsm_t *);

    95

    96

void ec_fsm_coe_dict_start(ec_fsm_t *);

    97

void ec_fsm_coe_dict_request(ec_fsm_t *);

    98

void ec_fsm_coe_dict_check(ec_fsm_t *);

    99

void ec_fsm_coe_dict_response(ec_fsm_t *);

   100

void ec_fsm_coe_dict_desc_request(ec_fsm_t *);

   101

void ec_fsm_coe_dict_desc_check(ec_fsm_t *);

   102

void ec_fsm_coe_dict_desc_response(ec_fsm_t *);

   103

void ec_fsm_coe_dict_entry_request(ec_fsm_t *);

   104

void ec_fsm_coe_dict_entry_check(ec_fsm_t *);

   105

void ec_fsm_coe_dict_entry_response(ec_fsm_t *);

   106

   107

void ec_fsm_coe_down_start(ec_fsm_t *);

   108

void ec_fsm_coe_down_request(ec_fsm_t *);

   109

void ec_fsm_coe_down_check(ec_fsm_t *);

   110

void ec_fsm_coe_down_response(ec_fsm_t *);

   111

   112

void ec_fsm_end(ec_fsm_t *);

   113

void ec_fsm_error(ec_fsm_t *);

   114

   115

void ec_canopen_abort_msg(uint32_t);

   116

   117

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

   118

   119

/**

   120

   Constructor.

   121

*/

   122

   123

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

   124

                ec_master_t *master /**< EtherCAT master */

   125

                )

   126

{

   127

    fsm->master = master;

   128

    fsm->master_state = ec_fsm_master_start;

   129

    fsm->master_slaves_responding = 0;

   130

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   131

    fsm->master_validation = 0;

   132

   133

    ec_datagram_init(&fsm->datagram);

   134

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

   135

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

   136

        return -1;

   137

    }

   138

   139

    return 0;

   140

}

   141

   142

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

   143

   144

/**

   145

   Destructor.

   146

*/

   147

   148

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

   149

{

   150

    ec_datagram_clear(&fsm->datagram);

   151

}

   152

   153

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

   154

   155

/**

   156

   Resets the state machine.

   157

*/

   158

   159

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

   160

{

   161

    fsm->master_state = ec_fsm_master_start;

   162

    fsm->master_slaves_responding = 0;

   163

    fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   164

}

   165

   166

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

   167

   168

/**

   169

   Executes the current state of the state machine.

   170

*/

   171

   172

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

   173

{

   174

    fsm->master_state(fsm);

   175

}

   176

   177

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

   178

   179

/**

   180

   Initializes the master startup state machine.

   181

*/

   182

   183

void ec_fsm_startup(ec_fsm_t *fsm)

   184

{

   185

    fsm->master_state = ec_fsm_startup_start;

   186

}

   187

   188

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

   189

   190

/**

   191

   Returns the running state of the master startup state machine.

   192

   \return non-zero if not terminated yet.

   193

*/

   194

   195

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

   196

{

   197

    return fsm->master_state != ec_fsm_end &&

   198

        fsm->master_state != ec_fsm_error;

   199

}

   200

   201

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

   202

   203

/**

   204

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

   205

   \return non-zero if successful.

   206

*/

   207

   208

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

   209

{

   210

    return fsm->master_state == ec_fsm_end;

   211

}

   212

   213

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

   214

   215

/**

   216

   Initializes the master configuration state machine.

   217

*/

   218

   219

void ec_fsm_configuration(ec_fsm_t *fsm)

   220

{

   221

    fsm->master_state = ec_fsm_configuration_start;

   222

}

   223

   224

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

   225

   226

/**

   227

   Returns the running state of the master configuration state machine.

   228

   \return non-zero if not terminated yet.

   229

*/

   230

   231

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

   232

{

   233

    return fsm->master_state != ec_fsm_end &&

   234

        fsm->master_state != ec_fsm_error;

   235

}

   236

   237

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

   238

   239

/**

   240

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

   241

   \return non-zero if successful.

   242

*/

   243

   244

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

   245

{

   246

    return fsm->master_state == ec_fsm_end;

   247

}

   248

   249

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

   250

 *  master startup state machine

   251

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

   252

   253

/**

   254

   Master state: START.

   255

   Starts with getting slave count and slave states.

   256

*/

   257

   258

void ec_fsm_startup_start(ec_fsm_t *fsm)

   259

{

   260

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

   261

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

   262

    fsm->master_state = ec_fsm_startup_broadcast;

   263

}

   264

   265

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

   266

   267

/**

   268

   Master state: BROADCAST.

   269

   Processes the broadcast read slave count and slaves states.

   270

*/

   271

   272

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

   273

{

   274

    ec_datagram_t *datagram = &fsm->datagram;

   275

    unsigned int i;

   276

    ec_slave_t *slave;

   277

    ec_master_t *master = fsm->master;

   278

   279

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   280

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

   281

        fsm->master_state = ec_fsm_error;

   282

        return;

   283

    }

   284

   285

    EC_INFO("Scanning bus.\n");

   286

   287

    ec_master_clear_slaves(master);

   288

   289

    master->slave_count = datagram->working_counter;

   290

   291

    if (!master->slave_count) {

   292

        // no slaves present -> finish state machine.

   293

        fsm->master_state = ec_fsm_end;

   294

        return;

   295

    }

   296

   297

    // init slaves

   298

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

   299

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

   300

                                             GFP_KERNEL))) {

   301

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

   302

            fsm->master_state = ec_fsm_error;

   303

            return;

   304

        }

   305

   306

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

   307

            fsm->master_state = ec_fsm_error;

   308

            return;

   309

        }

   310

   311

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

   312

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

   313

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

   314

            fsm->master_state = ec_fsm_error;

   315

            return;

   316

        }

   317

   318

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

   319

    }

   320

   321

    // begin scanning of slaves

   322

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

   323

    fsm->slave_state = ec_fsm_slavescan_start;

   324

    fsm->master_state = ec_fsm_startup_scan;

   325

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

   326

    return;

   327

}

   328

   329

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

   330

   331

/**

   332

   Master state: SCAN.

   333

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

   334

*/

   335

   336

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

   337

{

   338

    ec_master_t *master = fsm->master;

   339

    ec_slave_t *slave = fsm->slave;

   340

   341

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

   342

   343

    if (fsm->slave_state == ec_fsm_error) {

   344

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

   345

        fsm->master_state = ec_fsm_error;

   346

        return;

   347

    }

   348

   349

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

   350

   351

    // another slave to scan?

   352

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

   353

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

   354

        fsm->slave_state = ec_fsm_slavescan_start;

   355

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

   356

        return;

   357

    }

   358

   359

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

   360

   361

    ec_master_calc_addressing(master);

   362

   363

    fsm->master_state = ec_fsm_end;

   364

}

   365

   366

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

   367

 *  master configuration state machine

   368

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

   369

   370

/**

   371

   Master configuration state machine: START.

   372

*/

   373

   374

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

   375

{

   376

    ec_master_t *master = fsm->master;

   377

   378

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

   379

        fsm->master_state = ec_fsm_end;

   380

        return;

   381

    }

   382

   383

    // begin configuring slaves

   384

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

   385

    fsm->slave_state = ec_fsm_slaveconf_init;

   386

    fsm->change_new = EC_SLAVE_STATE_INIT;

   387

    fsm->change_state = ec_fsm_change_start;

   388

    fsm->master_state = ec_fsm_configuration_conf;

   389

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

   390

}

   391

   392

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

   393

   394

/**

   395

   Master state: CONF.

   396

*/

   397

   398

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

   399

{

   400

    ec_master_t *master = fsm->master;

   401

    ec_slave_t *slave = fsm->slave;

   402

   403

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

   404

   405

    if (fsm->slave_state == ec_fsm_error) {

   406

        fsm->master_state = ec_fsm_error;

   407

        return;

   408

    }

   409

   410

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

   411

   412

    // another slave to configure?

   413

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

   414

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

   415

        fsm->slave_state = ec_fsm_slaveconf_init;

   416

        fsm->change_new = EC_SLAVE_STATE_INIT;

   417

        fsm->change_state = ec_fsm_change_start;

   418

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

   419

        return;

   420

    }

   421

   422

    fsm->master_state = ec_fsm_end;

   423

}

   424

   425

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

   426

 *  operation / idle state machine

   427

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

   428

   429

/**

   430

   Master state: START.

   431

   Starts with getting slave count and slave states.

   432

*/

   433

   434

void ec_fsm_master_start(ec_fsm_t *fsm)

   435

{

   436

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

   437

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

   438

    fsm->master_state = ec_fsm_master_broadcast;

   439

}

   440

   441

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

   442

   443

/**

   444

   Master state: BROADCAST.

   445

   Processes the broadcast read slave count and slaves states.

   446

*/

   447

   448

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

   449

{

   450

    ec_datagram_t *datagram = &fsm->datagram;

   451

    unsigned int topology_change, states_change, i;

   452

    ec_slave_t *slave;

   453

    ec_master_t *master = fsm->master;

   454

   455

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   456

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

   457

            fsm->master_slaves_responding = 0;

   458

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

   459

                slave->online = 0;

   460

            }

   461

        }

   462

        fsm->master_state = ec_fsm_master_start;

   463

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

   464

        return;

   465

    }

   466

   467

    topology_change = (datagram->working_counter !=

   468

                       fsm->master_slaves_responding);

   469

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

   470

   471

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

   472

    fsm->master_slaves_responding = datagram->working_counter;

   473

   474

    if (topology_change) {

   475

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

   476

                fsm->master_slaves_responding,

   477

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

   478

   479

        if (master->mode == EC_MASTER_MODE_OPERATION) {

   480

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

   481

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

   482

            }

   483

            else {

   484

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

   485

            }

   486

        }

   487

    }

   488

   489

    if (states_change) {

   490

        char states[EC_STATE_STRING_SIZE];

   491

        ec_state_string(fsm->master_slave_states, states);

   492

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

   493

    }

   494

   495

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

   496

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