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

    fsm->spontaneous_change = 0;

    70

}

    71

    72

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

    73

    74

/**

    75

   Destructor.

    76

*/

    77

    78

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

    79

{

    80

}

    81

    82

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

    83

    84

/**

    85

   Starts the change state machine.

    86

*/

    87

    88

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

    89

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

    90

                         ec_slave_state_t state /**< requested state */

    91

                         )

    92

{

    93

    fsm->mode = EC_FSM_CHANGE_MODE_FULL;

    94

    fsm->slave = slave;

    95

    fsm->requested_state = state;

    96

    fsm->state = ec_fsm_change_state_start;

    97

}

    98

    99

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

   100

   101

/**

   102

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

   103

*/

   104

   105

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

   106

                       ec_slave_t *slave /**< EtherCAT slave */

   107

                       )

   108

{

   109

    fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;

   110

    fsm->slave = slave;

   111

    fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;

   112

    fsm->state = ec_fsm_change_state_start_ack;

   113

}

   114

   115

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

   116

   117

/**

   118

   Executes the current state of the state machine.

   119

   \return false, if the state machine has terminated

   120

*/

   121

   122

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

   123

{

   124

    fsm->state(fsm);

   125

   126

    return fsm->state != ec_fsm_change_state_end

   127

        && fsm->state != ec_fsm_change_state_error;

   128

}

   129

   130

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

   131

   132

/**

   133

   Returns, if the state machine terminated with success.

   134

   \return non-zero if successful.

   135

*/

   136

   137

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

   138

{

   139

    return fsm->state == ec_fsm_change_state_end;

   140

}

   141

   142

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

   143

 *  state change state machine

   144

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

   145

   146

/**

   147

   Change state: START.

   148

*/

   149

   150

void ec_fsm_change_state_start(ec_fsm_change_t *fsm

   151

                               /**< finite state machine */)

   152

{

   153

    ec_datagram_t *datagram = fsm->datagram;

   154

    ec_slave_t *slave = fsm->slave;

   155

   156

    fsm->take_time = 1;

   157

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

   158

   159

    // write new state to slave

   160

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

   161

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

   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

        return;

   180

   181

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   182

        fsm->state = ec_fsm_change_state_error;

   183

        EC_ERR("Failed to receive state datagram from slave %i"

   184

                " (datagram state %i)!\n",

   185

               fsm->slave->ring_position, datagram->state);

   186

        return;

   187

    }

   188

   189

    if (fsm->take_time) {

   190

        fsm->take_time = 0;

   191

        fsm->jiffies_start = datagram->jiffies_sent;

   192

    }

   193

   194

    if (datagram->working_counter == 0) {

   195

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

   196

            char state_str[EC_STATE_STRING_SIZE];

   197

            ec_state_string(fsm->requested_state, state_str);

   198

            fsm->state = ec_fsm_change_state_error;

   199

            EC_ERR("Failed to set state %s on slave %i: ",

   200

                    state_str, fsm->slave->ring_position);

   201

            ec_datagram_print_wc_error(datagram);

   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

        fsm->retries = EC_FSM_RETRIES;

   209

        return;

   210

    }

   211

   212

    if (unlikely(datagram->working_counter > 1)) {

   213

        char state_str[EC_STATE_STRING_SIZE];

   214

        ec_state_string(fsm->requested_state, state_str);

   215

        fsm->state = ec_fsm_change_state_error;

   216

        EC_ERR("Failed to set state %s on slave %i: ",

   217

                state_str, fsm->slave->ring_position);

   218

        ec_datagram_print_wc_error(datagram);

   219

        return;

   220

    }

   221

   222

    fsm->take_time = 1;

   223

   224

    // read AL status from slave

   225

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

   226

    fsm->retries = EC_FSM_RETRIES;

   227

    fsm->spontaneous_change = 0;

   228

    fsm->state = ec_fsm_change_state_status;

   229

}

   230

   231

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

   232

   233

/**

   234

   Change state: STATUS.

   235

*/

   236

   237

void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   238

                                /**< finite state machine */)

   239

