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_start(ec_fsm_change_t *);

    48 void ec_fsm_change_check(ec_fsm_change_t *);

    49 void ec_fsm_change_status(ec_fsm_change_t *);

    50 void ec_fsm_change_code(ec_fsm_change_t *);

    51 void ec_fsm_change_ack(ec_fsm_change_t *);

    52 void ec_fsm_change_check_ack(ec_fsm_change_t *);

    53 void ec_fsm_change_end(ec_fsm_change_t *);

    54 void ec_fsm_change_error(ec_fsm_change_t *);

    55 

    56 /*****************************************************************************/

    57 

    58 /**

    59    Constructor.

    60 */

    61 

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

    63                         ec_datagram_t *datagram /**< datagram */

    64                         )

    65 {

    66     fsm->state = NULL;

    67     fsm->datagram = datagram;

    68 }

    69 

    70 /*****************************************************************************/

    71 

    72 /**

    73    Destructor.

    74 */

    75 

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

    77 {

    78 }

    79 

    80 /*****************************************************************************/

    81 

    82 /**

    83    Resets the state machine.

    84 */

    85 

    86 void ec_fsm_change(ec_fsm_change_t *fsm, /**< finite state machine */

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

    88                    ec_slave_state_t state /**< requested state */

    89                    )

    90 {

    91     fsm->slave = slave;

    92     fsm->requested_state = state;

    93     fsm->state = ec_fsm_change_start;

    94 }

    95 

    96 /*****************************************************************************/

    97 

    98 /**

    99    Executes the current state of the state machine.

   100 */

   101 

   102 void ec_fsm_change_exec(ec_fsm_change_t *fsm /**< finite state machine */)

   103 {

   104     fsm->state(fsm);

   105 }

   106 

   107 /*****************************************************************************/

   108 

   109 /**

   110    Returns the running state of the state machine.

   111    \return non-zero if not terminated yet.

   112 */

   113 

   114 int ec_fsm_change_running(ec_fsm_change_t *fsm /**< Finite state machine */)

   115 {

   116     return fsm->state != ec_fsm_change_end

   117         && fsm->state != ec_fsm_change_error;

   118 }

   119 

   120 /*****************************************************************************/

   121 

   122 /**

   123    Returns, if the state machine terminated with success.

   124    \return non-zero if successful.

   125 */

   126 

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

   128 {

   129     return fsm->state == ec_fsm_change_end;

   130 }

   131 

   132 /******************************************************************************

   133  *  state change state machine

   134  *****************************************************************************/

   135 

   136 /**

   137    Change state: START.

   138 */

   139 

   140 void ec_fsm_change_start(ec_fsm_change_t *fsm /**< finite state machine */)

   141 {

   142     ec_datagram_t *datagram = fsm->datagram;

   143     ec_slave_t *slave = fsm->slave;

   144 

   145     fsm->take_time = 1;

   146 

   147     // write new state to slave

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

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

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

   151     fsm->state = ec_fsm_change_check;

   152 }

   153 

   154 /*****************************************************************************/

   155 

   156 /**

   157    Change state: CHECK.

   158 */

   159 

   160 void ec_fsm_change_check(ec_fsm_change_t *fsm /**< finite state machine */)

   161 {

   162     ec_datagram_t *datagram = fsm->datagram;

   163     ec_slave_t *slave = fsm->slave;

   164 

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

   166         fsm->state = ec_fsm_change_error;

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

   168                fsm->slave->ring_position);

   169         return;

   170     }

   171 

   172     if (fsm->take_time) {

   173         fsm->take_time = 0;

   174         fsm->jiffies_start = datagram->jiffies_sent;

   175     }

   176 

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

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

   179             fsm->state = ec_fsm_change_error;

   180             EC_ERR("Failed to set state 0x%02X on slave %i: Slave did not"

   181                    " respond.\n", fsm->requested_state,

   182                    fsm->slave->ring_position);

   183             return;

   184         }

   185 

   186         // repeat writing new state to slave

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

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

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

   190         return;

   191     }

   192 

   193     fsm->take_time = 1;

   194 

   195     // read AL status from slave

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

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

   198     fsm->state = ec_fsm_change_status;

   199 }

   200 

   201 /*****************************************************************************/

   202 

   203 /**

   204    Change state: STATUS.

   205 */

   206 

   207 void ec_fsm_change_status(ec_fsm_change_t *fsm /**< finite state machine */)

   208 {

   209     ec_datagram_t *datagram = fsm->datagram;

   210     ec_slave_t *slave = fsm->slave;

   211 

   212     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   214         fsm->state = ec_fsm_change_error;

   215         EC_ERR("Failed to check state 0x%02X on slave %i.\n",

   216                fsm->requested_state, slave->ring_position);

   217         return;

   218     }

   219 

   220     if (fsm->take_time) {

   221         fsm->take_time = 0;

   222         fsm->jiffies_start = datagram->jiffies_sent;

   223     }

   224 

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

   226 

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

   228         // state has been set successfully

   229         fsm->state = ec_fsm_change_end;

   230         return;

   231     }

   232 

   233     if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   234         // state change error

   235         EC_ERR("Failed to set state 0x%02X - Slave %i refused state change"

   236                " (code 0x%02X)!\n", fsm->requested_state, slave->ring_position,

   237                slave->current_state);

   238         // fetch AL status error code

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

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

   241         fsm->state = ec_fsm_change_code;

   242         return;

   243     }

   244 

   245     if (datagram->jiffies_received

   246         - fsm->jiffies_start >= 100 * HZ / 1000) { // 100ms

   247         // timeout while checking

   248         fsm->state = ec_fsm_change_error;

   249         EC_ERR("Timeout while setting state 0x%02X on slave %i.\n",

   250                fsm->requested_state, slave->ring_position);

   251         return;

   252     }

   253 

   254     // still old state: check again

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

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

   257 }

   258 

   259 /*****************************************************************************/

   260 

   261 /**

   262    Application layer status messages.

   263 */

   264 

   265 const ec_code_msg_t al_status_messages[] = {

   266     {0x0001, "Unspecified error"},

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

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

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

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

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

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

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

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

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

   276     {0x001A, "Synchronisation error"},

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

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

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

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

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

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

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

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

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

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

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

   288     {0x0032, "PLL error"},

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

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

   291     {0x0042, "MBOX EOE"},

   292     {0x0043, "MBOX COE"},

   293     {0x0044, "MBOX FOE"},

   294     {0x0045, "MBOX SOE"},

   295     {0x004F, "MBOX VOE"},

   296     {}

   297 };

   298 

   299 /*****************************************************************************/

   300 

   301 /**

   302    Change state: CODE.

   303 */

   304 

   305 void ec_fsm_change_code(ec_fsm_change_t *fsm /**< finite state machine */)

   306 {

   307     ec_datagram_t *datagram = fsm->datagram;

   308     ec_slave_t *slave = fsm->slave;

   309     uint32_t code;

   310     const ec_code_msg_t *al_msg;

   311 

   312     if (datagram->state != EC_DATAGRAM_RECEIVED

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

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

   315     }

   316     else {

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

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

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

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

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

   322                 break;

   323             }

   324             if (!al_msg->code)

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

   326         }

   327     }

   328 

   329     // acknowledge "old" slave state

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

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

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

   333     fsm->state = ec_fsm_change_ack;

   334 }

   335 

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

   337 

   338 /**

   339    Change state: ACK.

   340 */

   341 

   342 void ec_fsm_change_ack(ec_fsm_change_t *fsm /**< finite state machine */)

   343 {

   344     ec_datagram_t *datagram = fsm->datagram;

   345     ec_slave_t *slave = fsm->slave;

   346 

   347     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   349         fsm->state = ec_fsm_change_error;

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

   351         return;

   352     }

   353 

   354     fsm->take_time = 1;

   355 

   356     // read new AL status

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

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

   359     fsm->state = ec_fsm_change_check_ack;

   360 }

   361 

   362 /*****************************************************************************/

   363 

   364 /**

   365    Change state: CHECK ACK.

   366 */

   367 

   368 void ec_fsm_change_check_ack(ec_fsm_change_t *fsm /**< finite state machine */)

   369 {

   370     ec_datagram_t *datagram = fsm->datagram;

   371     ec_slave_t *slave = fsm->slave;

   372 

   373     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   375         fsm->state = ec_fsm_change_error;

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

   377         return;

   378     }

   379 

   380     if (fsm->take_time) {

   381         fsm->take_time = 0;

   382         fsm->jiffies_start = datagram->jiffies_sent;

   383     }

   384 

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

   386 

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

   388         fsm->state = ec_fsm_change_error;

   389         EC_INFO("Acknowledged state 0x%02X on slave %i.\n",

   390                 slave->current_state, slave->ring_position);

   391         return;

   392     }

   393 

   394     if (datagram->jiffies_received

   395         - fsm->jiffies_start >= 100 * HZ / 1000) { // 100ms

   396         // timeout while checking

   397         slave->current_state = EC_SLAVE_STATE_UNKNOWN;

   398         fsm->state = ec_fsm_change_error;

   399         EC_ERR("Timeout while acknowledging state 0x%02X on slave %i.\n",

   400                fsm->requested_state, slave->ring_position);

   401         return;

   402     }

   403 

   404     // reread new AL status

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

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

   407 }

   408 

   409 /*****************************************************************************/

   410 

   411 /**

   412    State: ERROR.

   413 */

   414 

   415 void ec_fsm_change_error(ec_fsm_change_t *fsm /**< finite state machine */)

   416 {

   417 }

   418 

   419 /*****************************************************************************/

   420 

   421 /**

   422    State: END.

   423 */

   424 

   425 void ec_fsm_change_end(ec_fsm_change_t *fsm /**< finite state machine */)

   426 {

   427 }

   428 

   429 /*****************************************************************************/