194 */ |
194 */ |
195 void ec_fsm_slave_config_state_start( |
195 void ec_fsm_slave_config_state_start( |
196 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
196 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
197 ) |
197 ) |
198 { |
198 { |
199 if (fsm->slave->master->debug_level) { |
199 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); |
200 ec_fsm_slave_config_enter_init(fsm); |
204 } |
201 } |
205 |
202 |
206 /*****************************************************************************/ |
203 /*****************************************************************************/ |
207 |
204 |
222 */ |
219 */ |
223 void ec_fsm_slave_config_state_init( |
220 void ec_fsm_slave_config_state_init( |
224 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
221 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
225 ) |
222 ) |
226 { |
223 { |
227 ec_master_t *master = fsm->slave->master; |
|
228 ec_slave_t *slave = fsm->slave; |
224 ec_slave_t *slave = fsm->slave; |
229 ec_datagram_t *datagram = fsm->datagram; |
225 ec_datagram_t *datagram = fsm->datagram; |
230 |
226 |
231 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
227 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
232 |
228 |
235 slave->error_flag = 1; |
231 slave->error_flag = 1; |
236 fsm->state = ec_fsm_slave_config_state_error; |
232 fsm->state = ec_fsm_slave_config_state_error; |
237 return; |
233 return; |
238 } |
234 } |
239 |
235 |
240 if (master->debug_level) { |
236 EC_SLAVE_DBG(slave, 1, "Now in INIT.\n"); |
241 EC_DBG("Slave %u is now in INIT.\n", slave->ring_position); |
|
242 } |
|
243 |
237 |
244 if (!slave->base_fmmu_count) { // skip FMMU configuration |
238 if (!slave->base_fmmu_count) { // skip FMMU configuration |
245 ec_fsm_slave_config_enter_clear_sync(fsm); |
239 ec_fsm_slave_config_enter_clear_sync(fsm); |
246 return; |
240 return; |
247 } |
241 } |
248 |
242 |
249 if (master->debug_level) |
243 EC_SLAVE_DBG(slave, 1, "Clearing FMMU configurations...\n"); |
250 EC_DBG("Clearing FMMU configurations of slave %u...\n", |
|
251 slave->ring_position); |
|
252 |
244 |
253 // clear FMMU configurations |
245 // clear FMMU configurations |
254 ec_datagram_fpwr(datagram, slave->station_address, |
246 ec_datagram_fpwr(datagram, slave->station_address, |
255 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
247 0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
256 ec_datagram_zero(datagram); |
248 ec_datagram_zero(datagram); |
271 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
263 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
272 return; |
264 return; |
273 |
265 |
274 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
266 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
275 fsm->state = ec_fsm_slave_config_state_error; |
267 fsm->state = ec_fsm_slave_config_state_error; |
276 EC_ERR("Failed receive FMMU clearing datagram for slave %u.\n", |
268 EC_SLAVE_ERR(fsm->slave, "Failed receive FMMU clearing datagram.\n"); |
277 fsm->slave->ring_position); |
|
278 return; |
269 return; |
279 } |
270 } |
280 |
271 |
281 if (datagram->working_counter != 1) { |
272 if (datagram->working_counter != 1) { |
282 fsm->slave->error_flag = 1; |
273 fsm->slave->error_flag = 1; |
283 fsm->state = ec_fsm_slave_config_state_error; |
274 fsm->state = ec_fsm_slave_config_state_error; |
284 EC_ERR("Failed to clear FMMUs of slave %u: ", |
275 EC_SLAVE_ERR(fsm->slave, "Failed to clear FMMUs: "); |
285 fsm->slave->ring_position); |
|
286 ec_datagram_print_wc_error(datagram); |
276 ec_datagram_print_wc_error(datagram); |
287 return; |
277 return; |
288 } |
278 } |
289 |
279 |
290 ec_fsm_slave_config_enter_clear_sync(fsm); |
280 ec_fsm_slave_config_enter_clear_sync(fsm); |
306 // no sync managers |
296 // no sync managers |
307 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
297 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
308 return; |
298 return; |
309 } |
299 } |
310 |
300 |
311 if (slave->master->debug_level) |
301 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 |
302 |
315 sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count; |
303 sync_size = EC_SYNC_PAGE_SIZE * slave->base_sync_count; |
316 |
304 |
317 // clear sync manager configurations |
305 // clear sync manager configurations |
318 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size); |
306 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, sync_size); |
334 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
322 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
335 return; |
323 return; |
336 |
324 |
337 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
325 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
338 fsm->state = ec_fsm_slave_config_state_error; |
326 fsm->state = ec_fsm_slave_config_state_error; |
339 EC_ERR("Failed receive sync manager clearing datagram" |
327 EC_SLAVE_ERR(fsm->slave, "Failed receive sync manager" |
340 " for slave %u.\n", fsm->slave->ring_position); |
328 " clearing datagram.\n"); |
341 return; |
329 return; |
342 } |
330 } |
343 |
331 |
344 if (datagram->working_counter != 1) { |
332 if (datagram->working_counter != 1) { |
345 fsm->slave->error_flag = 1; |
333 fsm->slave->error_flag = 1; |
346 fsm->state = ec_fsm_slave_config_state_error; |
334 fsm->state = ec_fsm_slave_config_state_error; |
347 EC_ERR("Failed to clear sync manager configurations of slave %u: ", |
335 EC_SLAVE_ERR(fsm->slave, |
348 fsm->slave->ring_position); |
336 "Failed to clear sync manager configurations: "); |
349 ec_datagram_print_wc_error(datagram); |
337 ec_datagram_print_wc_error(datagram); |
350 return; |
338 return; |
351 } |
339 } |
352 |
340 |
353 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
341 ec_fsm_slave_config_enter_dc_clear_assign(fsm); |
367 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
355 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
368 ec_fsm_slave_config_enter_mbox_sync(fsm); |
356 ec_fsm_slave_config_enter_mbox_sync(fsm); |
369 return; |
357 return; |
370 } |
358 } |
371 |
359 |
372 if (slave->master->debug_level) |
360 EC_SLAVE_DBG(slave, 1, "Clearing DC assignment...\n"); |
373 EC_DBG("Clearing DC assignment of slave %u...\n", |
|
374 slave->ring_position); |
|
375 |
361 |
376 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
362 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
377 ec_datagram_zero(datagram); |
363 ec_datagram_zero(datagram); |
378 fsm->retries = EC_FSM_RETRIES; |
364 fsm->retries = EC_FSM_RETRIES; |
379 fsm->state = ec_fsm_slave_config_state_dc_clear_assign; |
365 fsm->state = ec_fsm_slave_config_state_dc_clear_assign; |
392 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
378 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
393 return; |
379 return; |
394 |
380 |
395 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
381 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
396 fsm->state = ec_fsm_slave_config_state_error; |
382 fsm->state = ec_fsm_slave_config_state_error; |
397 EC_ERR("Failed receive DC assignment clearing datagram" |
383 EC_SLAVE_ERR(fsm->slave, "Failed receive DC assignment" |
398 " for slave %u.\n", fsm->slave->ring_position); |
384 " clearing datagram.\n"); |
399 return; |
385 return; |
400 } |
386 } |
401 |
387 |
402 if (fsm->slave->master->debug_level && datagram->working_counter != 1) { |
388 if (datagram->working_counter != 1) { |
403 // clearing the DC assignment does not succeed on simple slaves |
389 // clearing the DC assignment does not succeed on simple slaves |
404 EC_DBG("Failed to clear DC assignment of slave %u: ", |
390 EC_SLAVE_DBG(fsm->slave, 1, "Failed to clear DC assignment: "); |
405 fsm->slave->ring_position); |
|
406 ec_datagram_print_wc_error(datagram); |
391 ec_datagram_print_wc_error(datagram); |
407 } |
392 } |
408 |
393 |
409 // read DC system time (0x0910, 64 bit) |
394 // read DC system time (0x0910, 64 bit) |
410 // gap (64 bit) |
395 // gap (64 bit) |
435 // correct read system time by elapsed time since read operation |
420 // correct read system time by elapsed time since read operation |
436 correction = jiffies_since_read * 1000 / HZ * 1000000; |
421 correction = jiffies_since_read * 1000 / HZ * 1000000; |
437 system_time32 += correction; |
422 system_time32 += correction; |
438 time_diff = (u32) slave->master->app_time - system_time32; |
423 time_diff = (u32) slave->master->app_time - system_time32; |
439 |
424 |
440 if (slave->master->debug_level) |
425 EC_SLAVE_DBG(slave, 1, "Calculating DC time offset (32 bit):" |
441 EC_DBG("Slave %u: system_time=%u (corrected with %u)," |
426 " system_time=%u (corrected with %u), app_time=%u, diff=%i\n", |
442 " app_time=%u, diff=%i\n", |
427 system_time32, correction, |
443 slave->ring_position, system_time32, correction, |
428 (u32) slave->master->app_time, time_diff); |
444 (u32) slave->master->app_time, time_diff); |
|
445 |
429 |
446 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
430 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
447 new_offset = time_diff + old_offset32; |
431 new_offset = time_diff + old_offset32; |
448 if (slave->master->debug_level) |
432 EC_SLAVE_DBG(slave, 1, "Setting time offset to %u (was %u)\n", |
449 EC_DBG("Slave %u: Setting time offset to %u (was %u)\n", |
433 new_offset, old_offset32); |
450 slave->ring_position, new_offset, old_offset32); |
|
451 return (u64) new_offset; |
434 return (u64) new_offset; |
452 } else { |
435 } else { |
453 if (slave->master->debug_level) |
436 EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n"); |
454 EC_DBG("Slave %u: Not touching time offset.\n", |
|
455 slave->ring_position); |
|
456 return old_offset; |
437 return old_offset; |
457 } |
438 } |
458 } |
439 } |
459 |
440 |
460 /*****************************************************************************/ |
441 /*****************************************************************************/ |
475 // correct read system time by elapsed time since read operation |
456 // correct read system time by elapsed time since read operation |
476 correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; |
457 correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000; |
477 system_time += correction; |
458 system_time += correction; |
478 time_diff = fsm->slave->master->app_time - system_time; |
459 time_diff = fsm->slave->master->app_time - system_time; |
479 |
460 |
480 if (slave->master->debug_level) |
461 EC_SLAVE_DBG(slave, 1, "Calculating DC time offset (64 bit):" |
481 EC_DBG("Slave %u: system_time=%llu (corrected with %llu)," |
462 " system_time=%llu (corrected with %llu)," |
482 " app_time=%llu, diff=%lli\n", |
463 " app_time=%llu, diff=%lli\n", |
483 slave->ring_position, system_time, correction, |
464 system_time, correction, |
484 slave->master->app_time, time_diff); |
465 slave->master->app_time, time_diff); |
485 |
|
486 |
466 |
487 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
467 if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { |
488 new_offset = time_diff + old_offset; |
468 new_offset = time_diff + old_offset; |
489 if (slave->master->debug_level) |
469 EC_SLAVE_DBG(slave, 1, "Setting time offset to %llu (was %llu)\n", |
490 EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n", |
470 new_offset, old_offset); |
491 slave->ring_position, new_offset, old_offset); |
|
492 } else { |
471 } else { |
493 new_offset = old_offset; |
472 new_offset = old_offset; |
494 if (slave->master->debug_level) |
473 EC_SLAVE_DBG(slave, 1, "Not touching time offset.\n"); |
495 EC_DBG("Slave %u: Not touching time offset.\n", |
|
496 slave->ring_position); |
|
497 } |
474 } |
498 |
475 |
499 return new_offset; |
476 return new_offset; |
500 } |
477 } |
501 |
478 |
515 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
492 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
516 return; |
493 return; |
517 |
494 |
518 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
495 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
519 fsm->state = ec_fsm_slave_config_state_error; |
496 fsm->state = ec_fsm_slave_config_state_error; |
520 EC_ERR("Failed to receive DC times datagram for slave %u: ", |
497 EC_SLAVE_ERR(slave, "Failed to receive DC times datagram: "); |
521 slave->ring_position); |
|
522 ec_datagram_print_state(datagram); |
498 ec_datagram_print_state(datagram); |
523 return; |
499 return; |
524 } |
500 } |
525 |
501 |
526 if (datagram->working_counter != 1) { |
502 if (datagram->working_counter != 1) { |
527 slave->error_flag = 1; |
503 slave->error_flag = 1; |
528 fsm->state = ec_fsm_slave_config_state_error; |
504 fsm->state = ec_fsm_slave_config_state_error; |
529 EC_ERR("Failed to get DC times of slave %u: ", |
505 EC_SLAVE_ERR(slave, "Failed to get DC times: "); |
530 slave->ring_position); |
|
531 ec_datagram_print_wc_error(datagram); |
506 ec_datagram_print_wc_error(datagram); |
532 return; |
507 return; |
533 } |
508 } |
534 |
509 |
535 system_time = EC_READ_U64(datagram->data); // 0x0910 |
510 system_time = EC_READ_U64(datagram->data); // 0x0910 |
566 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
541 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
567 return; |
542 return; |
568 |
543 |
569 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
544 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
570 fsm->state = ec_fsm_slave_config_state_error; |
545 fsm->state = ec_fsm_slave_config_state_error; |
571 EC_ERR("Failed to receive DC system time offset datagram for" |
546 EC_SLAVE_ERR(slave, "Failed to receive DC system time offset" |
572 " slave %u: ", slave->ring_position); |
547 " datagram: "); |
573 ec_datagram_print_state(datagram); |
548 ec_datagram_print_state(datagram); |
574 return; |
549 return; |
575 } |
550 } |
576 |
551 |
577 if (datagram->working_counter != 1) { |
552 if (datagram->working_counter != 1) { |
578 slave->error_flag = 1; |
553 slave->error_flag = 1; |
579 fsm->state = ec_fsm_slave_config_state_error; |
554 fsm->state = ec_fsm_slave_config_state_error; |
580 EC_ERR("Failed to set DC system time offset of slave %u: ", |
555 EC_SLAVE_ERR(slave, "Failed to set DC system time offset: "); |
581 slave->ring_position); |
|
582 ec_datagram_print_wc_error(datagram); |
556 ec_datagram_print_wc_error(datagram); |
583 return; |
557 return; |
584 } |
558 } |
585 |
559 |
586 ec_fsm_slave_config_enter_mbox_sync(fsm); |
560 ec_fsm_slave_config_enter_mbox_sync(fsm); |
592 */ |
566 */ |
593 void ec_fsm_slave_config_enter_mbox_sync( |
567 void ec_fsm_slave_config_enter_mbox_sync( |
594 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
568 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
595 ) |
569 ) |
596 { |
570 { |
597 ec_master_t *master = fsm->slave->master; |
|
598 ec_slave_t *slave = fsm->slave; |
571 ec_slave_t *slave = fsm->slave; |
599 ec_datagram_t *datagram = fsm->datagram; |
572 ec_datagram_t *datagram = fsm->datagram; |
600 unsigned int i; |
573 unsigned int i; |
601 |
574 |
602 // slave is now in INIT |
575 // slave is now in INIT |
603 if (slave->current_state == slave->requested_state) { |
576 if (slave->current_state == slave->requested_state) { |
604 fsm->state = ec_fsm_slave_config_state_end; // successful |
577 fsm->state = ec_fsm_slave_config_state_end; // successful |
605 if (master->debug_level) { |
578 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
606 EC_DBG("Finished configuration of slave %u.\n", |
|
607 slave->ring_position); |
|
608 } |
|
609 return; |
579 return; |
610 } |
580 } |
611 |
581 |
612 if (!slave->sii.mailbox_protocols) { |
582 if (!slave->sii.mailbox_protocols) { |
613 // no mailbox protocols supported |
583 // no mailbox protocols supported |
614 if (master->debug_level) |
584 EC_SLAVE_DBG(slave, 1, "Slave does not support" |
615 EC_DBG("Slave %u does not support mailbox communication.\n", |
585 " mailbox communication.\n"); |
616 slave->ring_position); |
|
617 ec_fsm_slave_config_enter_boot_preop(fsm); |
586 ec_fsm_slave_config_enter_boot_preop(fsm); |
618 return; |
587 return; |
619 } |
588 } |
620 |
589 |
621 if (master->debug_level) { |
590 EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n"); |
622 EC_DBG("Configuring mailbox sync managers of slave %u.\n", |
|
623 slave->ring_position); |
|
624 } |
|
625 |
591 |
626 if (slave->requested_state == EC_SLAVE_STATE_BOOT) { |
592 if (slave->requested_state == EC_SLAVE_STATE_BOOT) { |
627 ec_sync_t sync; |
593 ec_sync_t sync; |
628 |
594 |
629 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
595 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
675 slave->configured_tx_mailbox_size = |
641 slave->configured_tx_mailbox_size = |
676 slave->sii.syncs[1].default_length; |
642 slave->sii.syncs[1].default_length; |
677 } else { // no mailbox sync manager configurations provided |
643 } else { // no mailbox sync manager configurations provided |
678 ec_sync_t sync; |
644 ec_sync_t sync; |
679 |
645 |
680 if (master->debug_level) |
646 EC_SLAVE_DBG(slave, 1, "Slave does not provide" |
681 EC_DBG("Slave %u does not provide" |
647 " mailbox sync manager configurations.\n"); |
682 " mailbox sync manager configurations.\n", |
|
683 slave->ring_position); |
|
684 |
648 |
685 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
649 ec_datagram_fpwr(datagram, slave->station_address, 0x0800, |
686 EC_SYNC_PAGE_SIZE * 2); |
650 EC_SYNC_PAGE_SIZE * 2); |
687 ec_datagram_zero(datagram); |
651 ec_datagram_zero(datagram); |
688 |
652 |
733 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
697 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
734 return; |
698 return; |
735 |
699 |
736 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
700 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
737 fsm->state = ec_fsm_slave_config_state_error; |
701 fsm->state = ec_fsm_slave_config_state_error; |
738 EC_ERR("Failed to receive sync manager configuration datagram for" |
702 EC_SLAVE_ERR(slave, "Failed to receive sync manager" |
739 " slave %u: ", slave->ring_position); |
703 " configuration datagram: "); |
740 ec_datagram_print_state(datagram); |
704 ec_datagram_print_state(datagram); |
741 return; |
705 return; |
742 } |
706 } |
743 |
707 |
744 if (fsm->take_time) { |
708 if (fsm->take_time) { |
755 unsigned long diff = datagram->jiffies_received - fsm->jiffies_start; |
719 unsigned long diff = datagram->jiffies_received - fsm->jiffies_start; |
756 |
720 |
757 if (diff >= HZ) { |
721 if (diff >= HZ) { |
758 slave->error_flag = 1; |
722 slave->error_flag = 1; |
759 fsm->state = ec_fsm_slave_config_state_error; |
723 fsm->state = ec_fsm_slave_config_state_error; |
760 EC_ERR("Timeout while configuring mailbox sync managers of" |
724 EC_SLAVE_ERR(slave, "Timeout while configuring" |
761 " slave %u.\n", slave->ring_position); |
725 " mailbox sync managers.\n"); |
762 return; |
726 return; |
763 } |
727 } else { |
764 else if (slave->master->debug_level) { |
728 EC_SLAVE_DBG(slave, 1, "Resending after %u ms...\n", |
765 EC_DBG("Resending after %u ms...\n", |
|
766 (unsigned int) diff * 1000 / HZ); |
729 (unsigned int) diff * 1000 / HZ); |
767 } |
730 } |
768 |
731 |
769 // send configuration datagram again |
732 // send configuration datagram again |
770 fsm->retries = EC_FSM_RETRIES; |
733 fsm->retries = EC_FSM_RETRIES; |
771 return; |
734 return; |
772 } |
735 } |
773 else if (datagram->working_counter != 1) { |
736 else if (datagram->working_counter != 1) { |
774 slave->error_flag = 1; |
737 slave->error_flag = 1; |
775 fsm->state = ec_fsm_slave_config_state_error; |
738 fsm->state = ec_fsm_slave_config_state_error; |
776 EC_ERR("Failed to set sync managers of slave %u: ", |
739 EC_SLAVE_ERR(slave, "Failed to set sync managers: "); |
777 slave->ring_position); |
|
778 ec_datagram_print_wc_error(datagram); |
740 ec_datagram_print_wc_error(datagram); |
779 return; |
741 return; |
780 } |
742 } |
781 |
743 |
782 ec_fsm_slave_config_enter_boot_preop(fsm); |
744 ec_fsm_slave_config_enter_boot_preop(fsm); |
808 void ec_fsm_slave_config_state_boot_preop( |
770 void ec_fsm_slave_config_state_boot_preop( |
809 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
771 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
810 ) |
772 ) |
811 { |
773 { |
812 ec_slave_t *slave = fsm->slave; |
774 ec_slave_t *slave = fsm->slave; |
813 ec_master_t *master = fsm->slave->master; |
|
814 |
775 |
815 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
776 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
816 |
777 |
817 if (!ec_fsm_change_success(fsm->fsm_change)) { |
778 if (!ec_fsm_change_success(fsm->fsm_change)) { |
818 if (!fsm->fsm_change->spontaneous_change) |
779 if (!fsm->fsm_change->spontaneous_change) |
822 } |
783 } |
823 |
784 |
824 // slave is now in BOOT or PREOP |
785 // slave is now in BOOT or PREOP |
825 slave->jiffies_preop = fsm->datagram->jiffies_received; |
786 slave->jiffies_preop = fsm->datagram->jiffies_received; |
826 |
787 |
827 if (master->debug_level) { |
788 EC_SLAVE_DBG(slave, 1, "Now in %s.\n", |
828 EC_DBG("Slave %u is now in %s.\n", slave->ring_position, |
789 slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT"); |
829 slave->requested_state != EC_SLAVE_STATE_BOOT |
|
830 ? "PREOP" : "BOOT"); |
|
831 } |
|
832 |
790 |
833 if (slave->current_state == slave->requested_state) { |
791 if (slave->current_state == slave->requested_state) { |
834 fsm->state = ec_fsm_slave_config_state_end; // successful |
792 fsm->state = ec_fsm_slave_config_state_end; // successful |
835 if (master->debug_level) { |
793 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
836 EC_DBG("Finished configuration of slave %u.\n", |
|
837 slave->ring_position); |
|
838 } |
|
839 return; |
794 return; |
840 } |
795 } |
841 |
796 |
842 ec_fsm_slave_config_enter_sdo_conf(fsm); |
797 ec_fsm_slave_config_enter_sdo_conf(fsm); |
843 } |
798 } |
882 ) |
837 ) |
883 { |
838 { |
884 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
839 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
885 |
840 |
886 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
841 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
887 EC_ERR("SDO configuration failed for slave %u.\n", |
842 EC_SLAVE_ERR(fsm->slave, "SDO configuration failed.\n"); |
888 fsm->slave->ring_position); |
|
889 fsm->slave->error_flag = 1; |
843 fsm->slave->error_flag = 1; |
890 fsm->state = ec_fsm_slave_config_state_error; |
844 fsm->state = ec_fsm_slave_config_state_error; |
891 return; |
845 return; |
892 } |
846 } |
893 |
847 |
959 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
913 ec_master_queue_external_datagram(slave->master, fsm_soe->datagram); |
960 return; |
914 return; |
961 } |
915 } |
962 |
916 |
963 if (!ec_fsm_soe_success(fsm_soe)) { |
917 if (!ec_fsm_soe_success(fsm_soe)) { |
964 EC_ERR("SoE configuration failed for slave %u.\n", |
918 EC_SLAVE_ERR(slave, "SoE configuration failed.\n"); |
965 fsm->slave->ring_position); |
|
966 fsm->slave->error_flag = 1; |
919 fsm->slave->error_flag = 1; |
967 fsm->state = ec_fsm_slave_config_state_error; |
920 fsm->state = ec_fsm_slave_config_state_error; |
968 return; |
921 return; |
969 } |
922 } |
970 |
923 |
1037 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
989 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1038 ) |
990 ) |
1039 { |
991 { |
1040 ec_slave_t *slave = fsm->slave; |
992 ec_slave_t *slave = fsm->slave; |
1041 ec_datagram_t *datagram = fsm->datagram; |
993 ec_datagram_t *datagram = fsm->datagram; |
1042 ec_master_t *master = slave->master; |
|
1043 ec_slave_config_t *config = slave->config; |
994 ec_slave_config_t *config = slave->config; |
1044 |
995 |
1045 if (config && config->watchdog_divider) { |
996 if (config && config->watchdog_divider) { |
1046 if (master->debug_level) |
997 EC_SLAVE_DBG(slave, 1, "Setting watchdog divider to %u.\n", |
1047 EC_DBG("Setting watchdog divider to %u.\n", |
998 config->watchdog_divider); |
1048 config->watchdog_divider); |
|
1049 |
999 |
1050 ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2); |
1000 ec_datagram_fpwr(datagram, slave->station_address, 0x0400, 2); |
1051 EC_WRITE_U16(datagram->data, config->watchdog_divider); |
1001 EC_WRITE_U16(datagram->data, config->watchdog_divider); |
1052 fsm->retries = EC_FSM_RETRIES; |
1002 fsm->retries = EC_FSM_RETRIES; |
1053 fsm->state = ec_fsm_slave_config_state_watchdog_divider; |
1003 fsm->state = ec_fsm_slave_config_state_watchdog_divider; |
1070 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1020 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1071 return; |
1021 return; |
1072 |
1022 |
1073 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1023 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1074 fsm->state = ec_fsm_slave_config_state_error; |
1024 fsm->state = ec_fsm_slave_config_state_error; |
1075 EC_ERR("Failed to receive watchdog divider configuration datagram for" |
1025 EC_SLAVE_ERR(slave, "Failed to receive watchdog divider" |
1076 " slave %u: ", slave->ring_position); |
1026 " configuration datagram: "); |
1077 ec_datagram_print_state(datagram); |
1027 ec_datagram_print_state(datagram); |
1078 return; |
1028 return; |
1079 } |
1029 } |
1080 |
1030 |
1081 if (datagram->working_counter != 1) { |
1031 if (datagram->working_counter != 1) { |
1082 slave->error_flag = 1; |
1032 slave->error_flag = 1; |
1083 EC_WARN("Failed to set watchdog divider of slave %u: ", |
1033 EC_SLAVE_WARN(slave, "Failed to set watchdog divider: "); |
1084 slave->ring_position); |
|
1085 ec_datagram_print_wc_error(datagram); |
1034 ec_datagram_print_wc_error(datagram); |
1086 return; |
1035 return; |
1087 } |
1036 } |
1088 |
1037 |
1089 ec_fsm_slave_config_enter_watchdog(fsm); |
1038 ec_fsm_slave_config_enter_watchdog(fsm); |
1100 ec_datagram_t *datagram = fsm->datagram; |
1049 ec_datagram_t *datagram = fsm->datagram; |
1101 ec_slave_t *slave = fsm->slave; |
1050 ec_slave_t *slave = fsm->slave; |
1102 ec_slave_config_t *config = slave->config; |
1051 ec_slave_config_t *config = slave->config; |
1103 |
1052 |
1104 if (config && config->watchdog_intervals) { |
1053 if (config && config->watchdog_intervals) { |
1105 if (slave->master->debug_level) |
1054 EC_SLAVE_DBG(slave, 1, "Setting process data" |
1106 EC_DBG("Setting process data watchdog intervals to %u.\n", |
1055 " watchdog intervals to %u.\n", config->watchdog_intervals); |
1107 config->watchdog_intervals); |
|
1108 |
1056 |
1109 ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2); |
1057 ec_datagram_fpwr(datagram, slave->station_address, 0x0420, 2); |
1110 EC_WRITE_U16(datagram->data, config->watchdog_intervals); |
1058 EC_WRITE_U16(datagram->data, config->watchdog_intervals); |
1111 |
1059 |
1112 fsm->retries = EC_FSM_RETRIES; |
1060 fsm->retries = EC_FSM_RETRIES; |
1131 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1079 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1132 return; |
1080 return; |
1133 |
1081 |
1134 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1082 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1135 fsm->state = ec_fsm_slave_config_state_error; |
1083 fsm->state = ec_fsm_slave_config_state_error; |
1136 EC_ERR("Failed to receive sync manager watchdog configuration " |
1084 EC_SLAVE_ERR(slave, "Failed to receive sync manager" |
1137 "datagram for slave %u: ", slave->ring_position); |
1085 " watchdog configuration datagram: "); |
1138 ec_datagram_print_state(datagram); |
1086 ec_datagram_print_state(datagram); |
1139 return; |
1087 return; |
1140 } |
1088 } |
1141 |
1089 |
1142 if (datagram->working_counter != 1) { |
1090 if (datagram->working_counter != 1) { |
1143 EC_WARN("Failed to set process data watchdog intervals of slave %u: ", |
1091 EC_SLAVE_WARN(slave, "Failed to set process data" |
1144 slave->ring_position); |
1092 " watchdog intervals: "); |
1145 ec_datagram_print_wc_error(datagram); |
1093 ec_datagram_print_wc_error(datagram); |
1146 } |
1094 } |
1147 |
1095 |
1148 ec_fsm_slave_config_enter_pdo_sync(fsm); |
1096 ec_fsm_slave_config_enter_pdo_sync(fsm); |
1149 } |
1097 } |
1218 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1166 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1219 return; |
1167 return; |
1220 |
1168 |
1221 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1169 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1222 fsm->state = ec_fsm_slave_config_state_error; |
1170 fsm->state = ec_fsm_slave_config_state_error; |
1223 EC_ERR("Failed to receive process data sync manager configuration" |
1171 EC_SLAVE_ERR(slave, "Failed to receive process data sync" |
1224 " datagram for slave %u: ", slave->ring_position); |
1172 " manager configuration datagram: "); |
1225 ec_datagram_print_state(datagram); |
1173 ec_datagram_print_state(datagram); |
1226 return; |
1174 return; |
1227 } |
1175 } |
1228 |
1176 |
1229 if (datagram->working_counter != 1) { |
1177 if (datagram->working_counter != 1) { |
1230 slave->error_flag = 1; |
1178 slave->error_flag = 1; |
1231 fsm->state = ec_fsm_slave_config_state_error; |
1179 fsm->state = ec_fsm_slave_config_state_error; |
1232 EC_ERR("Failed to set process data sync managers of slave %u: ", |
1180 EC_SLAVE_ERR(slave, "Failed to set process data sync managers: "); |
1233 slave->ring_position); |
|
1234 ec_datagram_print_wc_error(datagram); |
1181 ec_datagram_print_wc_error(datagram); |
1235 return; |
1182 return; |
1236 } |
1183 } |
1237 |
1184 |
1238 ec_fsm_slave_config_enter_fmmu(fsm); |
1185 ec_fsm_slave_config_enter_fmmu(fsm); |
1258 } |
1205 } |
1259 |
1206 |
1260 if (slave->base_fmmu_count < slave->config->used_fmmus) { |
1207 if (slave->base_fmmu_count < slave->config->used_fmmus) { |
1261 slave->error_flag = 1; |
1208 slave->error_flag = 1; |
1262 fsm->state = ec_fsm_slave_config_state_error; |
1209 fsm->state = ec_fsm_slave_config_state_error; |
1263 EC_ERR("Slave %u has less FMMUs (%u) than requested (%u).\n", |
1210 EC_SLAVE_ERR(slave, "Slave has less FMMUs (%u)" |
1264 slave->ring_position, slave->base_fmmu_count, |
1211 " than requested (%u).\n", slave->base_fmmu_count, |
1265 slave->config->used_fmmus); |
1212 slave->config->used_fmmus); |
1266 return; |
1213 return; |
1267 } |
1214 } |
1268 |
1215 |
1269 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1216 if (!slave->base_fmmu_count) { // skip FMMU configuration |
1278 for (i = 0; i < slave->config->used_fmmus; i++) { |
1225 for (i = 0; i < slave->config->used_fmmus; i++) { |
1279 fmmu = &slave->config->fmmu_configs[i]; |
1226 fmmu = &slave->config->fmmu_configs[i]; |
1280 if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) { |
1227 if (!(sync = ec_slave_get_sync(slave, fmmu->sync_index))) { |
1281 slave->error_flag = 1; |
1228 slave->error_flag = 1; |
1282 fsm->state = ec_fsm_slave_config_state_error; |
1229 fsm->state = ec_fsm_slave_config_state_error; |
1283 EC_ERR("Failed to determine PDO sync manager for FMMU on slave" |
1230 EC_SLAVE_ERR(slave, "Failed to determine PDO sync manager" |
1284 " %u!\n", slave->ring_position); |
1231 " for FMMU!\n"); |
1285 return; |
1232 return; |
1286 } |
1233 } |
1287 ec_fmmu_config_page(fmmu, sync, |
1234 ec_fmmu_config_page(fmmu, sync, |
1288 datagram->data + EC_FMMU_PAGE_SIZE * i); |
1235 datagram->data + EC_FMMU_PAGE_SIZE * i); |
1289 } |
1236 } |
1306 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1253 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1307 return; |
1254 return; |
1308 |
1255 |
1309 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1256 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1310 fsm->state = ec_fsm_slave_config_state_error; |
1257 fsm->state = ec_fsm_slave_config_state_error; |
1311 EC_ERR("Failed to receive FMMUs datagram for slave %u: ", |
1258 EC_SLAVE_ERR(slave, "Failed to receive FMMUs datagram: "); |
1312 slave->ring_position); |
|
1313 ec_datagram_print_state(datagram); |
1259 ec_datagram_print_state(datagram); |
1314 return; |
1260 return; |
1315 } |
1261 } |
1316 |
1262 |
1317 if (datagram->working_counter != 1) { |
1263 if (datagram->working_counter != 1) { |
1318 slave->error_flag = 1; |
1264 slave->error_flag = 1; |
1319 fsm->state = ec_fsm_slave_config_state_error; |
1265 fsm->state = ec_fsm_slave_config_state_error; |
1320 EC_ERR("Failed to set FMMUs of slave %u: ", |
1266 EC_SLAVE_ERR(slave, "Failed to set FMMUs: "); |
1321 slave->ring_position); |
|
1322 ec_datagram_print_wc_error(datagram); |
1267 ec_datagram_print_wc_error(datagram); |
1323 return; |
1268 return; |
1324 } |
1269 } |
1325 |
1270 |
1326 ec_fsm_slave_config_enter_dc_cycle(fsm); |
1271 ec_fsm_slave_config_enter_dc_cycle(fsm); |
1343 return; |
1288 return; |
1344 } |
1289 } |
1345 |
1290 |
1346 if (config->dc_assign_activate) { |
1291 if (config->dc_assign_activate) { |
1347 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
1292 if (!slave->base_dc_supported || !slave->has_dc_system_time) { |
1348 EC_WARN("Slave %u seems not to support distributed clocks!\n", |
1293 EC_SLAVE_WARN(slave, "Slave seems not to support" |
1349 slave->ring_position); |
1294 " distributed clocks!\n"); |
1350 } |
1295 } |
1351 |
1296 |
1352 if (slave->master->debug_level) |
1297 EC_SLAVE_DBG(slave, 1, "Setting DC cycle times to %u / %u.\n", |
1353 EC_DBG("Slave %u: Setting DC cycle times to %u / %u.\n", |
1298 config->dc_sync[0].cycle_time, config->dc_sync[1].cycle_time); |
1354 slave->ring_position, |
|
1355 config->dc_sync[0].cycle_time, |
|
1356 config->dc_sync[1].cycle_time); |
|
1357 |
1299 |
1358 // set DC cycle times |
1300 // set DC cycle times |
1359 ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); |
1301 ec_datagram_fpwr(datagram, slave->station_address, 0x09A0, 8); |
1360 EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time); |
1302 EC_WRITE_U32(datagram->data, config->dc_sync[0].cycle_time); |
1361 EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time); |
1303 EC_WRITE_U32(datagram->data + 4, config->dc_sync[1].cycle_time); |
1390 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1332 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1391 return; |
1333 return; |
1392 |
1334 |
1393 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1335 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1394 fsm->state = ec_fsm_slave_config_state_error; |
1336 fsm->state = ec_fsm_slave_config_state_error; |
1395 EC_ERR("Failed to receive DC cycle times datagram for slave %u: ", |
1337 EC_SLAVE_ERR(slave, "Failed to receive DC cycle times datagram: "); |
1396 slave->ring_position); |
|
1397 ec_datagram_print_state(datagram); |
1338 ec_datagram_print_state(datagram); |
1398 return; |
1339 return; |
1399 } |
1340 } |
1400 |
1341 |
1401 if (datagram->working_counter != 1) { |
1342 if (datagram->working_counter != 1) { |
1402 slave->error_flag = 1; |
1343 slave->error_flag = 1; |
1403 fsm->state = ec_fsm_slave_config_state_error; |
1344 fsm->state = ec_fsm_slave_config_state_error; |
1404 EC_ERR("Failed to set DC cycle times of slave %u: ", |
1345 EC_SLAVE_ERR(slave, "Failed to set DC cycle times: "); |
1405 slave->ring_position); |
|
1406 ec_datagram_print_wc_error(datagram); |
1346 ec_datagram_print_wc_error(datagram); |
1407 return; |
1347 return; |
1408 } |
1348 } |
1409 |
1349 |
1410 // set DC start time |
1350 // set DC start time |
1422 |
1362 |
1423 start = start_time + |
1363 start = start_time + |
1424 sync0->cycle_time - remainder + sync0->shift_time; |
1364 sync0->cycle_time - remainder + sync0->shift_time; |
1425 |
1365 |
1426 if (master->debug_level) { |
1366 if (master->debug_level) { |
1427 EC_DBG("app_start_time=%llu\n", master->app_start_time); |
1367 EC_SLAVE_DBG(slave, 1, "app_start_time=%llu\n", |
1428 EC_DBG(" start_time=%llu\n", start_time); |
1368 master->app_start_time); |
1429 EC_DBG(" cycle_time=%u\n", sync0->cycle_time); |
1369 EC_SLAVE_DBG(slave, 1, " start_time=%llu\n", |
1430 EC_DBG(" shift_time=%u\n", sync0->shift_time); |
1370 start_time); |
1431 EC_DBG(" remainder=%u\n", remainder); |
1371 EC_SLAVE_DBG(slave, 1, " cycle_time=%u\n", |
1432 EC_DBG(" start=%llu\n", start); |
1372 sync0->cycle_time); |
|
1373 EC_SLAVE_DBG(slave, 1, " shift_time=%u\n", |
|
1374 sync0->shift_time); |
|
1375 EC_SLAVE_DBG(slave, 1, " remainder=%u\n", |
|
1376 remainder); |
|
1377 EC_SLAVE_DBG(slave, 1, " start=%llu\n", |
|
1378 start); |
1433 } |
1379 } |
1434 start_time = start; |
1380 start_time = start; |
1435 } else { |
1381 } else { |
1436 EC_WARN("No application time supplied. Cyclic start time will " |
1382 EC_SLAVE_WARN(slave, "No application time supplied." |
1437 "not be in phase for slave %u.\n", slave->ring_position); |
1383 " Cyclic start time will not be in phase.\n"); |
1438 } |
1384 } |
1439 } |
1385 } |
1440 |
1386 |
1441 if (master->debug_level) |
1387 EC_SLAVE_DBG(slave, 1, "Setting DC cyclic operation" |
1442 EC_DBG("Slave %u: Setting DC cyclic operation start time to %llu.\n", |
1388 " start time to %llu.\n", start_time); |
1443 slave->ring_position, start_time); |
|
1444 |
1389 |
1445 ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); |
1390 ec_datagram_fpwr(datagram, slave->station_address, 0x0990, 8); |
1446 EC_WRITE_U64(datagram->data, start_time); |
1391 EC_WRITE_U64(datagram->data, start_time); |
1447 fsm->retries = EC_FSM_RETRIES; |
1392 fsm->retries = EC_FSM_RETRIES; |
1448 fsm->state = ec_fsm_slave_config_state_dc_start; |
1393 fsm->state = ec_fsm_slave_config_state_dc_start; |
1468 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1413 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1469 return; |
1414 return; |
1470 |
1415 |
1471 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1416 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1472 fsm->state = ec_fsm_slave_config_state_error; |
1417 fsm->state = ec_fsm_slave_config_state_error; |
1473 EC_ERR("Failed to receive DC start time datagram for slave %u: ", |
1418 EC_SLAVE_ERR(slave, "Failed to receive DC start time datagram: "); |
1474 slave->ring_position); |
|
1475 ec_datagram_print_state(datagram); |
1419 ec_datagram_print_state(datagram); |
1476 return; |
1420 return; |
1477 } |
1421 } |
1478 |
1422 |
1479 if (datagram->working_counter != 1) { |
1423 if (datagram->working_counter != 1) { |
1480 slave->error_flag = 1; |
1424 slave->error_flag = 1; |
1481 fsm->state = ec_fsm_slave_config_state_error; |
1425 fsm->state = ec_fsm_slave_config_state_error; |
1482 EC_ERR("Failed to set DC start time of slave %u: ", |
1426 EC_SLAVE_ERR(slave, "Failed to set DC start time: "); |
1483 slave->ring_position); |
|
1484 ec_datagram_print_wc_error(datagram); |
1427 ec_datagram_print_wc_error(datagram); |
1485 return; |
1428 return; |
1486 } |
1429 } |
1487 |
1430 |
1488 if (slave->master->debug_level) |
1431 EC_SLAVE_DBG(slave, 1, "Setting DC AssignActivate to 0x%04x.\n", |
1489 EC_DBG("Slave %u: Setting DC AssignActivate to 0x%04x.\n", |
1432 config->dc_assign_activate); |
1490 slave->ring_position, config->dc_assign_activate); |
|
1491 |
1433 |
1492 // assign sync unit to EtherCAT or PDI |
1434 // assign sync unit to EtherCAT or PDI |
1493 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
1435 ec_datagram_fpwr(datagram, slave->station_address, 0x0980, 2); |
1494 EC_WRITE_U16(datagram->data, config->dc_assign_activate); |
1436 EC_WRITE_U16(datagram->data, config->dc_assign_activate); |
1495 fsm->retries = EC_FSM_RETRIES; |
1437 fsm->retries = EC_FSM_RETRIES; |
1510 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1452 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1511 return; |
1453 return; |
1512 |
1454 |
1513 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1455 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1514 fsm->state = ec_fsm_slave_config_state_error; |
1456 fsm->state = ec_fsm_slave_config_state_error; |
1515 EC_ERR("Failed to receive DC activation datagram for slave %u: ", |
1457 EC_SLAVE_ERR(slave, "Failed to receive DC activation datagram: "); |
1516 slave->ring_position); |
|
1517 ec_datagram_print_state(datagram); |
1458 ec_datagram_print_state(datagram); |
1518 return; |
1459 return; |
1519 } |
1460 } |
1520 |
1461 |
1521 if (datagram->working_counter != 1) { |
1462 if (datagram->working_counter != 1) { |
1522 slave->error_flag = 1; |
1463 slave->error_flag = 1; |
1523 fsm->state = ec_fsm_slave_config_state_error; |
1464 fsm->state = ec_fsm_slave_config_state_error; |
1524 EC_ERR("Failed to set DC cyclic operation state of slave %u: ", |
1465 EC_SLAVE_ERR(slave, "Failed to activate DC: "); |
1525 slave->ring_position); |
|
1526 ec_datagram_print_wc_error(datagram); |
1466 ec_datagram_print_wc_error(datagram); |
1527 return; |
1467 return; |
1528 } |
1468 } |
1529 |
1469 |
1530 ec_fsm_slave_config_enter_safeop(fsm); |
1470 ec_fsm_slave_config_enter_safeop(fsm); |
1549 */ |
1489 */ |
1550 void ec_fsm_slave_config_state_safeop( |
1490 void ec_fsm_slave_config_state_safeop( |
1551 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1491 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1552 ) |
1492 ) |
1553 { |
1493 { |
1554 ec_master_t *master = fsm->slave->master; |
|
1555 ec_slave_t *slave = fsm->slave; |
1494 ec_slave_t *slave = fsm->slave; |
1556 |
1495 |
1557 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1496 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1558 |
1497 |
1559 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1498 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1563 return; |
1502 return; |
1564 } |
1503 } |
1565 |
1504 |
1566 // slave is now in SAFEOP |
1505 // slave is now in SAFEOP |
1567 |
1506 |
1568 if (master->debug_level) { |
1507 EC_SLAVE_DBG(slave, 1, "Now in SAFEOP.\n"); |
1569 EC_DBG("Slave %u is now in SAFEOP.\n", slave->ring_position); |
|
1570 } |
|
1571 |
1508 |
1572 if (fsm->slave->current_state == fsm->slave->requested_state) { |
1509 if (fsm->slave->current_state == fsm->slave->requested_state) { |
1573 fsm->state = ec_fsm_slave_config_state_end; // successful |
1510 fsm->state = ec_fsm_slave_config_state_end; // successful |
1574 if (master->debug_level) { |
1511 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
1575 EC_DBG("Finished configuration of slave %u.\n", |
|
1576 slave->ring_position); |
|
1577 } |
|
1578 return; |
1512 return; |
1579 } |
1513 } |
1580 |
1514 |
1581 // set state to OP |
1515 // set state to OP |
1582 fsm->state = ec_fsm_slave_config_state_op; |
1516 fsm->state = ec_fsm_slave_config_state_op; |
1590 */ |
1524 */ |
1591 void ec_fsm_slave_config_state_op( |
1525 void ec_fsm_slave_config_state_op( |
1592 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1526 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1593 ) |
1527 ) |
1594 { |
1528 { |
1595 ec_master_t *master = fsm->slave->master; |
|
1596 ec_slave_t *slave = fsm->slave; |
1529 ec_slave_t *slave = fsm->slave; |
1597 |
1530 |
1598 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1531 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
1599 |
1532 |
1600 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1533 if (!ec_fsm_change_success(fsm->fsm_change)) { |
1604 return; |
1537 return; |
1605 } |
1538 } |
1606 |
1539 |
1607 // slave is now in OP |
1540 // slave is now in OP |
1608 |
1541 |
1609 if (master->debug_level) { |
1542 EC_SLAVE_DBG(slave, 1, "Now in OP. Finished configuration.\n"); |
1610 EC_DBG("Slave %u is now in OP.\n", slave->ring_position); |
|
1611 EC_DBG("Finished configuration of slave %u.\n", slave->ring_position); |
|
1612 } |
|
1613 |
1543 |
1614 fsm->state = ec_fsm_slave_config_state_end; // successful |
1544 fsm->state = ec_fsm_slave_config_state_end; // successful |
1615 } |
1545 } |
1616 |
1546 |
1617 /*****************************************************************************/ |
1547 /*****************************************************************************/ |
1620 */ |
1550 */ |
1621 void ec_fsm_slave_config_reconfigure( |
1551 void ec_fsm_slave_config_reconfigure( |
1622 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1552 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
1623 ) |
1553 ) |
1624 { |
1554 { |
1625 if (fsm->slave->master->debug_level) { |
1555 EC_SLAVE_DBG(fsm->slave, 1, "Slave configuration detached during " |
1626 EC_DBG("Slave configuration for slave %u detached during " |
1556 "configuration. Reconfiguring."); |
1627 "configuration. Reconfiguring.", fsm->slave->ring_position); |
|
1628 } |
|
1629 |
1557 |
1630 ec_fsm_slave_config_enter_init(fsm); // reconfigure |
1558 ec_fsm_slave_config_enter_init(fsm); // reconfigure |
1631 } |
1559 } |
1632 |
1560 |
1633 /****************************************************************************** |
1561 /****************************************************************************** |