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 slave state machines.

    37 */

    38 

    39 /*****************************************************************************/

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "fsm_slave.h"

    45 

    46 /*****************************************************************************/

    47 

    48 void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);

    49 void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);

    50 void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *);

    51 void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *);

    52 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *);

    53 void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *);

    54 void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *);

    55 

    56 void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *);

    57 void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *);

    58 void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *);

    59 void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *);

    60 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);

    61 void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *);

    62 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);

    63 void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *);

    64 void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *);

    65 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);

    66 

    67 void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *);

    68 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);

    69 void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *);

    70 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    71 void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *);

    72 void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *);

    73 

    74 void ec_fsm_slave_state_end(ec_fsm_slave_t *);

    75 void ec_fsm_slave_state_error(ec_fsm_slave_t *);

    76 

    77 /*****************************************************************************/

    78 

    79 /**

    80    Constructor.

    81 */

    82 

    83 void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */

    84         ec_datagram_t *datagram /**< datagram structure to use */

    85         )

    86 {

    87     fsm->datagram = datagram;

    88 

    89     // init sub state machines

    90     ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);

    91     ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);

    92     ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);

    93 }

    94 

    95 /*****************************************************************************/

    96 

    97 /**

    98    Destructor.

    99 */

   100 

   101 void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */)

   102 {

   103     // clear sub state machines

   104     ec_fsm_sii_clear(&fsm->fsm_sii);

   105     ec_fsm_change_clear(&fsm->fsm_change);

   106     ec_fsm_coe_clear(&fsm->fsm_coe);

   107 }

   108 

   109 /*****************************************************************************/

   110 

   111 /**

   112  * Start slave scan state machine.

   113  */

   114 

   115 void ec_fsm_slave_start_scan(ec_fsm_slave_t *fsm, /**< slave state machine */

   116         ec_slave_t *slave /**< slave to configure */

   117         )

   118 {

   119     fsm->slave = slave;

   120     fsm->state = ec_fsm_slave_scan_state_start;

   121 }

   122 

   123 /*****************************************************************************/

   124 

   125 /**

   126  * Start slave configuration state machine.

   127  */

   128 

   129 void ec_fsm_slave_start_conf(ec_fsm_slave_t *fsm, /**< slave state machine */

   130         ec_slave_t *slave /**< slave to configure */

   131         )

   132 {

   133     fsm->slave = slave;

   134     fsm->state = ec_fsm_slave_conf_state_start;

   135 }

   136 

   137 /*****************************************************************************/

   138 

   139 /**

   140    \return false, if state machine has terminated

   141 */

   142 

   143 int ec_fsm_slave_running(const ec_fsm_slave_t *fsm /**< slave state machine */)

   144 {

   145     return fsm->state != ec_fsm_slave_state_end

   146         && fsm->state != ec_fsm_slave_state_error;

   147 }

   148 

   149 /*****************************************************************************/

   150 

   151 /**

   152    Executes the current state of the state machine.

   153    If the state machine's datagram is not sent or received yet, the execution

   154    of the state machine is delayed to the next cycle.

   155    \return false, if state machine has terminated

   156 */

   157 

   158 int ec_fsm_slave_exec(ec_fsm_slave_t *fsm /**< slave state machine */)

   159 {

   160     if (fsm->datagram->state == EC_DATAGRAM_SENT

   161         || fsm->datagram->state == EC_DATAGRAM_QUEUED) {

   162         // datagram was not sent or received yet.

   163         return ec_fsm_slave_running(fsm);

   164     }

   165 

   166     fsm->state(fsm);

   167     return ec_fsm_slave_running(fsm);

   168 }

   169 

   170 /*****************************************************************************/

   171 

   172 /**

   173    \return true, if the state machine terminated gracefully

   174 */

   175 

   176 int ec_fsm_slave_success(const ec_fsm_slave_t *fsm /**< slave state machine */)

   177 {

   178     return fsm->state == ec_fsm_slave_state_end;

   179 }

   180 

   181 /******************************************************************************

   182  *  slave scan state machine

   183  *****************************************************************************/

   184 

   185 /**

   186    Slave scan state: START.

   187    First state of the slave state machine. Writes the station address to the

   188    slave, according to its ring position.

   189 */

   190 

   191 void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *fsm /**< slave state machine */)

   192 {

   193     // write station address

   194     ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2);

   195     EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address);

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

   197     fsm->retries = EC_FSM_RETRIES;

   198     fsm->state = ec_fsm_slave_scan_state_address;

   199 }

   200 

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

   202 

   203 /**

   204    Slave scan state: ADDRESS.

   205 */

   206 

   207 void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *fsm /**< slave state machine */)

   208 {

   209     ec_datagram_t *datagram = fsm->datagram;

   210 

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

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

   213         return;

   214     }

   215 

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

   217         fsm->state = ec_fsm_slave_state_error;

   218         EC_ERR("Failed to receive station address datagram for slave %i.\n",

   219                fsm->slave->ring_position);

   220         return;

   221     }

   222 

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

   224         fsm->slave->error_flag = 1;

   225         fsm->state = ec_fsm_slave_state_error;

   226         EC_ERR("Failed to write station address - slave %i did not respond.\n",

   227                fsm->slave->ring_position);

   228         return;

   229     }

   230 

   231     // Read AL state

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

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

   234     fsm->retries = EC_FSM_RETRIES;

   235     fsm->state = ec_fsm_slave_scan_state_state;

   236 }

   237 

   238 /*****************************************************************************/

   239 

   240 /**

   241    Slave scan state: STATE.

   242 */

   243 

   244 void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *fsm /**< slave state machine */)

   245 {

   246     ec_datagram_t *datagram = fsm->datagram;

   247     ec_slave_t *slave = fsm->slave;

   248 

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

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

   251         return;

   252     }

   253 

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

   255         fsm->state = ec_fsm_slave_state_error;

   256         EC_ERR("Failed to receive AL state datagram from slave %i.\n",

   257                fsm->slave->ring_position);

   258         return;

   259     }

   260 

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

   262         fsm->slave->error_flag = 1;

   263         fsm->state = ec_fsm_slave_state_error;

   264         EC_ERR("Failed to read AL state - slave %i did not respond.\n",

   265                fsm->slave->ring_position);

   266         return;

   267     }

   268 

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

   270     if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   271         char state_str[EC_STATE_STRING_SIZE];

   272         ec_state_string(slave->current_state, state_str);

   273         EC_WARN("Slave %i has state error bit set (%s)!\n",

   274                 slave->ring_position, state_str);

   275     }

   276 

   277     // read base data

   278     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6);

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

   280     fsm->retries = EC_FSM_RETRIES;

   281     fsm->state = ec_fsm_slave_scan_state_base;

   282 }

   283 

   284 /*****************************************************************************/

   285 

   286 /**

   287    Slave scan state: BASE.

   288 */

   289 

   290 void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *fsm /**< slave state machine */)

   291 {

   292     ec_datagram_t *datagram = fsm->datagram;

   293     ec_slave_t *slave = fsm->slave;

   294 

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

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

   297         return;

   298     }

   299 

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

   301         fsm->state = ec_fsm_slave_state_error;

   302         EC_ERR("Failed to receive base data datagram for slave %i.\n",

   303                slave->ring_position);

   304         return;

   305     }

   306 

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

   308         fsm->slave->error_flag = 1;

   309         fsm->state = ec_fsm_slave_state_error;

   310         EC_ERR("Failed to read base data - slave %i did not respond.\n",

   311                slave->ring_position);

   312         return;

   313     }

   314 

   315     slave->base_type       = EC_READ_U8 (datagram->data);

   316     slave->base_revision   = EC_READ_U8 (datagram->data + 1);

   317     slave->base_build      = EC_READ_U16(datagram->data + 2);

   318     slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4);

   319     slave->base_sync_count = EC_READ_U8 (datagram->data + 5);

   320 

   321     if (slave->base_fmmu_count > EC_MAX_FMMUS)

   322         slave->base_fmmu_count = EC_MAX_FMMUS;

   323 

   324     // read data link status

   325     ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2);

   326     ec_master_queue_datagram(slave->master, datagram);

   327     fsm->retries = EC_FSM_RETRIES;

   328     fsm->state = ec_fsm_slave_scan_state_datalink;

   329 }

   330 

   331 /*****************************************************************************/

   332 

   333 /**

   334    Slave scan state: DATALINK.

   335 */

   336 

   337 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *fsm /**< slave state machine */)

   338 {

   339     ec_datagram_t *datagram = fsm->datagram;

   340     ec_slave_t *slave = fsm->slave;

   341     uint16_t dl_status;

   342     unsigned int i;

   343 

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

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

   346         return;

   347     }

   348 

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

   350         fsm->state = ec_fsm_slave_state_error;

   351         EC_ERR("Failed to receive DL status datagram from slave %i.\n",

   352                slave->ring_position);

   353         return;

   354     }

   355 

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

   357         fsm->slave->error_flag = 1;

   358         fsm->state = ec_fsm_slave_state_error;

   359         EC_ERR("Failed to read DL status - slave %i did not respond.\n",

   360                slave->ring_position);

   361         return;

   362     }

   363 

   364     dl_status = EC_READ_U16(datagram->data);

   365     for (i = 0; i < 4; i++) {

   366         slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;

   367         slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;

   368         slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;

   369     }

   370 

   371     // Start fetching EEPROM size

   372 

   373     fsm->sii_offset = 0x0040; // first category header

   374     ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

   375     fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   376     fsm->state(fsm); // execute state immediately

   377 }

   378 

   379 /*****************************************************************************/

   380 

   381 /**

   382    Slave scan state: EEPROM SIZE.

   383 */

   384 

   385 void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *fsm /**< slave state machine */)

   386 {

   387     ec_slave_t *slave = fsm->slave;

   388     uint16_t cat_type, cat_size;

   389 

   390     if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   391 

   392     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   393         fsm->slave->error_flag = 1;

   394         fsm->state = ec_fsm_slave_state_error;

   395         EC_ERR("Failed to read EEPROM size of slave %i.\n",

   396                slave->ring_position);

   397         return;

   398     }

   399 

   400     cat_type = EC_READ_U16(fsm->fsm_sii.value);

   401     cat_size = EC_READ_U16(fsm->fsm_sii.value + 2);

   402 

   403     if (cat_type != 0xFFFF) { // not the last category

   404         fsm->sii_offset += cat_size + 2;

   405         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   406                         EC_FSM_SII_NODE);

   407         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   408         return;

   409     }

   410 

   411     slave->eeprom_size = (fsm->sii_offset + 1) * 2;

   412 

   413     if (slave->eeprom_data) {

   414         EC_INFO("Freeing old EEPROM data on slave %i...\n",

   415                 slave->ring_position);

   416         kfree(slave->eeprom_data);

   417     }

   418 

   419     if (!(slave->eeprom_data =

   420           (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) {

   421         fsm->slave->error_flag = 1;

   422         fsm->state = ec_fsm_slave_state_error;

   423         EC_ERR("Failed to allocate EEPROM data on slave %i.\n",

   424                slave->ring_position);

   425         return;

   426     }

   427 

   428     // Start fetching EEPROM contents

   429 

   430     fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   431     fsm->sii_offset = 0x0000;

   432     ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

   433     ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   434 }

   435 

   436 /*****************************************************************************/

   437 

   438 /**

   439    Slave scan state: EEPROM DATA.

   440 */

   441 

   442 void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *fsm /**< slave state machine */)

   443 {

   444     ec_slave_t *slave = fsm->slave;

   445     uint16_t *cat_word, cat_type, cat_size;

   446 

   447     if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   448 

   449     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   450         fsm->slave->error_flag = 1;

   451         fsm->state = ec_fsm_slave_state_error;

   452         EC_ERR("Failed to fetch EEPROM contents of slave %i.\n",

   453                slave->ring_position);

   454         return;

   455     }

   456 

   457     // 2 words fetched

   458 

   459     if (fsm->sii_offset + 2 <= slave->eeprom_size / 2) { // 2 words fit

   460         memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   461                fsm->fsm_sii.value, 4);

   462     }

   463     else { // copy the last word

   464         memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   465                fsm->fsm_sii.value, 2);

   466     }

   467 

   468     if (fsm->sii_offset + 2 < slave->eeprom_size / 2) {

   469         // fetch the next 2 words

   470         fsm->sii_offset += 2;

   471         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   472                         EC_FSM_SII_NODE);

   473         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   474         return;

   475     }

   476 

   477     // Evaluate EEPROM contents

   478 

   479     slave->sii_alias =

   480         EC_READ_U16(slave->eeprom_data + 2 * 0x0004);

   481     slave->sii_vendor_id =

   482         EC_READ_U32(slave->eeprom_data + 2 * 0x0008);

   483     slave->sii_product_code =

   484         EC_READ_U32(slave->eeprom_data + 2 * 0x000A);

   485     slave->sii_revision_number =

   486         EC_READ_U32(slave->eeprom_data + 2 * 0x000C);

   487     slave->sii_serial_number =

   488         EC_READ_U32(slave->eeprom_data + 2 * 0x000E);

   489     slave->sii_rx_mailbox_offset =

   490         EC_READ_U16(slave->eeprom_data + 2 * 0x0018);

   491     slave->sii_rx_mailbox_size =

   492         EC_READ_U16(slave->eeprom_data + 2 * 0x0019);

   493     slave->sii_tx_mailbox_offset =

   494         EC_READ_U16(slave->eeprom_data + 2 * 0x001A);

   495     slave->sii_tx_mailbox_size =

   496         EC_READ_U16(slave->eeprom_data + 2 * 0x001B);

   497     slave->sii_mailbox_protocols =

   498         EC_READ_U16(slave->eeprom_data + 2 * 0x001C);

   499 

   500     // evaluate category data

   501     cat_word = (uint16_t *) slave->eeprom_data + 0x0040;

   502     while (EC_READ_U16(cat_word) != 0xFFFF) {

   503         cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   504         cat_size = EC_READ_U16(cat_word + 1);

   505 

   506         switch (cat_type) {

   507             case 0x000A:

   508                 if (ec_slave_fetch_strings(slave, (uint8_t *) (cat_word + 2)))

   509                     goto end;

   510                 break;

   511             case 0x001E:

   512                 ec_slave_fetch_general(slave, (uint8_t *) (cat_word + 2));

   513                 break;

   514             case 0x0028:

   515                 break;

   516             case 0x0029:

   517                 if (ec_slave_fetch_sync(slave, (uint8_t *) (cat_word + 2),

   518                                         cat_size))

   519                     goto end;

   520                 break;

   521             case 0x0032:

   522                 if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2),

   523                                        cat_size, EC_TX_PDO))

   524                     goto end;

   525                 break;

   526             case 0x0033:

   527                 if (ec_slave_fetch_pdo(slave, (uint8_t *) (cat_word + 2),

   528                                        cat_size, EC_RX_PDO))

   529                     goto end;

   530                 break;

   531             default:

   532                 if (fsm->slave->master->debug_level)

   533                     EC_WARN("Unknown category type 0x%04X in slave %i.\n",

   534                             cat_type, slave->ring_position);

   535         }

   536 

   537         cat_word += cat_size + 2;

   538     }

   539 

   540     fsm->state = ec_fsm_slave_state_end;

   541     return;

   542 

   543 end:

   544     EC_ERR("Failed to analyze category data.\n");

   545     fsm->slave->error_flag = 1;

   546     fsm->state = ec_fsm_slave_state_error;

   547 }

   548 

   549 /******************************************************************************

   550  *  slave configuration state machine

   551  *****************************************************************************/

   552 

   553 /**

   554    Slave configuration state: START.

   555 */

   556 

   557 void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *fsm /**< slave state machine */)

   558 {

   559     if (fsm->slave->master->debug_level) {

   560         EC_DBG("Configuring slave %i...\n", fsm->slave->ring_position);

   561     }

   562 

   563     ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);

   564     ec_fsm_change_exec(&fsm->fsm_change);

   565     fsm->state = ec_fsm_slave_conf_state_init;

   566 }

   567 

   568 /*****************************************************************************/

   569 

   570 /**

   571    Slave configuration state: INIT.

   572 */

   573 

   574 void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *fsm /**< slave state machine */)

   575 {

   576     ec_master_t *master = fsm->slave->master;

   577     ec_slave_t *slave = fsm->slave;

   578     ec_datagram_t *datagram = fsm->datagram;

   579 

   580     if (ec_fsm_change_exec(&fsm->fsm_change)) return;

   581 

   582     if (!ec_fsm_change_success(&fsm->fsm_change)) {

   583         slave->error_flag = 1;

   584         fsm->state = ec_fsm_slave_state_error;

   585         return;

   586     }

   587 

   588     slave->self_configured = 1;

   589 

   590     if (master->debug_level) {

   591         EC_DBG("Slave %i is now in INIT.\n", slave->ring_position);

   592     }

   593 

   594     // check and reset CRC fault counters

   595     //ec_slave_check_crc(slave);

   596     // TODO: Implement state machine for CRC checking.

   597 

   598     if (!slave->base_fmmu_count) { // skip FMMU configuration

   599         ec_fsm_slave_conf_enter_sync(fsm);

   600         return;

   601     }

   602 

   603     if (master->debug_level)

   604         EC_DBG("Clearing FMMU configurations of slave %i...\n",

   605                slave->ring_position);

   606 

   607     // clear FMMU configurations

   608     ec_datagram_npwr(datagram, slave->station_address,

   609                      0x0600, EC_FMMU_SIZE * slave->base_fmmu_count);

   610     memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);

   611     ec_master_queue_datagram(master, datagram);

   612     fsm->retries = EC_FSM_RETRIES;

   613     fsm->state = ec_fsm_slave_conf_state_clear_fmmus;

   614 }

   615 

   616 /*****************************************************************************/

   617 

   618 /**

   619    Slave configuration state: CLEAR FMMU.

   620 */

   621 

   622 void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *fsm

   623                                         /**< slave state machine */)

   624 {

   625     ec_datagram_t *datagram = fsm->datagram;

   626 

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

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

   629         return;

   630     }

   631 

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

   633         fsm->state = ec_fsm_slave_state_error;

   634         EC_ERR("Failed receive FMMU clearing datagram for slave %i.\n",

   635                fsm->slave->ring_position);

   636         return;

   637     }

   638 

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

   640         fsm->slave->error_flag = 1;

   641         fsm->state = ec_fsm_slave_state_error;

   642         EC_ERR("Failed to clear FMMUs - slave %i did not respond.\n",

   643                fsm->slave->ring_position);

   644         return;

   645     }

   646 

   647     ec_fsm_slave_conf_enter_sync(fsm);

   648 }

   649 

   650 /*****************************************************************************/

   651 

   652 /**

   653 */

   654 

   655 void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *fsm /**< slave state machine */)

   656 {

   657     ec_master_t *master = fsm->slave->master;

   658     ec_slave_t *slave = fsm->slave;

   659     ec_datagram_t *datagram = fsm->datagram;

   660     const ec_sii_sync_t *sync;

   661     ec_sii_sync_t mbox_sync;

   662 

   663     // slave is now in INIT

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

   665         fsm->state = ec_fsm_slave_state_end; // successful

   666         if (master->debug_level) {

   667             EC_DBG("Finished configuration of slave %i.\n",

   668                    slave->ring_position);

   669         }

   670         return;

   671     }

   672 

   673     if (!slave->base_sync_count) { // no sync managers

   674         ec_fsm_slave_conf_enter_preop(fsm);

   675         return;

   676     }

   677 

   678     if (master->debug_level) {

   679         EC_DBG("Configuring sync managers of slave %i.\n",

   680                slave->ring_position);

   681     }

   682 

   683     // configure sync managers

   684     ec_datagram_npwr(datagram, slave->station_address, 0x0800,

   685                      EC_SYNC_SIZE * slave->base_sync_count);

   686     memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);

   687 

   688     if (list_empty(&slave->sii_syncs)) {

   689         if (slave->sii_rx_mailbox_offset && slave->sii_tx_mailbox_offset) {

   690             if (slave->master->debug_level)

   691                 EC_DBG("Guessing sync manager settings for slave %i.\n",

   692                        slave->ring_position);

   693             mbox_sync.index = 0;

   694             mbox_sync.physical_start_address = slave->sii_tx_mailbox_offset;

   695             mbox_sync.length = slave->sii_tx_mailbox_size;

   696             mbox_sync.control_register = 0x26;

   697             mbox_sync.enable = 0x01;

   698             mbox_sync.est_length = 0;

   699             ec_sync_config(&mbox_sync, slave,

   700                            datagram->data + EC_SYNC_SIZE * mbox_sync.index);

   701             mbox_sync.index = 1;

   702             mbox_sync.physical_start_address = slave->sii_rx_mailbox_offset;

   703             mbox_sync.length = slave->sii_rx_mailbox_size;

   704             mbox_sync.control_register = 0x22;

   705             mbox_sync.enable = 0x01;

   706             mbox_sync.est_length = 0;

   707             ec_sync_config(&mbox_sync, slave,

   708                            datagram->data + EC_SYNC_SIZE * mbox_sync.index);

   709         }

   710     }

   711     else if (slave->sii_mailbox_protocols) { // mailboxes present

   712         list_for_each_entry(sync, &slave->sii_syncs, list) {

   713             // only configure mailbox sync-managers

   714             if (sync->index != 0 && sync->index != 1) continue;

   715             ec_sync_config(sync, slave,

   716                            datagram->data + EC_SYNC_SIZE * sync->index);

   717         }

   718     }

   719 

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

   721     fsm->retries = EC_FSM_RETRIES;

   722     fsm->state = ec_fsm_slave_conf_state_sync;

   723 }

   724 

   725 /*****************************************************************************/

   726 

   727 /**

   728    Slave configuration state: SYNC.

   729 */

   730 

   731 void ec_fsm_slave_conf_state_sync(ec_fsm_slave_t *fsm /**< slave state machine */)

   732 {

   733     ec_datagram_t *datagram = fsm->datagram;

   734     ec_slave_t *slave = fsm->slave;

   735 

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

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

   738         return;

   739     }

   740 

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

   742         fsm->state = ec_fsm_slave_state_error;

   743         EC_ERR("Failed to receive sync manager configuration datagram for"

   744                " slave %i.\n", slave->ring_position);

   745         return;

   746     }

   747 

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

   749         slave->error_flag = 1;

   750         fsm->state = ec_fsm_slave_state_error;

   751         EC_ERR("Failed to set sync managers - slave %i did not respond.\n",

   752                slave->ring_position);

   753         return;

   754     }

   755 

   756     ec_fsm_slave_conf_enter_preop(fsm);

   757 }

   758 

   759 /*****************************************************************************/

   760 

   761 /**

   762  */

   763 

   764 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *fsm /**< slave state machine */)

   765 {

   766     fsm->state = ec_fsm_slave_conf_state_preop;

   767     ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP);

   768     ec_fsm_change_exec(&fsm->fsm_change); // execute immediately

   769 }

   770 

   771 /*****************************************************************************/

   772 

   773 /**

   774    Slave configuration state: PREOP.

   775 */

   776 

   777 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *fsm /**< slave state machine */)

   778 {

   779     ec_slave_t *slave = fsm->slave;

   780     ec_master_t *master = fsm->slave->master;

   781 

   782     if (ec_fsm_change_exec(&fsm->fsm_change)) return;

   783 

   784     if (!ec_fsm_change_success(&fsm->fsm_change)) {

   785         slave->error_flag = 1;

   786         fsm->state = ec_fsm_slave_state_error;

   787         return;

   788     }

   789 

   790     // slave is now in PREOP

   791     slave->jiffies_preop = fsm->datagram->jiffies_received;

   792 

   793     if (master->debug_level) {

   794         EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position);

   795     }

   796 

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

   798         fsm->state = ec_fsm_slave_state_end; // successful

   799         if (master->debug_level) {

   800             EC_DBG("Finished configuration of slave %i.\n",

   801                    slave->ring_position);

   802         }

   803         return;

   804     }

   805 

   806     ec_fsm_slave_conf_enter_sync2(fsm);

   807 }

   808 

   809 /*****************************************************************************/

   810 

   811 /**

   812 */

   813 

   814 void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *fsm /**< slave state machine */)

   815 {

   816     ec_slave_t *slave = fsm->slave;

   817     ec_datagram_t *datagram = fsm->datagram;

   818     ec_sii_sync_t *sync;

   819 

   820     if (list_empty(&slave->sii_syncs)) {

   821         ec_fsm_slave_conf_enter_fmmu(fsm);

   822         return;

   823     }

   824 

   825     // configure sync managers for process data

   826     ec_datagram_npwr(datagram, slave->station_address, 0x0800,

   827                      EC_SYNC_SIZE * slave->base_sync_count);

   828     memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);

   829 

   830     list_for_each_entry(sync, &slave->sii_syncs, list) {

   831         ec_sync_config(sync, slave,

   832                        datagram->data + EC_SYNC_SIZE * sync->index);

   833     }

   834 

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

   836     fsm->retries = EC_FSM_RETRIES;

   837     fsm->state = ec_fsm_slave_conf_state_sync2;

   838 }

   839 

   840 /*****************************************************************************/

   841 

   842 /**

   843    Slave configuration state: SYNC2.

   844 */

   845 

   846 void ec_fsm_slave_conf_state_sync2(ec_fsm_slave_t *fsm /**< slave state machine */)

   847 {

   848     ec_datagram_t *datagram = fsm->datagram;

   849     ec_slave_t *slave = fsm->slave;

   850 

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

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

   853         return;

   854     }

   855 

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

   857         fsm->state = ec_fsm_slave_state_error;

   858         EC_ERR("Failed to receive process data sync manager configuration"

   859                " datagram for slave %i.\n",

   860                slave->ring_position);

   861         return;

   862     }

   863 

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

   865         slave->error_flag = 1;

   866         fsm->state = ec_fsm_slave_state_error;

   867         EC_ERR("Failed to set process data sync managers - slave %i did not"

   868                " respond.\n", slave->ring_position);

   869         return;

   870     }

   871 

   872     ec_fsm_slave_conf_enter_fmmu(fsm);

   873 }

   874 

   875 /*****************************************************************************/

   876 

   877 /**

   878 */

   879 

   880 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *fsm /**< slave state machine */)

   881 {

   882     ec_slave_t *slave = fsm->slave;

   883     ec_master_t *master = slave->master;

   884     ec_datagram_t *datagram = fsm->datagram;

   885     unsigned int j;

   886 

   887     if (!slave->base_fmmu_count) { // skip FMMU configuration

   888         ec_fsm_slave_conf_enter_sdoconf(fsm);

   889         return;

   890     }

   891 

   892     // configure FMMUs

   893     ec_datagram_npwr(datagram, slave->station_address,

   894                      0x0600, EC_FMMU_SIZE * slave->base_fmmu_count);

   895     memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);

   896     for (j = 0; j < slave->fmmu_count; j++) {

   897         ec_fmmu_config(&slave->fmmus[j], slave,

   898                        datagram->data + EC_FMMU_SIZE * j);

   899     }

   900 

   901     ec_master_queue_datagram(master, datagram);

   902     fsm->retries = EC_FSM_RETRIES;

   903     fsm->state = ec_fsm_slave_conf_state_fmmu;

   904 }

   905 

   906 /*****************************************************************************/

   907 

   908 /**

   909    Slave configuration state: FMMU.

   910 */

   911 

   912 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *fsm /**< slave state machine */)

   913 {

   914     ec_datagram_t *datagram = fsm->datagram;

   915     ec_slave_t *slave = fsm->slave;

   916 

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

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

   919         return;

   920     }

   921 

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

   923         fsm->state = ec_fsm_slave_state_error;

   924         EC_ERR("Failed to receive FMMUs datagram for slave %i.\n",

   925                fsm->slave->ring_position);

   926         return;

   927     }

   928 

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

   930         fsm->slave->error_flag = 1;

   931         fsm->state = ec_fsm_slave_state_error;

   932         EC_ERR("Failed to set FMMUs - slave %i did not respond.\n",

   933                fsm->slave->ring_position);

   934         return;

   935     }

   936 

   937     // No CoE configuration to be applied? Jump to SAVEOP state.

   938     if (list_empty(&slave->sdo_confs)) { // skip SDO configuration

   939         ec_fsm_slave_conf_enter_saveop(fsm);

   940         return;

   941     }

   942 

   943     ec_fsm_slave_conf_enter_sdoconf(fsm);

   944 }

   945 

   946 /*****************************************************************************/

   947 

   948 /**

   949  */

   950 

   951 void ec_fsm_slave_conf_enter_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */)

   952 {

   953     ec_slave_t *slave = fsm->slave;

   954 

   955     if (list_empty(&slave->sdo_confs)) { // skip SDO configuration

   956         ec_fsm_slave_conf_enter_saveop(fsm);

   957         return;

   958     }

   959 

   960     // start SDO configuration

   961     fsm->state = ec_fsm_slave_conf_state_sdoconf;

   962     fsm->sdodata = list_entry(fsm->slave->sdo_confs.next, ec_sdo_data_t, list);

   963     ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);

   964     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   965 }

   966 

   967 /*****************************************************************************/

   968 

   969 /**

   970    Slave configuration state: SDOCONF.

   971 */

   972 

   973 void ec_fsm_slave_conf_state_sdoconf(ec_fsm_slave_t *fsm /**< slave state machine */)

   974 {

   975     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;

   976 

   977     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {

   978         fsm->slave->error_flag = 1;

   979         fsm->state = ec_fsm_slave_state_error;

   980         return;

   981     }

   982 

   983     // Another SDO to configure?

   984     if (fsm->sdodata->list.next != &fsm->slave->sdo_confs) {

   985         fsm->sdodata = list_entry(fsm->sdodata->list.next,

   986                                   ec_sdo_data_t, list);

   987         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);

   988         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   989         return;

   990     }

   991 

   992     // All SDOs are now configured.

   993 

   994     // set state to SAVEOP

   995     ec_fsm_slave_conf_enter_saveop(fsm);

   996 }

   997 

   998 /*****************************************************************************/

   999 

  1000 /**

  1001  */

  1002 

  1003 void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *fsm /**< slave state machine */)

  1004 {

  1005     fsm->state = ec_fsm_slave_conf_state_saveop;

  1006     ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAVEOP);

  1007     ec_fsm_change_exec(&fsm->fsm_change); // execute immediately

  1008 }

  1009 

  1010 /*****************************************************************************/

  1011 

  1012 /**

  1013    Slave configuration state: SAVEOP.

  1014 */

  1015 

  1016 void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *fsm /**< slave state machine */)

  1017 {

  1018     ec_master_t *master = fsm->slave->master;

  1019     ec_slave_t *slave = fsm->slave;

  1020 

  1021     if (ec_fsm_change_exec(&fsm->fsm_change)) return;

  1022 

  1023     if (!ec_fsm_change_success(&fsm->fsm_change)) {

  1024         fsm->slave->error_flag = 1;

  1025         fsm->state = ec_fsm_slave_state_error;

  1026         return;

  1027     }

  1028 

  1029     // slave is now in SAVEOP

  1030 

  1031     if (master->debug_level) {

  1032         EC_DBG("Slave %i is now in SAVEOP.\n", slave->ring_position);

  1033     }

  1034 

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

  1036         fsm->state = ec_fsm_slave_state_end; // successful

  1037         if (master->debug_level) {

  1038             EC_DBG("Finished configuration of slave %i.\n",

  1039                    slave->ring_position);

  1040         }

  1041         return;

  1042     }

  1043 

  1044     // set state to OP

  1045     fsm->state = ec_fsm_slave_conf_state_op;

  1046     ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP);

  1047     ec_fsm_change_exec(&fsm->fsm_change); // execute immediately

  1048 }

  1049 

  1050 /*****************************************************************************/

  1051 

  1052 /**

  1053    Slave configuration state: OP

  1054 */

  1055 

  1056 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *fsm /**< slave state machine */)

  1057 {

  1058     ec_master_t *master = fsm->slave->master;

  1059     ec_slave_t *slave = fsm->slave;

  1060 

  1061     if (ec_fsm_change_exec(&fsm->fsm_change)) return;

  1062 

  1063     if (!ec_fsm_change_success(&fsm->fsm_change)) {

  1064         slave->error_flag = 1;

  1065         fsm->state = ec_fsm_slave_state_error;

  1066         return;

  1067     }

  1068 

  1069     // slave is now in OP

  1070 

  1071     if (master->debug_level) {

  1072         EC_DBG("Slave %i is now in OP.\n", slave->ring_position);

  1073         EC_DBG("Finished configuration of slave %i.\n", slave->ring_position);

  1074     }

  1075 

  1076     fsm->state = ec_fsm_slave_state_end; // successful

  1077 }

  1078 

  1079 /******************************************************************************

  1080  *  Common state functions

  1081  *****************************************************************************/

  1082 

  1083 /**

  1084    State: ERROR.

  1085 */

  1086 

  1087 void ec_fsm_slave_state_error(ec_fsm_slave_t *fsm /**< slave state machine */)

  1088 {

  1089 }

  1090 

  1091 /*****************************************************************************/

  1092 

  1093 /**

  1094    State: END.

  1095 */

  1096 

  1097 void ec_fsm_slave_state_end(ec_fsm_slave_t *fsm /**< slave state machine */)

  1098 {

  1099 }

  1100 

  1101 /*****************************************************************************/