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 }

    70 

    71 /*****************************************************************************/

    72 

    73 /**

    74    Destructor.

    75 */

    76 

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

    78 {

    79 }

    80 

    81 /*****************************************************************************/

    82 

    83 /**

    84    Starts the change state machine.

    85 */

    86 

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

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

    89                          ec_slave_state_t state /**< requested state */

    90                          )

    91 {

    92     fsm->mode = EC_FSM_CHANGE_MODE_FULL;

    93     fsm->slave = slave;

    94     fsm->requested_state = state;

    95     fsm->state = ec_fsm_change_state_start;

    96 }

    97 

    98 /*****************************************************************************/

    99 

   100 /**

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

   102 */

   103 

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

   105                        ec_slave_t *slave /**< EtherCAT slave */

   106                        )

   107 {

   108     fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;

   109     fsm->slave = slave;

   110     fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;

   111     fsm->state = ec_fsm_change_state_start_ack;

   112 }

   113 

   114 /*****************************************************************************/

   115 

   116 /**

   117    Executes the current state of the state machine.

   118    \return false, if the state machine has terminated

   119 */

   120 

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

   122 {

   123     fsm->state(fsm);

   124 

   125     return fsm->state != ec_fsm_change_state_end

   126         && fsm->state != ec_fsm_change_state_error;

   127 }

   128 

   129 /*****************************************************************************/

   130 

   131 /**

   132    Returns, if the state machine terminated with success.

   133    \return non-zero if successful.

   134 */

   135 

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

   137 {

   138     return fsm->state == ec_fsm_change_state_end;

   139 }

   140 

   141 /******************************************************************************

   142  *  state change state machine

   143  *****************************************************************************/

   144 

   145 /**

   146    Change state: START.

   147 */

   148 

   149 void ec_fsm_change_state_start(ec_fsm_change_t *fsm

   150                                /**< finite state machine */)

   151 {

   152     ec_datagram_t *datagram = fsm->datagram;

   153     ec_slave_t *slave = fsm->slave;

   154 

   155     fsm->take_time = 1;

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

   157 

   158     // write new state to slave

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

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

   161     ec_master_queue_datagram(fsm->slave->master, datagram);

   162     fsm->state = ec_fsm_change_state_check;

   163 }

   164 

   165 /*****************************************************************************/

   166 

   167 /**

   168    Change state: CHECK.

   169 */

   170 

   171 void ec_fsm_change_state_check(ec_fsm_change_t *fsm

   172                                /**< finite state machine */)

   173 {

   174     ec_datagram_t *datagram = fsm->datagram;

   175     ec_slave_t *slave = fsm->slave;

   176 

   177     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   178         fsm->state = ec_fsm_change_state_error;

   179         EC_ERR("Failed to send state datagram to slave %i!\n",

   180                fsm->slave->ring_position);

   181         return;

   182     }

   183 

   184     if (fsm->take_time) {

   185         fsm->take_time = 0;

   186         fsm->jiffies_start = datagram->jiffies_sent;

   187     }

   188 

   189     if (datagram->working_counter != 1) {

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

   191             char state_str[EC_STATE_STRING_SIZE];

   192             ec_state_string(fsm->requested_state, state_str);

   193             fsm->state = ec_fsm_change_state_error;

   194             EC_ERR("Failed to set state %s on slave %i: Slave did not"

   195                    " respond.\n", state_str, fsm->slave->ring_position);

   196             return;

   197         }

   198 

   199         // repeat writing new state to slave

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

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

   202         ec_master_queue_datagram(fsm->slave->master, datagram);

   203         return;

   204     }

   205 

   206     fsm->take_time = 1;

   207 

   208     // read AL status from slave

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

   210     ec_master_queue_datagram(fsm->slave->master, datagram);

   211     fsm->state = ec_fsm_change_state_status;

   212 }

   213 

   214 /*****************************************************************************/

   215 

   216 /**

   217    Change state: STATUS.

   218 */

   219 

   220 void ec_fsm_change_state_status(ec_fsm_change_t *fsm

   221                                 /**< finite state machine */)

   222 {

   223     ec_datagram_t *datagram = fsm->datagram;

   224     ec_slave_t *slave = fsm->slave;

   225 

   226     if (datagram->state != EC_DATAGRAM_RECEIVED

   227         || datagram->working_counter != 1) {

   228         char req_state[EC_STATE_STRING_SIZE];

   229         ec_state_string(fsm->requested_state, req_state);

   230         fsm->state = ec_fsm_change_state_error;

   231         EC_ERR("Failed to check state %s on slave %i.\n",

   232                req_state, slave->ring_position);

   233         return;

   234     }

   235 

   236     if (fsm->take_time) {

   237         fsm->take_time = 0;

   238         fsm->jiffies_start = datagram->jiffies_sent;

   239     }

   240 

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

   242 

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

   244         // state has been set successfully

   245         fsm->state = ec_fsm_change_state_end;

   246         return;

   247     }

   248 

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

   250         char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];

   251 

   252         ec_state_string(slave->current_state, cur_state);

   253 

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

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

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

   257             // state change

   258             fsm->old_state = slave->current_state;

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

   260                     slave->ring_position, cur_state);

   261             goto again;

   262         }

   263 

   264         // state change error

   265 

   266         slave->error_flag = 1;

   267         ec_state_string(fsm->requested_state, req_state);

   268 

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

   270                req_state, slave->ring_position, cur_state);

   271         // fetch AL status error code

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

   273         ec_master_queue_datagram(fsm->slave->master, datagram);

   274         fsm->state = ec_fsm_change_state_code;

   275         return;

   276     }

   277 

   278     // still old state

   279 

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

   281         // timeout while checking

   282         char state_str[EC_STATE_STRING_SIZE];

   283         ec_state_string(fsm->requested_state, state_str);

   284         fsm->state = ec_fsm_change_state_error;

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

   286                state_str, slave->ring_position);

   287         return;

   288     }

   289 

   290  again:

   291     // no timeout yet. check again

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

   293     ec_master_queue_datagram(fsm->slave->master, datagram);

   294 }

   295 

   296 /*****************************************************************************/

   297 

   298 /**

   299    Application layer status messages.

   300 */

   301 

   302 const ec_code_msg_t al_status_messages[] = {

   303     {0x0001, "Unspecified error"},

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

   305     {0x0012, "Unknown requested state"},

   306     {0x0013, "Bootstrap not supported"},

   307     {0x0014, "No valid firmware"},

   308     {0x0015, "Invalid mailbox configuration"},

   309     {0x0016, "Invalid mailbox configuration"},

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

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

   312     {0x0019, "No valid outputs"},

   313     {0x001A, "Synchronisation error"},

   314     {0x001B, "Sync manager watchdog"},

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

   316     {0x001D, "Invalid output configuration"},

   317     {0x001E, "Invalid input configuration"},

   318     {0x001F, "Invalid watchdog configuration"},

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

   320     {0x0021, "Slave needs INIT"},

   321     {0x0022, "Slave needs PREOP"},

   322     {0x0023, "Slave needs SAVEOP"},

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

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

   325     {0x0032, "PLL error"},

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

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

   328     {0x0042, "MBOX EOE"},

   329     {0x0043, "MBOX COE"},

   330     {0x0044, "MBOX FOE"},

   331     {0x0045, "MBOX SOE"},

   332     {0x004F, "MBOX VOE"},

   333     {}

   334 };

   335 

   336 /*****************************************************************************/

   337 

   338 /**

   339    Change state: CODE.

   340 */

   341 

   342 void ec_fsm_change_state_code(ec_fsm_change_t *fsm

   343                               /**< finite state machine */)

   344 {

   345     ec_datagram_t *datagram = fsm->datagram;

   346     uint32_t code;

   347     const ec_code_msg_t *al_msg;

   348 

   349     if (datagram->state != EC_DATAGRAM_RECEIVED

   350         || datagram->working_counter != 1) {

   351         EC_WARN("Reception of AL status code datagram failed.\n");

   352     }

   353     else {

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

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

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

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

   358                        al_msg->code, al_msg->message);

   359                 break;

   360             }

   361             if (!al_msg->code)

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

   363         }

   364     }

   365 

   366     // acknowledge "old" slave state

   367     ec_fsm_change_state_start_ack(fsm); // execute immediately

   368 }

   369 

   370 /*****************************************************************************/

   371 

   372 /**

   373    Change state: START ACK.

   374 */

   375 

   376 void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm

   377                                    /**< finite state machine */)

   378 {

   379     ec_slave_t *slave = fsm->slave;

   380     ec_datagram_t *datagram = fsm->datagram;

   381 

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

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

   384     ec_master_queue_datagram(fsm->slave->master, datagram);

   385     fsm->state = ec_fsm_change_state_ack;

   386 }

   387 

   388 /*****************************************************************************/

   389 

   390 /**

   391    Change state: ACK.

   392 */

   393 

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

   395 {

   396     ec_datagram_t *datagram = fsm->datagram;

   397     ec_slave_t *slave = fsm->slave;

   398 

   399     if (datagram->state != EC_DATAGRAM_RECEIVED

   400         || datagram->working_counter != 1) {

   401         fsm->state = ec_fsm_change_state_error;

   402         EC_ERR("Reception of state ack datagram failed.\n");

   403         return;

   404     }

   405 

   406     fsm->take_time = 1;

   407 

   408     // read new AL status

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

   410     ec_master_queue_datagram(fsm->slave->master, datagram);

   411     fsm->state = ec_fsm_change_state_check_ack;

   412 }

   413 

   414 /*****************************************************************************/

   415 

   416 /**

   417    Change state: CHECK ACK.

   418 */

   419 

   420 void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm

   421                                    /**< finite state machine */)

   422 {

   423     ec_datagram_t *datagram = fsm->datagram;

   424     ec_slave_t *slave = fsm->slave;

   425 

   426     if (datagram->state != EC_DATAGRAM_RECEIVED

   427         || datagram->working_counter != 1) {

   428         fsm->state = ec_fsm_change_state_error;

   429         EC_ERR("Reception of state ack check datagram failed.\n");

   430         return;

   431     }

   432 

   433     if (fsm->take_time) {

   434         fsm->take_time = 0;

   435         fsm->jiffies_start = datagram->jiffies_sent;

   436     }

   437 

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

   439 

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

   441         char state_str[EC_STATE_STRING_SIZE];

   442         ec_state_string(slave->current_state, state_str);

   443         if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) {

   444             fsm->state = ec_fsm_change_state_error;

   445         }

   446         else { // EC_FSM_CHANGE_MODE_ACK_ONLY

   447             fsm->state = ec_fsm_change_state_end;

   448         }

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

   450                 state_str, slave->ring_position);

   451         return;

   452     }

   453 

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

   455         // timeout while checking

   456         char state_str[EC_STATE_STRING_SIZE];

   457         ec_state_string(slave->current_state, state_str);

   458         fsm->state = ec_fsm_change_state_error;

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

   460                state_str, slave->ring_position);

   461         return;

   462     }

   463 

   464     // reread new AL status

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

   466     ec_master_queue_datagram(fsm->slave->master, datagram);

   467 }

   468 

   469 /*****************************************************************************/

   470 

   471 /**

   472    State: ERROR.

   473 */

   474 

   475 void ec_fsm_change_state_error(ec_fsm_change_t *fsm

   476                                /**< finite state machine */)

   477 {

   478 }

   479 

   480 /*****************************************************************************/

   481 

   482 /**

   483    State: END.

   484 */

   485 

   486 void ec_fsm_change_state_end(ec_fsm_change_t *fsm

   487                              /**< finite state machine */)

   488 {

   489 }

   490 

   491 /*****************************************************************************/