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 state change FSM.

    37

*/

    38

    39

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

    40

    41

#include "globals.h"

    42

#include "master.h"

    43

#include "fsm_change.h"

    44

    45

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

    46

    47

void ec_fsm_change_state_start(ec_fsm_change_t *);

    48

void ec_fsm_change_state_check(ec_fsm_change_t *);

    49

void ec_fsm_change_state_status(ec_fsm_change_t *);

    50

void ec_fsm_change_state_code(ec_fsm_change_t *);

    51

void ec_fsm_change_state_start_ack(ec_fsm_change_t *);

    52

void ec_fsm_change_state_ack(ec_fsm_change_t *);

    53

void ec_fsm_change_state_check_ack(ec_fsm_change_t *);

    54

void ec_fsm_change_state_end(ec_fsm_change_t *);

    55

void ec_fsm_change_state_error(ec_fsm_change_t *);

    56

    57

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

    58

    59

/**

    60

   Constructor.

    61

*/

    62

    63

void ec_fsm_change_init(ec_fsm_change_t *fsm, /**< finite state machine */

    64

                        ec_datagram_t *datagram /**< datagram */

    65

                        )

    66

{

    67

    fsm->state = NULL;

    68

    fsm->datagram = datagram;

    69

}

    70

    71

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

    72

    73

/**

    74

   Destructor.

    75

*/

    76

    77

void ec_fsm_change_clear(ec_fsm_change_t *fsm /**< finite state machine */)

    78

{

    79

}

    80

    81

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

    82

    83

/**

    84

   Starts the change state machine.

    85

*/

    86

    87

void ec_fsm_change_start(ec_fsm_change_t *fsm, /**< finite state machine */

    88

                         ec_slave_t *slave, /**< EtherCAT slave */

    89

                         ec_slave_state_t state /**< requested state */

    90

                         )

    91

{

    92

    fsm->mode = EC_FSM_CHANGE_MODE_FULL;

    93

    fsm->slave = slave;

    94

    fsm->requested_state = state;

    95

    fsm->state = ec_fsm_change_state_start;

    96

}

    97

    98

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

    99

   100

/**

   101

   Starts the change state machine to only acknowlegde a slave's state.

   102

*/

   103

   104

void ec_fsm_change_ack(ec_fsm_change_t *fsm, /**< finite state machine */

   105

                       ec_slave_t *slave /**< EtherCAT slave */

   106

                       )

   107

{

   108

    fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;

   109

    fsm->slave = slave;

   110

    fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;

   111

    fsm->state = ec_fsm_change_state_start_ack;

   112

}

   113

   114

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

   115

   116

/**

   117

   Executes the current state of the state machine.

   118

   \return false, if the state machine has terminated

   119

*/

   120

   121

int ec_fsm_change_exec(ec_fsm_change_t *fsm /**< finite state machine */)

   122

{

   123

    fsm->state(fsm);

   124

   125

    return fsm->state != ec_fsm_change_state_end

   126

        && fsm->state != ec_fsm_change_state_error;

   127

}

   128

   129

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

   130

   131

/**

   132

   Returns, if the state machine terminated with success.

   133

   \return non-zero if successful.

   134

*/

   135

   136

int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */)

   137

{

   138

    return fsm->state == ec_fsm_change_state_end;

   139

}

   140

   141

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

   142

 *  state change state machine

   143

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

   144

   145

/**

   146

   Change state: START.

   147

*/

   148

   149

void ec_fsm_change_state_start(ec_fsm_change_t *fsm

   150

                               /**< finite state machine */)

   151

{

   152

    ec_datagram_t *datagram = fsm->datagram;

   153

    ec_slave_t *slave = fsm->slave;

   154

   155

    fsm->take_time = 1;

   156

    fsm->old_state = fsm->slave->current_state;

   157

   158

    // write new state to slave

   159

    ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);

   160

    EC_WRITE_U16(datagram->data, fsm->requested_state);

   161

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

   162

    fsm->retries = EC_FSM_RETRIES;

   163

    fsm->state = ec_fsm_change_state_check;

   164

}

   165

   166

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

   167

   168

