44 |
44 |
45 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
45 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
46 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
46 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
47 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
47 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
48 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
48 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
|
49 void ec_fsm_slave_config_state_clear_dc_assign(ec_fsm_slave_config_t *); |
49 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
50 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
50 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
53 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
53 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
54 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
60 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
62 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
62 |
63 |
63 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
|
66 void ec_fsm_slave_config_enter_clear_dc_assign(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
284 ec_datagram_t *datagram = fsm->datagram; |
286 ec_datagram_t *datagram = fsm->datagram; |
285 size_t sync_size; |
287 size_t sync_size; |
286 |
288 |
287 if (!slave->sii.sync_count) { // FIXME use base_sync_count? |
289 if (!slave->sii.sync_count) { // FIXME use base_sync_count? |
288 // no sync managers |
290 // no sync managers |
289 ec_fsm_slave_config_enter_mbox_sync(fsm); |
291 ec_fsm_slave_config_enter_clear_dc_assign(fsm); |
290 return; |
292 return; |
291 } |
293 } |
292 |
294 |
293 if (slave->master->debug_level) |
295 if (slave->master->debug_level) |
294 EC_DBG("Clearing sync manager configurations of slave %u...\n", |
296 EC_DBG("Clearing sync manager configurations of slave %u...\n", |
325 |
327 |
326 if (datagram->working_counter != 1) { |
328 if (datagram->working_counter != 1) { |
327 fsm->slave->error_flag = 1; |
329 fsm->slave->error_flag = 1; |
328 fsm->state = ec_fsm_slave_config_state_error; |
330 fsm->state = ec_fsm_slave_config_state_error; |
329 EC_ERR("Failed to clear sync manager configurations of slave %u: ", |
331 EC_ERR("Failed to clear sync manager configurations of slave %u: ", |
|
332 fsm->slave->ring_position); |
|
333 ec_datagram_print_wc_error(datagram); |
|
334 return; |
|
335 } |
|
336 |
|
337 ec_fsm_slave_config_enter_clear_dc_assign(fsm); |
|
338 } |
|
339 |
|
340 /*****************************************************************************/ |
|
341 |
|
342 /** Clear the DC assignment. |
|
343 */ |
|
344 void ec_fsm_slave_config_enter_clear_dc_assign( |
|
345 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
346 ) |
|
347 { |
|
348 ec_slave_t *slave = fsm->slave; |
|
349 ec_datagram_t *datagram = fsm->datagram; |
|
350 |
|
351 if (!slave->base_dc_supported) { |
|
352 ec_fsm_slave_config_enter_mbox_sync(fsm); |
|
353 return; |
|
354 } |
|
355 |
|
356 if (slave->master->debug_level) |
|
357 EC_DBG("Clearing DC assignment of slave %u...\n", |
|
358 slave->ring_position); |
|
359 |
|
360 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
|
361 ec_datagram_zero(datagram); |
|
362 fsm->retries = EC_FSM_RETRIES; |
|
363 fsm->state = ec_fsm_slave_config_state_clear_dc_assign; |
|
364 } |
|
365 |
|
366 /*****************************************************************************/ |
|
367 |
|
368 /** Slave configuration state: CLEAR DC ASSIGN. |
|
369 */ |
|
370 void ec_fsm_slave_config_state_clear_dc_assign( |
|
371 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
372 ) |
|
373 { |
|
374 ec_datagram_t *datagram = fsm->datagram; |
|
375 |
|
376 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
377 return; |
|
378 |
|
379 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
380 fsm->state = ec_fsm_slave_config_state_error; |
|
381 EC_ERR("Failed receive DC assignment clearing datagram" |
|
382 " for slave %u.\n", fsm->slave->ring_position); |
|
383 return; |
|
384 } |
|
385 |
|
386 if (datagram->working_counter != 1) { |
|
387 fsm->slave->error_flag = 1; |
|
388 fsm->state = ec_fsm_slave_config_state_error; |
|
389 EC_ERR("Failed to clear DC assignment of slave %u: ", |
330 fsm->slave->ring_position); |
390 fsm->slave->ring_position); |
331 ec_datagram_print_wc_error(datagram); |
391 ec_datagram_print_wc_error(datagram); |
332 return; |
392 return; |
333 } |
393 } |
334 |
394 |