IgH EtherCAT Master: fsm_change.c Source File

00001 /******************************************************************************
00002  *
00003  *  $Id$
00004  *
00005  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
00006  *
00007  *  This file is part of the IgH EtherCAT Master.
00008  *
00009  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
00010  *  modify it under the terms of the GNU General Public License version 2, as
00011  *  published by the Free Software Foundation.
00012  *
00013  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
00016  *  Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License along
00019  *  with the IgH EtherCAT Master; if not, write to the Free Software
00020  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  *  ---
00023  *
00024  *  The license mentioned above concerns the source code only. Using the
00025  *  EtherCAT technology and brand is only permitted in compliance with the
00026  *  industrial property and similar rights of Beckhoff Automation GmbH.
00027  *
00028  *****************************************************************************/
00029 
00035 /*****************************************************************************/
00036 
00037 #include "globals.h"
00038 #include "master.h"
00039 #include "fsm_change.h"
00040 
00041 /*****************************************************************************/
00042 
00045 #define EC_AL_STATE_CHANGE_TIMEOUT 5
00046 
00047 /*****************************************************************************/
00048 
00049 void ec_fsm_change_state_start(ec_fsm_change_t *);
00050 void ec_fsm_change_state_check(ec_fsm_change_t *);
00051 void ec_fsm_change_state_status(ec_fsm_change_t *);
00052 void ec_fsm_change_state_start_code(ec_fsm_change_t *);
00053 void ec_fsm_change_state_code(ec_fsm_change_t *);
00054 void ec_fsm_change_state_ack(ec_fsm_change_t *);
00055 void ec_fsm_change_state_check_ack(ec_fsm_change_t *);
00056 void ec_fsm_change_state_end(ec_fsm_change_t *);
00057 void ec_fsm_change_state_error(ec_fsm_change_t *);
00058 
00059 /*****************************************************************************/
00060 
00065 void ec_fsm_change_init(ec_fsm_change_t *fsm, 
00066                         ec_datagram_t *datagram 
00067                         )
00068 {
00069     fsm->state = NULL;
00070     fsm->datagram = datagram;
00071     fsm->spontaneous_change = 0;
00072 }
00073 
00074 /*****************************************************************************/
00075 
00080 void ec_fsm_change_clear(ec_fsm_change_t *fsm )
00081 {
00082 }
00083 
00084 /*****************************************************************************/
00085 
00090 void ec_fsm_change_start(ec_fsm_change_t *fsm, 
00091                          ec_slave_t *slave, 
00092                          ec_slave_state_t state 
00093                          )
00094 {
00095     fsm->mode = EC_FSM_CHANGE_MODE_FULL;
00096     fsm->slave = slave;
00097     fsm->requested_state = state;
00098     fsm->state = ec_fsm_change_state_start;
00099 }
00100 
00101 /*****************************************************************************/
00102 
00107 void ec_fsm_change_ack(ec_fsm_change_t *fsm, 
00108                        ec_slave_t *slave 
00109                        )
00110 {
00111     fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY;
00112     fsm->slave = slave;
00113     fsm->requested_state = EC_SLAVE_STATE_UNKNOWN;
00114     fsm->state = ec_fsm_change_state_start_code;
00115 }
00116 
00117 /*****************************************************************************/
00118 
00124 int ec_fsm_change_exec(ec_fsm_change_t *fsm )
00125 {
00126     fsm->state(fsm);
00127 
00128     return fsm->state != ec_fsm_change_state_end
00129         && fsm->state != ec_fsm_change_state_error;
00130 }
00131 
00132 /*****************************************************************************/
00133 
00139 int ec_fsm_change_success(ec_fsm_change_t *fsm )
00140 {
00141     return fsm->state == ec_fsm_change_state_end;
00142 }
00143 
00144 /******************************************************************************
00145  *  state change state machine
00146  *****************************************************************************/
00147 
00152 void ec_fsm_change_state_start(ec_fsm_change_t *fsm)
00154 {
00155     ec_datagram_t *datagram = fsm->datagram;
00156     ec_slave_t *slave = fsm->slave;
00157 
00158     fsm->take_time = 1;
00159     fsm->old_state = fsm->slave->current_state;
00160 
00161     // write new state to slave
00162     ec_datagram_fpwr(datagram, slave->station_address, 0x0120, 2);
00163     EC_WRITE_U16(datagram->data, fsm->requested_state);
00164     fsm->retries = EC_FSM_RETRIES;
00165     fsm->state = ec_fsm_change_state_check;
00166 }
00167 
00168 /*****************************************************************************/
00169 
00174 void ec_fsm_change_state_check(ec_fsm_change_t *fsm)
00176 {
00177     ec_datagram_t *datagram = fsm->datagram;
00178     ec_slave_t *slave = fsm->slave;
00179 
00180     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00181         return;
00182 
00183     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00184         fsm->state = ec_fsm_change_state_error;
00185         EC_SLAVE_ERR(slave, "Failed to receive state datagram: ");
00186         ec_datagram_print_state(datagram);
00187         return;
00188     }
00189 
00190     if (fsm->take_time) {
00191         fsm->take_time = 0;
00192         fsm->jiffies_start = datagram->jiffies_sent;
00193     }
00194 
00195     if (datagram->working_counter == 0) {
00196         if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) {
00197             char state_str[EC_STATE_STRING_SIZE];
00198             ec_state_string(fsm->requested_state, state_str, 0);
00199             fsm->state = ec_fsm_change_state_error;
00200             EC_SLAVE_ERR(slave, "Failed to set state %s: ", state_str);
00201             ec_datagram_print_wc_error(datagram);
00202             return;
00203         }
00204 
00205         // repeat writing new state to slave
00206         ec_datagram_fpwr(datagram, slave->station_address, 0x0120, 2);
00207         EC_WRITE_U16(datagram->data, fsm->requested_state);
00208         fsm->retries = EC_FSM_RETRIES;
00209         return;
00210     }
00211 
00212     if (unlikely(datagram->working_counter > 1)) {
00213         char state_str[EC_STATE_STRING_SIZE];
00214         ec_state_string(fsm->requested_state, state_str, 0);
00215         fsm->state = ec_fsm_change_state_error;
00216         EC_SLAVE_ERR(slave, "Failed to set state %s: ", state_str);
00217         ec_datagram_print_wc_error(datagram);
00218         return;
00219     }
00220 
00221     fsm->take_time = 1;
00222 
00223     // read AL status from slave
00224     ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
00225     ec_datagram_zero(datagram);
00226     fsm->retries = EC_FSM_RETRIES;
00227     fsm->spontaneous_change = 0;
00228     fsm->state = ec_fsm_change_state_status;
00229 }
00230 
00231 /*****************************************************************************/
00232 
00237 void ec_fsm_change_state_status(ec_fsm_change_t *fsm)
00239 {
00240     ec_datagram_t *datagram = fsm->datagram;
00241     ec_slave_t *slave = fsm->slave;
00242 
00243     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00244         return;
00245 
00246     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00247         fsm->state = ec_fsm_change_state_error;
00248         EC_SLAVE_ERR(slave, "Failed to receive state checking datagram: ");
00249         ec_datagram_print_state(datagram);
00250         return;
00251     }
00252 
00253     if (datagram->working_counter != 1) {
00254         char req_state[EC_STATE_STRING_SIZE];
00255         ec_state_string(fsm->requested_state, req_state, 0);
00256         fsm->state = ec_fsm_change_state_error;
00257         EC_SLAVE_ERR(slave, "Failed to check state %s: ", req_state);
00258         ec_datagram_print_wc_error(datagram);
00259         return;
00260     }
00261 
00262     if (fsm->take_time) {
00263         fsm->take_time = 0;
00264         fsm->jiffies_start = datagram->jiffies_sent;
00265     }
00266 
00267     slave->current_state = EC_READ_U8(datagram->data);
00268 
00269     if (slave->current_state == fsm->requested_state) {
00270         // state has been set successfully
00271         fsm->state = ec_fsm_change_state_end;
00272         return;
00273     }
00274 
00275     if (slave->current_state != fsm->old_state) { // state changed
00276         char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE];
00277 
00278         ec_state_string(slave->current_state, cur_state, 0);
00279 
00280         if ((slave->current_state & 0x0F) != (fsm->old_state & 0x0F)) {
00281             // Slave spontaneously changed its state just before the new state
00282             // was written. Accept current state as old state and wait for
00283             // state change
00284             fsm->spontaneous_change = 1;
00285             fsm->old_state = slave->current_state;
00286             EC_SLAVE_WARN(slave, "Changed to %s in the meantime.\n",
00287                     cur_state);
00288             goto check_again;
00289         }
00290 
00291         // state change error
00292 
00293         slave->error_flag = 1;
00294         ec_state_string(fsm->requested_state, req_state, 0);
00295 
00296         EC_SLAVE_ERR(slave, "Failed to set %s state, slave refused state"
00297                 " change (%s).\n", req_state, cur_state);
00298 
00299         ec_fsm_change_state_start_code(fsm);
00300         return;
00301     }
00302 
00303     // still old state
00304 
00305     if (datagram->jiffies_received - fsm->jiffies_start >=
00306             EC_AL_STATE_CHANGE_TIMEOUT * HZ) {
00307         // timeout while checking
00308         char state_str[EC_STATE_STRING_SIZE];
00309         ec_state_string(fsm->requested_state, state_str, 0);
00310         fsm->state = ec_fsm_change_state_error;
00311         EC_SLAVE_ERR(slave, "Timeout while setting state %s.\n", state_str);
00312         return;
00313     }
00314 
00315  check_again:
00316     // no timeout yet. check again
00317     ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
00318     ec_datagram_zero(datagram);
00319     fsm->retries = EC_FSM_RETRIES;
00320 }
00321 
00322 /*****************************************************************************/
00323 
00326 void ec_fsm_change_state_start_code(
00327         ec_fsm_change_t *fsm 
00328         )
00329 {
00330     ec_slave_t *slave = fsm->slave;
00331     ec_datagram_t *datagram = fsm->datagram;
00332 
00333     // fetch AL status error code
00334     ec_datagram_fprd(datagram, slave->station_address, 0x0134, 2);
00335     ec_datagram_zero(datagram);
00336     fsm->retries = EC_FSM_RETRIES;
00337     fsm->state = ec_fsm_change_state_code;
00338 }
00339 
00340 /*****************************************************************************/
00341 
00346 const ec_code_msg_t al_status_messages[] = {
00347     {0x0000, "No error"},
00348     {0x0001, "Unspecified error"},
00349     {0x0002, "No Memory"},
00350     {0x0011, "Invalid requested state change"},
00351     {0x0012, "Unknown requested state"},
00352     {0x0013, "Bootstrap not supported"},
00353     {0x0014, "No valid firmware"},
00354     {0x0015, "Invalid mailbox configuration"},
00355     {0x0016, "Invalid mailbox configuration"},
00356     {0x0017, "Invalid sync manager configuration"},
00357     {0x0018, "No valid inputs available"},
00358     {0x0019, "No valid outputs"},
00359     {0x001A, "Synchronization error"},
00360     {0x001B, "Sync manager watchdog"},
00361     {0x001C, "Invalid sync manager types"},
00362     {0x001D, "Invalid output configuration"},
00363     {0x001E, "Invalid input configuration"},
00364     {0x001F, "Invalid watchdog configuration"},
00365     {0x0020, "Slave needs cold start"},
00366     {0x0021, "Slave needs INIT"},
00367     {0x0022, "Slave needs PREOP"},
00368     {0x0023, "Slave needs SAFEOP"},
00369     {0x0024, "Invalid Input Mapping"},
00370     {0x0025, "Invalid Output Mapping"},
00371     {0x0026, "Inconsistent Settings"},
00372     {0x0027, "Freerun not supported"},
00373     {0x0028, "Synchronization not supported"},
00374     {0x0029, "Freerun needs 3 Buffer Mode"},
00375     {0x002A, "Background Watchdog"},
00376     {0x002B, "No Valid Inputs and Outputs"},
00377     {0x002C, "Fatal Sync Error"},
00378     {0x002D, "No Sync Error"},
00379     {0x0030, "Invalid DC SYNCH configuration"},
00380     {0x0031, "Invalid DC latch configuration"},
00381     {0x0032, "PLL error"},
00382     {0x0033, "DC Sync IO Error"},
00383     {0x0034, "DC Sync Timeout Error"},
00384     {0x0035, "DC Invalid Sync Cycle Time"},
00385     {0x0036, "DC Sync0 Cycle Time"},
00386     {0x0037, "DC Sync1 Cycle Time"},
00387     {0x0041, "MBX_AOE"},
00388     {0x0042, "MBX_EOE"},
00389     {0x0043, "MBX_COE"},
00390     {0x0044, "MBX_FOE"},
00391     {0x0045, "MBX_SOE"},
00392     {0x004F, "MBX_VOE"},
00393     {0x0050, "EEPROM No Access"},
00394     {0x0051, "EEPROM Error"},
00395     {0x0060, "Slave Restarted Locally"},
00396     {0xffff}
00397 };
00398 
00399 
00400 /*****************************************************************************/
00401 
00406 void ec_fsm_change_state_code(ec_fsm_change_t *fsm)
00408 {
00409     ec_datagram_t *datagram = fsm->datagram;
00410     uint32_t code;
00411     const ec_code_msg_t *al_msg;
00412 
00413     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00414         return;
00415 
00416     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00417         fsm->state = ec_fsm_change_state_error;
00418         EC_SLAVE_ERR(fsm->slave, "Failed to receive"
00419                 " AL status code datagram: ");
00420         ec_datagram_print_state(datagram);
00421         return;
00422     }
00423 
00424     if (datagram->working_counter != 1) {
00425         EC_SLAVE_WARN(fsm->slave, "Reception of AL status code"
00426                 " datagram failed: ");
00427         ec_datagram_print_wc_error(datagram);
00428     } else {
00429         code = EC_READ_U16(datagram->data);
00430         for (al_msg = al_status_messages; al_msg->code != 0xffff; al_msg++) {
00431             if (al_msg->code != code) {
00432                 continue;
00433             }
00434 
00435             EC_SLAVE_ERR(fsm->slave, "AL status message 0x%04X: \"%s\".\n",
00436                     al_msg->code, al_msg->message);
00437             break;
00438         }
00439         if (al_msg->code == 0xffff) { /* not found in our list. */
00440             EC_SLAVE_ERR(fsm->slave, "Unknown AL status code 0x%04X.\n",
00441                     code);
00442         }
00443     }
00444 
00445     // acknowledge "old" slave state
00446     ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x0120, 2);
00447     EC_WRITE_U16(datagram->data, fsm->slave->current_state);
00448     fsm->retries = EC_FSM_RETRIES;
00449     fsm->state = ec_fsm_change_state_ack;
00450 }
00451 
00452 /*****************************************************************************/
00453 
00458 void ec_fsm_change_state_ack(ec_fsm_change_t *fsm )
00459 {
00460     ec_datagram_t *datagram = fsm->datagram;
00461     ec_slave_t *slave = fsm->slave;
00462 
00463     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00464         return;
00465 
00466     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00467         fsm->state = ec_fsm_change_state_error;
00468         EC_SLAVE_ERR(slave, "Failed to receive state ack datagram: ");
00469         ec_datagram_print_state(datagram);
00470         return;
00471     }
00472 
00473     if (datagram->working_counter != 1) {
00474         fsm->state = ec_fsm_change_state_error;
00475         EC_SLAVE_ERR(slave, "Reception of state ack datagram failed: ");
00476         ec_datagram_print_wc_error(datagram);
00477         return;
00478     }
00479 
00480     fsm->take_time = 1;
00481 
00482     // read new AL status
00483     ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
00484     ec_datagram_zero(datagram);
00485     fsm->retries = EC_FSM_RETRIES;
00486     fsm->state = ec_fsm_change_state_check_ack;
00487 }
00488 
00489 /*****************************************************************************/
00490 
00495 void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm)
00497 {
00498     ec_datagram_t *datagram = fsm->datagram;
00499     ec_slave_t *slave = fsm->slave;
00500 
00501     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00502         return;
00503 
00504     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00505         fsm->state = ec_fsm_change_state_error;
00506         EC_SLAVE_ERR(slave, "Failed to receive state ack check datagram: ");
00507         ec_datagram_print_state(datagram);
00508         return;
00509     }
00510 
00511     if (datagram->working_counter != 1) {
00512         fsm->state = ec_fsm_change_state_error;
00513         EC_SLAVE_ERR(slave, "Reception of state ack check datagram failed: ");
00514         ec_datagram_print_wc_error(datagram);
00515         return;
00516     }
00517 
00518     if (fsm->take_time) {
00519         fsm->take_time = 0;
00520         fsm->jiffies_start = datagram->jiffies_sent;
00521     }
00522 
00523     slave->current_state = EC_READ_U8(datagram->data);
00524 
00525     if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) {
00526         char state_str[EC_STATE_STRING_SIZE];
00527         ec_state_string(slave->current_state, state_str, 0);
00528         if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) {
00529             fsm->state = ec_fsm_change_state_error;
00530         }
00531         else { // EC_FSM_CHANGE_MODE_ACK_ONLY
00532             fsm->state = ec_fsm_change_state_end;
00533         }
00534         EC_SLAVE_INFO(slave, "Acknowledged state %s.\n", state_str);
00535         return;
00536     }
00537 
00538     if (datagram->jiffies_received - fsm->jiffies_start >=
00539             EC_AL_STATE_CHANGE_TIMEOUT * HZ) {
00540         // timeout while checking
00541         char state_str[EC_STATE_STRING_SIZE];
00542         ec_state_string(slave->current_state, state_str, 0);
00543         fsm->state = ec_fsm_change_state_error;
00544         EC_SLAVE_ERR(slave, "Timeout while acknowledging state %s.\n",
00545                 state_str);
00546         return;
00547     }
00548 
00549     // reread new AL status
00550     ec_datagram_fprd(datagram, slave->station_address, 0x0130, 2);
00551     ec_datagram_zero(datagram);
00552     fsm->retries = EC_FSM_RETRIES;
00553 }
00554 
00555 /*****************************************************************************/
00556 
00561 void ec_fsm_change_state_error(ec_fsm_change_t *fsm)
00563 {
00564 }
00565 
00566 /*****************************************************************************/
00567 
00572 void ec_fsm_change_state_end(ec_fsm_change_t *fsm)
00574 {
00575 }
00576 
00577 /*****************************************************************************/