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

void ec_fsm_change_state_start(ec_fsm_change_t *);

    44

void ec_fsm_change_state_check(ec_fsm_change_t *);

    45

void ec_fsm_change_state_status(ec_fsm_change_t *);

    46

void ec_fsm_change_state_code(ec_fsm_change_t *);

    47

void ec_fsm_change_state_start_ack(ec_fsm_change_t *);

    48

void ec_fsm_change_state_ack(ec_fsm_change_t *);

    49

void ec_fsm_change_state_check_ack(ec_fsm_change_t *);

    50

void ec_fsm_change_state_end(ec_fsm_change_t *);

    51

void ec_fsm_change_state_error(ec_fsm_change_t *);

    52

    53

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

    54

    55

/**

    56

   Constructor.

    57

*/

    58

    59

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

    60

                        ec_datagram_t *datagram /**< datagram */

    61

                        )

    62

{

    63

    fsm->state = NULL;

    64

    fsm->datagram = datagram;

    65

    fsm->spontaneous_change = 0;

    66

}

    67

    68

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

    69

    70

/**

    71

   Destructor.

    72

*/

    73

    74

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

    75

{

    76

}

    77

    78

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

    79

    80

/**

    81

   Starts the change state machine.

    82

*/

    83

    84

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

    85

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

    86

                         ec_slave_state_t state /**< requested state */

    87

                         )

    88

{

    89

    fsm->mode = EC_FSM_CHANGE_MODE_FULL;

    90

    fsm->slave = slave;

    91

    fsm->requested_state = state;

    92

    fsm->state = ec_fsm_change_state_start;

    93

}

    94

    95

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

    96

    97

/**

    98

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

    99

*/

   100

   101

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

   102

                       ec_slave_t *slave /**< EtherCAT slave */

   103

                       )

   104

{

   105

    fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;

   106

    fsm->slave = slave;

   107

    fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;

   108

    fsm->state = ec_fsm_change_state_start_ack;

   109

}

   110

   111

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

   112

   113

/**

   114

   Executes the current state of the state machine.

   115

   \return false, if the state machine has terminated

   116

*/

   117

   118

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

   119

{

   120

    fsm->state(fsm);

   121

   122

    return fsm->state != ec_fsm_change_state_end

   123

        && fsm->state != ec_fsm_change_state_error;

   124

}

   125

   126

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

   127

   128

/**

   129

   Returns, if the state machine terminated with success.

   130

   \return non-zero if successful.

   131

*/

   132

   133

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

   134

{

   135

    return fsm->state == ec_fsm_change_state_end;

   136

}

   137

   138

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

   139

 *  state change state machine

   140

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

   141

   142

/**

   143

   Change state: START.

   144

*/

   145

   146

void ec_fsm_change_state_start(ec_fsm_change_t *fsm

   147

                               /**< finite state machine */)

   148

{

   149

    ec_datagram_t *datagram = fsm->datagram;

   150

    ec_slave_t *slave = fsm->slave;

   151

   152

    fsm->take_time = 1;

   153

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

   154

   155

    // write new state to slave

   156

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

   157

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

   158

    fsm->retries = EC_FSM_RETRIES;

   159

    fsm->state = ec_fsm_change_state_check;

   160

}

   161

   162

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

   163

   164

/**

   165

   Change state: CHECK.

   166

*/

   167

   168

void ec_fsm_change_state_check(ec_fsm_change_t *fsm

   169

                               /**< finite state machine */)

   170

