1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   EtherCAT state change FSM.

    33

*/

    34

    35

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

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "fsm_change.h"

    40

    41

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

    42

    43

/** Timeout while waiting for AL state change [s].

    44

 */

    45

#define EC_AL_STATE_CHANGE_TIMEOUT 2

    46

    47

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

    48

    49

void ec_fsm_change_state_start(ec_fsm_change_t *);

    50

void ec_fsm_change_state_check(ec_fsm_change_t *);

    51

void ec_fsm_change_state_status(ec_fsm_change_t *);

    52

void ec_fsm_change_state_code(ec_fsm_change_t *);

    53

void ec_fsm_change_state_start_ack(ec_fsm_change_t *);

    54

void ec_fsm_change_state_ack(ec_fsm_change_t *);

    55

void ec_fsm_change_state_check_ack(ec_fsm_change_t *);

    56

void ec_fsm_change_state_end(ec_fsm_change_t *);

    57

void ec_fsm_change_state_error(ec_fsm_change_t *);

    58

    59

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

    60

    61

/**

    62

   Constructor.

    63

*/

    64

    65

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

    66

                        ec_datagram_t *datagram /**< datagram */

    67

                        )

    68

{

    69

    fsm->state = NULL;

    70

    fsm->datagram = datagram;

    71

    fsm->spontaneous_change = 0;

    72

}

    73

    74

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

    75

    76

/**

    77

   Destructor.

    78

*/

    79

    80

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

    81

{

    82

}

    83

    84

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

    85

    86

/**

    87

   Starts the change state machine.

    88

*/

    89

    90

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

    91

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

    92

                         ec_slave_state_t state /**< requested state */

    93

                         )

    94

{

    95

    fsm->mode = EC_FSM_CHANGE_MODE_FULL;

    96

    fsm->slave = slave;

    97

    fsm->requested_state = state;

    98

    fsm->state = ec_fsm_change_state_start;

    99

}

   100

   101

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

   102

   103

/**

   104

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

   105

*/

   106

   107

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

   108

                       ec_slave_t *slave /**< EtherCAT slave */

   109

                       )

   110

{

   111

    fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;

   112

    fsm->slave = slave;

   113

    fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;

   114

    fsm->state = ec_fsm_change_state_start_ack;

   115

}

   116

   117

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

   118

   119

/**

   120

   Executes the current state of the state machine.

   121

   \return false, if the state machine has terminated

   122

*/

   123

   124

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

   125

{

   126

    fsm->state(fsm);

   127

   128

    return fsm->state != ec_fsm_change_state_end

   129

        && fsm->state != ec_fsm_change_state_error;

   130

}

   131

   132

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

   133

   134

/**

   135

   Returns, if the state machine terminated with success.

   136

   \return non-zero if successful.

   137

*/

   138

   139

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

   140

{

   141

    return fsm->state == ec_fsm_change_state_end;

   142

}

   143

   144

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

   145

 *  state change state machine

   146

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

   147

   148

/**

   149

   Change state: START.

   150

*/

   151

   152

void ec_fsm_change_state_start(ec_fsm_change_t *fsm

   153

                               /**< finite state machine */)

   154

{

   155

    ec_datagram_t *datagram = fsm->datagram;

   156

    ec_slave_t *slave = fsm->slave;

   157

   158

    fsm->take_time = 1;

   159

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

   160

   161

    // write new state to slave

   162

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

   163

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

   164

    fsm->retries = EC_FSM_RETRIES;

   165

    fsm->state = ec_fsm_change_state_check;

   166

}

   167

   168

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

   169

   170

/**

   171

   Change state: CHECK.

   172

*/

   173

   174

void ec_fsm_change_state_check(ec_fsm_change_t *fsm

   175

                               /**< finite state machine */)

   176

