master/fsm.c
changeset 505 bc443ca0077f
parent 484 9fde4a17b820
child 520 d778acff9592
equal deleted inserted replaced
504:63d11bdfe3df 505:bc443ca0077f
   137 
   137 
   138 /*****************************************************************************/
   138 /*****************************************************************************/
   139 
   139 
   140 /**
   140 /**
   141    Executes the current state of the state machine.
   141    Executes the current state of the state machine.
       
   142    If the state machine's datagram is not sent or received yet, the execution
       
   143    of the state machine is delayed to the next cycle.
   142    \return false, if state machine has terminated
   144    \return false, if state machine has terminated
   143 */
   145 */
   144 
   146 
   145 int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)
   147 int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)
   146 {
   148 {
       
   149     if (fsm->datagram.state == EC_DATAGRAM_SENT
       
   150         || fsm->datagram.state == EC_DATAGRAM_QUEUED) {
       
   151         // datagram was not sent or received yet.
       
   152         return ec_fsm_running(fsm);
       
   153     }
       
   154 
   147     fsm->master_state(fsm);
   155     fsm->master_state(fsm);
   148 
       
   149     return ec_fsm_running(fsm);
   156     return ec_fsm_running(fsm);
   150 }
   157 }
   151 
   158 
   152 /*****************************************************************************/
   159 /*****************************************************************************/
   153 
   160 
   200     ec_datagram_t *datagram = &fsm->datagram;
   207     ec_datagram_t *datagram = &fsm->datagram;
   201     unsigned int topology_change, states_change, i;
   208     unsigned int topology_change, states_change, i;
   202     ec_slave_t *slave;
   209     ec_slave_t *slave;
   203     ec_master_t *master = fsm->master;
   210     ec_master_t *master = fsm->master;
   204 
   211 
   205     if (datagram->state != EC_DATAGRAM_RECEIVED) {
   212     if (datagram->state == EC_DATAGRAM_TIMED_OUT) {
   206         if (!master->device->link_state) {
   213         // always retry
   207             fsm->master_slaves_responding = 0;
   214         ec_master_queue_datagram(fsm->master, &fsm->datagram);
   208             list_for_each_entry(slave, &master->slaves, list) {
   215         return;
   209                 slave->online = 0;
   216     }
   210             }
   217 
   211         }
   218     if (datagram->state != EC_DATAGRAM_RECEIVED) { // EC_DATAGRAM_ERROR
   212         else {
   219         // link is down
   213             EC_ERR("Failed to receive broadcast datagram.\n");
   220         fsm->master_slaves_responding = 0;
       
   221         list_for_each_entry(slave, &master->slaves, list) {
       
   222             slave->online = 0;
   214         }
   223         }
   215         fsm->master_state = ec_fsm_master_error;
   224         fsm->master_state = ec_fsm_master_error;
   216         return;
   225         return;
   217     }
   226     }
   218 
   227 
   290 
   299 
   291     // fetch state from each slave
   300     // fetch state from each slave
   292     fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
   301     fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);
   293     ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2);
   302     ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2);
   294     ec_master_queue_datagram(master, &fsm->datagram);
   303     ec_master_queue_datagram(master, &fsm->datagram);
       
   304     fsm->retries = EC_FSM_RETRIES;
   295     fsm->master_state = ec_fsm_master_read_states;
   305     fsm->master_state = ec_fsm_master_read_states;
   296 }
   306 }
   297 
   307 
   298 /*****************************************************************************/
   308 /*****************************************************************************/
   299 
   309 
   434         // process next slave
   444         // process next slave
   435         fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
   445         fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);
   436         ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address,
   446         ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address,
   437                          0x0130, 2);
   447                          0x0130, 2);
   438         ec_master_queue_datagram(master, &fsm->datagram);
   448         ec_master_queue_datagram(master, &fsm->datagram);
       
   449         fsm->retries = EC_FSM_RETRIES;
   439         fsm->master_state = ec_fsm_master_read_states;
   450         fsm->master_state = ec_fsm_master_read_states;
   440         return;
   451         return;
   441     }
   452     }
   442 
   453 
   443     // all slave states read
   454     // all slave states read
   471 void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */)
   482 void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */)
   472 {
   483 {
   473     ec_slave_t *slave = fsm->slave;
   484     ec_slave_t *slave = fsm->slave;
   474     ec_datagram_t *datagram = &fsm->datagram;
   485     ec_datagram_t *datagram = &fsm->datagram;
   475     uint8_t new_state;
   486     uint8_t new_state;
       
   487 
       
   488     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
       
   489         ec_master_queue_datagram(fsm->master, &fsm->datagram);
       
   490         return;
       
   491     }
   476 
   492 
   477     if (datagram->state != EC_DATAGRAM_RECEIVED) {
   493     if (datagram->state != EC_DATAGRAM_RECEIVED) {
   478         EC_ERR("Failed to receive AL state datagram for slave %i!\n",
   494         EC_ERR("Failed to receive AL state datagram for slave %i!\n",
   479                slave->ring_position);
   495                slave->ring_position);
   480         fsm->master_state = ec_fsm_master_error;
   496         fsm->master_state = ec_fsm_master_error;
   611 
   627 
   612     // write station address
   628     // write station address
   613     ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
   629     ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
   614     EC_WRITE_U16(datagram->data, fsm->slave->station_address);
   630     EC_WRITE_U16(datagram->data, fsm->slave->station_address);
   615     ec_master_queue_datagram(fsm->master, datagram);
   631     ec_master_queue_datagram(fsm->master, datagram);
       
   632     fsm->retries = EC_FSM_RETRIES;
   616     fsm->master_state = ec_fsm_master_rewrite_addresses;
   633     fsm->master_state = ec_fsm_master_rewrite_addresses;
   617 }
   634 }
   618 
   635 
   619 /*****************************************************************************/
   636 /*****************************************************************************/
   620 
   637 
   672                                      )
   689                                      )
   673 {
   690 {
   674     ec_slave_t *slave = fsm->slave;
   691     ec_slave_t *slave = fsm->slave;
   675     ec_datagram_t *datagram = &fsm->datagram;
   692     ec_datagram_t *datagram = &fsm->datagram;
   676 
   693 
   677     if (datagram->state != EC_DATAGRAM_RECEIVED
   694     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
   678         || datagram->working_counter != 1) {
   695         ec_master_queue_datagram(fsm->master, &fsm->datagram);
   679         EC_ERR("Failed to write station address on slave %i.\n",
   696         return;
       
   697     }
       
   698 
       
   699     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
   700         EC_ERR("Failed to receive address datagram for slave %i.\n",
   680                slave->ring_position);
   701                slave->ring_position);
       
   702         fsm->master_state = ec_fsm_master_error;
       
   703         return;
       
   704     }
       
   705 
       
   706     if (datagram->working_counter != 1) {
       
   707         EC_ERR("Failed to write station address - slave %i did not respond.\n",
       
   708                slave->ring_position);
       
   709         fsm->master_state = ec_fsm_master_error;
       
   710         return;
   681     }
   711     }
   682 
   712 
   683     if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
   713     if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?
   684         fsm->master_state = ec_fsm_master_start;
   714         fsm->master_state = ec_fsm_master_start;
   685         fsm->master_state(fsm); // execute immediately
   715         fsm->master_state(fsm); // execute immediately
   892 
   922 
   893     // write station address
   923     // write station address
   894     ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
   924     ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x0010, 2);
   895     EC_WRITE_U16(datagram->data, fsm->slave->station_address);
   925     EC_WRITE_U16(datagram->data, fsm->slave->station_address);
   896     ec_master_queue_datagram(fsm->master, datagram);
   926     ec_master_queue_datagram(fsm->master, datagram);
       
   927     fsm->retries = EC_FSM_RETRIES;
   897     fsm->slave_state = ec_fsm_slavescan_address;
   928     fsm->slave_state = ec_fsm_slavescan_address;
   898 }
   929 }
   899 
   930 
   900 /*****************************************************************************/
   931 /*****************************************************************************/
   901 
   932 
   905 
   936 
   906 void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */)
   937 void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */)
   907 {
   938 {
   908     ec_datagram_t *datagram = &fsm->datagram;
   939     ec_datagram_t *datagram = &fsm->datagram;
   909 
   940 
   910     if (datagram->state != EC_DATAGRAM_RECEIVED
   941     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
   911         || datagram->working_counter != 1) {
   942         ec_master_queue_datagram(fsm->master, &fsm->datagram);
       
   943         return;
       
   944     }
       
   945 
       
   946     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
   947         fsm->slave_state = ec_fsm_slave_state_error;
       
   948         EC_ERR("Failed to receive station address datagram for slave %i.\n",
       
   949                fsm->slave->ring_position);
       
   950         return;
       
   951     }
       
   952 
       
   953     if (datagram->working_counter != 1) {
   912         fsm->slave->error_flag = 1;
   954         fsm->slave->error_flag = 1;
   913         fsm->slave_state = ec_fsm_slave_state_error;
   955         fsm->slave_state = ec_fsm_slave_state_error;
   914         EC_ERR("Failed to write station address of slave %i.\n",
   956         EC_ERR("Failed to write station address - slave %i did not respond.\n",
   915                fsm->slave->ring_position);
   957                fsm->slave->ring_position);
   916         return;
   958         return;
   917     }
   959     }
   918 
   960 
   919     // Read AL state
   961     // Read AL state
   920     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2);
   962     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2);
   921     ec_master_queue_datagram(fsm->master, datagram);
   963     ec_master_queue_datagram(fsm->master, datagram);
       
   964     fsm->retries = EC_FSM_RETRIES;
   922     fsm->slave_state = ec_fsm_slavescan_state;
   965     fsm->slave_state = ec_fsm_slavescan_state;
   923 }
   966 }
   924 
   967 
   925 /*****************************************************************************/
   968 /*****************************************************************************/
   926 
   969 
   931 void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */)
   974 void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */)
   932 {
   975 {
   933     ec_datagram_t *datagram = &fsm->datagram;
   976     ec_datagram_t *datagram = &fsm->datagram;
   934     ec_slave_t *slave = fsm->slave;
   977     ec_slave_t *slave = fsm->slave;
   935 
   978 
   936     if (datagram->state != EC_DATAGRAM_RECEIVED
   979     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
   937         || datagram->working_counter != 1) {
   980         ec_master_queue_datagram(fsm->master, datagram);
       
   981         return;
       
   982     }
       
   983 
       
   984     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
   985         fsm->slave_state = ec_fsm_slave_state_error;
       
   986         EC_ERR("Failed to receive AL state datagram from slave %i.\n",
       
   987                fsm->slave->ring_position);
       
   988         return;
       
   989     }
       
   990 
       
   991     if (datagram->working_counter != 1) {
   938         fsm->slave->error_flag = 1;
   992         fsm->slave->error_flag = 1;
   939         fsm->slave_state = ec_fsm_slave_state_error;
   993         fsm->slave_state = ec_fsm_slave_state_error;
   940         EC_ERR("Failed to read AL state of slave %i.\n",
   994         EC_ERR("Failed to read AL state - slave %i did not respond.\n",
   941                fsm->slave->ring_position);
   995                fsm->slave->ring_position);
   942         return;
   996         return;
   943     }
   997     }
   944 
   998 
   945     slave->current_state = EC_READ_U8(datagram->data);
   999     slave->current_state = EC_READ_U8(datagram->data);
   951     }
  1005     }
   952 
  1006 
   953     // read base data
  1007     // read base data
   954     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6);
  1008     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6);
   955     ec_master_queue_datagram(fsm->master, datagram);
  1009     ec_master_queue_datagram(fsm->master, datagram);
       
  1010     fsm->retries = EC_FSM_RETRIES;
   956     fsm->slave_state = ec_fsm_slavescan_base;
  1011     fsm->slave_state = ec_fsm_slavescan_base;
   957 }
  1012 }
   958 
  1013 
   959 /*****************************************************************************/
  1014 /*****************************************************************************/
   960 
  1015 
   965 void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */)
  1020 void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */)
   966 {
  1021 {
   967     ec_datagram_t *datagram = &fsm->datagram;
  1022     ec_datagram_t *datagram = &fsm->datagram;
   968     ec_slave_t *slave = fsm->slave;
  1023     ec_slave_t *slave = fsm->slave;
   969 
  1024 
   970     if (datagram->state != EC_DATAGRAM_RECEIVED
  1025     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
   971         || datagram->working_counter != 1) {
  1026         ec_master_queue_datagram(fsm->master, datagram);
       
  1027         return;
       
  1028     }
       
  1029 
       
  1030     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
  1031         fsm->slave_state = ec_fsm_slave_state_error;
       
  1032         EC_ERR("Failed to receive base data datagram for slave %i.\n",
       
  1033                slave->ring_position);
       
  1034         return;
       
  1035     }
       
  1036 
       
  1037     if (datagram->working_counter != 1) {
   972         fsm->slave->error_flag = 1;
  1038         fsm->slave->error_flag = 1;
   973         fsm->slave_state = ec_fsm_slave_state_error;
  1039         fsm->slave_state = ec_fsm_slave_state_error;
   974         EC_ERR("Failed to read base data of slave %i.\n",
  1040         EC_ERR("Failed to read base data - slave %i did not respond.\n",
   975                slave->ring_position);
  1041                slave->ring_position);
   976         return;
  1042         return;
   977     }
  1043     }
   978 
  1044 
   979     slave->base_type       = EC_READ_U8 (datagram->data);
  1045     slave->base_type       = EC_READ_U8 (datagram->data);
   986         slave->base_fmmu_count = EC_MAX_FMMUS;
  1052         slave->base_fmmu_count = EC_MAX_FMMUS;
   987 
  1053 
   988     // read data link status
  1054     // read data link status
   989     ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2);
  1055     ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2);
   990     ec_master_queue_datagram(slave->master, datagram);
  1056     ec_master_queue_datagram(slave->master, datagram);
       
  1057     fsm->retries = EC_FSM_RETRIES;
   991     fsm->slave_state = ec_fsm_slavescan_datalink;
  1058     fsm->slave_state = ec_fsm_slavescan_datalink;
   992 }
  1059 }
   993 
  1060 
   994 /*****************************************************************************/
  1061 /*****************************************************************************/
   995 
  1062 
  1002     ec_datagram_t *datagram = &fsm->datagram;
  1069     ec_datagram_t *datagram = &fsm->datagram;
  1003     ec_slave_t *slave = fsm->slave;
  1070     ec_slave_t *slave = fsm->slave;
  1004     uint16_t dl_status;
  1071     uint16_t dl_status;
  1005     unsigned int i;
  1072     unsigned int i;
  1006 
  1073 
  1007     if (datagram->state != EC_DATAGRAM_RECEIVED
  1074     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
  1008         || datagram->working_counter != 1) {
  1075         ec_master_queue_datagram(fsm->master, datagram);
       
  1076         return;
       
  1077     }
       
  1078 
       
  1079     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
  1080         fsm->slave_state = ec_fsm_slave_state_error;
       
  1081         EC_ERR("Failed to receive DL status datagram from slave %i.\n",
       
  1082                slave->ring_position);
       
  1083         return;
       
  1084     }
       
  1085 
       
  1086     if (datagram->working_counter != 1) {
  1009         fsm->slave->error_flag = 1;
  1087         fsm->slave->error_flag = 1;
  1010         fsm->slave_state = ec_fsm_slave_state_error;
  1088         fsm->slave_state = ec_fsm_slave_state_error;
  1011         EC_ERR("Failed to read DL status of slave %i.\n",
  1089         EC_ERR("Failed to read DL status - slave %i did not respond.\n",
  1012                slave->ring_position);
  1090                slave->ring_position);
  1013         return;
  1091         return;
  1014     }
  1092     }
  1015 
  1093 
  1016     dl_status = EC_READ_U16(datagram->data);
  1094     dl_status = EC_READ_U16(datagram->data);
  1259     // clear FMMU configurations
  1337     // clear FMMU configurations
  1260     ec_datagram_npwr(datagram, slave->station_address,
  1338     ec_datagram_npwr(datagram, slave->station_address,
  1261                      0x0600, EC_FMMU_SIZE * slave->base_fmmu_count);
  1339                      0x0600, EC_FMMU_SIZE * slave->base_fmmu_count);
  1262     memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
  1340     memset(datagram->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
  1263     ec_master_queue_datagram(master, datagram);
  1341     ec_master_queue_datagram(master, datagram);
       
  1342     fsm->retries = EC_FSM_RETRIES;
  1264     fsm->slave_state = ec_fsm_slaveconf_state_clear_fmmus;
  1343     fsm->slave_state = ec_fsm_slaveconf_state_clear_fmmus;
  1265 }
  1344 }
  1266 
  1345 
  1267 /*****************************************************************************/
  1346 /*****************************************************************************/
  1268 
  1347 
  1273 void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *fsm
  1352 void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *fsm
  1274                                         /**< finite state machine */)
  1353                                         /**< finite state machine */)
  1275 {
  1354 {
  1276     ec_datagram_t *datagram = &fsm->datagram;
  1355     ec_datagram_t *datagram = &fsm->datagram;
  1277 
  1356 
  1278     if (datagram->state != EC_DATAGRAM_RECEIVED
  1357     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
  1279         || datagram->working_counter != 1) {
  1358         ec_master_queue_datagram(fsm->master, datagram);
       
  1359         return;
       
  1360     }
       
  1361 
       
  1362     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
  1363         fsm->slave_state = ec_fsm_slave_state_error;
       
  1364         EC_ERR("Failed receive FMMU clearing datagram for slave %i.\n",
       
  1365                fsm->slave->ring_position);
       
  1366         return;
       
  1367     }
       
  1368 
       
  1369     if (datagram->working_counter != 1) {
  1280         fsm->slave->error_flag = 1;
  1370         fsm->slave->error_flag = 1;
  1281         fsm->slave_state = ec_fsm_slave_state_error;
  1371         fsm->slave_state = ec_fsm_slave_state_error;
  1282         EC_ERR("Failed to clear FMMUs on slave %i.\n",
  1372         EC_ERR("Failed to clear FMMUs - slave %i did not respond.\n",
  1283                fsm->slave->ring_position);
  1373                fsm->slave->ring_position);
  1284         return;
  1374         return;
  1285     }
  1375     }
  1286 
  1376 
  1287     ec_fsm_slaveconf_enter_sync(fsm);
  1377     ec_fsm_slaveconf_enter_sync(fsm);
  1356                            datagram->data + EC_SYNC_SIZE * sync->index);
  1446                            datagram->data + EC_SYNC_SIZE * sync->index);
  1357         }
  1447         }
  1358     }
  1448     }
  1359 
  1449 
  1360     ec_master_queue_datagram(fsm->master, datagram);
  1450     ec_master_queue_datagram(fsm->master, datagram);
       
  1451     fsm->retries = EC_FSM_RETRIES;
  1361     fsm->slave_state = ec_fsm_slaveconf_state_sync;
  1452     fsm->slave_state = ec_fsm_slaveconf_state_sync;
  1362 }
  1453 }
  1363 
  1454 
  1364 /*****************************************************************************/
  1455 /*****************************************************************************/
  1365 
  1456 
  1370 void ec_fsm_slaveconf_state_sync(ec_fsm_t *fsm /**< finite state machine */)
  1461 void ec_fsm_slaveconf_state_sync(ec_fsm_t *fsm /**< finite state machine */)
  1371 {
  1462 {
  1372     ec_datagram_t *datagram = &fsm->datagram;
  1463     ec_datagram_t *datagram = &fsm->datagram;
  1373     ec_slave_t *slave = fsm->slave;
  1464     ec_slave_t *slave = fsm->slave;
  1374 
  1465 
  1375     if (datagram->state != EC_DATAGRAM_RECEIVED
  1466     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
  1376         || datagram->working_counter != 1) {
  1467         ec_master_queue_datagram(fsm->master, datagram);
       
  1468         return;
       
  1469     }
       
  1470 
       
  1471     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
  1472         fsm->slave_state = ec_fsm_slave_state_error;
       
  1473         EC_ERR("Failed to receive sync manager configuration datagram for"
       
  1474                " slave %i.\n", slave->ring_position);
       
  1475         return;
       
  1476     }
       
  1477 
       
  1478     if (datagram->working_counter != 1) {
  1377         slave->error_flag = 1;
  1479         slave->error_flag = 1;
  1378         fsm->slave_state = ec_fsm_slave_state_error;
  1480         fsm->slave_state = ec_fsm_slave_state_error;
  1379         EC_ERR("Failed to set sync managers on slave %i.\n",
  1481         EC_ERR("Failed to set sync managers - slave %i did not respond.\n",
  1380                slave->ring_position);
  1482                slave->ring_position);
  1381         return;
  1483         return;
  1382     }
  1484     }
  1383 
  1485 
  1384     ec_fsm_slaveconf_enter_preop(fsm);
  1486     ec_fsm_slaveconf_enter_preop(fsm);
  1459         ec_sync_config(sync, slave,
  1561         ec_sync_config(sync, slave,
  1460                        datagram->data + EC_SYNC_SIZE * sync->index);
  1562                        datagram->data + EC_SYNC_SIZE * sync->index);
  1461     }
  1563     }
  1462 
  1564 
  1463     ec_master_queue_datagram(fsm->master, datagram);
  1565     ec_master_queue_datagram(fsm->master, datagram);
       
  1566     fsm->retries = EC_FSM_RETRIES;
  1464     fsm->slave_state = ec_fsm_slaveconf_state_sync2;
  1567     fsm->slave_state = ec_fsm_slaveconf_state_sync2;
  1465 }
  1568 }
  1466 
  1569 
  1467 /*****************************************************************************/
  1570 /*****************************************************************************/
  1468 
  1571 
  1473 void ec_fsm_slaveconf_state_sync2(ec_fsm_t *fsm /**< finite state machine */)
  1576 void ec_fsm_slaveconf_state_sync2(ec_fsm_t *fsm /**< finite state machine */)
  1474 {
  1577 {
  1475     ec_datagram_t *datagram = &fsm->datagram;
  1578     ec_datagram_t *datagram = &fsm->datagram;
  1476     ec_slave_t *slave = fsm->slave;
  1579     ec_slave_t *slave = fsm->slave;
  1477 
  1580 
  1478     if (datagram->state != EC_DATAGRAM_RECEIVED
  1581     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
  1479         || datagram->working_counter != 1) {
  1582         ec_master_queue_datagram(fsm->master, datagram);
       
  1583         return;
       
  1584     }
       
  1585 
       
  1586     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
  1587         fsm->slave_state = ec_fsm_slave_state_error;
       
  1588         EC_ERR("Failed to receive process data sync manager configuration"
       
  1589                " datagram for slave %i.\n",
       
  1590                slave->ring_position);
       
  1591         return;
       
  1592     }
       
  1593 
       
  1594     if (datagram->working_counter != 1) {
  1480         slave->error_flag = 1;
  1595         slave->error_flag = 1;
  1481         fsm->slave_state = ec_fsm_slave_state_error;
  1596         fsm->slave_state = ec_fsm_slave_state_error;
  1482         EC_ERR("Failed to set process data sync managers on slave %i.\n",
  1597         EC_ERR("Failed to set process data sync managers - slave %i did not"
  1483                slave->ring_position);
  1598                " respond.\n", slave->ring_position);
  1484         return;
  1599         return;
  1485     }
  1600     }
  1486 
  1601 
  1487     ec_fsm_slaveconf_enter_fmmu(fsm);
  1602     ec_fsm_slaveconf_enter_fmmu(fsm);
  1488 }
  1603 }
  1512         ec_fmmu_config(&slave->fmmus[j], slave,
  1627         ec_fmmu_config(&slave->fmmus[j], slave,
  1513                        datagram->data + EC_FMMU_SIZE * j);
  1628                        datagram->data + EC_FMMU_SIZE * j);
  1514     }
  1629     }
  1515 
  1630 
  1516     ec_master_queue_datagram(master, datagram);
  1631     ec_master_queue_datagram(master, datagram);
       
  1632     fsm->retries = EC_FSM_RETRIES;
  1517     fsm->slave_state = ec_fsm_slaveconf_state_fmmu;
  1633     fsm->slave_state = ec_fsm_slaveconf_state_fmmu;
  1518 }
  1634 }
  1519 
  1635 
  1520 /*****************************************************************************/
  1636 /*****************************************************************************/
  1521 
  1637 
  1526 void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *fsm /**< finite state machine */)
  1642 void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *fsm /**< finite state machine */)
  1527 {
  1643 {
  1528     ec_datagram_t *datagram = &fsm->datagram;
  1644     ec_datagram_t *datagram = &fsm->datagram;
  1529     ec_slave_t *slave = fsm->slave;
  1645     ec_slave_t *slave = fsm->slave;
  1530 
  1646 
  1531     if (datagram->state != EC_DATAGRAM_RECEIVED
  1647     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
  1532         || datagram->working_counter != 1) {
  1648         ec_master_queue_datagram(fsm->master, datagram);
       
  1649         return;
       
  1650     }
       
  1651 
       
  1652     if (datagram->state != EC_DATAGRAM_RECEIVED) {
       
  1653         fsm->slave_state = ec_fsm_slave_state_error;
       
  1654         EC_ERR("Failed to receive FMMUs datagram for slave %i.\n",
       
  1655                fsm->slave->ring_position);
       
  1656         return;
       
  1657     }
       
  1658 
       
  1659     if (datagram->working_counter != 1) {
  1533         fsm->slave->error_flag = 1;
  1660         fsm->slave->error_flag = 1;
  1534         fsm->slave_state = ec_fsm_slave_state_error;
  1661         fsm->slave_state = ec_fsm_slave_state_error;
  1535         EC_ERR("Failed to set FMMUs on slave %i.\n",
  1662         EC_ERR("Failed to set FMMUs - slave %i did not respond.\n",
  1536                fsm->slave->ring_position);
  1663                fsm->slave->ring_position);
  1537         return;
  1664         return;
  1538     }
  1665     }
  1539 
  1666 
  1540     // No CoE configuration to be applied? Jump to SAVEOP state.
  1667     // No CoE configuration to be applied? Jump to SAVEOP state.