{

   171

    ec_datagram_t *datagram = fsm->datagram;

   172

    ec_slave_t *slave = fsm->slave;

   173

   174

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

   175

        return;

   176

   177

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   178

        fsm->state = ec_fsm_change_state_error;

   179

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

   180

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

   181

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

   182

        return;

   183

    }

   184

   185

    if (fsm->take_time) {

   186

        fsm->take_time = 0;

   187

        fsm->jiffies_start = datagram->jiffies_sent;

   188

    }

   189

   190

    if (datagram->working_counter == 0) {

   191

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

   192

            char state_str[EC_STATE_STRING_SIZE];

   193

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

   194

            fsm->state = ec_fsm_change_state_error;

   195

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

   196

                    state_str, fsm->slave->ring_position);

   197

            ec_datagram_print_wc_error(datagram);

   198

            return;

   199

        }

   200

   201

        // repeat writing new state to slave

   202

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

   203

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

   204

        fsm->retries = EC_FSM_RETRIES;

   205

        return;

   206

    }

   207

   208

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

   209

        char state_str[EC_STATE_STRING_SIZE];

   210

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

   211

        fsm->state = ec_fsm_change_state_error;

   212

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

   213

                state_str, fsm->slave->ring_position);

   214

        ec_datagram_print_wc_error(datagram);

   215

        return;

   216

    }

   217

   218

    fsm->take_time = 1;

   219

   220

    // read AL status from slave

   221

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

   222

    ec_datagram_zero(datagram);

   223

    fsm->retries = EC_FSM_RETRIES;

   224

    fsm->spontaneous_change = 0;

   225

    fsm->state = ec_fsm_change_state_status;

   226

}

   227

   228

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

   229

   230

/**

   231

   Change state: STATUS.

   232

*/

   233

   234

void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   235

                                /**< finite state machine */)

   236

{

   237

    ec_datagram_t *datagram = fsm->datagram;

   238

    ec_slave_t *slave = fsm->slave;

   239

   240

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

   241

        return;

   242

   243

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   244

        fsm->state = ec_fsm_change_state_error;

   245

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

   246

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

   247

               slave->ring_position, datagram->state);

   248

        return;

   249

    }

   250

   251

    if (datagram->working_counter != 1) {

   252

        char req_state[EC_STATE_STRING_SIZE];

   253

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

   254

        fsm->state = ec_fsm_change_state_error;

   255

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

   256

               req_state, slave->ring_position);

   257

        ec_datagram_print_wc_error(datagram);

   258

        return;

   259

    }

   260

   261

    if (fsm->take_time) {

   262

        fsm->take_time = 0;

   263

        fsm->jiffies_start = datagram->jiffies_sent;

   264

    }

   265

   266

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

   267

   268

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

   269

        // state has been set successfully

   270

        fsm->state = ec_fsm_change_state_end;

   271

        return;

   272

    }

   273

   274

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

   275

        char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   276

   277

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

   278

   279

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

   280

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

   281

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

   282

            // state change

   283

            fsm->spontaneous_change = 1;

   284

            fsm->old_state = slave->current_state;

   285

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

   286

                    slave->ring_position, cur_state);

   287

            goto check_again;

   288

        }

   289

   290

        // state change error

   291

   292

        slave->error_flag = 1;

   293

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

   294

   295

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

   296

               req_state, slave->ring_position, cur_state);

   297

        // fetch AL status error code

   298

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

   299

        ec_datagram_zero(datagram);

   300

        fsm->retries = EC_FSM_RETRIES;

   301

        fsm->state = ec_fsm_change_state_code;

   302

        return;

   303

    }

   304

   305

    // still old state

   306

   307

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

   308

        // timeout while checking

   309

        char state_str[EC_STATE_STRING_SIZE];

   310

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

   311

        fsm->state = ec_fsm_change_state_error;

   312

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

   313

                state_str, slave->ring_position);

   314

        return;

   315

    }

   316

   317

 check_again:

   318

    // no timeout yet. check again

   319

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

   320

    ec_datagram_zero(datagram);

   321

    fsm->retries = EC_FSM_RETRIES;

   322

}

   323

   324

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

   325

   326

/**

   327

   Application layer status messages.

   328

*/

   329

   330