{

   177

    ec_datagram_t *datagram = fsm->datagram;

   178

    ec_slave_t *slave = fsm->slave;

   179

   180

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

   181

        return;

   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 %u"

   186

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

   187

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

   188

        return;

   189

    }

   190

   191

    if (fsm->take_time) {

   192

        fsm->take_time = 0;

   193

        fsm->jiffies_start = datagram->jiffies_sent;

   194

    }

   195

   196

    if (datagram->working_counter == 0) {

   197

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

   198

            char state_str[EC_STATE_STRING_SIZE];

   199

            ec_state_string(fsm->requested_state, state_str, 0);

   200

            fsm->state = ec_fsm_change_state_error;

   201

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

   202

                    state_str, fsm->slave->ring_position);

   203

            ec_datagram_print_wc_error(datagram);

   204

            return;

   205

        }

   206

   207

        // repeat writing new state to slave

   208

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

   209

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

   210

        fsm->retries = EC_FSM_RETRIES;

   211

        return;

   212

    }

   213

   214

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

   215

        char state_str[EC_STATE_STRING_SIZE];

   216

        ec_state_string(fsm->requested_state, state_str, 0);

   217

        fsm->state = ec_fsm_change_state_error;

   218

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

   219

                state_str, fsm->slave->ring_position);

   220

        ec_datagram_print_wc_error(datagram);

   221

        return;

   222

    }

   223

   224

    fsm->take_time = 1;

   225

   226

    // read AL status from slave

   227

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

   228

    ec_datagram_zero(datagram);

   229

    fsm->retries = EC_FSM_RETRIES;

   230

    fsm->spontaneous_change = 0;

   231

    fsm->state = ec_fsm_change_state_status;

   232

}

   233

   234

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

   235

   236

/**

   237

   Change state: STATUS.

   238

*/

   239

   240

void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   241

                                /**< finite state machine */)

   242

{

   243

    ec_datagram_t *datagram = fsm->datagram;

   244

    ec_slave_t *slave = fsm->slave;

   245

   246

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

   247

        return;

   248

   249

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   250

        fsm->state = ec_fsm_change_state_error;

   251

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

   252

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

   253

               slave->ring_position, datagram->state);

   254

        return;

   255

    }

   256

   257

    if (datagram->working_counter != 1) {

   258

        char req_state[EC_STATE_STRING_SIZE];

   259

        ec_state_string(fsm->requested_state, req_state, 0);

   260

        fsm->state = ec_fsm_change_state_error;

   261

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

   262

               req_state, slave->ring_position);

   263

        ec_datagram_print_wc_error(datagram);

   264

        return;

   265

    }

   266

   267

    if (fsm->take_time) {

   268

        fsm->take_time = 0;

   269

        fsm->jiffies_start = datagram->jiffies_sent;

   270

    }

   271

   272

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

   273

   274

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

   275

        // state has been set successfully

   276

        fsm->state = ec_fsm_change_state_end;

   277

        return;

   278

    }

   279

   280

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

   281

        char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   282

   283

        ec_state_string(slave->current_state, cur_state, 0);

   284

   285

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

   286

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

   287

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

   288

            // state change

   289

            fsm->spontaneous_change = 1;

   290

            fsm->old_state = slave->current_state;

   291

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

   292

                    slave->ring_position, cur_state);

   293

            goto check_again;

   294

        }

   295

   296

        // state change error

   297

   298

        slave->error_flag = 1;

   299

        ec_state_string(fsm->requested_state, req_state, 0);

   300

   301

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

   302

               req_state, slave->ring_position, cur_state);

   303

        // fetch AL status error code

   304

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

   305

        ec_datagram_zero(datagram);

   306

        fsm->retries = EC_FSM_RETRIES;

   307

        fsm->state = ec_fsm_change_state_code;

   308

        return;

   309

    }

   310

   311

    // still old state

   312

   313

    if (datagram->jiffies_received - fsm->jiffies_start >=

   314

            EC_AL_STATE_CHANGE_TIMEOUT * HZ) {

   315

        // timeout while checking

   316

        char state_str[EC_STATE_STRING_SIZE];

   317

        ec_state_string(fsm->requested_state, state_str, 0);

   318

        fsm->state = ec_fsm_change_state_error;

   319

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

   320

                state_str, slave->ring_position);

   321

        return;

   322

    }

   323

   324

 check_again:

   325

    // no timeout yet. check again

   326

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

   327

    ec_datagram_zero(datagram);

   328

    fsm->retries = EC_FSM_RETRIES;

   329

}

   330

   331

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

   332

   333

/**

   334

   Application layer status messages.

   335

*/

   336

   337