/**

   169

   Change state: CHECK.

   170

*/

   171

   172

void ec_fsm_change_state_check(ec_fsm_change_t *fsm

   173

                               /**< finite state machine */)

   174

{

   175

    ec_datagram_t *datagram = fsm->datagram;

   176

    ec_slave_t *slave = fsm->slave;

   177

   178

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

   179

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

   180

        return;

   181

    }

   182

   183

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   184

        fsm->state = ec_fsm_change_state_error;

   185

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

   186

               fsm->slave->ring_position);

   187

        return;

   188

    }

   189

   190

    if (fsm->take_time) {

   191

        fsm->take_time = 0;

   192

        fsm->jiffies_start = datagram->jiffies_sent;

   193

    }

   194

   195

    if (datagram->working_counter != 1) {

   196

        if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) {

   197

            char state_str[EC_STATE_STRING_SIZE];

   198

            ec_state_string(fsm->requested_state, state_str);

   199

            fsm->state = ec_fsm_change_state_error;

   200

            EC_ERR("Failed to set state %s on slave %i: Slave did not"

   201

                   " respond.\n", state_str, fsm->slave->ring_position);

   202

            return;

   203

        }

   204

   205

        // repeat writing new state to slave

   206

        ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);

   207

        EC_WRITE_U16(datagram->data, fsm->requested_state);

   208

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

   209

        fsm->retries = EC_FSM_RETRIES;

   210

        return;

   211

    }

   212

   213

    fsm->take_time = 1;

   214

   215

    // read AL status from slave

   216

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

   217

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

   218

    fsm->retries = EC_FSM_RETRIES;

   219

    fsm->state = ec_fsm_change_state_status;

   220

}

   221

   222

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

   223

   224

/**

   225

   Change state: STATUS.

   226

*/

   227

   228

void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   229

                                /**< finite state machine */)

   230

{

   231

    ec_datagram_t *datagram = fsm->datagram;

   232

    ec_slave_t *slave = fsm->slave;

   233

   234

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

   235

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

   236

        return;

   237

    }

   238

   239

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   240

        fsm->state = ec_fsm_change_state_error;

   241

        EC_ERR("Failed to receive state checking datagram from slave %i.\n",

   242

               slave->ring_position);

   243

        return;

   244

    }

   245

   246

    if (datagram->working_counter != 1) {

   247

        char req_state[EC_STATE_STRING_SIZE];

   248

        ec_state_string(fsm->requested_state, req_state);

   249

        fsm->state = ec_fsm_change_state_error;

   250

        EC_ERR("Failed to check state %s on slave %i.\n",

   251

               req_state, slave->ring_position);

   252

        return;

   253

    }

   254

   255

    if (fsm->take_time) {

   256

        fsm->take_time = 0;

   257

        fsm->jiffies_start = datagram->jiffies_sent;

   258

    }

   259

   260

    slave->current_state = EC_READ_U8(datagram->data);

   261

   262

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

   263

        // state has been set successfully

   264

        fsm->state = ec_fsm_change_state_end;

   265

        return;

   266

    }

   267

   268

    if (slave->current_state != fsm->old_state) { // state changed

   269

        char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   270

   271

        ec_state_string(slave->current_state, cur_state);

   272

   273

        if ((slave->current_state & 0x0F) != (fsm->old_state & 0x0F)) {

   274

            // Slave spontaneously changed its state just before the new state

   275

            // was written. Accept current state as old state and wait for

   276

            // state change

   277

            fsm->old_state = slave->current_state;

   278

            EC_WARN("Slave %i changed to %s in the meantime.\n",

   279

                    slave->ring_position, cur_state);

   280

            goto again;

   281

        }

   282

   283

        // state change error

   284

   285

        slave->error_flag = 1;

   286

        ec_state_string(fsm->requested_state, req_state);

   287

   288

        EC_ERR("Failed to set %s state, slave %i refused state change (%s).\n",

   289

               req_state, slave->ring_position, cur_state);

   290

        // fetch AL status error code

   291

        ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2);

   292

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

   293

        fsm->retries = EC_FSM_RETRIES;

   294

        fsm->state = ec_fsm_change_state_code;

   295

        return;

   296

    }

   297

   298

    // still old state

   299

   300

    if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s

   301

        // timeout while checking

   302

        char state_str[EC_STATE_STRING_SIZE];

   303

        ec_state_string(fsm->requested_state, state_str);

   304

        fsm->state = ec_fsm_change_state_error;

   305

        EC_ERR("Timeout while setting state %s on slave %i.\n",

   306

               state_str, slave->ring_position);

   307

        return;

   308

    }

   309

   310

 again:

   311

    // no timeout yet. check again

   312

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

   313

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

   314

    fsm->retries = EC_FSM_RETRIES;

   315

}

   316

   317

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

   318

   319

