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

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

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   EtherCAT state change FSM.

    30

*/

    31

    32

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

    33

    34

#include "globals.h"

    35

#include "master.h"

    36

#include "fsm_change.h"

    37

    38

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

    39

    40

void ec_fsm_change_state_start(ec_fsm_change_t *);

    41

void ec_fsm_change_state_check(ec_fsm_change_t *);

    42

void ec_fsm_change_state_status(ec_fsm_change_t *);

    43

void ec_fsm_change_state_code(ec_fsm_change_t *);

    44

void ec_fsm_change_state_start_ack(ec_fsm_change_t *);

    45

void ec_fsm_change_state_ack(ec_fsm_change_t *);

    46

void ec_fsm_change_state_check_ack(ec_fsm_change_t *);

    47

void ec_fsm_change_state_end(ec_fsm_change_t *);

    48

void ec_fsm_change_state_error(ec_fsm_change_t *);

    49

    50

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

    51

    52

/**

    53

   Constructor.

    54

*/

    55

    56

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

    57

                        ec_datagram_t *datagram /**< datagram */

    58

                        )

    59

{

    60

    fsm->state = NULL;

    61

    fsm->datagram = datagram;

    62

    fsm->spontaneous_change = 0;

    63

}

    64

    65

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

    66

    67

/**

    68

   Destructor.

    69

*/

    70

    71

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

    72

{

    73

}

    74

    75

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

    76

    77

/**

    78

   Starts the change state machine.

    79

*/

    80

    81

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

    82

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

    83

                         ec_slave_state_t state /**< requested state */

    84

                         )

    85

{

    86

    fsm->mode = EC_FSM_CHANGE_MODE_FULL;

    87

    fsm->slave = slave;

    88

    fsm->requested_state = state;

    89

    fsm->state = ec_fsm_change_state_start;

    90

}

    91

    92

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

    93

    94

/**

    95

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

    96

*/

    97

    98

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

    99

                       ec_slave_t *slave /**< EtherCAT slave */

   100

                       )

   101

{

   102

    fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;

   103

    fsm->slave = slave;

   104

    fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;

   105

    fsm->state = ec_fsm_change_state_start_ack;

   106

}

   107

   108

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

   109

   110

/**

   111

   Executes the current state of the state machine.

   112

   \return false, if the state machine has terminated

   113

*/

   114

   115

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

   116

{

   117

    fsm->state(fsm);

   118

   119

    return fsm->state != ec_fsm_change_state_end

   120

        && fsm->state != ec_fsm_change_state_error;

   121

}

   122

   123

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

   124

   125

/**

   126

   Returns, if the state machine terminated with success.

   127

   \return non-zero if successful.

   128

*/

   129

   130

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

   131

{

   132

    return fsm->state == ec_fsm_change_state_end;

   133

}

   134

   135

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

   136

 *  state change state machine

   137

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

   138

   139

/**

   140

   Change state: START.

   141

*/

   142

   143

void ec_fsm_change_state_start(ec_fsm_change_t *fsm

   144

                               /**< finite state machine */)

   145

{

   146

    ec_datagram_t *datagram = fsm->datagram;

   147

    ec_slave_t *slave = fsm->slave;

   148

   149

    fsm->take_time = 1;

   150

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

   151

   152

    // write new state to slave

   153

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

   154

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

   155

    fsm->retries = EC_FSM_RETRIES;

   156

    fsm->state = ec_fsm_change_state_check;

   157

}

   158

   159

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

   160

   161

/**

   162

   Change state: CHECK.

   163

*/

   164

   165

void ec_fsm_change_state_check(ec_fsm_change_t *fsm

   166

                               /**< finite state machine */)

   167

