733 slave->ring_position); |
733 slave->ring_position); |
734 } |
734 } |
735 return; |
735 return; |
736 } |
736 } |
737 |
737 |
|
738 ec_fsm_slave_config_enter_sdo_conf(fsm); |
|
739 } |
|
740 |
|
741 /*****************************************************************************/ |
|
742 |
|
743 /** Check for SDO configurations to be applied. |
|
744 */ |
|
745 void ec_fsm_slave_config_enter_sdo_conf( |
|
746 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
747 ) |
|
748 { |
|
749 ec_slave_t *slave = fsm->slave; |
|
750 |
738 if (!slave->config) { |
751 if (!slave->config) { |
739 EC_WARN("Slave %u is not configured.\n", slave->ring_position); |
752 ec_fsm_slave_config_enter_pdo_sync(fsm); |
740 ec_fsm_slave_config_enter_safeop(fsm); |
753 return; |
741 return; |
754 } |
742 } |
|
743 |
|
744 ec_fsm_slave_config_enter_sdo_conf(fsm); |
|
745 } |
|
746 |
|
747 /*****************************************************************************/ |
|
748 |
|
749 /** Check for SDO configurations to be applied. |
|
750 */ |
|
751 void ec_fsm_slave_config_enter_sdo_conf( |
|
752 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
753 ) |
|
754 { |
|
755 ec_slave_t *slave = fsm->slave; |
|
756 |
755 |
757 // No CoE configuration to be applied? |
756 // No CoE configuration to be applied? |
758 // FIXME check for config |
|
759 if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration |
757 if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration |
760 ec_fsm_slave_config_enter_pdo_conf(fsm); |
758 ec_fsm_slave_config_enter_pdo_conf(fsm); |
761 return; |
759 return; |
762 } |
760 } |
763 |
761 |
860 ec_slave_t *slave = fsm->slave; |
858 ec_slave_t *slave = fsm->slave; |
861 ec_datagram_t *datagram = fsm->datagram; |
859 ec_datagram_t *datagram = fsm->datagram; |
862 unsigned int i, offset, num_pdo_syncs; |
860 unsigned int i, offset, num_pdo_syncs; |
863 uint8_t sync_index; |
861 uint8_t sync_index; |
864 const ec_sync_t *sync; |
862 const ec_sync_t *sync; |
865 const ec_sync_config_t *sync_config; |
|
866 uint16_t size; |
863 uint16_t size; |
|
864 ec_direction_t dir; |
867 |
865 |
868 if (slave->sii.mailbox_protocols) { |
866 if (slave->sii.mailbox_protocols) { |
869 offset = 2; // slave has mailboxes |
867 offset = 2; // slave has mailboxes |
870 } else { |
868 } else { |
871 offset = 0; |
869 offset = 0; |
886 ec_datagram_zero(datagram); |
884 ec_datagram_zero(datagram); |
887 |
885 |
888 for (i = 0; i < num_pdo_syncs; i++) { |
886 for (i = 0; i < num_pdo_syncs; i++) { |
889 sync_index = i + offset; |
887 sync_index = i + offset; |
890 sync = &slave->sii.syncs[sync_index]; |
888 sync = &slave->sii.syncs[sync_index]; |
891 sync_config = &slave->config->sync_configs[sync_index]; |
889 |
892 size = ec_pdo_list_total_size(&sync_config->pdos); |
890 if (slave->config) { |
893 ec_sync_page(sync, sync_index, size, sync_config->dir, |
891 const ec_sync_config_t *sync_config; |
894 datagram->data + EC_SYNC_PAGE_SIZE * i); |
892 sync_config = &slave->config->sync_configs[sync_index]; |
|
893 size = ec_pdo_list_total_size(&sync_config->pdos); |
|
894 dir = sync_config->dir; |
|
895 } else { |
|
896 size = sync->default_length; |
|
897 dir = EC_DIR_INVALID; |
|
898 } |
|
899 |
|
900 ec_sync_page(sync, sync_index, size, dir, datagram->data + |
|
901 EC_SYNC_PAGE_SIZE * i); |
895 } |
902 } |
896 |
903 |
897 fsm->retries = EC_FSM_RETRIES; |
904 fsm->retries = EC_FSM_RETRIES; |
898 fsm->state = ec_fsm_slave_config_state_pdo_sync; |
905 fsm->state = ec_fsm_slave_config_state_pdo_sync; |
899 } |
906 } |
944 ec_datagram_t *datagram = fsm->datagram; |
951 ec_datagram_t *datagram = fsm->datagram; |
945 unsigned int i; |
952 unsigned int i; |
946 const ec_fmmu_config_t *fmmu; |
953 const ec_fmmu_config_t *fmmu; |
947 const ec_sync_t *sync; |
954 const ec_sync_t *sync; |
948 |
955 |
949 if (!slave->config) { // config removed in the meantime |
956 if (!slave->config) { |
950 ec_fsm_slave_config_reconfigure(fsm); |
957 ec_fsm_slave_config_enter_safeop(fsm); |
951 return; |
958 return; |
952 } |
959 } |
953 |
960 |
954 if (slave->base_fmmu_count < slave->config->used_fmmus) { |
961 if (slave->base_fmmu_count < slave->config->used_fmmus) { |
955 slave->error_flag = 1; |
962 slave->error_flag = 1; |