{

   240

    ec_datagram_t *datagram = fsm->datagram;

   241

    ec_slave_t *slave = fsm->slave;

   242

   243

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

   244

        return;

   245

   246

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   247

        fsm->state = ec_fsm_change_state_error;

   248

        EC_ERR("Failed to receive state checking datagram from slave %i"

   249

                " (datagram state %i).\n",

   250

               slave->ring_position, datagram->state);

   251

        return;

   252

    }

   253

   254

    if (datagram->working_counter != 1) {

   255

        char req_state[EC_STATE_STRING_SIZE];

   256

        ec_state_string(fsm->requested_state, req_state);

   257

        fsm->state = ec_fsm_change_state_error;

   258

        EC_ERR("Failed to check state %s on slave %i: ",

   259

               req_state, slave->ring_position);

   260

        ec_datagram_print_wc_error(datagram);

   261

        return;

   262

    }

   263

   264

    if (fsm->take_time) {

   265

        fsm->take_time = 0;

   266

        fsm->jiffies_start = datagram->jiffies_sent;

   267

    }

   268

   269

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

   270

   271

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

   272

        // state has been set successfully

   273

        fsm->state = ec_fsm_change_state_end;

   274

        return;

   275

    }

   276

   277

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

   278

        char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   279

   280

        ec_state_string(slave->current_state, cur_state);

   281

   282

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

   283

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

   284

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

   285

            // state change

   286

            fsm->spontaneous_change = 1;

   287

            fsm->old_state = slave->current_state;

   288

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

   289

                    slave->ring_position, cur_state);

   290

            goto check_again;

   291

        }

   292

   293

        // state change error

   294

   295

        slave->error_flag = 1;

   296

        ec_state_string(fsm->requested_state, req_state);

   297

   298

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

   299

               req_state, slave->ring_position, cur_state);

   300

        // fetch AL status error code

   301

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

   302

        fsm->retries = EC_FSM_RETRIES;

   303

        fsm->state = ec_fsm_change_state_code;

   304

        return;

   305

    }

   306

   307

    // still old state

   308

   309

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

   310

        // timeout while checking

   311

        char state_str[EC_STATE_STRING_SIZE];

   312

        ec_state_string(fsm->requested_state, state_str);

   313

        fsm->state = ec_fsm_change_state_error;

   314

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

   315

                state_str, slave->ring_position);

   316

        return;

   317

    }

   318

   319

 check_again:

   320

    // no timeout yet. check again

   321

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

   322

    fsm->retries = EC_FSM_RETRIES;

   323

}

   324

   325

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

   326

   327

/**

   328

   Application layer status messages.

   329

*/

   330

   331

const ec_code_msg_t al_status_messages[] = {

   332

    {0x0001, "Unspecified error"},

   333

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

   334

    {0x0012, "Unknown requested state"},

   335

    {0x0013, "Bootstrap not supported"},

   336

    {0x0014, "No valid firmware"},

   337

    {0x0015, "Invalid mailbox configuration"},

   338

    {0x0016, "Invalid mailbox configuration"},

   339

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

   340

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

   341

    {0x0019, "No valid outputs"},

   342

    {0x001A, "Synchronisation error"},

   343

    {0x001B, "Sync manager watchdog"},

   344

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

   345

    {0x001D, "Invalid output configuration"},

   346

    {0x001E, "Invalid input configuration"},

   347

    {0x001F, "Invalid watchdog configuration"},

   348

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

   349

    {0x0021, "Slave needs INIT"},

   350

    {0x0022, "Slave needs PREOP"},

   351

    {0x0023, "Slave needs SAVEOP"},

   352

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

   353

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

   354

    {0x0032, "PLL error"},

   355

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

   356

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

   357

    {0x0042, "MBOX EOE"},

   358

    {0x0043, "MBOX COE"},

   359

    {0x0044, "MBOX FOE"},

   360

    {0x0045, "MBOX SOE"},

   361

    {0x004F, "MBOX VOE"},

   362

    {}

   363

};

   364

   365

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

   366

   367

/**

   368

   Change state: CODE.

   369

*/

   370

   371

void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   372

                              /**< finite state machine */)

   373

