changeset 1989 | 6aa393418fb3 |
parent 1987 | f452c93f7723 |
parent 1925 | 29161abef052 |
child 1990 | b0dcdfbd4238 |
1988:ea38efeeb7b3 | 1989:6aa393418fb3 |
---|---|
42 #include "slave_config.h" |
42 #include "slave_config.h" |
43 #include "fsm_slave_config.h" |
43 #include "fsm_slave_config.h" |
44 |
44 |
45 /*****************************************************************************/ |
45 /*****************************************************************************/ |
46 |
46 |
47 /** Time difference [ns] to tolerate without setting a new system time offset. |
47 /** Maximum clock difference (in ns) before going to SAFEOP. |
48 */ |
48 * |
49 #define EC_SYSTEM_TIME_TOLERANCE_NS 100000000 |
49 * Wait for DC time difference to drop under this absolute value before |
50 * requesting SAFEOP. |
|
51 */ |
|
52 #define EC_DC_MAX_SYNC_DIFF_NS 5000 |
|
53 |
|
54 /** Maximum time (in ms) to wait for clock discipline. |
|
55 */ |
|
56 #define EC_DC_SYNC_WAIT_MS 5000 |
|
57 |
|
58 /** Time offset (in ns), that is added to cyclic start time. |
|
59 */ |
|
60 #define EC_DC_START_OFFSET 100000000ULL |
|
50 |
61 |
51 /*****************************************************************************/ |
62 /*****************************************************************************/ |
52 |
63 |
53 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *); |
54 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_state_dc_read_offset(ec_fsm_slave_config_t *); |
|
59 void ec_fsm_slave_config_state_dc_write_offset(ec_fsm_slave_config_t *); |
|
60 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
62 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
63 void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_soe_conf(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
74 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
75 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
76 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
77 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); |
78 void ec_fsm_slave_config_state_dc_cycle(ec_fsm_slave_config_t *); |
79 void ec_fsm_slave_config_state_dc_sync_check(ec_fsm_slave_config_t *); |
|
70 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); |
80 void ec_fsm_slave_config_state_dc_start(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); |
81 void ec_fsm_slave_config_state_dc_assign(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
82 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
83 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
74 |
84 |
194 */ |
204 */ |
195 void ec_fsm_slave_config_state_start( |
205 void ec_fsm_slave_config_state_start( |
196 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
206 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
197 ) |
207 ) |
198 { |
208 { |
199 if (fsm->slave->master->debug_level) { |
209 EC_SLAVE_DBG(fsm->slave, 1, "Configuring...\n"); |
200 EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position); |
|
201 } |
|
202 |
|
203 ec_fsm_slave_config_enter_init(fsm); |
210 ec_fsm_slave_config_enter_init(fsm); |
204 } |
211 } |
205 |
212 |
206 /*****************************************************************************/ |
213 /*****************************************************************************/ |
207 |
214 |
222 */ |
229 */ |
223 void ec_fsm_slave_config_state_init( |
230 void ec_fsm_slave_config_state_init( |
224 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
231 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
225 ) |
232 ) |
226 { |
233 { |
227 ec_master_t *master = fsm->slave->master; |
|
228 ec_slave_t *slave = fsm->slave; |
234 ec_slave_t *slave = fsm->slave; |
229 ec_datagram_t *datagram = fsm->datagram; |
235 ec_datagram_t *datagram = fsm->datagram; |
230 |
236 |
231 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
237 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
232 |
238 |
235 slave->error_flag = 1; |
241 slave->error_flag = 1; |
236 fsm->state = ec_fsm_slave_config_state_error; |
242 fsm->state = ec_fsm_slave_config_state_error; |
237 return; |
243 return; |
238 } |
244 } |
239 |
245 |
240 if (master->debug_level) { |
246 EC_SLAVE_DBG(slave, 1, "Now in INIT.\n"); |
241 EC_DBG("Slave %u is now in INIT.\n", slave->ring_position); |
|
242 } |
|
243 |
247 |
244 if (!slave->base_fmmu_count) { // skip FMMU configuration |
248 if (!slave->base_fmmu_count) { // skip FMMU configuration |
245 ec_fsm_slave_config_enter_clear_sync(fsm); |
249 ec_fsm_slave_config_enter_clear_sync(fsm); |
246 return; |
250 return; |
247 } |
251 } |
248 |
252 |
249 if (master->debug_level) |
253 EC_SLAVE_DBG(slave, 1, "Clearing FMMU configurations...\n"); |
250 EC_DBG("Clearing FMMU configurations of slave %u...\n", |
|
251 slave->ring_position); |
|
252 |
254 |
253 // clear FMMU configurations |
255 // clear FMMU configurations |
254 ec_datagram_fpwr(datagram, slave->station_address, |
256 ec_datagram_fpwr(datagram, slave->station_address, |
255 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
257 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
256 ec_datagram_zero(datagram); |
258 ec_datagram_zero(datagram); |
271 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
273 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
272 return; |
274 return; |
273 |
275 |
274 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
276 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
275 fsm->state = ec_fsm_slave_config_state_error; |
277 fsm->state = ec_fsm_slave_config_state_error; |
276 EC_ERR("Failed receive FMMU clearing datagram for slave %u.\n", |
278 EC_SLAVE_ERR(fsm->slave, "Failed receive FMMU clearing datagram.\n"); |
277 fsm->slave->ring_position); |
|
278 return; |
279 return; |
279 } |
280 } |
280 |
281 |
281 if (datagram->working_counter != 1) { |
282 if (datagram->working_counter != 1) { |
282 fsm->slave->error_flag = 1; |
283 fsm->slave->error_flag = 1; |
283 fsm->state = ec_fsm_slave_config_state_error; |
284 fsm->state = ec_fsm_slave_config_state_error; |
284 EC_ERR("Failed to clear FMMUs of slave %u: ", |
285 EC_SLAVE_ERR(fsm->slave, "Failed to clear FMMUs: "); |
285 fsm->slave->ring_position); |
|
286 ec_datagram_print_wc_error(datagram); |
286 ec_datagram_print_wc_error(datagram); |
287 return; |
287 return; |
288 } |
288 } |
289 |
289 |
290 ec_fsm_slave_config_enter_clear_sync(fsm); |
290 ec_fsm_slave_config_enter_clear_sync(fsm); |
306 // no sync managers |
306 // no sync managers |
307 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
307 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
308 return; |
308 return; |
309 } |
309 } |
310 |
310 |
311 if (slave->master->debug_level) |
311 EC_SLAVE_DBG(slave, 1, "Clearing sync manager configurations...\n"); |
312 EC_DBG("Clearing sync manager configurations of slave %u...\n", |
|
313 slave->ring_position); |
|
314 |
312 |
315 sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count; |
313 sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count; |
316 |
314 |
317 // clear sync manager configurations |
315 // clear sync manager configurations |
318 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size); |
316 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size); |
334 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
332 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
335 return; |
333 return; |
336 |
334 |
337 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
335 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
338 fsm->state = ec_fsm_slave_config_state_error; |
336 fsm->state = ec_fsm_slave_config_state_error; |
339 EC_ERR("Failed receive sync manager clearing datagram" |
337 EC_SLAVE_ERR(fsm->slave, "Failed receive sync manager" |
340 " for slave %u.\n", fsm->slave->ring_position); |
338 " clearing datagram.\n"); |
341 return; |
339 return; |
342 } |
340 } |
343 |
341 |
344 if (datagram->working_counter != 1) { |
342 if (datagram->working_counter != 1) { |
345 fsm->slave->error_flag = 1; |
343 fsm->slave->error_flag = 1; |
346 fsm->state = ec_fsm_slave_config_state_error; |
344 fsm->state = ec_fsm_slave_config_state_error; |
347 EC_ERR("Failed to clear sync manager configurations of slave %u: ", |
345 EC_SLAVE_ERR(fsm->slave, |
348 fsm->slave->ring_position); |
346 "Failed to clear sync manager configurations: "); |
349 ec_datagram_print_wc_error(datagram); |
347 ec_datagram_print_wc_error(datagram); |
350 return; |
348 return; |
351 } |
349 } |
352 |
350 |
353 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
351 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
367 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
365 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
368 ec_fsm_slave_config_enter_mbox_sync(fsm); |
366 ec_fsm_slave_config_enter_mbox_sync(fsm); |
369 return; |
367 return; |
370 } |
368 } |
371 |
369 |
372 if (slave->master->debug_level) |
370 EC_SLAVE_DBG(slave, 1, "Clearing DC assignment...\n"); |
373 EC_DBG("Clearing DC assignment of slave %u...\n", |
|
374 slave->ring_position); |
|
375 |
371 |
376 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
372 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
377 ec_datagram_zero(datagram); |
373 ec_datagram_zero(datagram); |
378 fsm->retries = EC_FSM_RETRIES; |
374 fsm->retries = EC_FSM_RETRIES; |
379 fsm->state = ec_fsm_slave_config_state_dc_clear_assign; |
375 fsm->state = ec_fsm_slave_config_state_dc_clear_assign; |
392 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
388 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
393 return; |
389 return; |
394 |
390 |
395 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
391 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
396 fsm->state = ec_fsm_slave_config_state_error; |
392 fsm->state = ec_fsm_slave_config_state_error; |
397 EC_ERR("Failed receive DC assignment clearing datagram" |
393 EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment" |
398 " for slave %u.\n", fsm->slave->ring_position); |
394 " clearing datagram.\n"); |
399 return; |
395 return; |
400 } |
396 } |
401 |
397 |
402 if (fsm->slave->master->debug_level && datagram->working_counter != 1) { |
398 if (datagram->working_counter != 1) { |
403 // clearing the DC assignment does not succeed on simple slaves |
399 // clearing the DC assignment does not succeed on simple slaves |
404 EC_DBG("Failed to clear DC assignment of slave %u: ", |
400 EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: "); |
405 fsm->slave->ring_position); |
|
406 ec_datagram_print_wc_error(datagram); |
401 ec_datagram_print_wc_error(datagram); |
407 } |
402 } |
408 |
403 |
409 // read DC system time (0x0910, 64 bit) |
|
410 // gap (64 bit) |
|
411 // and time offset (0x0920, 64 bit) |
|
412 ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); |
|
413 fsm->retries = EC_FSM_RETRIES; |
|
414 fsm->state = ec_fsm_slave_config_state_dc_read_offset; |
|
415 } |
|
416 |
|
417 /*****************************************************************************/ |
|
418 |
|
419 /** Configure 32 bit time offset. |
|
420 */ |
|
421 u64 ec_fsm_slave_config_dc_offset32( |
|
422 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
|
423 u64 system_time, /**< System time register. */ |
|
424 u64 old_offset, /**< Time offset register. */ |
|
425 u64 correction /**< Correction. */ |
|
426 ) |
|
427 { |
|
428 ec_slave_t *slave = fsm->slave; |
|
429 u32 correction32, system_time32, old_offset32, new_offset; |
|
430 s32 time_diff; |
|
431 |
|
432 system_time32 = (u32) system_time; |
|
433 old_offset32 = (u32) old_offset; |
|
434 |
|
435 // correct read system time by elapsed time since read operation |
|
436 correction32 = (u32)correction; |
|
437 system_time32 -= correction32; |
|
438 time_diff = (u32) slave->master->app_time - system_time32; |
|
439 |
|
440 if (slave->master->debug_level) |
|
441 EC_DBG("Slave %u: system_time=%u (corrected with %u)," |
|
442 " app_time=%u, diff=%i\n", |
|
443 slave->ring_position, system_time32, correction32, |
|
444 (u32) slave->master->app_time, time_diff); |
|
445 |
|
446 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
|
447 new_offset = time_diff + old_offset32; |
|
448 if (slave->master->debug_level) |
|
449 EC_DBG("Slave %u: Setting time offset to %u (was %u)\n", |
|
450 slave->ring_position, new_offset, old_offset32); |
|
451 return (u64) new_offset; |
|
452 } else { |
|
453 if (slave->master->debug_level) |
|
454 EC_DBG("Slave %u: Not touching time offset.\n", |
|
455 slave->ring_position); |
|
456 return old_offset; |
|
457 } |
|
458 } |
|
459 |
|
460 /*****************************************************************************/ |
|
461 |
|
462 /** Configure 64 bit time offset. |
|
463 */ |
|
464 u64 ec_fsm_slave_config_dc_offset64( |
|
465 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
|
466 u64 system_time, /**< System time register. */ |
|
467 u64 old_offset, /**< Time offset register. */ |
|
468 u64 correction /**< Correction. */ |
|
469 ) |
|
470 { |
|
471 ec_slave_t *slave = fsm->slave; |
|
472 u64 new_offset; |
|
473 s64 time_diff; |
|
474 |
|
475 system_time -= correction; |
|
476 time_diff = fsm->slave->master->app_time - system_time; |
|
477 |
|
478 if (slave->master->debug_level) |
|
479 EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," |
|
480 " app_time=%llu, diff=%lli\n", |
|
481 slave->ring_position, system_time, correction, |
|
482 slave->master->app_time, time_diff); |
|
483 |
|
484 |
|
485 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
|
486 new_offset = time_diff + old_offset; |
|
487 if (slave->master->debug_level) |
|
488 EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n", |
|
489 slave->ring_position, new_offset, old_offset); |
|
490 } else { |
|
491 new_offset = old_offset; |
|
492 if (slave->master->debug_level) |
|
493 EC_DBG("Slave %u: Not touching time offset.\n", |
|
494 slave->ring_position); |
|
495 } |
|
496 |
|
497 return new_offset; |
|
498 } |
|
499 |
|
500 /*****************************************************************************/ |
|
501 |
|
502 /** Slave configuration state: DC READ OFFSET. |
|
503 */ |
|
504 void ec_fsm_slave_config_state_dc_read_offset( |
|
505 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
506 ) |
|
507 { |
|
508 ec_datagram_t *datagram = fsm->datagram; |
|
509 ec_slave_t *slave = fsm->slave; |
|
510 u64 system_time, old_offset, new_offset, correction; |
|
511 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
512 return; |
|
513 |
|
514 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
515 fsm->state = ec_fsm_slave_config_state_error; |
|
516 EC_ERR("Failed to receive DC times datagram for slave %u: ", |
|
517 slave->ring_position); |
|
518 ec_datagram_print_state(datagram); |
|
519 return; |
|
520 } |
|
521 |
|
522 if (datagram->working_counter != 1) { |
|
523 slave->error_flag = 1; |
|
524 fsm->state = ec_fsm_slave_config_state_error; |
|
525 EC_ERR("Failed to get DC times of slave %u: ", |
|
526 slave->ring_position); |
|
527 ec_datagram_print_wc_error(datagram); |
|
528 return; |
|
529 } |
|
530 |
|
531 system_time = EC_READ_U64(datagram->data); // 0x0910 |
|
532 old_offset = EC_READ_U64(datagram->data + 16); // 0x0920 |
|
533 /* correct read system time by elapsed time since read operation |
|
534 and the app_time set time */ |
|
535 #ifdef EC_HAVE_CYCLES |
|
536 correction = |
|
537 (datagram->cycles_sent - slave->master->dc_cycles_app_time) |
|
538 * 1000000LL; |
|
539 do_div(correction,cpu_khz); |
|
540 #else |
|
541 correction = |
|
542 (u64) ((datagram->jiffies_sent-slave->master->dc_jiffies_app_time) * 1000 / HZ) |
|
543 * 1000000; |
|
544 #endif |
|
545 |
|
546 if (slave->base_dc_range == EC_DC_32) { |
|
547 new_offset = ec_fsm_slave_config_dc_offset32(fsm, |
|
548 system_time, old_offset, correction); |
|
549 } else { |
|
550 new_offset = ec_fsm_slave_config_dc_offset64(fsm, |
|
551 system_time, old_offset, correction); |
|
552 } |
|
553 |
|
554 // set DC system time offset and transmission delay |
|
555 ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12); |
|
556 EC_WRITE_U64(datagram->data, new_offset); |
|
557 EC_WRITE_U32(datagram->data + 8, slave->transmission_delay); |
|
558 fsm->retries = EC_FSM_RETRIES; |
|
559 fsm->state = ec_fsm_slave_config_state_dc_write_offset; |
|
560 } |
|
561 |
|
562 /*****************************************************************************/ |
|
563 |
|
564 /** Slave configuration state: DC WRITE OFFSET. |
|
565 */ |
|
566 void ec_fsm_slave_config_state_dc_write_offset( |
|
567 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
568 ) |
|
569 { |
|
570 ec_datagram_t *datagram = fsm->datagram; |
|
571 ec_slave_t *slave = fsm->slave; |
|
572 |
|
573 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
574 return; |
|
575 |
|
576 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
577 fsm->state = ec_fsm_slave_config_state_error; |
|
578 EC_ERR("Failed to receive DC system time offset datagram for" |
|
579 " slave %u: ", slave->ring_position); |
|
580 ec_datagram_print_state(datagram); |
|
581 return; |
|
582 } |
|
583 |
|
584 if (datagram->working_counter != 1) { |
|
585 slave->error_flag = 1; |
|
586 fsm->state = ec_fsm_slave_config_state_error; |
|
587 EC_ERR("Failed to set DC system time offset of slave %u: ", |
|
588 slave->ring_position); |
|
589 ec_datagram_print_wc_error(datagram); |
|
590 return; |
|
591 } |
|
592 |
|
593 ec_fsm_slave_config_enter_mbox_sync(fsm); |
404 ec_fsm_slave_config_enter_mbox_sync(fsm); |
594 } |
405 } |
595 |
406 |
596 /*****************************************************************************/ |
407 /*****************************************************************************/ |
597 |
408 |
599 */ |
410 */ |
600 void ec_fsm_slave_config_enter_mbox_sync( |
411 void ec_fsm_slave_config_enter_mbox_sync( |
601 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
412 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
602 ) |
413 ) |
603 { |
414 { |
604 ec_master_t *master = fsm->slave->master; |
|
605 ec_slave_t *slave = fsm->slave; |
415 ec_slave_t *slave = fsm->slave; |
606 ec_datagram_t *datagram = fsm->datagram; |
416 ec_datagram_t *datagram = fsm->datagram; |
607 unsigned int i; |
417 unsigned int i; |
608 |
418 |
609 // slave is now in INIT |
419 // slave is now in INIT |
610 if (slave->current_state == slave->requested_state) { |
420 if (slave->current_state == slave->requested_state) { |
611 fsm->state = ec_fsm_slave_config_state_end; // successful |
421 fsm->state = ec_fsm_slave_config_state_end; // successful |
612 if (master->debug_level) { |
422 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
613 EC_DBG("Finished configuration of slave %u.\n", |
|
614 slave->ring_position); |
|
615 } |
|
616 return; |
423 return; |
617 } |
424 } |
618 |
425 |
619 if (!slave->sii.mailbox_protocols) { |
426 if (!slave->sii.mailbox_protocols) { |
620 // no mailbox protocols supported |
427 // no mailbox protocols supported |
621 if (master->debug_level) |
428 EC_SLAVE_DBG(slave, 1, "Slave does not support" |
622 EC_DBG("Slave %u does not support mailbox communication.\n", |
429 " mailbox communication.\n"); |
623 slave->ring_position); |
|
624 ec_fsm_slave_config_enter_boot_preop(fsm); |
430 ec_fsm_slave_config_enter_boot_preop(fsm); |
625 return; |
431 return; |
626 } |
432 } |
627 |
433 |
628 if (master->debug_level) { |
434 EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n"); |
629 EC_DBG("Configuring mailbox sync managers of slave %u.\n", |
|
630 slave->ring_position); |
|
631 } |
|
632 |
435 |
633 if (slave->requested_state == EC_SLAVE_STATE_BOOT) { |
436 if (slave->requested_state == EC_SLAVE_STATE_BOOT) { |
634 ec_sync_t sync; |
437 ec_sync_t sync; |
635 |
438 |
636 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
439 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
682 slave->configured_tx_mailbox_size = |
485 slave->configured_tx_mailbox_size = |
683 slave->sii.syncs[1].default_length; |
486 slave->sii.syncs[1].default_length; |
684 } else { // no mailbox sync manager configurations provided |
487 } else { // no mailbox sync manager configurations provided |
685 ec_sync_t sync; |
488 ec_sync_t sync; |
686 |
489 |
687 if (master->debug_level) |
490 EC_SLAVE_DBG(slave, 1, "Slave does not provide" |
688 EC_DBG("Slave %u does not provide" |
491 " mailbox sync manager configurations.\n"); |
689 " mailbox sync manager configurations.\n", |
|
690 slave->ring_position); |
|
691 |
492 |
692 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
493 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
693 EC_SYNC_PAGE_SIZE * 2); |
494 EC_SYNC_PAGE_SIZE * 2); |
694 ec_datagram_zero(datagram); |
495 ec_datagram_zero(datagram); |
695 |
496 |
740 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
541 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
741 return; |
542 return; |
742 |
543 |
743 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
544 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
744 fsm->state = ec_fsm_slave_config_state_error; |
545 fsm->state = ec_fsm_slave_config_state_error; |
745 EC_ERR("Failed to receive sync manager configuration datagram for" |
546 EC_SLAVE_ERR(slave, "Failed to receive sync manager" |
746 " slave %u: ", slave->ring_position); |
547 " configuration datagram: "); |
747 ec_datagram_print_state(datagram); |
548 ec_datagram_print_state(datagram); |
748 return; |
549 return; |
749 } |
550 } |
750 |
551 |
751 if (fsm->take_time) { |
552 if (fsm->take_time) { |
762 unsigned long diff = datagram->jiffies_received - fsm->jiffies_start; |
563 unsigned long diff = datagram->jiffies_received - fsm->jiffies_start; |
763 |
564 |
764 if (diff >= HZ) { |
565 if (diff >= HZ) { |
765 slave->error_flag = 1; |
566 slave->error_flag = 1; |
766 fsm->state = ec_fsm_slave_config_state_error; |
567 fsm->state = ec_fsm_slave_config_state_error; |
767 EC_ERR("Timeout while configuring mailbox sync managers of" |
568 EC_SLAVE_ERR(slave, "Timeout while configuring" |
768 " slave %u.\n", slave->ring_position); |
569 " mailbox sync managers.\n"); |
769 return; |
570 return; |
770 } |
571 } else { |
771 else if (slave->master->debug_level) { |
572 EC_SLAVE_DBG(slave, 1, "Resending after %u ms...\n", |
772 EC_DBG("Resending after %u ms...\n", |
|
773 (unsigned int) diff * 1000 / HZ); |
573 (unsigned int) diff * 1000 / HZ); |
774 } |
574 } |
775 |
575 |
776 // send configuration datagram again |
576 // send configuration datagram again |
777 fsm->retries = EC_FSM_RETRIES; |
577 fsm->retries = EC_FSM_RETRIES; |
778 return; |
578 return; |
779 } |
579 } |
780 else if (datagram->working_counter != 1) { |
580 else if (datagram->working_counter != 1) { |
781 slave->error_flag = 1; |
581 slave->error_flag = 1; |
782 fsm->state = ec_fsm_slave_config_state_error; |
582 fsm->state = ec_fsm_slave_config_state_error; |
783 EC_ERR("Failed to set sync managers of slave %u: ", |
583 EC_SLAVE_ERR(slave, "Failed to set sync managers: "); |
784 slave->ring_position); |
|
785 ec_datagram_print_wc_error(datagram); |
584 ec_datagram_print_wc_error(datagram); |
786 return; |
585 return; |
787 } |
586 } |
788 |
587 |
789 ec_fsm_slave_config_enter_boot_preop(fsm); |
588 ec_fsm_slave_config_enter_boot_preop(fsm); |
815 void ec_fsm_slave_config_state_boot_preop( |
614 void ec_fsm_slave_config_state_boot_preop( |
816 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
615 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
817 ) |
616 ) |
818 { |
617 { |
819 ec_slave_t *slave = fsm->slave; |
618 ec_slave_t *slave = fsm->slave; |
820 ec_master_t *master = fsm->slave->master; |
|
821 |
619 |
822 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
620 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
823 |
621 |
824 if (!ec_fsm_change_success(fsm->fsm_change)) { |
622 if (!ec_fsm_change_success(fsm->fsm_change)) { |
825 if (!fsm->fsm_change->spontaneous_change) |
623 if (!fsm->fsm_change->spontaneous_change) |
829 } |
627 } |
830 |
628 |
831 // slave is now in BOOT or PREOP |
629 // slave is now in BOOT or PREOP |
832 slave->jiffies_preop = fsm->datagram->jiffies_received; |
630 slave->jiffies_preop = fsm->datagram->jiffies_received; |
833 |
631 |
834 if (master->debug_level) { |
632 EC_SLAVE_DBG(slave, 1, "Now in %s.\n", |
835 EC_DBG("Slave %u is now in %s.\n", slave->ring_position, |
633 slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT"); |
836 slave->requested_state != EC_SLAVE_STATE_BOOT |
|
837 ? "PREOP" : "BOOT"); |
|
838 } |
|
839 |
634 |
840 if (slave->current_state == slave->requested_state) { |
635 if (slave->current_state == slave->requested_state) { |
841 fsm->state = ec_fsm_slave_config_state_end; // successful |
636 fsm->state = ec_fsm_slave_config_state_end; // successful |
842 if (master->debug_level) { |
637 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
843 EC_DBG("Finished configuration of slave %u.\n", |
|
844 slave->ring_position); |
|
845 } |
|
846 return; |
638 return; |
847 } |
639 } |
848 |
640 |
849 ec_fsm_slave_config_enter_sdo_conf(fsm); |
641 ec_fsm_slave_config_enter_sdo_conf(fsm); |
850 } |
642 } |
889 ) |
681 ) |
890 { |
682 { |
891 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
683 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
892 |
684 |
893 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
685 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
894 EC_ERR("SDO configuration failed for slave %u.\n", |
686 EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); |
895 fsm->slave->ring_position); |
|
896 fsm->slave->error_flag = 1; |
687 fsm->slave->error_flag = 1; |
897 fsm->state = ec_fsm_slave_config_state_error; |
688 fsm->state = ec_fsm_slave_config_state_error; |
898 return; |
689 return; |
899 } |
690 } |
900 |
691 |
966 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
757 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
967 return; |
758 return; |
968 } |
759 } |
969 |
760 |
970 if (!ec_fsm_soe_success(fsm_soe)) { |
761 if (!ec_fsm_soe_success(fsm_soe)) { |
971 EC_ERR("SoE configuration failed for slave %u.\n", |
762 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
972 fsm->slave->ring_position); |
|
973 fsm->slave->error_flag = 1; |
763 fsm->slave->error_flag = 1; |
974 fsm->state = ec_fsm_slave_config_state_error; |
764 fsm->state = ec_fsm_slave_config_state_error; |
975 return; |
765 return; |
976 } |
766 } |
977 |
767 |
1027 ec_fsm_slave_config_reconfigure(fsm); |
817 ec_fsm_slave_config_reconfigure(fsm); |
1028 return; |
818 return; |
1029 } |
819 } |
1030 |
820 |
1031 if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { |
821 if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { |
1032 EC_WARN("PDO configuration failed on slave %u.\n", |
822 EC_SLAVE_WARN(fsm->slave, "PDO configuration failed.\n"); |
1033 fsm->slave->ring_position); |
|
1034 } |
823 } |
1035 |
824 |
1036 ec_fsm_slave_config_enter_watchdog_divider(fsm); |
825 ec_fsm_slave_config_enter_watchdog_divider(fsm); |
1037 } |
826 } |
1038 |
827 |
1044 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
833 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1045 ) |
834 ) |
1046 { |
835 { |
1047 ec_slave_t *slave = fsm->slave; |
836 ec_slave_t *slave = fsm->slave; |
1048 ec_datagram_t *datagram = fsm->datagram; |
837 ec_datagram_t *datagram = fsm->datagram; |
1049 ec_master_t *master = slave->master; |
|
1050 ec_slave_config_t *config = slave->config; |
838 ec_slave_config_t *config = slave->config; |
1051 |
839 |
1052 if (config && config->watchdog_divider) { |
840 if (config && config->watchdog_divider) { |
1053 if (master->debug_level) |
841 EC_SLAVE_DBG(slave, 1, "Setting watchdog divider to %u.\n", |
1054 EC_DBG("Setting watchdog divider to %u.\n", |
842 config->watchdog_divider); |
1055 config->watchdog_divider); |
|
1056 |
843 |
1057 ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2); |
844 ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2); |
1058 EC_WRITE_U16(datagram->data, config->watchdog_divider); |
845 EC_WRITE_U16(datagram->data, config->watchdog_divider); |
1059 fsm->retries = EC_FSM_RETRIES; |
846 fsm->retries = EC_FSM_RETRIES; |
1060 fsm->state = ec_fsm_slave_config_state_watchdog_divider; |
847 fsm->state = ec_fsm_slave_config_state_watchdog_divider; |
1077 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
864 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1078 return; |
865 return; |
1079 |
866 |
1080 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
867 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1081 fsm->state = ec_fsm_slave_config_state_error; |
868 fsm->state = ec_fsm_slave_config_state_error; |
1082 EC_ERR("Failed to receive watchdog divider configuration datagram for" |
869 EC_SLAVE_ERR(slave, "Failed to receive watchdog divider" |
1083 " slave %u: ", slave->ring_position); |
870 " configuration datagram: "); |
1084 ec_datagram_print_state(datagram); |
871 ec_datagram_print_state(datagram); |
1085 return; |
872 return; |
1086 } |
873 } |
1087 |
874 |
1088 if (datagram->working_counter != 1) { |
875 if (datagram->working_counter != 1) { |
1089 slave->error_flag = 1; |
876 slave->error_flag = 1; |
1090 EC_WARN("Failed to set watchdog divider of slave %u: ", |
877 EC_SLAVE_WARN(slave, "Failed to set watchdog divider: "); |
1091 slave->ring_position); |
|
1092 ec_datagram_print_wc_error(datagram); |
878 ec_datagram_print_wc_error(datagram); |
1093 return; |
879 return; |
1094 } |
880 } |
1095 |
881 |
1096 ec_fsm_slave_config_enter_watchdog(fsm); |
882 ec_fsm_slave_config_enter_watchdog(fsm); |
1107 ec_datagram_t *datagram = fsm->datagram; |
893 ec_datagram_t *datagram = fsm->datagram; |
1108 ec_slave_t *slave = fsm->slave; |
894 ec_slave_t *slave = fsm->slave; |
1109 ec_slave_config_t *config = slave->config; |
895 ec_slave_config_t *config = slave->config; |
1110 |
896 |
1111 if (config && config->watchdog_intervals) { |
897 if (config && config->watchdog_intervals) { |
1112 if (slave->master->debug_level) |
898 EC_SLAVE_DBG(slave, 1, "Setting process data" |
1113 EC_DBG("Setting process data watchdog intervals to %u.\n", |
899 " watchdog intervals to %u.\n", config->watchdog_intervals); |
1114 config->watchdog_intervals); |
|
1115 |
900 |
1116 ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2); |
901 ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2); |
1117 EC_WRITE_U16(datagram->data, config->watchdog_intervals); |
902 EC_WRITE_U16(datagram->data, config->watchdog_intervals); |
1118 |
903 |
1119 fsm->retries = EC_FSM_RETRIES; |
904 fsm->retries = EC_FSM_RETRIES; |
1138 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
923 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1139 return; |
924 return; |
1140 |
925 |
1141 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
926 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1142 fsm->state = ec_fsm_slave_config_state_error; |
927 fsm->state = ec_fsm_slave_config_state_error; |
1143 EC_ERR("Failed to receive sync manager watchdog configuration " |
928 EC_SLAVE_ERR(slave, "Failed to receive sync manager" |
1144 "datagram for slave %u: ", slave->ring_position); |
929 " watchdog configuration datagram: "); |
1145 ec_datagram_print_state(datagram); |
930 ec_datagram_print_state(datagram); |
1146 return; |
931 return; |
1147 } |
932 } |
1148 |
933 |
1149 if (datagram->working_counter != 1) { |
934 if (datagram->working_counter != 1) { |
1150 EC_WARN("Failed to set process data watchdog intervals of slave %u: ", |
935 EC_SLAVE_WARN(slave, "Failed to set process data" |
1151 slave->ring_position); |
936 " watchdog intervals: "); |
1152 ec_datagram_print_wc_error(datagram); |
937 ec_datagram_print_wc_error(datagram); |
1153 } |
938 } |
1154 |
939 |
1155 ec_fsm_slave_config_enter_pdo_sync(fsm); |
940 ec_fsm_slave_config_enter_pdo_sync(fsm); |
1156 } |
941 } |
1225 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1010 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1226 return; |
1011 return; |
1227 |
1012 |
1228 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1013 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1229 fsm->state = ec_fsm_slave_config_state_error; |
1014 fsm->state = ec_fsm_slave_config_state_error; |
1230 EC_ERR("Failed to receive process data sync manager configuration" |
1015 EC_SLAVE_ERR(slave, "Failed to receive process data sync" |
1231 " datagram for slave %u: ", slave->ring_position); |
1016 " manager configuration datagram: "); |
1232 ec_datagram_print_state(datagram); |
1017 ec_datagram_print_state(datagram); |
1233 return; |
1018 return; |
1234 } |
1019 } |
1235 |
1020 |
1236 if (datagram->working_counter != 1) { |
1021 if (datagram->working_counter != 1) { |
1237 slave->error_flag = 1; |
1022 slave->error_flag = 1; |
1238 fsm->state = ec_fsm_slave_config_state_error; |
1023 fsm->state = ec_fsm_slave_config_state_error; |
1239 EC_ERR("Failed to set process data sync managers of slave %u: ", |
1024 EC_SLAVE_ERR(slave, "Failed to set process data sync managers: "); |
1240 slave->ring_position); |
|
1241 ec_datagram_print_wc_error(datagram); |
1025 ec_datagram_print_wc_error(datagram); |
1242 return; |
1026 return; |
1243 } |
1027 } |
1244 |
1028 |
1245 ec_fsm_slave_config_enter_fmmu(fsm); |
1029 ec_fsm_slave_config_enter_fmmu(fsm); |
1265 } |
1049 } |
1266 |
1050 |
1267 if (slave->base_fmmu_count < slave->config->used_fmmus) { |
1051 if (slave->base_fmmu_count < slave->config->used_fmmus) { |
1268 slave->error_flag = 1; |
1052 slave->error_flag = 1; |
1269 fsm->state = ec_fsm_slave_config_state_error; |
1053 fsm->state = ec_fsm_slave_config_state_error; |
1270 EC_ERR("Slave %u has less FMMUs (%u) than requested (%u).\n", |
1054 EC_SLAVE_ERR(slave, "Slave has less FMMUs (%u)" |
1271 slave->ring_position, slave->base_fmmu_count, |
1055 " than requested (%u).\n", slave->base_fmmu_count, |
1272 slave->config->used_fmmus); |
1056 slave->config->used_fmmus); |
1273 return; |
1057 return; |
1274 } |
1058 } |
1275 |
1059 |
1276 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1060 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1285 for (i = 0; i < slave->config->used_fmmus; i++) { |
1069 for (i = 0; i < slave->config->used_fmmus; i++) { |
1286 fmmu = &slave->config->fmmu_configs[i]; |
1070 fmmu = &slave->config->fmmu_configs[i]; |
1287 if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) { |
1071 if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) { |
1288 slave->error_flag = 1; |
1072 slave->error_flag = 1; |
1289 fsm->state = ec_fsm_slave_config_state_error; |
1073 fsm->state = ec_fsm_slave_config_state_error; |
1290 EC_ERR("Failed to determine PDO sync manager for FMMU on slave" |
1074 EC_SLAVE_ERR(slave, "Failed to determine PDO sync manager" |
1291 " %u!\n", slave->ring_position); |
1075 " for FMMU!\n"); |
1292 return; |
1076 return; |
1293 } |
1077 } |
1294 ec_fmmu_config_page(fmmu, sync, |
1078 ec_fmmu_config_page(fmmu, sync, |
1295 datagram->data + EC_FMMU_PAGE_SIZE * i); |
1079 datagram->data + EC_FMMU_PAGE_SIZE * i); |
1296 } |
1080 } |
1313 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1097 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1314 return; |
1098 return; |
1315 |
1099 |
1316 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1100 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1317 fsm->state = ec_fsm_slave_config_state_error; |
1101 fsm->state = ec_fsm_slave_config_state_error; |
1318 EC_ERR("Failed to receive FMMUs datagram for slave %u: ", |
1102 EC_SLAVE_ERR(slave, "Failed to receive FMMUs datagram: "); |
1319 slave->ring_position); |
|
1320 ec_datagram_print_state(datagram); |
1103 ec_datagram_print_state(datagram); |
1321 return; |
1104 return; |
1322 } |
1105 } |
1323 |
1106 |
1324 if (datagram->working_counter != 1) { |
1107 if (datagram->working_counter != 1) { |
1325 slave->error_flag = 1; |
1108 slave->error_flag = 1; |
1326 fsm->state = ec_fsm_slave_config_state_error; |
1109 fsm->state = ec_fsm_slave_config_state_error; |
1327 EC_ERR("Failed to set FMMUs of slave %u: ", |
1110 EC_SLAVE_ERR(slave, "Failed to set FMMUs: "); |
1328 slave->ring_position); |
|
1329 ec_datagram_print_wc_error(datagram); |
1111 ec_datagram_print_wc_error(datagram); |
1330 return; |
1112 return; |
1331 } |
1113 } |
1332 |
1114 |
1333 ec_fsm_slave_config_enter_dc_cycle(fsm); |
1115 ec_fsm_slave_config_enter_dc_cycle(fsm); |
1350 return; |
1132 return; |
1351 } |
1133 } |
1352 |
1134 |
1353 if (config->dc_assign_activate) { |
1135 if (config->dc_assign_activate) { |
1354 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
1136 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
1355 EC_WARN("Slave %u seems not to support distributed clocks!\n", |
1137 EC_SLAVE_WARN(slave, "Slave seems not to support" |
1356 slave->ring_position); |
1138 " distributed clocks!\n"); |
1357 } |
1139 } |
1358 |
1140 |
1359 if (slave->master->debug_level) |
1141 EC_SLAVE_DBG(slave, 1, "Setting DC cycle times to %u / %u.\n", |
1360 EC_DBG("Slave %u: Setting DC cycle times to %u / %u.\n", |
1142 config->dc_sync[0].cycle_time, config->dc_sync[1].cycle_time); |
1361 slave->ring_position, |
|
1362 config->dc_sync[0].cycle_time, |
|
1363 config->dc_sync[1].cycle_time); |
|
1364 |
1143 |
1365 // set DC cycle times |
1144 // set DC cycle times |
1366 ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); |
1145 ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); |
1367 EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time); |
1146 EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time); |
1368 EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time); |
1147 EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time); |
1382 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1161 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1383 ) |
1162 ) |
1384 { |
1163 { |
1385 ec_datagram_t *datagram = fsm->datagram; |
1164 ec_datagram_t *datagram = fsm->datagram; |
1386 ec_slave_t *slave = fsm->slave; |
1165 ec_slave_t *slave = fsm->slave; |
1166 ec_slave_config_t *config = slave->config; |
|
1167 |
|
1168 if (!config) { // config removed in the meantime |
|
1169 ec_fsm_slave_config_reconfigure(fsm); |
|
1170 return; |
|
1171 } |
|
1172 |
|
1173 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
1174 return; |
|
1175 |
|
1176 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1177 fsm->state = ec_fsm_slave_config_state_error; |
|
1178 EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: "); |
|
1179 ec_datagram_print_state(datagram); |
|
1180 return; |
|
1181 } |
|
1182 |
|
1183 if (datagram->working_counter != 1) { |
|
1184 slave->error_flag = 1; |
|
1185 fsm->state = ec_fsm_slave_config_state_error; |
|
1186 EC_SLAVE_ERR(slave, "Failed to set DC cycle times: "); |
|
1187 ec_datagram_print_wc_error(datagram); |
|
1188 return; |
|
1189 } |
|
1190 |
|
1191 EC_SLAVE_DBG(slave, 1, "Checking for synchrony.\n"); |
|
1192 |
|
1193 fsm->jiffies_start = jiffies; |
|
1194 ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); |
|
1195 fsm->retries = EC_FSM_RETRIES; |
|
1196 fsm->state = ec_fsm_slave_config_state_dc_sync_check; |
|
1197 } |
|
1198 |
|
1199 /*****************************************************************************/ |
|
1200 |
|
1201 /** Slave configuration state: DC SYNC CHECK. |
|
1202 */ |
|
1203 void ec_fsm_slave_config_state_dc_sync_check( |
|
1204 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
1205 ) |
|
1206 { |
|
1207 ec_datagram_t *datagram = fsm->datagram; |
|
1208 ec_slave_t *slave = fsm->slave; |
|
1387 ec_master_t *master = slave->master; |
1209 ec_master_t *master = slave->master; |
1388 ec_slave_config_t *config = slave->config; |
1210 ec_slave_config_t *config = slave->config; |
1211 uint32_t abs_sync_diff; |
|
1212 unsigned long diff_ms; |
|
1389 ec_sync_signal_t *sync0 = &config->dc_sync[0]; |
1213 ec_sync_signal_t *sync0 = &config->dc_sync[0]; |
1390 u64 start_time; |
1214 u64 start_time; |
1391 |
1215 |
1392 if (!config) { // config removed in the meantime |
1216 if (!config) { // config removed in the meantime |
1393 ec_fsm_slave_config_reconfigure(fsm); |
1217 ec_fsm_slave_config_reconfigure(fsm); |
1397 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1221 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1398 return; |
1222 return; |
1399 |
1223 |
1400 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1224 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1401 fsm->state = ec_fsm_slave_config_state_error; |
1225 fsm->state = ec_fsm_slave_config_state_error; |
1402 EC_ERR("Failed to receive DC cycle times datagram for slave %u: ", |
1226 EC_SLAVE_ERR(slave, "Failed to receive DC sync check datagram: "); |
1403 slave->ring_position); |
|
1404 ec_datagram_print_state(datagram); |
1227 ec_datagram_print_state(datagram); |
1405 return; |
1228 return; |
1406 } |
1229 } |
1407 |
1230 |
1408 if (datagram->working_counter != 1) { |
1231 if (datagram->working_counter != 1) { |
1409 slave->error_flag = 1; |
1232 slave->error_flag = 1; |
1410 fsm->state = ec_fsm_slave_config_state_error; |
1233 fsm->state = ec_fsm_slave_config_state_error; |
1411 EC_ERR("Failed to set DC cycle times of slave %u: ", |
1234 EC_SLAVE_ERR(slave, "Failed to check DC synchrony: "); |
1412 slave->ring_position); |
|
1413 ec_datagram_print_wc_error(datagram); |
1235 ec_datagram_print_wc_error(datagram); |
1414 return; |
1236 return; |
1415 } |
1237 } |
1416 |
1238 |
1239 abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff; |
|
1240 diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
|
1241 |
|
1242 if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) { |
|
1243 |
|
1244 if (diff_ms >= EC_DC_SYNC_WAIT_MS) { |
|
1245 EC_SLAVE_WARN(slave, "Slave did not sync after %u ms.\n", |
|
1246 (u32) diff_ms); |
|
1247 } else { |
|
1248 EC_SLAVE_DBG(slave, 1, "Sync after %4u ms: %10u ns\n", |
|
1249 (u32) diff_ms, abs_sync_diff); |
|
1250 |
|
1251 // check synchrony again |
|
1252 ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); |
|
1253 fsm->retries = EC_FSM_RETRIES; |
|
1254 return; |
|
1255 } |
|
1256 } else { |
|
1257 EC_SLAVE_DBG(slave, 1, "%u ns difference after %u ms.\n", |
|
1258 abs_sync_diff, (u32) diff_ms); |
|
1259 } |
|
1260 |
|
1417 // set DC start time |
1261 // set DC start time |
1418 start_time = master->app_time + 100000000ULL; // now + X ns |
1262 start_time = master->app_time + EC_DC_START_OFFSET; // now + X ns |
1419 // FIXME use slave's local system time here? |
1263 // FIXME use slave's local system time here? |
1420 |
1264 |
1421 if (sync0->cycle_time) { |
1265 if (sync0->cycle_time) { |
1422 // find correct phase |
1266 // find correct phase |
1423 if (master->has_start_time) { |
1267 if (master->has_app_time) { |
1424 u64 diff, start; |
1268 u64 diff, start; |
1425 u32 remainder; |
1269 u32 remainder; |
1426 |
1270 |
1427 diff = start_time - master->app_start_time; |
1271 diff = start_time - master->app_start_time; |
1428 remainder = do_div(diff, sync0->cycle_time); |
1272 remainder = do_div(diff, sync0->cycle_time); |
1429 |
1273 |
1430 start = start_time + |
1274 start = start_time + |
1431 sync0->cycle_time - remainder + sync0->shift_time; |
1275 sync0->cycle_time - remainder + sync0->shift_time; |
1432 |
1276 |
1433 if (master->debug_level) { |
1277 EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n", |
1434 EC_DBG("app_start_time=%llu\n", master->app_start_time); |
1278 master->app_start_time); |
1435 EC_DBG(" start_time=%llu\n", start_time); |
1279 EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", start_time); |
1436 EC_DBG(" cycle_time=%u\n", sync0->cycle_time); |
1280 EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", sync0->cycle_time); |
1437 EC_DBG(" shift_time=%u\n", sync0->shift_time); |
1281 EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", sync0->shift_time); |
1438 EC_DBG(" remainder=%u\n", remainder); |
1282 EC_SLAVE_DBG(slave, 1, " remainder=%u\n", remainder); |
1439 EC_DBG(" start=%llu\n", start); |
1283 EC_SLAVE_DBG(slave, 1, " start=%llu\n", start); |
1440 } |
|
1441 start_time = start; |
1284 start_time = start; |
1442 } else { |
1285 } else { |
1443 EC_WARN("No application time supplied. Cyclic start time will " |
1286 EC_SLAVE_WARN(slave, "No application time supplied." |
1444 "not be in phase for slave %u.\n", slave->ring_position); |
1287 " Cyclic start time will not be in phase.\n"); |
1445 } |
1288 } |
1446 } |
1289 } |
1447 |
1290 |
1448 if (master->debug_level) |
1291 EC_SLAVE_DBG(slave, 1, "Setting DC cyclic operation" |
1449 EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", |
1292 " start time to %llu.\n", start_time); |
1450 slave->ring_position, start_time); |
|
1451 |
1293 |
1452 ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); |
1294 ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); |
1453 EC_WRITE_U64(datagram->data, start_time); |
1295 EC_WRITE_U64(datagram->data, start_time); |
1454 fsm->retries = EC_FSM_RETRIES; |
1296 fsm->retries = EC_FSM_RETRIES; |
1455 fsm->state = ec_fsm_slave_config_state_dc_start; |
1297 fsm->state = ec_fsm_slave_config_state_dc_start; |
1475 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1317 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1476 return; |
1318 return; |
1477 |
1319 |
1478 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1320 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1479 fsm->state = ec_fsm_slave_config_state_error; |
1321 fsm->state = ec_fsm_slave_config_state_error; |
1480 EC_ERR("Failed to receive DC start time datagram for slave %u: ", |
1322 EC_SLAVE_ERR(slave, "Failed to receive DC start time datagram: "); |
1481 slave->ring_position); |
|
1482 ec_datagram_print_state(datagram); |
1323 ec_datagram_print_state(datagram); |
1483 return; |
1324 return; |
1484 } |
1325 } |
1485 |
1326 |
1486 if (datagram->working_counter != 1) { |
1327 if (datagram->working_counter != 1) { |
1487 slave->error_flag = 1; |
1328 slave->error_flag = 1; |
1488 fsm->state = ec_fsm_slave_config_state_error; |
1329 fsm->state = ec_fsm_slave_config_state_error; |
1489 EC_ERR("Failed to set DC start time of slave %u: ", |
1330 EC_SLAVE_ERR(slave, "Failed to set DC start time: "); |
1490 slave->ring_position); |
|
1491 ec_datagram_print_wc_error(datagram); |
1331 ec_datagram_print_wc_error(datagram); |
1492 return; |
1332 return; |
1493 } |
1333 } |
1494 |
1334 |
1495 if (slave->master->debug_level) |
1335 EC_SLAVE_DBG(slave, 1, "Setting DC AssignActivate to 0x%04x.\n", |
1496 EC_DBG("Slave %u: Setting DC AssignActivate to 0x%04x.\n", |
1336 config->dc_assign_activate); |
1497 slave->ring_position, config->dc_assign_activate); |
|
1498 |
1337 |
1499 // assign sync unit to EtherCAT or PDI |
1338 // assign sync unit to EtherCAT or PDI |
1500 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
1339 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
1501 EC_WRITE_U16(datagram->data, config->dc_assign_activate); |
1340 EC_WRITE_U16(datagram->data, config->dc_assign_activate); |
1502 fsm->retries = EC_FSM_RETRIES; |
1341 fsm->retries = EC_FSM_RETRIES; |
1517 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1356 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1518 return; |
1357 return; |
1519 |
1358 |
1520 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1359 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1521 fsm->state = ec_fsm_slave_config_state_error; |
1360 fsm->state = ec_fsm_slave_config_state_error; |
1522 EC_ERR("Failed to receive DC activation datagram for slave %u: ", |
1361 EC_SLAVE_ERR(slave, "Failed to receive DC activation datagram: "); |
1523 slave->ring_position); |
|
1524 ec_datagram_print_state(datagram); |
1362 ec_datagram_print_state(datagram); |
1525 return; |
1363 return; |
1526 } |
1364 } |
1527 |
1365 |
1528 if (datagram->working_counter != 1) { |
1366 if (datagram->working_counter != 1) { |
1529 slave->error_flag = 1; |
1367 slave->error_flag = 1; |
1530 fsm->state = ec_fsm_slave_config_state_error; |
1368 fsm->state = ec_fsm_slave_config_state_error; |
1531 EC_ERR("Failed to set DC cyclic operation state of slave %u: ", |
1369 EC_SLAVE_ERR(slave, "Failed to activate DC: "); |
1532 slave->ring_position); |
|
1533 ec_datagram_print_wc_error(datagram); |
1370 ec_datagram_print_wc_error(datagram); |
1534 return; |
1371 return; |
1535 } |
1372 } |
1536 |
1373 |
1537 ec_fsm_slave_config_enter_safeop(fsm); |
1374 ec_fsm_slave_config_enter_safeop(fsm); |
1556 */ |
1393 */ |
1557 void ec_fsm_slave_config_state_safeop( |
1394 void ec_fsm_slave_config_state_safeop( |
1558 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1395 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1559 ) |
1396 ) |
1560 { |
1397 { |
1561 ec_master_t *master = fsm->slave->master; |
|
1562 ec_slave_t *slave = fsm->slave; |
1398 ec_slave_t *slave = fsm->slave; |
1563 |
1399 |
1564 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1400 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1565 |
1401 |
1566 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1402 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1570 return; |
1406 return; |
1571 } |
1407 } |
1572 |
1408 |
1573 // slave is now in SAFEOP |
1409 // slave is now in SAFEOP |
1574 |
1410 |
1575 if (master->debug_level) { |
1411 EC_SLAVE_DBG(slave, 1, "Now in SAFEOP.\n"); |
1576 EC_DBG("Slave %u is now in SAFEOP.\n", slave->ring_position); |
|
1577 } |
|
1578 |
1412 |
1579 if (fsm->slave->current_state == fsm->slave->requested_state) { |
1413 if (fsm->slave->current_state == fsm->slave->requested_state) { |
1580 fsm->state = ec_fsm_slave_config_state_end; // successful |
1414 fsm->state = ec_fsm_slave_config_state_end; // successful |
1581 if (master->debug_level) { |
1415 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
1582 EC_DBG("Finished configuration of slave %u.\n", |
|
1583 slave->ring_position); |
|
1584 } |
|
1585 return; |
1416 return; |
1586 } |
1417 } |
1587 |
1418 |
1588 // set state to OP |
1419 // set state to OP |
1589 fsm->state = ec_fsm_slave_config_state_op; |
1420 fsm->state = ec_fsm_slave_config_state_op; |
1597 */ |
1428 */ |
1598 void ec_fsm_slave_config_state_op( |
1429 void ec_fsm_slave_config_state_op( |
1599 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1430 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1600 ) |
1431 ) |
1601 { |
1432 { |
1602 ec_master_t *master = fsm->slave->master; |
|
1603 ec_slave_t *slave = fsm->slave; |
1433 ec_slave_t *slave = fsm->slave; |
1604 |
1434 |
1605 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1435 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1606 |
1436 |
1607 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1437 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1611 return; |
1441 return; |
1612 } |
1442 } |
1613 |
1443 |
1614 // slave is now in OP |
1444 // slave is now in OP |
1615 |
1445 |
1616 if (master->debug_level) { |
1446 EC_SLAVE_DBG(slave, 1, "Now in OP. Finished configuration.\n"); |
1617 EC_DBG("Slave %u is now in OP.\n", slave->ring_position); |
|
1618 EC_DBG("Finished configuration of slave %u.\n", slave->ring_position); |
|
1619 } |
|
1620 |
1447 |
1621 fsm->state = ec_fsm_slave_config_state_end; // successful |
1448 fsm->state = ec_fsm_slave_config_state_end; // successful |
1622 } |
1449 } |
1623 |
1450 |
1624 /*****************************************************************************/ |
1451 /*****************************************************************************/ |
1627 */ |
1454 */ |
1628 void ec_fsm_slave_config_reconfigure( |
1455 void ec_fsm_slave_config_reconfigure( |
1629 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1456 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1630 ) |
1457 ) |
1631 { |
1458 { |
1632 if (fsm->slave->master->debug_level) { |
1459 EC_SLAVE_DBG(fsm->slave, 1, "Slave configuration detached during " |
1633 EC_DBG("Slave configuration for slave %u detached during " |
1460 "configuration. Reconfiguring."); |
1634 "configuration. Reconfiguring.", fsm->slave->ring_position); |
|
1635 } |
|
1636 |
1461 |
1637 ec_fsm_slave_config_enter_init(fsm); // reconfigure |
1462 ec_fsm_slave_config_enter_init(fsm); // reconfigure |
1638 } |
1463 } |
1639 |
1464 |
1640 /****************************************************************************** |
1465 /****************************************************************************** |