{

   168

    ec_datagram_t *datagram = fsm->datagram;

   169

    ec_slave_t *slave = fsm->slave;

   170

   171

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

   172

        return;

   173

   174

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   175

        fsm->state = ec_fsm_change_state_error;

   176

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

   177

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

   178

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

   179

        return;

   180

    }

   181

   182

    if (fsm->take_time) {

   183

        fsm->take_time = 0;

   184

        fsm->jiffies_start = datagram->jiffies_sent;

   185

    }

   186

   187

    if (datagram->working_counter == 0) {

   188

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

   189

            char state_str[EC_STATE_STRING_SIZE];

   190

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

   191

            fsm->state = ec_fsm_change_state_error;

   192

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

   193

                    state_str, fsm->slave->ring_position);

   194

            ec_datagram_print_wc_error(datagram);

   195

            return;

   196

        }

   197

   198

        // repeat writing new state to slave

   199

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

   200

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

   201

        fsm->retries = EC_FSM_RETRIES;

   202

        return;

   203

    }

   204

   205

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

   206

        char state_str[EC_STATE_STRING_SIZE];

   207

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

   208

        fsm->state = ec_fsm_change_state_error;

   209

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

   210

                state_str, fsm->slave->ring_position);

   211

        ec_datagram_print_wc_error(datagram);

   212

        return;

   213

    }

   214

   215

    fsm->take_time = 1;

   216

   217

    // read AL status from slave

   218

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

   219

    ec_datagram_zero(datagram);

   220

    fsm->retries = EC_FSM_RETRIES;

   221

    fsm->spontaneous_change = 0;

   222

    fsm->state = ec_fsm_change_state_status;

   223

}

   224

   225

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

   226

   227

/**

   228

   Change state: STATUS.

   229

*/

   230

   231

void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   232

                                /**< finite state machine */)

   233

{

   234

    ec_datagram_t *datagram = fsm->datagram;

   235

    ec_slave_t *slave = fsm->slave;

   236

   237

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

   238

        return;

   239

   240

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   241

        fsm->state = ec_fsm_change_state_error;

   242

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

   243

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

   244

               slave->ring_position, datagram->state);

   245

        return;

   246

    }

   247

   248

    if (datagram->working_counter != 1) {

   249

        char req_state[EC_STATE_STRING_SIZE];

   250

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

   251

        fsm->state = ec_fsm_change_state_error;

   252

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

   253

               req_state, slave->ring_position);

   254

        ec_datagram_print_wc_error(datagram);

   255

        return;

   256

    }

   257

   258

    if (fsm->take_time) {

   259

        fsm->take_time = 0;

   260

        fsm->jiffies_start = datagram->jiffies_sent;

   261

    }

   262

   263

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

   264

   265

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

   266

        // state has been set successfully

   267

        fsm->state = ec_fsm_change_state_end;

   268

        return;

   269

    }

   270

   271

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

   272

        char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   273

   274

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

   275

   276

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

   277

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

   278

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

   279

            // state change

   280

            fsm->spontaneous_change = 1;

   281

            fsm->old_state = slave->current_state;

   282

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

   283

                    slave->ring_position, cur_state);

   284

            goto check_again;

   285

        }

   286

   287

        // state change error

   288

   289

        slave->error_flag = 1;

   290

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

   291

   292

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

   293

               req_state, slave->ring_position, cur_state);

   294

        // fetch AL status error code

   295

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

   296

        ec_datagram_zero(datagram);

   297

        fsm->retries = EC_FSM_RETRIES;

   298

        fsm->state = ec_fsm_change_state_code;

   299

        return;

   300

    }

   301

   302

    // still old state

   303

   304

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

   305

        // timeout while checking

   306

        char state_str[EC_STATE_STRING_SIZE];

   307

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

   308

        fsm->state = ec_fsm_change_state_error;

   309

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

   310

                state_str, slave->ring_position);

   311

        return;

   312

    }

   313

   314

 check_again:

   315

    // no timeout yet. check again

   316

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

   317

    ec_datagram_zero(datagram);

   318

    fsm->retries = EC_FSM_RETRIES;

   319

}

   320

   321

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

   322

   323

/**

   324

   Application layer status messages.

   325

*/

   326

   327

