IgH EtherCAT Master: fsm_sii.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 "mailbox.h"
00039 #include "master.h"
00040 #include "fsm_sii.h"
00041 
00049 #define SII_TIMEOUT 20
00050 
00053 #define SII_INHIBIT 5
00054 
00055 //#define SII_DEBUG
00056 
00057 /*****************************************************************************/
00058 
00059 void ec_fsm_sii_state_start_reading(ec_fsm_sii_t *);
00060 void ec_fsm_sii_state_read_check(ec_fsm_sii_t *);
00061 void ec_fsm_sii_state_read_fetch(ec_fsm_sii_t *);
00062 void ec_fsm_sii_state_start_writing(ec_fsm_sii_t *);
00063 void ec_fsm_sii_state_write_check(ec_fsm_sii_t *);
00064 void ec_fsm_sii_state_write_check2(ec_fsm_sii_t *);
00065 void ec_fsm_sii_state_end(ec_fsm_sii_t *);
00066 void ec_fsm_sii_state_error(ec_fsm_sii_t *);
00067 
00068 /*****************************************************************************/
00069 
00074 void ec_fsm_sii_init(ec_fsm_sii_t *fsm, 
00075                      ec_datagram_t *datagram 
00076                      )
00077 {
00078     fsm->state = NULL;
00079     fsm->datagram = datagram;
00080 }
00081 
00082 /*****************************************************************************/
00083 
00088 void ec_fsm_sii_clear(ec_fsm_sii_t *fsm )
00089 {
00090 }
00091 
00092 /*****************************************************************************/
00093 
00098 void ec_fsm_sii_read(ec_fsm_sii_t *fsm, 
00099                      ec_slave_t *slave, 
00100                      uint16_t word_offset, 
00101                      ec_fsm_sii_addressing_t mode 
00102                      )
00103 {
00104     fsm->state = ec_fsm_sii_state_start_reading;
00105     fsm->slave = slave;
00106     fsm->word_offset = word_offset;
00107     fsm->mode = mode;
00108 }
00109 
00110 /*****************************************************************************/
00111 
00116 void ec_fsm_sii_write(ec_fsm_sii_t *fsm, 
00117                       ec_slave_t *slave, 
00118                       uint16_t word_offset, 
00119                       const uint16_t *value, 
00120                       ec_fsm_sii_addressing_t mode 
00121                       )
00122 {
00123     fsm->state = ec_fsm_sii_state_start_writing;
00124     fsm->slave = slave;
00125     fsm->word_offset = word_offset;
00126     fsm->mode = mode;
00127     memcpy(fsm->value, value, 2);
00128 }
00129 
00130 /*****************************************************************************/
00131 
00137 int ec_fsm_sii_exec(ec_fsm_sii_t *fsm )
00138 {
00139     fsm->state(fsm);
00140 
00141     return fsm->state != ec_fsm_sii_state_end
00142         && fsm->state != ec_fsm_sii_state_error;
00143 }
00144 
00145 /*****************************************************************************/
00146 
00152 int ec_fsm_sii_success(ec_fsm_sii_t *fsm )
00153 {
00154     return fsm->state == ec_fsm_sii_state_end;
00155 }
00156 
00157 /******************************************************************************
00158  * state functions
00159  *****************************************************************************/
00160 
00166 void ec_fsm_sii_state_start_reading(
00167         ec_fsm_sii_t *fsm 
00168         )
00169 {
00170     ec_datagram_t *datagram = fsm->datagram;
00171 
00172     // initiate read operation
00173     switch (fsm->mode) {
00174         case EC_FSM_SII_USE_INCREMENT_ADDRESS:
00175             ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4);
00176             break;
00177         case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
00178             ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 4);
00179             break;
00180     }
00181 
00182     EC_WRITE_U8 (datagram->data,     0x80); // two address octets
00183     EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
00184     EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
00185 
00186 #ifdef SII_DEBUG
00187     EC_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",
00188             fsm->word_offset);
00189     ec_print_data(datagram->data, 4);
00190 #endif
00191 
00192     fsm->retries = EC_FSM_RETRIES;
00193     fsm->state = ec_fsm_sii_state_read_check;
00194 }
00195 
00196 /*****************************************************************************/
00197 
00203 void ec_fsm_sii_state_read_check(
00204         ec_fsm_sii_t *fsm 
00205         )
00206 {
00207     ec_datagram_t *datagram = fsm->datagram;
00208 
00209     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00210         return;
00211 
00212     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00213         fsm->state = ec_fsm_sii_state_error;
00214         EC_SLAVE_ERR(fsm->slave, "Failed to receive SII read datagram: ");
00215         ec_datagram_print_state(datagram);
00216         return;
00217     }
00218 
00219     if (datagram->working_counter != 1) {
00220         fsm->state = ec_fsm_sii_state_error;
00221         EC_SLAVE_ERR(fsm->slave, "Reception of SII read datagram failed: ");
00222         ec_datagram_print_wc_error(datagram);
00223         return;
00224     }
00225 
00226     fsm->jiffies_start = datagram->jiffies_sent;
00227     fsm->check_once_more = 1;
00228 
00229     // issue check/fetch datagram
00230     switch (fsm->mode) {
00231         case EC_FSM_SII_USE_INCREMENT_ADDRESS:
00232             ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10);
00233             break;
00234         case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
00235             ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 10);
00236             break;
00237     }
00238 
00239     ec_datagram_zero(datagram);
00240     fsm->retries = EC_FSM_RETRIES;
00241     fsm->state = ec_fsm_sii_state_read_fetch;
00242 }
00243 
00244 /*****************************************************************************/
00245 
00250 void ec_fsm_sii_state_read_fetch(
00251         ec_fsm_sii_t *fsm 
00252         )
00253 {
00254     ec_datagram_t *datagram = fsm->datagram;
00255 
00256     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00257         return;
00258 
00259     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00260         fsm->state = ec_fsm_sii_state_error;
00261         EC_SLAVE_ERR(fsm->slave,
00262                 "Failed to receive SII check/fetch datagram: ");
00263         ec_datagram_print_state(datagram);
00264         return;
00265     }
00266 
00267     if (datagram->working_counter != 1) {
00268         fsm->state = ec_fsm_sii_state_error;
00269         EC_SLAVE_ERR(fsm->slave,
00270                 "Reception of SII check/fetch datagram failed: ");
00271         ec_datagram_print_wc_error(datagram);
00272         return;
00273     }
00274 
00275 #ifdef SII_DEBUG
00276     EC_SLAVE_DBG(fsm->slave, 0, "checking SII read state:\n");
00277     ec_print_data(datagram->data, 10);
00278 #endif
00279 
00280     if (EC_READ_U8(datagram->data + 1) & 0x20) {
00281         EC_SLAVE_ERR(fsm->slave, "Error on last command while"
00282                 " reading from SII word 0x%04x.\n", fsm->word_offset);
00283         fsm->state = ec_fsm_sii_state_error;
00284         return;
00285     }
00286 
00287     // check "busy bit"
00288     if (EC_READ_U8(datagram->data + 1) & 0x81) { /* busy bit or
00289                                                     read operation busy */
00290         // still busy... timeout?
00291         unsigned long diff_ms =
00292             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
00293         if (diff_ms >= SII_TIMEOUT) {
00294             if (fsm->check_once_more) {
00295                 fsm->check_once_more = 0;
00296             } else {
00297                 EC_SLAVE_ERR(fsm->slave, "SII: Read timeout.\n");
00298                 fsm->state = ec_fsm_sii_state_error;
00299                 return;
00300             }
00301         }
00302 
00303         // issue check/fetch datagram again
00304         fsm->retries = EC_FSM_RETRIES;
00305         return;
00306     }
00307 
00308     // SII value received.
00309     memcpy(fsm->value, datagram->data + 6, 4);
00310     fsm->state = ec_fsm_sii_state_end;
00311 }
00312 
00313 /*****************************************************************************/
00314 
00320 void ec_fsm_sii_state_start_writing(
00321         ec_fsm_sii_t *fsm 
00322         )
00323 {
00324     ec_datagram_t *datagram = fsm->datagram;
00325 
00326     // initiate write operation
00327     ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 8);
00328     EC_WRITE_U8 (datagram->data,     0x81); /* two address octets
00329                                                + enable write access */
00330     EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation
00331     EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
00332     memset(datagram->data + 4, 0x00, 2);
00333     memcpy(datagram->data + 6, fsm->value, 2);
00334 
00335 #ifdef SII_DEBUG
00336     EC_SLAVE_DBG(fsm->slave, 0, "writing SII data:\n");
00337     ec_print_data(datagram->data, 8);
00338 #endif
00339 
00340     fsm->retries = EC_FSM_RETRIES;
00341     fsm->state = ec_fsm_sii_state_write_check;
00342 }
00343 
00344 /*****************************************************************************/
00345 
00350 void ec_fsm_sii_state_write_check(
00351         ec_fsm_sii_t *fsm 
00352         )
00353 {
00354     ec_datagram_t *datagram = fsm->datagram;
00355 
00356     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00357         return;
00358 
00359     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00360         fsm->state = ec_fsm_sii_state_error;
00361         EC_SLAVE_ERR(fsm->slave, "Failed to receive SII write datagram: ");
00362         ec_datagram_print_state(datagram);
00363         return;
00364     }
00365 
00366     if (datagram->working_counter != 1) {
00367         fsm->state = ec_fsm_sii_state_error;
00368         EC_SLAVE_ERR(fsm->slave, "Reception of SII write datagram failed: ");
00369         ec_datagram_print_wc_error(datagram);
00370         return;
00371     }
00372 
00373     fsm->jiffies_start = datagram->jiffies_sent;
00374     fsm->check_once_more = 1;
00375 
00376     // issue check datagram
00377     ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 2);
00378     ec_datagram_zero(datagram);
00379     fsm->retries = EC_FSM_RETRIES;
00380     fsm->state = ec_fsm_sii_state_write_check2;
00381 }
00382 
00383 /*****************************************************************************/
00384 
00389 void ec_fsm_sii_state_write_check2(
00390         ec_fsm_sii_t *fsm 
00391         )
00392 {
00393     ec_datagram_t *datagram = fsm->datagram;
00394     unsigned long diff_ms;
00395 
00396     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
00397         return;
00398 
00399     if (datagram->state != EC_DATAGRAM_RECEIVED) {
00400         fsm->state = ec_fsm_sii_state_error;
00401         EC_SLAVE_ERR(fsm->slave,
00402                 "Failed to receive SII write check datagram: ");
00403         ec_datagram_print_state(datagram);
00404         return;
00405     }
00406 
00407     if (datagram->working_counter != 1) {
00408         fsm->state = ec_fsm_sii_state_error;
00409         EC_SLAVE_ERR(fsm->slave,
00410                 "Reception of SII write check datagram failed: ");
00411         ec_datagram_print_wc_error(datagram);
00412         return;
00413     }
00414 
00415 #ifdef SII_DEBUG
00416     EC_SLAVE_DBG(fsm->slave, 0, "checking SII write state:\n");
00417     ec_print_data(datagram->data, 2);
00418 #endif
00419 
00420     if (EC_READ_U8(datagram->data + 1) & 0x20) {
00421         EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n");
00422         fsm->state = ec_fsm_sii_state_error;
00423         return;
00424     }
00425 
00426     /* FIXME: some slaves never answer with the busy flag set...
00427      * wait a few ms for the write operation to complete. */
00428     diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
00429     if (diff_ms < SII_INHIBIT) {
00430 #ifdef SII_DEBUG
00431         EC_SLAVE_DBG(fsm->slave, 0, "too early.\n");
00432 #endif
00433         // issue check datagram again
00434         fsm->retries = EC_FSM_RETRIES;
00435         return;
00436     }
00437 
00438     if (EC_READ_U8(datagram->data + 1) & 0x82) { /* busy bit or
00439                                                     write operation busy bit */
00440         // still busy... timeout?
00441         if (diff_ms >= SII_TIMEOUT) {
00442             if (fsm->check_once_more) {
00443                 fsm->check_once_more = 0;
00444             } else {
00445                 EC_SLAVE_ERR(fsm->slave, "SII: Write timeout.\n");
00446                 fsm->state = ec_fsm_sii_state_error;
00447                 return;
00448             }
00449         }
00450 
00451         // issue check datagram again
00452         fsm->retries = EC_FSM_RETRIES;
00453         return;
00454     }
00455 
00456     if (EC_READ_U8(datagram->data + 1) & 0x40) {
00457         EC_SLAVE_ERR(fsm->slave, "SII: Write operation failed!\n");
00458         fsm->state = ec_fsm_sii_state_error;
00459         return;
00460     }
00461 
00462     // success
00463     fsm->state = ec_fsm_sii_state_end;
00464 }
00465 
00466 /*****************************************************************************/
00467 
00472 void ec_fsm_sii_state_error(
00473         ec_fsm_sii_t *fsm 
00474         )
00475 {
00476 }
00477 
00478 /*****************************************************************************/
00479 
00484 void ec_fsm_sii_state_end(
00485         ec_fsm_sii_t *fsm 
00486         )
00487 {
00488 }
00489 
00490 /*****************************************************************************/