881 ec_fsm_slave_conf_enter_safeop(fsm); |
881 ec_fsm_slave_conf_enter_safeop(fsm); |
882 return; |
882 return; |
883 } |
883 } |
884 |
884 |
885 // No CoE configuration to be applied? |
885 // No CoE configuration to be applied? |
886 if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration |
886 if (list_empty(&slave->config->sdo_configs)) { // skip Sdo configuration |
887 ec_fsm_slave_conf_enter_mapping(fsm); |
887 ec_fsm_slave_conf_enter_mapping(fsm); |
888 return; |
888 return; |
889 } |
889 } |
890 |
890 |
891 // start SDO configuration |
891 // start Sdo configuration |
892 fsm->state = ec_fsm_slave_conf_state_sdo_conf; |
892 fsm->state = ec_fsm_slave_conf_state_sdo_conf; |
893 fsm->sdodata = |
893 fsm->sdodata = |
894 list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list); |
894 list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list); |
895 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
895 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
896 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
896 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
907 ) |
907 ) |
908 { |
908 { |
909 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
909 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
910 |
910 |
911 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
911 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
912 EC_ERR("SDO configuration failed for slave %u.\n", |
912 EC_ERR("Sdo configuration failed for slave %u.\n", |
913 fsm->slave->ring_position); |
913 fsm->slave->ring_position); |
914 fsm->slave->error_flag = 1; |
914 fsm->slave->error_flag = 1; |
915 fsm->state = ec_fsm_slave_state_error; |
915 fsm->state = ec_fsm_slave_state_error; |
916 return; |
916 return; |
917 } |
917 } |
918 |
918 |
919 // Another SDO to configure? |
919 // Another Sdo to configure? |
920 if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) { |
920 if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) { |
921 fsm->sdodata = |
921 fsm->sdodata = |
922 list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list); |
922 list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list); |
923 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
923 ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata); |
924 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
924 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
925 return; |
925 return; |
926 } |
926 } |
927 |
927 |
928 // All SDOs are now configured. |
928 // All Sdos are now configured. |
929 ec_fsm_slave_conf_enter_mapping(fsm); |
929 ec_fsm_slave_conf_enter_mapping(fsm); |
930 } |
930 } |
931 |
931 |
932 /*****************************************************************************/ |
932 /*****************************************************************************/ |
933 |
933 |
934 /** |
934 /** |
935 * Check for PDO mappings to be applied. |
935 * Check for Pdo mappings to be applied. |
936 */ |
936 */ |
937 |
937 |
938 void ec_fsm_slave_conf_enter_mapping( |
938 void ec_fsm_slave_conf_enter_mapping( |
939 ec_fsm_slave_t *fsm /**< slave state machine */ |
939 ec_fsm_slave_t *fsm /**< slave state machine */ |
940 ) |
940 ) |
941 { |
941 { |
942 // start configuring PDO mapping |
942 // start configuring Pdo mapping |
943 fsm->state = ec_fsm_slave_conf_state_mapping; |
943 fsm->state = ec_fsm_slave_conf_state_mapping; |
944 ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave); |
944 ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave); |
945 ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately |
945 ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately |
946 } |
946 } |
947 |
947 |
956 ) |
956 ) |
957 { |
957 { |
958 if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return; |
958 if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return; |
959 |
959 |
960 if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) { |
960 if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) { |
961 EC_ERR("PDO mapping configuration failed for slave %u.\n", |
961 EC_ERR("Pdo mapping configuration failed for slave %u.\n", |
962 fsm->slave->ring_position); |
962 fsm->slave->ring_position); |
963 fsm->slave->error_flag = 1; |
963 fsm->slave->error_flag = 1; |
964 fsm->state = ec_fsm_slave_state_error; |
964 fsm->state = ec_fsm_slave_state_error; |
965 return; |
965 return; |
966 } |
966 } |
1099 memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
1099 memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count); |
1100 for (i = 0; i < slave->config->used_fmmus; i++) { |
1100 for (i = 0; i < slave->config->used_fmmus; i++) { |
1101 fmmu = &slave->config->fmmu_configs[i]; |
1101 fmmu = &slave->config->fmmu_configs[i]; |
1102 if (!(sync = ec_slave_get_pdo_sync(slave, fmmu->dir))) { |
1102 if (!(sync = ec_slave_get_pdo_sync(slave, fmmu->dir))) { |
1103 fsm->state = ec_fsm_slave_state_error; |
1103 fsm->state = ec_fsm_slave_state_error; |
1104 EC_ERR("Failed to determine PDO sync manager for FMMU on slave" |
1104 EC_ERR("Failed to determine Pdo sync manager for FMMU on slave" |
1105 " %u!\n", slave->ring_position); |
1105 " %u!\n", slave->ring_position); |
1106 return; |
1106 return; |
1107 } |
1107 } |
1108 ec_fmmu_config_page(fmmu, sync, |
1108 ec_fmmu_config_page(fmmu, sync, |
1109 datagram->data + EC_FMMU_PAGE_SIZE * i); |
1109 datagram->data + EC_FMMU_PAGE_SIZE * i); |