const ec_code_msg_t al_status_messages[] = {

   328

    {0x0001, "Unspecified error"},

   329

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

   330

    {0x0012, "Unknown requested state"},

   331

    {0x0013, "Bootstrap not supported"},

   332

    {0x0014, "No valid firmware"},

   333

    {0x0015, "Invalid mailbox configuration"},

   334

    {0x0016, "Invalid mailbox configuration"},

   335

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

   336

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

   337

    {0x0019, "No valid outputs"},

   338

    {0x001A, "Synchronisation error"},

   339

    {0x001B, "Sync manager watchdog"},

   340

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

   341

    {0x001D, "Invalid output configuration"},

   342

    {0x001E, "Invalid input configuration"},

   343

    {0x001F, "Invalid watchdog configuration"},

   344

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

   345

    {0x0021, "Slave needs INIT"},

   346

    {0x0022, "Slave needs PREOP"},

   347

    {0x0023, "Slave needs SAFEOP"},

   348

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

   349

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

   350

    {0x0032, "PLL error"},

   351

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

   352

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

   353

    {0x0042, "MBOX EOE"},

   354

    {0x0043, "MBOX COE"},

   355

    {0x0044, "MBOX FOE"},

   356

    {0x0045, "MBOX SOE"},

   357

    {0x004F, "MBOX VOE"},

   358

    {}

   359

};

   360

   361

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

   362

   363

/**

   364

   Change state: CODE.

   365

*/

   366

   367

void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   368

                              /**< finite state machine */)

   369

{

   370

    ec_datagram_t *datagram = fsm->datagram;

   371

    uint32_t code;

   372

    const ec_code_msg_t *al_msg;

   373

   374

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

   375

        return;

   376

   377

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   378

        fsm->state = ec_fsm_change_state_error;

   379

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

   380

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

   381

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

   382

        return;

   383

    }

   384

   385

    if (datagram->working_counter != 1) {

   386

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

   387

        ec_datagram_print_wc_error(datagram);

   388

    } else {

   389

        code = EC_READ_U16(datagram->data);

   390

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

   391

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

   392

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

   393

                    al_msg->code, al_msg->message);

   394

            break;

   395

        }

   396

        if (!al_msg->code)

   397

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

   398

    }

   399

   400

    // acknowledge "old" slave state

   401

    ec_fsm_change_state_start_ack(fsm); // execute immediately

   402

}

   403

   404

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

   405

   406

/**

   407

   Change state: START ACK.

   408

*/

   409

   410

void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   411

                                   /**< finite state machine */)

   412

{

   413

    ec_slave_t *slave = fsm->slave;

   414

    ec_datagram_t *datagram = fsm->datagram;

   415

   416

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

   417

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

   418

    fsm->retries = EC_FSM_RETRIES;

   419

    fsm->state = ec_fsm_change_state_ack;

   420

}

   421

   422

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

   423

   424

/**

   425

   Change state: ACK.

   426

*/

   427

   428

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

   429

{

   430

    ec_datagram_t *datagram = fsm->datagram;

   431

    ec_slave_t *slave = fsm->slave;

   432

   433

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

   434

        return;

   435

   436

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   437

        fsm->state = ec_fsm_change_state_error;

   438

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

   439

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

   440

               slave->ring_position, datagram->state);

   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 on slave %u: ",

   447

                slave->ring_position);

   448

        ec_datagram_print_wc_error(datagram);

   449

        return;

   450

    }

   451

   452

    fsm->take_time = 1;

   453

   454

    // read new AL status

   455

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

   456

    ec_datagram_zero(datagram);

   457

    fsm->retries = EC_FSM_RETRIES;

   458

    fsm->state = ec_fsm_change_state_check_ack;

   459

}

   460

   461

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

   462

   463

/**

   464

   Change state: CHECK ACK.

   465

*/

   466

   467

void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   468

                                   /**< finite state machine */)

   469

{

   470

    ec_datagram_t *datagram = fsm->datagram;

   471

    ec_slave_t *slave = fsm->slave;

   472

   473

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

   474

        return;

   475

   476

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   477

        fsm->state = ec_fsm_change_state_error;

   478

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

   479

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

   480

               slave->ring_position, datagram->state);

   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 on slave %u: ",

   487

                slave->ring_position);

   488

        ec_datagram_print_wc_error(datagram);

   489

        return;

   490

    }

   491

   492

    if (fsm->take_time) {

   493

        fsm->take_time = 0;

   494

        fsm->jiffies_start = datagram->jiffies_sent;

   495

    }

   496

   497

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

   498

   499

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

   500

        char state_str[EC_STATE_STRING_SIZE];

   501

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

   502