{

   374

    ec_datagram_t *datagram = fsm->datagram;

   375

    uint32_t code;

   376

    const ec_code_msg_t *al_msg;

   377

   378

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

   379

        return;

   380

   381

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   382

        fsm->state = ec_fsm_change_state_error;

   383

        EC_ERR("Failed to receive AL status code datagram from slave %i"

   384

                " (datagram state %i).\n",

   385

               fsm->slave->ring_position, datagram->state);

   386

        return;

   387

    }

   388

   389

    if (datagram->working_counter != 1) {

   390

        EC_WARN("Reception of AL status code datagram failed: ");

   391

        ec_datagram_print_wc_error(datagram);

   392

    }

   393

    else {

   394

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

   395

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

   396

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

   397

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

   398

                       al_msg->code, al_msg->message);

   399

                break;

   400

            }

   401

            if (!al_msg->code)

   402

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

   403

        }

   404

    }

   405

   406

    // acknowledge "old" slave state

   407

    ec_fsm_change_state_start_ack(fsm); // execute immediately

   408

}

   409

   410

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

   411

   412

/**

   413

   Change state: START ACK.

   414

*/

   415

   416

void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   417

                                   /**< finite state machine */)

   418

{

   419

    ec_slave_t *slave = fsm->slave;

   420

    ec_datagram_t *datagram = fsm->datagram;

   421

   422

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

   423

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

   424

    fsm->retries = EC_FSM_RETRIES;

   425

    fsm->state = ec_fsm_change_state_ack;

   426

}

   427

   428

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

   429

   430

/**

   431

   Change state: ACK.

   432

*/

   433

   434

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

   435

{

   436

    ec_datagram_t *datagram = fsm->datagram;

   437

    ec_slave_t *slave = fsm->slave;

   438

   439

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

   440

        return;

   441

   442

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   443

        fsm->state = ec_fsm_change_state_error;

   444

        EC_ERR("Failed to receive state ack datagram for slave %i"

   445

                " (datagram state %i).\n",

   446

               slave->ring_position, datagram->state);

   447

        return;

   448

    }

   449

   450

    if (datagram->working_counter != 1) {

   451

        fsm->state = ec_fsm_change_state_error;

   452

        EC_ERR("Reception of state ack datagram failed on slave %i: ",

   453

                slave->ring_position);

   454

        ec_datagram_print_wc_error(datagram);

   455

        return;

   456

    }

   457

   458

    fsm->take_time = 1;

   459

   460

    // read new AL status

   461

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

   462

    fsm->retries = EC_FSM_RETRIES;

   463

    fsm->state = ec_fsm_change_state_check_ack;

   464

}

   465

   466

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

   467

   468

/**

   469

   Change state: CHECK ACK.

   470

*/

   471

   472

void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   473

                                   /**< finite state machine */)

   474

{

   475

    ec_datagram_t *datagram = fsm->datagram;

   476

    ec_slave_t *slave = fsm->slave;

   477

   478

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

   479

        return;

   480

   481

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   482

        fsm->state = ec_fsm_change_state_error;

   483

        EC_ERR("Failed to receive state ack check datagram from slave %i"

   484

                " (datagram state %i).\n",

   485

               slave->ring_position, datagram->state);

   486

        return;

   487

    }

   488

   489

    if (datagram->working_counter != 1) {

   490

        fsm->state = ec_fsm_change_state_error;

   491

        EC_ERR("Reception of state ack check datagram failed on slave %i: ",

   492

                slave->ring_position);

   493

        ec_datagram_print_wc_error(datagram);

   494

        return;

   495

    }

   496

   497

    if (fsm->take_time) {

   498

        fsm->take_time = 0;

   499

        fsm->jiffies_start = datagram->jiffies_sent;

   500

    }

   501

   502

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

   503

   504

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

   505

        char state_str[EC_STATE_STRING_SIZE];

   506

        ec_state_string(slave->current_state, state_str);

   507

        if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) {

   508

            fsm->state = ec_fsm_change_state_error;

   509

        }

   510

        else { // EC_FSM_CHANGE_MODE_ACK_ONLY

   511

            fsm->state = ec_fsm_change_state_end;

   512

        }

   513

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

   514

                state_str, slave->ring_position);

   515

        return;

   516

    }

   517

   518

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

   519

        // timeout while checking