/**

   320

   Application layer status messages.

   321

*/

   322

   323

const ec_code_msg_t al_status_messages[] = {

   324

    {0x0001, "Unspecified error"},

   325

    {0x0011, "Invalud requested state change"},

   326

    {0x0012, "Unknown requested state"},

   327

    {0x0013, "Bootstrap not supported"},

   328

    {0x0014, "No valid firmware"},

   329

    {0x0015, "Invalid mailbox configuration"},

   330

    {0x0016, "Invalid mailbox configuration"},

   331

    {0x0017, "Invalid sync manager configuration"},

   332

    {0x0018, "No valid inputs available"},

   333

    {0x0019, "No valid outputs"},

   334

    {0x001A, "Synchronisation error"},

   335

    {0x001B, "Sync manager watchdog"},

   336

    {0x001C, "Invalid sync manager types"},

   337

    {0x001D, "Invalid output configuration"},

   338

    {0x001E, "Invalid input configuration"},

   339

    {0x001F, "Invalid watchdog configuration"},

   340

    {0x0020, "Slave needs cold start"},

   341

    {0x0021, "Slave needs INIT"},

   342

    {0x0022, "Slave needs PREOP"},

   343

    {0x0023, "Slave needs SAVEOP"},

   344

    {0x0030, "Invalid DC SYNCH configuration"},

   345

    {0x0031, "Invalid DC latch configuration"},

   346

    {0x0032, "PLL error"},

   347

    {0x0033, "Invalid DC IO error"},

   348

    {0x0034, "Invalid DC timeout error"},

   349

    {0x0042, "MBOX EOE"},

   350

    {0x0043, "MBOX COE"},

   351

    {0x0044, "MBOX FOE"},

   352

    {0x0045, "MBOX SOE"},

   353

    {0x004F, "MBOX VOE"},

   354

    {}

   355

};

   356

   357

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

   358

   359

/**

   360

   Change state: CODE.

   361

*/

   362

   363

void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   364

                              /**< finite state machine */)

   365

{

   366

    ec_datagram_t *datagram = fsm->datagram;

   367

    uint32_t code;

   368

    const ec_code_msg_t *al_msg;

   369

   370

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

   371

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

   372

        return;

   373

    }

   374

   375

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   376

        fsm->state = ec_fsm_change_state_error;

   377

        EC_ERR("Failed to receive AL status code datagram from slave %i.\n",

   378

               fsm->slave->ring_position);

   379

        return;

   380

    }

   381

   382

    if (datagram->working_counter != 1) {

   383

        EC_WARN("Reception of AL status code datagram failed.\n");

   384

    }

   385

    else {

   386

        if ((code = EC_READ_U16(datagram->data))) {

   387

            for (al_msg = al_status_messages; al_msg->code; al_msg++) {

   388

                if (al_msg->code != code) continue;

   389

                EC_ERR("AL status message 0x%04X: \"%s\".\n",

   390

                       al_msg->code, al_msg->message);

   391

                break;

   392

            }

   393

            if (!al_msg->code)

   394

                EC_ERR("Unknown AL status code 0x%04X.\n", code);

   395

        }

   396

    }

   397

   398

    // acknowledge "old" slave state

   399

    ec_fsm_change_state_start_ack(fsm); // execute immediately

   400

}

   401

   402

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

   403

   404

