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

    fsm->retries = EC_FSM_RETRIES;

   220

    fsm->spontaneous_change = 0;

   221

    fsm->state = ec_fsm_change_state_status;

   222

}

   223

   224

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

   225

   226

/**

   227

   Change state: STATUS.

   228

*/

   229

   230

void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   231

                                /**< finite state machine */)

   232

{

   233

    ec_datagram_t *datagram = fsm->datagram;

   234

    ec_slave_t *slave = fsm->slave;

   235

   236

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

   237

        return;

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

   242

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

   243

               slave->ring_position, datagram->state);

   244

        return;

   245

    }

   246

   247

    if (datagram->working_counter != 1) {

   248

        char req_state[EC_STATE_STRING_SIZE];

   249

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

   250

        fsm->state = ec_fsm_change_state_error;

   251

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

   252

               req_state, slave->ring_position);

   253

        ec_datagram_print_wc_error(datagram);

   254

        return;

   255

    }

   256

   257

    if (fsm->take_time) {

   258

        fsm->take_time = 0;

   259

        fsm->jiffies_start = datagram->jiffies_sent;

   260

    }

   261

   262

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

   263

   264

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

   265

        // state has been set successfully

   266

        fsm->state = ec_fsm_change_state_end;

   267

        return;

   268

    }

   269

   270

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

   271

        char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   272

   273

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

   274

   275

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

   276

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

   277

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

   278

            // state change

   279

            fsm->spontaneous_change = 1;

   280

            fsm->old_state = slave->current_state;

   281

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

   282

                    slave->ring_position, cur_state);

   283

            goto check_again;

   284

        }

   285

   286

        // state change error

   287

   288

        slave->error_flag = 1;

   289

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

   290

   291

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

   292

               req_state, slave->ring_position, cur_state);

   293

        // fetch AL status error code

   294

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

   295

        fsm->retries = EC_FSM_RETRIES;

   296

        fsm->state = ec_fsm_change_state_code;

   297

        return;

   298

    }

   299

   300

    // still old state

   301

   302

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

   303

        // timeout while checking

   304

        char state_str[EC_STATE_STRING_SIZE];

   305

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

   306

        fsm->state = ec_fsm_change_state_error;

   307

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

   308

                state_str, slave->ring_position);

   309

        return;

   310

    }

   311

   312

 check_again:

   313

    // no timeout yet. check again

   314

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

   315

    fsm->retries = EC_FSM_RETRIES;

   316

}

   317

   318

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

   319

   320

/**

   321

   Application layer status messages.

   322

*/

   323

   324

const ec_code_msg_t al_status_messages[] = {

   325

    {0x0001, "Unspecified error"},

   326

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

   327

    {0x0012, "Unknown requested state"},

   328

    {0x0013, "Bootstrap not supported"},

   329

    {0x0014, "No valid firmware"},

   330

    {0x0015, "Invalid mailbox configuration"},

   331

    {0x0016, "Invalid mailbox configuration"},

   332

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

   333

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

   334

    {0x0019, "No valid outputs"},

   335

    {0x001A, "Synchronisation error"},

   336

    {0x001B, "Sync manager watchdog"},

   337

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

   338

    {0x001D, "Invalid output configuration"},

   339

    {0x001E, "Invalid input configuration"},

   340

    {0x001F, "Invalid watchdog configuration"},

   341

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

   342

    {0x0021, "Slave needs INIT"},

   343

    {0x0022, "Slave needs PREOP"},

   344

    {0x0023, "Slave needs SAFEOP"},

   345

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

   346

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

   347

    {0x0032, "PLL error"},

   348

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

   349

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

   350

    {0x0042, "MBOX EOE"},

   351

    {0x0043, "MBOX COE"},

   352

    {0x0044, "MBOX FOE"},

   353

    {0x0045, "MBOX SOE"},

   354

    {0x004F, "MBOX VOE"},

   355

    {}

   356

};

   357

   358

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

   359

   360

/**

   361

   Change state: CODE.

   362

*/

   363

   364

void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   365

                              /**< finite state machine */)

   366

{

   367

    ec_datagram_t *datagram = fsm->datagram;

   368

    uint32_t code;

   369

    const ec_code_msg_t *al_msg;

   370

   371

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

   372

        return;

   373

   374

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   375

        fsm->state = ec_fsm_change_state_error;

   376

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

   377

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

   378

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

   379

        return;

   380

    }

   381

   382

    if (datagram->working_counter != 1) {

   383

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

   384

        ec_datagram_print_wc_error(datagram);

   385

    } else {

   386

        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

    // acknowledge "old" slave state

   398

    ec_fsm_change_state_start_ack(fsm); // execute immediately

   399

}

   400

   401

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

   402

   403

/**

   404

   Change state: START ACK.

   405

*/

   406

   407

void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   408

                                   /**< finite state machine */)

   409

{

   410

    ec_slave_t *slave = fsm->slave;

   411

    ec_datagram_t *datagram = fsm->datagram;

   412

   413

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

   414

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

   415

    fsm->retries = EC_FSM_RETRIES;

   416

    fsm->state = ec_fsm_change_state_ack;

   417

}

   418

   419

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

   420

   421

/**

   422

   Change state: ACK.

   423

*/

   424

   425

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

   426

{

   427

    ec_datagram_t *datagram = fsm->datagram;

   428

    ec_slave_t *slave = fsm->slave;

   429

   430

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

   431

        return;

   432

   433

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   434

        fsm->state = ec_fsm_change_state_error;

   435

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

   436

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

   437

               slave->ring_position, datagram->state);

   438

        return;

   439

    }

   440

   441

    if (datagram->working_counter != 1) {

   442

        fsm->state = ec_fsm_change_state_error;

   443

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

   444

                slave->ring_position);

   445

        ec_datagram_print_wc_error(datagram);

   446

        return;

   447

    }

   448

   449

    fsm->take_time = 1;

   450

   451

    // read new AL status

   452

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

   453

    fsm->retries = EC_FSM_RETRIES;

   454

    fsm->state = ec_fsm_change_state_check_ack;

   455

}

   456

   457

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

   458

   459

/**

   460

   Change state: CHECK ACK.

   461

*/

   462

   463

void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   464

                                   /**< finite state machine */)

   465

{

   466

    ec_datagram_t *datagram = fsm->datagram;

   467

    ec_slave_t *slave = fsm->slave;

   468

   469

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

   470

        return;

   471

   472

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   473

        fsm->state = ec_fsm_change_state_error;

   474

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

   475

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

   476

               slave->ring_position, datagram->state);

   477

        return;

   478

    }

   479

   480

    if (datagram->working_counter != 1) {

   481

        fsm->state = ec_fsm_change_state_error;

   482

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

   483

                slave->ring_position);

   484

        ec_datagram_print_wc_error(datagram);

   485

        return;

   486

    }

   487

   488

    if (fsm->take_time) {

   489

        fsm->take_time = 0;

   490

        fsm->jiffies_start = datagram->jiffies_sent;

   491

    }

   492

   493

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

   494

   495

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

   496

        char state_str[EC_STATE_STRING_SIZE];

   497

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

   498

        if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) {

   499

            fsm->state = ec_fsm_change_state_error;

   500

        }

   501

        else { // EC_FSM_CHANGE_MODE_ACK_ONLY

   502

            fsm->state = ec_fsm_change_state_end;