40 /*****************************************************************************/ |
40 /*****************************************************************************/ |
41 |
41 |
42 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
42 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
43 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
43 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
44 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
44 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
|
45 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
45 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
46 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
46 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
47 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
47 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
48 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
48 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
49 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
49 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
50 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
50 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
53 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
53 |
54 |
54 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
|
56 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
59 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
60 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
59 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
211 if (master->debug_level) { |
213 if (master->debug_level) { |
212 EC_DBG("Slave %u is now in INIT.\n", slave->ring_position); |
214 EC_DBG("Slave %u is now in INIT.\n", slave->ring_position); |
213 } |
215 } |
214 |
216 |
215 if (!slave->base_fmmu_count) { // skip FMMU configuration |
217 if (!slave->base_fmmu_count) { // skip FMMU configuration |
216 ec_fsm_slave_config_enter_mbox_sync(fsm); |
218 ec_fsm_slave_config_enter_clear_sync(fsm); |
217 return; |
219 return; |
218 } |
220 } |
219 |
221 |
220 if (master->debug_level) |
222 if (master->debug_level) |
221 EC_DBG("Clearing FMMU configurations of slave %u...\n", |
223 EC_DBG("Clearing FMMU configurations of slave %u...\n", |
251 |
253 |
252 if (datagram->working_counter != 1) { |
254 if (datagram->working_counter != 1) { |
253 fsm->slave->error_flag = 1; |
255 fsm->slave->error_flag = 1; |
254 fsm->state = ec_fsm_slave_config_state_error; |
256 fsm->state = ec_fsm_slave_config_state_error; |
255 EC_ERR("Failed to clear FMMUs of slave %u: ", |
257 EC_ERR("Failed to clear FMMUs of slave %u: ", |
|
258 fsm->slave->ring_position); |
|
259 ec_datagram_print_wc_error(datagram); |
|
260 return; |
|
261 } |
|
262 |
|
263 ec_fsm_slave_config_enter_clear_sync(fsm); |
|
264 } |
|
265 |
|
266 /*****************************************************************************/ |
|
267 |
|
268 /** Clear the sync manager configurations. |
|
269 */ |
|
270 void ec_fsm_slave_config_enter_clear_sync( |
|
271 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
272 ) |
|
273 { |
|
274 ec_slave_t *slave = fsm->slave; |
|
275 ec_datagram_t *datagram = fsm->datagram; |
|
276 size_t sync_size; |
|
277 |
|
278 if (!slave->sii.sync_count) { |
|
279 // no mailbox protocols supported |
|
280 ec_fsm_slave_config_enter_mbox_sync(fsm); |
|
281 return; |
|
282 } |
|
283 |
|
284 sync_size = EC_SYNC_PAGE_SIZE * slave->sii.sync_count; |
|
285 |
|
286 // clear sync manager configurations |
|
287 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size); |
|
288 memset(datagram->data, 0x00, sync_size); |
|
289 fsm->retries = EC_FSM_RETRIES; |
|
290 fsm->state = ec_fsm_slave_config_state_clear_sync; |
|
291 } |
|
292 |
|
293 /*****************************************************************************/ |
|
294 |
|
295 /** Slave configuration state: CLEAR SYNC. |
|
296 */ |
|
297 void ec_fsm_slave_config_state_clear_sync( |
|
298 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
299 ) |
|
300 { |
|
301 ec_datagram_t *datagram = fsm->datagram; |
|
302 |
|
303 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
304 return; |
|
305 |
|
306 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
307 fsm->state = ec_fsm_slave_config_state_error; |
|
308 EC_ERR("Failed receive sync manager clearing datagram" |
|
309 " for slave %u.\n", fsm->slave->ring_position); |
|
310 return; |
|
311 } |
|
312 |
|
313 if (datagram->working_counter != 1) { |
|
314 fsm->slave->error_flag = 1; |
|
315 fsm->state = ec_fsm_slave_config_state_error; |
|
316 EC_ERR("Failed to clear sync manager configurations of slave %u: ", |
256 fsm->slave->ring_position); |
317 fsm->slave->ring_position); |
257 ec_datagram_print_wc_error(datagram); |
318 ec_datagram_print_wc_error(datagram); |
258 return; |
319 return; |
259 } |
320 } |
260 |
321 |