48 void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *); |
48 void ec_fsm_slave_scan_state_state(ec_fsm_slave_scan_t *); |
49 void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *); |
49 void ec_fsm_slave_scan_state_base(ec_fsm_slave_scan_t *); |
50 void ec_fsm_slave_scan_state_dc_cap(ec_fsm_slave_scan_t *); |
50 void ec_fsm_slave_scan_state_dc_cap(ec_fsm_slave_scan_t *); |
51 void ec_fsm_slave_scan_state_dc_times(ec_fsm_slave_scan_t *); |
51 void ec_fsm_slave_scan_state_dc_times(ec_fsm_slave_scan_t *); |
52 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *); |
52 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *); |
|
53 #ifdef EC_SII_ASSIGN |
|
54 void ec_fsm_slave_scan_state_assign_sii(ec_fsm_slave_scan_t *); |
|
55 #endif |
53 void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *); |
56 void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *); |
54 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *); |
57 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *); |
55 #ifdef EC_REGALIAS |
58 #ifdef EC_REGALIAS |
56 void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *); |
59 void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *); |
57 #endif |
60 #endif |
437 fsm->state = ec_fsm_slave_scan_state_datalink; |
440 fsm->state = ec_fsm_slave_scan_state_datalink; |
438 } |
441 } |
439 |
442 |
440 /*****************************************************************************/ |
443 /*****************************************************************************/ |
441 |
444 |
|
445 /** Enter slave scan state SII_SIZE. |
|
446 */ |
|
447 void ec_fsm_slave_scan_enter_sii_size( |
|
448 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
|
449 ) |
|
450 { |
|
451 // Start fetching SII size |
|
452 |
|
453 fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header |
|
454 ec_fsm_sii_read(&fsm->fsm_sii, fsm->slave, fsm->sii_offset, |
|
455 EC_FSM_SII_USE_CONFIGURED_ADDRESS); |
|
456 fsm->state = ec_fsm_slave_scan_state_sii_size; |
|
457 fsm->state(fsm); // execute state immediately |
|
458 } |
|
459 |
|
460 /*****************************************************************************/ |
|
461 |
|
462 #ifdef EC_SII_ASSIGN |
|
463 |
|
464 /** Enter slave scan state ASSIGN_SII. |
|
465 */ |
|
466 void ec_fsm_slave_scan_enter_assign_sii( |
|
467 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
|
468 ) |
|
469 { |
|
470 ec_datagram_t *datagram = fsm->datagram; |
|
471 ec_slave_t *slave = fsm->slave; |
|
472 |
|
473 // assign SII to ECAT |
|
474 ec_datagram_fpwr(datagram, slave->station_address, 0x0500, 1); |
|
475 EC_WRITE_U8(datagram->data, 0x00); // EtherCAT |
|
476 fsm->retries = EC_FSM_RETRIES; |
|
477 fsm->state = ec_fsm_slave_scan_state_assign_sii; |
|
478 } |
|
479 |
|
480 #endif |
|
481 |
|
482 /*****************************************************************************/ |
|
483 |
442 /** |
484 /** |
443 Slave scan state: DATALINK. |
485 Slave scan state: DATALINK. |
444 */ |
486 */ |
445 |
487 |
446 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *fsm /**< slave state machine */) |
488 void ec_fsm_slave_scan_state_datalink( |
|
489 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
|
490 ) |
447 { |
491 { |
448 ec_datagram_t *datagram = fsm->datagram; |
492 ec_datagram_t *datagram = fsm->datagram; |
449 ec_slave_t *slave = fsm->slave; |
493 ec_slave_t *slave = fsm->slave; |
450 uint16_t dl_status; |
494 uint16_t dl_status; |
451 unsigned int i; |
495 unsigned int i; |
476 dl_status & (1 << (8 + i * 2)) ? 1 : 0; |
520 dl_status & (1 << (8 + i * 2)) ? 1 : 0; |
477 slave->ports[i].link.signal_detected = |
521 slave->ports[i].link.signal_detected = |
478 dl_status & (1 << (9 + i * 2)) ? 1 : 0; |
522 dl_status & (1 << (9 + i * 2)) ? 1 : 0; |
479 } |
523 } |
480 |
524 |
481 // Start fetching SII size |
525 #ifdef EC_SII_ASSIGN |
482 |
526 ec_fsm_slave_scan_enter_assign_sii(fsm); |
483 fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header |
527 #else |
484 ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, |
528 ec_fsm_slave_scan_enter_sii_size(fsm); |
485 EC_FSM_SII_USE_CONFIGURED_ADDRESS); |
529 #endif |
486 fsm->state = ec_fsm_slave_scan_state_sii_size; |
530 } |
487 fsm->state(fsm); // execute state immediately |
531 |
488 } |
532 /*****************************************************************************/ |
|
533 |
|
534 #ifdef EC_SII_ASSIGN |
|
535 |
|
536 /** |
|
537 Slave scan state: ASSIGN_SII. |
|
538 */ |
|
539 |
|
540 void ec_fsm_slave_scan_state_assign_sii( |
|
541 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
|
542 ) |
|
543 { |
|
544 ec_datagram_t *datagram = fsm->datagram; |
|
545 ec_slave_t *slave = fsm->slave; |
|
546 |
|
547 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
|
548 return; |
|
549 } |
|
550 |
|
551 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
552 EC_SLAVE_WARN(slave, "Failed to receive SII assignment datagram: "); |
|
553 ec_datagram_print_state(datagram); |
|
554 // Try to go on, probably assignment is correct |
|
555 goto continue_with_sii_size; |
|
556 } |
|
557 |
|
558 if (datagram->working_counter != 1) { |
|
559 EC_SLAVE_WARN(slave, "Failed to assign SII to EtherCAT: "); |
|
560 ec_datagram_print_wc_error(datagram); |
|
561 // Try to go on, probably assignment is correct |
|
562 } |
|
563 |
|
564 continue_with_sii_size: |
|
565 ec_fsm_slave_scan_enter_sii_size(fsm); |
|
566 } |
|
567 |
|
568 #endif |
489 |
569 |
490 /*****************************************************************************/ |
570 /*****************************************************************************/ |
491 |
571 |
492 /** |
572 /** |
493 Slave scan state: SII SIZE. |
573 Slave scan state: SII SIZE. |
494 */ |
574 */ |
495 |
575 |
496 void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state machine */) |
576 void ec_fsm_slave_scan_state_sii_size( |
|
577 ec_fsm_slave_scan_t *fsm /**< slave state machine */ |
|
578 ) |
497 { |
579 { |
498 ec_slave_t *slave = fsm->slave; |
580 ec_slave_t *slave = fsm->slave; |
499 uint16_t cat_type, cat_size; |
581 uint16_t cat_type, cat_size; |
500 |
582 |
501 if (ec_fsm_sii_exec(&fsm->fsm_sii)) |
583 if (ec_fsm_sii_exec(&fsm->fsm_sii)) |