const ec_code_msg_t al_status_messages[] = {

   331

    {0x0001, "Unspecified error"},

   332

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

   333

    {0x0012, "Unknown requested state"},

   334

    {0x0013, "Bootstrap not supported"},

   335

    {0x0014, "No valid firmware"},

   336

    {0x0015, "Invalid mailbox configuration"},

   337

    {0x0016, "Invalid mailbox configuration"},

   338

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

   339

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

   340

    {0x0019, "No valid outputs"},

   341

    {0x001A, "Synchronisation error"},

   342

    {0x001B, "Sync manager watchdog"},

   343

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

   344

    {0x001D, "Invalid output configuration"},

   345

    {0x001E, "Invalid input configuration"},

   346

    {0x001F, "Invalid watchdog configuration"},

   347

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

   348

    {0x0021, "Slave needs INIT"},

   349

    {0x0022, "Slave needs PREOP"},

   350

    {0x0023, "Slave needs SAFEOP"},

   351

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

   352

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

   353

    {0x0032, "PLL error"},

   354

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

   355

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

   356

    {0x0042, "MBOX EOE"},

   357

    {0x0043, "MBOX COE"},

   358

    {0x0044, "MBOX FOE"},

   359

    {0x0045, "MBOX SOE"},

   360

    {0x004F, "MBOX VOE"},

   361

    {}

   362

};

   363

   364

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

   365

   366

/**

   367

   Change state: CODE.

   368

*/

   369

   370

void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   371

                              /**< finite state machine */)

   372

{

   373

    ec_datagram_t *datagram = fsm->datagram;

   374

    uint32_t code;

   375

    const ec_code_msg_t *al_msg;

   376

   377

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

   378

        return;

   379

   380

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   381

        fsm->state = ec_fsm_change_state_error;

   382

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

   383

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

   384

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

   385

        return;

   386

    }

   387

   388

    if (datagram->working_counter != 1) {

   389

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

   390

        ec_datagram_print_wc_error(datagram);

   391

    } else {

   392

        code = EC_READ_U16(datagram->data);

   393

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

   394

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

   395

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

   396

                    al_msg->code, al_msg->message);

   397

            break;

   398

        }

   399

        if (!al_msg->code)

   400

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

   401

    }

   402

   403

    // acknowledge "old" slave state

   404

    ec_fsm_change_state_start_ack(fsm); // execute immediately

   405

}

   406

   407

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

   408

   409

/**

   410

   Change state: START ACK.

   411

*/

   412

   413

void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   414

                                   /**< finite state machine */)

   415

{

   416

    ec_slave_t *slave = fsm->slave;

   417

    ec_datagram_t *datagram = fsm->datagram;

   418

   419

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

   420

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

   421

    fsm->retries = EC_FSM_RETRIES;

   422

    fsm->state = ec_fsm_change_state_ack;

   423

}

   424

   425

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

   426

   427

/**

   428

   Change state: ACK.

   429

*/

   430

   431

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

   432

{

   433

    ec_datagram_t *datagram = fsm->datagram;

   434

    ec_slave_t *slave = fsm->slave;

   435

   436

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

   437

        return;

   438

   439

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   440

        fsm->state = ec_fsm_change_state_error;

   441

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

   442

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

   443

               slave->ring_position, datagram->state);

   444

        return;

   445

    }

   446

   447

    if (datagram->working_counter != 1) {

   448

        fsm->state = ec_fsm_change_state_error;

   449

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

   450

                slave->ring_position);

   451

        ec_datagram_print_wc_error(datagram);

   452

        return;

   453

    }

   454

   455

    fsm->take_time = 1;

   456

   457

    // read new AL status

   458

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

   459

    ec_datagram_zero(datagram);

   460

    fsm->retries = EC_FSM_RETRIES;

   461

    fsm->state = ec_fsm_change_state_check_ack;

   462

}

   463

   464

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

   465

   466

/**

   467

   Change state: CHECK ACK.

   468

*/

   469

   470

void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   471

                                   /**< finite state machine */)

   472

{

   473

    ec_datagram_t *datagram = fsm->datagram;

   474

    ec_slave_t *slave = fsm->slave;

   475

   476

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

   477

        return;

   478

   479

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   480

        fsm->state = ec_fsm_change_state_error;

   481

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

   482

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

   483

               slave->ring_position, datagram->state);

   484

        return;

   485

    }

   486

   487

    if (datagram->working_counter != 1) {

   488

        fsm->state = ec_fsm_change_state_error;

   489

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

   490

                slave->ring_position);

   491

        ec_datagram_print_wc_error(datagram);

   492

        return;

   493

    }

   494

   495

    if (fsm->take_time) {

   496

        fsm->take_time = 0;

   497

        fsm->jiffies_start = datagram->jiffies_sent;

   498

    }

   499

   500

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

   501

   502

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