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. |