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 configuration state machine.

    37 */

    38 

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

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "slave_config.h"

    45 #include "fsm_slave_config.h"

    46 

    47 /*****************************************************************************/

    48 

    49 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);

    50 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);

    51 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);

    52 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *);

    53 void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *);

    54 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *);

    55 void ec_fsm_slave_config_state_mapping(ec_fsm_slave_config_t *);

    56 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *);

    57 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *);

    58 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *);

    59 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *);

    60 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *);

    61 

    62 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *);

    63 void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *);

    64 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *);

    65 void ec_fsm_slave_config_enter_mapping(ec_fsm_slave_config_t *);

    66 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *);

    67 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *);

    68 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *);

    69 

    70 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *);

    71 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *);

    72 

    73 /*****************************************************************************/

    74 

    75 /** Constructor.

    76  */

    77 void ec_fsm_slave_config_init(ec_fsm_slave_config_t *fsm, /**< slave state machine */

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

    79         )

    80 {

    81     fsm->datagram = datagram;

    82 

    83     // init sub state machines

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

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

    86     ec_fsm_pdo_mapping_init(&fsm->fsm_pdo_map, &fsm->fsm_coe);

    87     ec_fsm_pdo_config_init(&fsm->fsm_pdo_conf, &fsm->fsm_coe);

    88 }

    89 

    90 /*****************************************************************************/

    91 

    92 /** Destructor.

    93  */

    94 void ec_fsm_slave_config_clear(ec_fsm_slave_config_t *fsm /**< slave state machine */)

    95 {

    96     // clear sub state machines

    97     ec_fsm_change_clear(&fsm->fsm_change);

    98     ec_fsm_coe_clear(&fsm->fsm_coe);

    99     ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_map);

   100     ec_fsm_pdo_config_clear(&fsm->fsm_pdo_conf);

   101 }

   102 

   103 /*****************************************************************************/

   104 

   105 /**

   106  * Start slave configuration state machine.

   107  */

   108 

   109 void ec_fsm_slave_config_start(ec_fsm_slave_config_t *fsm, /**< slave state machine */

   110         ec_slave_t *slave /**< slave to configure */

   111         )

   112 {

   113     fsm->slave = slave;

   114     fsm->state = ec_fsm_slave_config_state_start;

   115 }

   116 

   117 /*****************************************************************************/

   118 

   119 /**

   120    \return false, if state machine has terminated

   121 */

   122 

   123 int ec_fsm_slave_config_running(const ec_fsm_slave_config_t *fsm /**< slave state machine */)

   124 {

   125     return fsm->state != ec_fsm_slave_config_state_end

   126         && fsm->state != ec_fsm_slave_config_state_error;

   127 }

   128 

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

   130 

   131 /**

   132    Executes the current state of the state machine.

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

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

   135    \return false, if state machine has terminated

   136 */

   137 

   138 int ec_fsm_slave_config_exec(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   139 {

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

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

   142         // datagram was not sent or received yet.

   143         return ec_fsm_slave_config_running(fsm);

   144     }

   145 

   146     fsm->state(fsm);

   147     return ec_fsm_slave_config_running(fsm);

   148 }

   149 

   150 /*****************************************************************************/

   151 

   152 /**

   153    \return true, if the state machine terminated gracefully

   154 */

   155 

   156 int ec_fsm_slave_config_success(const ec_fsm_slave_config_t *fsm /**< slave state machine */)

   157 {

   158     return fsm->state == ec_fsm_slave_config_state_end;

   159 }

   160 

   161 /******************************************************************************

   162  * Slave configuration state machine

   163  *****************************************************************************/

   164 

   165 /**

   166    Slave configuration state: START.

   167 */

   168 

   169 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   170 {

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

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

   173     }

   174 

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

   176     ec_fsm_change_exec(&fsm->fsm_change);

   177     fsm->state = ec_fsm_slave_config_state_init;

   178 }

   179 

   180 /*****************************************************************************/

   181 

   182 /**

   183    Slave configuration state: INIT.

   184 */

   185 

   186 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   187 {

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

   189     ec_slave_t *slave = fsm->slave;

   190     ec_datagram_t *datagram = fsm->datagram;

   191 

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

   193 

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

   195         if (!fsm->fsm_change.spontaneous_change)

   196             slave->error_flag = 1;

   197         fsm->state = ec_fsm_slave_config_state_error;

   198         return;

   199     }

   200 

   201     slave->self_configured = 1;

   202 

   203     if (master->debug_level) {

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

   205     }

   206 

   207     // check and reset CRC fault counters

   208     //ec_slave_check_crc(slave);

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

   210 

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

   212         ec_fsm_slave_config_enter_mbox_sync(fsm);

   213         return;

   214     }

   215 

   216     if (master->debug_level)

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

   218                slave->ring_position);

   219 

   220     // clear FMMU configurations

   221     ec_datagram_fpwr(datagram, slave->station_address,

   222             0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   223     memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   224     fsm->retries = EC_FSM_RETRIES;

   225     fsm->state = ec_fsm_slave_config_state_clear_fmmus;

   226 }

   227 

   228 /*****************************************************************************/

   229 

   230 /**

   231    Slave configuration state: CLEAR FMMU.

   232 */

   233 

   234 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *fsm

   235                                         /**< slave state machine */)

   236 {

   237     ec_datagram_t *datagram = fsm->datagram;

   238 

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

   240         return;

   241 

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

   243         fsm->state = ec_fsm_slave_config_state_error;

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

   245                fsm->slave->ring_position);

   246         return;

   247     }

   248 

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

   250         fsm->slave->error_flag = 1;

   251         fsm->state = ec_fsm_slave_config_state_error;

   252         EC_ERR("Failed to clear FMMUs of slave %i: ",

   253                fsm->slave->ring_position);

   254         ec_datagram_print_wc_error(datagram);

   255         return;

   256     }

   257 

   258     ec_fsm_slave_config_enter_mbox_sync(fsm);

   259 }

   260 

   261 /*****************************************************************************/

   262 

   263 /**

   264  * Check for mailbox sync managers to be configured.

   265  */

   266 

   267 void ec_fsm_slave_config_enter_mbox_sync(

   268         ec_fsm_slave_config_t *fsm /**< slave state machine */

   269         )

   270 {

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

   272     ec_slave_t *slave = fsm->slave;

   273     ec_datagram_t *datagram = fsm->datagram;

   274     unsigned int i;

   275 

   276     // slave is now in INIT

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

   278         fsm->state = ec_fsm_slave_config_state_end; // successful

   279         if (master->debug_level) {

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

   281                    slave->ring_position);

   282         }

   283         return;

   284     }

   285 

   286     if (!slave->sii_mailbox_protocols) {

   287         // no mailbox protocols supported

   288         if (master->debug_level)

   289             EC_DBG("Slave %u does not support mailbox communication.\n",

   290                     slave->ring_position);

   291         ec_fsm_slave_config_enter_preop(fsm);

   292         return;

   293     }

   294 

   295     if (master->debug_level) {

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

   297                slave->ring_position);

   298     }

   299 

   300     if (slave->sii_sync_count >= 2) { // mailbox configuration provided

   301         ec_datagram_fpwr(datagram, slave->station_address, 0x0800,

   302                 EC_SYNC_PAGE_SIZE * slave->sii_sync_count);

   303         memset(datagram->data, 0x00,

   304                 EC_SYNC_PAGE_SIZE * slave->sii_sync_count);

   305 

   306         for (i = 0; i < 2; i++) {

   307             ec_sync_config(&slave->sii_syncs[i], slave->sii_syncs[i].length,

   308                     datagram->data + EC_SYNC_PAGE_SIZE * i);

   309         }

   310     } else { // no mailbox sync manager configurations provided

   311         ec_sync_t sync;

   312 

   313         if (master->debug_level)

   314             EC_DBG("Slave %u does not provide"

   315                     " mailbox sync manager configurations.\n",

   316                     slave->ring_position);

   317 

   318         ec_datagram_fpwr(datagram, slave->station_address, 0x0800,

   319                 EC_SYNC_PAGE_SIZE * 2);

   320         memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * 2);

   321 

   322         ec_sync_init(&sync, slave, 0);

   323         sync.physical_start_address = slave->sii_rx_mailbox_offset;

   324         sync.control_register = 0x26;

   325         sync.enable = 1;

   326         ec_sync_config(&sync, slave->sii_rx_mailbox_size,

   327                 datagram->data + EC_SYNC_PAGE_SIZE * sync.index);

   328 

   329         ec_sync_init(&sync, slave, 1);

   330         sync.physical_start_address = slave->sii_tx_mailbox_offset;

   331         sync.control_register = 0x22;

   332         sync.enable = 1;

   333         ec_sync_config(&sync, slave->sii_tx_mailbox_size,

   334                 datagram->data + EC_SYNC_PAGE_SIZE * sync.index);

   335     }

   336 

   337     fsm->retries = EC_FSM_RETRIES;

   338     fsm->state = ec_fsm_slave_config_state_mbox_sync;

   339 }

   340 

   341 /*****************************************************************************/

   342 

   343 /**

   344    Slave configuration state: SYNC.

   345 */

   346 

   347 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   348 {

   349     ec_datagram_t *datagram = fsm->datagram;

   350     ec_slave_t *slave = fsm->slave;

   351 

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

   353         return;

   354 

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

   356         fsm->state = ec_fsm_slave_config_state_error;

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

   358                " slave %i (datagram state %i).\n",

   359                slave->ring_position, datagram->state);

   360         return;

   361     }

   362 

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

   364         slave->error_flag = 1;

   365         fsm->state = ec_fsm_slave_config_state_error;

   366         EC_ERR("Failed to set sync managers of slave %i: ",

   367                slave->ring_position);

   368         ec_datagram_print_wc_error(datagram);

   369         return;

   370     }

   371 

   372     ec_fsm_slave_config_enter_preop(fsm);

   373 }

   374 

   375 /*****************************************************************************/

   376 

   377 /**

   378  * Request PREOP state.

   379  */

   380 

   381 void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   382 {

   383     fsm->state = ec_fsm_slave_config_state_preop;

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

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

   386 }

   387 

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

   389 

   390 /**

   391    Slave configuration state: PREOP.

   392 */

   393 

   394 void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   395 {

   396     ec_slave_t *slave = fsm->slave;

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

   398 

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

   400 

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

   402         if (!fsm->fsm_change.spontaneous_change)

   403             slave->error_flag = 1;

   404         fsm->state = ec_fsm_slave_config_state_error;

   405         return;

   406     }

   407 

   408     // slave is now in PREOP

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

   410 

   411     if (master->debug_level) {

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

   413     }

   414 

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

   416         fsm->state = ec_fsm_slave_config_state_end; // successful

   417         if (master->debug_level) {

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

   419                    slave->ring_position);

   420         }

   421         return;

   422     }

   423 

   424     ec_fsm_slave_config_enter_sdo_conf(fsm);

   425 }

   426 

   427 /*****************************************************************************/

   428 

   429 /**

   430  * Check for Sdo configurations to be applied.

   431  */

   432 

   433 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   434 {

   435     ec_slave_t *slave = fsm->slave;

   436 

   437     if (!slave->config) {

   438         EC_DBG("Slave %u is not configured.\n", slave->ring_position);

   439         ec_fsm_slave_config_enter_safeop(fsm);

   440         return;

   441     }

   442 

   443     // No CoE configuration to be applied?

   444     if (list_empty(&slave->config->sdo_configs)) { // skip Sdo configuration

   445         ec_fsm_slave_config_enter_mapping(fsm);

   446         return;

   447     }

   448 

   449     // start Sdo configuration

   450     fsm->state = ec_fsm_slave_config_state_sdo_conf;

   451     fsm->sdodata =

   452         list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list);

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

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

   455 }

   456 

   457 /*****************************************************************************/

   458 

   459 /**

   460    Slave configuration state: SDO_CONF.

   461 */

   462 

   463 void ec_fsm_slave_config_state_sdo_conf(

   464         ec_fsm_slave_config_t *fsm /**< slave state machine */

   465         )

   466 {

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

   468 

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

   470         EC_ERR("Sdo configuration failed for slave %u.\n",

   471                 fsm->slave->ring_position);

   472         fsm->slave->error_flag = 1;

   473         fsm->state = ec_fsm_slave_config_state_error;

   474         return;

   475     }

   476 

   477     // Another Sdo to configure?

   478     if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) {

   479         fsm->sdodata =

   480             list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list);

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

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

   483         return;

   484     }

   485 

   486     // All Sdos are now configured.

   487     ec_fsm_slave_config_enter_mapping(fsm);

   488 }

   489 

   490 /*****************************************************************************/

   491 

   492 /**

   493  * Check for Pdo mappings to be applied.

   494  */

   495 

   496 void ec_fsm_slave_config_enter_mapping(

   497         ec_fsm_slave_config_t *fsm /**< slave state machine */

   498         )

   499 {

   500     // start configuring Pdo mapping

   501     fsm->state = ec_fsm_slave_config_state_mapping;

   502     ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave);

   503     ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately

   504 }

   505 

   506 /*****************************************************************************/

   507 

   508 /**

   509    Slave configuration state: MAPPING.

   510 */

   511 

   512 void ec_fsm_slave_config_state_mapping(

   513         ec_fsm_slave_config_t *fsm /**< slave state machine */

   514         )

   515 {

   516     if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return;

   517 

   518     if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) {

   519         EC_ERR("Pdo mapping configuration failed for slave %u.\n",

   520                 fsm->slave->ring_position);

   521         fsm->slave->error_flag = 1;

   522         fsm->state = ec_fsm_slave_config_state_error;

   523         return;

   524     }

   525 

   526     // Start Pdo configuration

   527     fsm->state = ec_fsm_slave_config_state_pdo_conf;

   528     ec_fsm_pdo_config_start(&fsm->fsm_pdo_conf, fsm->slave);

   529     ec_fsm_pdo_config_exec(&fsm->fsm_pdo_conf); // execute immediately

   530 }

   531 

   532 /*****************************************************************************/

   533 

   534 /**

   535    Slave configuration state: PDO_CONF.

   536 */

   537 

   538 void ec_fsm_slave_config_state_pdo_conf(

   539         ec_fsm_slave_config_t *fsm /**< slave state machine */

   540         )

   541 {

   542     if (ec_fsm_pdo_config_exec(&fsm->fsm_pdo_conf)) return;

   543 

   544     if (!ec_fsm_pdo_config_success(&fsm->fsm_pdo_conf)) {

   545         EC_ERR("Pdo configuration failed for slave %u.\n",

   546                 fsm->slave->ring_position);

   547         fsm->slave->error_flag = 1;

   548         fsm->state = ec_fsm_slave_config_state_error;

   549         return;

   550     }

   551 

   552     ec_fsm_slave_config_enter_pdo_sync(fsm);

   553 }

   554 

   555 /*****************************************************************************/

   556 

   557 /**

   558  * Check for Pdo sync managers to be configured.

   559  */

   560 

   561 void ec_fsm_slave_config_enter_pdo_sync(

   562         ec_fsm_slave_config_t *fsm /**< slave state machine */

   563         )

   564 {

   565     ec_slave_t *slave = fsm->slave;

   566     ec_datagram_t *datagram = fsm->datagram;

   567     unsigned int i, offset, num_syncs;

   568     const ec_sync_t *sync;

   569     ec_direction_t dir;

   570     uint16_t size;

   571 

   572     if (!slave->sii_sync_count) {

   573         ec_fsm_slave_config_enter_fmmu(fsm);

   574         return;

   575     }

   576 

   577     if (slave->sii_mailbox_protocols) {

   578         offset = 2; // slave has mailboxes

   579     } else {

   580         offset = 0;

   581     }

   582     num_syncs = slave->sii_sync_count - offset;

   583 

   584     // configure sync managers for process data

   585     ec_datagram_fpwr(datagram, slave->station_address,

   586             0x0800 + EC_SYNC_PAGE_SIZE * offset,

   587             EC_SYNC_PAGE_SIZE * num_syncs);

   588     memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * num_syncs);

   589 

   590     for (i = 0; i < num_syncs; i++) {

   591         sync = &slave->sii_syncs[i + offset];

   592         dir = ec_sync_direction(sync);

   593         size = ec_pdo_mapping_total_size(&slave->config->mapping[dir]);

   594         ec_sync_config(sync, size, datagram->data + EC_SYNC_PAGE_SIZE * i);

   595     }

   596 

   597     fsm->retries = EC_FSM_RETRIES;

   598     fsm->state = ec_fsm_slave_config_state_pdo_sync;

   599 }

   600 

   601 /*****************************************************************************/

   602 

   603 /**

   604  * Configure Pdo sync managers.

   605  */

   606 

   607 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   608 {

   609     ec_datagram_t *datagram = fsm->datagram;

   610     ec_slave_t *slave = fsm->slave;

   611 

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

   613         return;

   614 

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

   616         fsm->state = ec_fsm_slave_config_state_error;

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

   618                " datagram for slave %i (datagram state %i).\n",

   619                slave->ring_position, datagram->state);

   620         return;

   621     }

   622 

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

   624         slave->error_flag = 1;

   625         fsm->state = ec_fsm_slave_config_state_error;

   626         EC_ERR("Failed to set process data sync managers of slave %i: ",

   627                 slave->ring_position);

   628         ec_datagram_print_wc_error(datagram);

   629         return;

   630     }

   631 

   632     ec_fsm_slave_config_enter_fmmu(fsm);

   633 }

   634 

   635 /*****************************************************************************/

   636 

   637 /**

   638  * Check for FMMUs to be configured.

   639  */

   640 

   641 void ec_fsm_slave_config_enter_fmmu(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   642 {

   643     ec_slave_t *slave = fsm->slave;

   644     ec_datagram_t *datagram = fsm->datagram;

   645     unsigned int i;

   646     const ec_fmmu_config_t *fmmu;

   647     const ec_sync_t *sync;

   648 

   649     if (slave->base_fmmu_count < slave->config->used_fmmus) {

   650         slave->error_flag = 1;

   651         fsm->state = ec_fsm_slave_config_state_error;

   652         EC_ERR("Slave %u has less FMMUs (%u) than requested (%u).\n",

   653                 slave->ring_position, slave->base_fmmu_count,

   654                 slave->config->used_fmmus);

   655         return;

   656     }

   657 

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

   659         ec_fsm_slave_config_enter_safeop(fsm);

   660         return;

   661     }

   662 

   663     // configure FMMUs

   664     ec_datagram_fpwr(datagram, slave->station_address,

   665                      0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   666     memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   667     for (i = 0; i < slave->config->used_fmmus; i++) {

   668         fmmu = &slave->config->fmmu_configs[i];

   669         if (!(sync = ec_slave_get_pdo_sync(slave, fmmu->dir))) {

   670             slave->error_flag = 1;

   671             fsm->state = ec_fsm_slave_config_state_error;

   672             EC_ERR("Failed to determine Pdo sync manager for FMMU on slave"

   673                     " %u!\n", slave->ring_position);

   674             return;

   675         }

   676         ec_fmmu_config_page(fmmu, sync,

   677                 datagram->data + EC_FMMU_PAGE_SIZE * i);

   678     }

   679 

   680     fsm->retries = EC_FSM_RETRIES;

   681     fsm->state = ec_fsm_slave_config_state_fmmu;

   682 }

   683 

   684 /*****************************************************************************/

   685 

   686 /**

   687    Slave configuration state: FMMU.

   688 */

   689 

   690 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   691 {

   692     ec_datagram_t *datagram = fsm->datagram;

   693     ec_slave_t *slave = fsm->slave;

   694 

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

   696         return;

   697 

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

   699         fsm->state = ec_fsm_slave_config_state_error;

   700         EC_ERR("Failed to receive FMMUs datagram for slave %i"

   701                 " (datagram state %i).\n",

   702                slave->ring_position, datagram->state);

   703         return;

   704     }

   705 

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

   707         slave->error_flag = 1;

   708         fsm->state = ec_fsm_slave_config_state_error;

   709         EC_ERR("Failed to set FMMUs of slave %i: ",

   710                slave->ring_position);

   711         ec_datagram_print_wc_error(datagram);

   712         return;

   713     }

   714 

   715     ec_fsm_slave_config_enter_safeop(fsm);

   716 }

   717 

   718 /*****************************************************************************/

   719 

   720 /**

   721  * Request SAFEOP state.

   722  */

   723 

   724 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   725 {

   726     fsm->state = ec_fsm_slave_config_state_safeop;

   727     ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP);

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

   729 }

   730 

   731 /*****************************************************************************/

   732 

   733 /**

   734    Slave configuration state: SAFEOP.

   735 */

   736 

   737 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   738 {

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

   740     ec_slave_t *slave = fsm->slave;

   741 

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

   743 

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

   745         if (!fsm->fsm_change.spontaneous_change)

   746             fsm->slave->error_flag = 1;

   747         fsm->state = ec_fsm_slave_config_state_error;

   748         return;

   749     }

   750 

   751     // slave is now in SAFEOP

   752 

   753     if (master->debug_level) {

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

   755     }

   756 

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

   758         fsm->state = ec_fsm_slave_config_state_end; // successful

   759         if (master->debug_level) {

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

   761                    slave->ring_position);

   762         }

   763         return;

   764     }

   765 

   766     // set state to OP

   767     fsm->state = ec_fsm_slave_config_state_op;

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

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

   770 }

   771 

   772 /*****************************************************************************/

   773 

   774 /**

   775    Slave configuration state: OP

   776 */

   777 

   778 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   779 {

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

   781     ec_slave_t *slave = fsm->slave;

   782 

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

   784 

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

   786         if (!fsm->fsm_change.spontaneous_change)

   787             slave->error_flag = 1;

   788         fsm->state = ec_fsm_slave_config_state_error;

   789         return;

   790     }

   791 

   792     // slave is now in OP

   793 

   794     if (master->debug_level) {

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

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

   797     }

   798 

   799     fsm->state = ec_fsm_slave_config_state_end; // successful

   800 }

   801 

   802 /******************************************************************************

   803  *  Common state functions

   804  *****************************************************************************/

   805 

   806 /**

   807    State: ERROR.

   808 */

   809 

   810 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   811 {

   812 }

   813 

   814 /*****************************************************************************/

   815 

   816 /**

   817    State: END.

   818 */

   819 

   820 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *fsm /**< slave state machine */)

   821 {

   822 }

   823 

   824 /*****************************************************************************/