/**

   405

   Change state: START ACK.

   406

*/

   407

   408

void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   409

                                   /**< finite state machine */)

   410

{

   411

    ec_slave_t *slave = fsm->slave;

   412

    ec_datagram_t *datagram = fsm->datagram;

   413

   414

    ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2);

   415

    EC_WRITE_U16(datagram->data, slave->current_state);

   416

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

   417

    fsm->retries = EC_FSM_RETRIES;

   418

    fsm->state = ec_fsm_change_state_ack;

   419

}

   420

   421

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

   422

   423

/**

   424

   Change state: ACK.

   425

*/

   426

   427

void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */)

   428

{

   429

    ec_datagram_t *datagram = fsm->datagram;

   430

    ec_slave_t *slave = fsm->slave;

   431

   432

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

   433

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

   434

        return;

   435

    }

   436

   437

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   438

        fsm->state = ec_fsm_change_state_error;

   439

        EC_ERR("Failed to receive state ack datagram for slave %i.\n",

   440

               slave->ring_position);

   441

        return;

   442

    }

   443

   444

    if (datagram->working_counter != 1) {

   445

        fsm->state = ec_fsm_change_state_error;

   446

        EC_ERR("Reception of state ack datagram failed - slave %i did not"

   447

               " respond.\n", slave->ring_position);

   448

        return;

   449

    }

   450

   451

    fsm->take_time = 1;

   452

   453

    // read new AL status

   454

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

   455

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

   456

    fsm->retries = EC_FSM_RETRIES;

   457

    fsm->state = ec_fsm_change_state_check_ack;

   458

}

   459

   460

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

   461

   462

/**

   463

   Change state: CHECK ACK.

   464

*/

   465

   466

void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   467

                                   /**< finite state machine */)

   468

{

   469

    ec_datagram_t *datagram = fsm->datagram;

   470

    ec_slave_t *slave = fsm->slave;

   471

   472

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

   473

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

   474

        return;

   475

    }

   476

   477

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   478

        fsm->state = ec_fsm_change_state_error;

   479

        EC_ERR("Failed to receive state ack check datagram from slave %i.\n",

   480

               slave->ring_position);

   481

        return;

   482

    }

   483

   484

    if (datagram->working_counter != 1) {

   485

        fsm->state = ec_fsm_change_state_error;

   486

        EC_ERR("Reception of state ack check datagram failed - slave %i did"

   487

               " not respond.\n", slave->ring_position);

   488

        return;

   489

    }

   490

   491

    if (fsm->take_time) {

   492

        fsm->take_time = 0;

   493

        fsm->jiffies_start = datagram->jiffies_sent;

   494

    }

   495

   496

    slave->current_state = EC_READ_U8(datagram->data);

   497

   498

    if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) {

   499

        char state_str[EC_STATE_STRING_SIZE];

   500

        ec_state_string(slave->current_state, state_str);

   501

        if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) {

   502

            fsm->state = ec_fsm_change_state_error;

   503

        }

   504

        else { // EC_FSM_CHANGE_MODE_ACK_ONLY

   505

            fsm->state = ec_fsm_change_state_end;

   506

        }

   507

        EC_INFO("Acknowledged state %s on slave %i.\n",

   508

                state_str, slave->ring_position);

   509

        return;

   510

    }

   511

   512

    if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s

   513

        // timeout while checking

   514

        char state_str[EC_STATE_STRING_SIZE];

   515

        ec_state_string(slave->current_state, state_str);

   516

        fsm->state = ec_fsm_change_state_error;

   517

        EC_ERR("Timeout while acknowledging state %s on slave %i.\n",

   518

               state_str, slave->ring_position);

   519

        return;

   520

    }

   521

   522

    // reread new AL status

   523

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