const ec_code_msg_t al_status_messages[] = {

   338

    {0x0001, "Unspecified error"},

   339

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

   340

    {0x0012, "Unknown requested state"},

   341

    {0x0013, "Bootstrap not supported"},

   342

    {0x0014, "No valid firmware"},

   343

    {0x0015, "Invalid mailbox configuration"},

   344

    {0x0016, "Invalid mailbox configuration"},

   345

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

   346

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

   347

    {0x0019, "No valid outputs"},

   348

    {0x001A, "Synchronisation error"},

   349

    {0x001B, "Sync manager watchdog"},

   350

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

   351

    {0x001D, "Invalid output configuration"},

   352

    {0x001E, "Invalid input configuration"},

   353

    {0x001F, "Invalid watchdog configuration"},

   354

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

   355

    {0x0021, "Slave needs INIT"},

   356

    {0x0022, "Slave needs PREOP"},

   357

    {0x0023, "Slave needs SAFEOP"},

   358

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

   359

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

   360

    {0x0032, "PLL error"},

   361

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

   362

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

   363

    {0x0042, "MBOX EOE"},

   364

    {0x0043, "MBOX COE"},

   365

    {0x0044, "MBOX FOE"},

   366

    {0x0045, "MBOX SOE"},

   367

    {0x004F, "MBOX VOE"},

   368

    {}

   369

};

   370

   371

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

   372

   373

/**

   374

   Change state: CODE.

   375

*/

   376

   377

void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   378

                              /**< finite state machine */)

   379

{

   380

    ec_datagram_t *datagram = fsm->datagram;

   381

    uint32_t code;

   382

    const ec_code_msg_t *al_msg;

   383

   384

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

   385

        return;

   386

   387

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   388

        fsm->state = ec_fsm_change_state_error;

   389

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

   390

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

   391

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

   392

        return;

   393

    }

   394

   395

    if (datagram->working_counter != 1) {

   396

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

   397

        ec_datagram_print_wc_error(datagram);

   398

    } else {

   399

        code = EC_READ_U16(datagram->data);

   400

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

   401

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

   402

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

   403

                    al_msg->code, al_msg->message);

   404

            break;

   405

        }

   406

        if (!al_msg->code)

   407

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

   408

    }

   409

   410

    // acknowledge "old" slave state

   411

    ec_fsm_change_state_start_ack(fsm); // execute immediately

   412

}

   413

   414

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

   415

   416

/**

   417

   Change state: START ACK.

   418

*/

   419

   420

void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   421

                                   /**< finite state machine */)

   422

{

   423

    ec_slave_t *slave = fsm->slave;

   424

    ec_datagram_t *datagram = fsm->datagram;

   425

   426

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

   427

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

   428

    fsm->retries = EC_FSM_RETRIES;

   429

    fsm->state = ec_fsm_change_state_ack;

   430

}

   431

   432

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

   433

   434

/**

   435

   Change state: ACK.

   436

*/

   437

   438

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

   439

{

   440

    ec_datagram_t *datagram = fsm->datagram;

   441

    ec_slave_t *slave = fsm->slave;

   442

   443

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

   444

        return;

   445

   446

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   447

        fsm->state = ec_fsm_change_state_error;

   448

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

   449

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

   450

               slave->ring_position, datagram->state);

   451

        return;

   452

    }

   453

   454

    if (datagram->working_counter != 1) {

   455

        fsm->state = ec_fsm_change_state_error;

   456

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

   457

                slave->ring_position);

   458

        ec_datagram_print_wc_error(datagram);

   459

        return;

   460

    }

   461

   462

    fsm->take_time = 1;

   463

   464

    // read new AL status

   465

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

   466

    ec_datagram_zero(datagram);

   467

    fsm->retries = EC_FSM_RETRIES;

   468

    fsm->state = ec_fsm_change_state_check_ack;

   469

}

   470

   471

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

   472

   473

/**

   474

   Change state: CHECK ACK.

   475

*/

   476

   477

void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   478

                                   /**< finite state machine */)

   479

{

   480

    ec_datagram_t *datagram = fsm->datagram;

   481

    ec_slave_t *slave = fsm->slave;

   482

   483

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

   484

        return;

   485

   486

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   487

        fsm->state = ec_fsm_change_state_error;

   488

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

   489

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

   490

               slave->ring_position, datagram->state);

   491

        return;

   492

    }

   493

   494

    if (datagram->working_counter != 1) {

   495

        fsm->state = ec_fsm_change_state_error;

   496

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

   497

                slave->ring_position);

   498

        ec_datagram_print_wc_error(datagram);

   499

        return;

   500

    }

   501