153 |
153 |
154 INIT_LIST_HEAD(&slave->sii_strings); |
154 INIT_LIST_HEAD(&slave->sii_strings); |
155 INIT_LIST_HEAD(&slave->sii_syncs); |
155 INIT_LIST_HEAD(&slave->sii_syncs); |
156 INIT_LIST_HEAD(&slave->sii_pdos); |
156 INIT_LIST_HEAD(&slave->sii_pdos); |
157 INIT_LIST_HEAD(&slave->sdo_dictionary); |
157 INIT_LIST_HEAD(&slave->sdo_dictionary); |
|
158 INIT_LIST_HEAD(&slave->sdo_confs); |
158 INIT_LIST_HEAD(&slave->varsize_fields); |
159 INIT_LIST_HEAD(&slave->varsize_fields); |
159 |
160 |
160 for (i = 0; i < 4; i++) { |
161 for (i = 0; i < 4; i++) { |
161 slave->dl_link[i] = 0; |
162 slave->dl_link[i] = 0; |
162 slave->dl_loop[i] = 0; |
163 slave->dl_loop[i] = 0; |
180 ec_sii_sync_t *sync, *next_sync; |
181 ec_sii_sync_t *sync, *next_sync; |
181 ec_sii_pdo_t *pdo, *next_pdo; |
182 ec_sii_pdo_t *pdo, *next_pdo; |
182 ec_sii_pdo_entry_t *entry, *next_ent; |
183 ec_sii_pdo_entry_t *entry, *next_ent; |
183 ec_sdo_t *sdo, *next_sdo; |
184 ec_sdo_t *sdo, *next_sdo; |
184 ec_sdo_entry_t *en, *next_en; |
185 ec_sdo_entry_t *en, *next_en; |
|
186 ec_sdo_data_t *sdodata, *next_sdodata; |
185 ec_varsize_t *var, *next_var; |
187 ec_varsize_t *var, *next_var; |
186 |
188 |
187 slave = container_of(kobj, ec_slave_t, kobj); |
189 slave = container_of(kobj, ec_slave_t, kobj); |
188 |
190 |
189 // free all string objects |
191 // free all string objects |
227 list_for_each_entry_safe(en, next_en, &sdo->entries, list) { |
229 list_for_each_entry_safe(en, next_en, &sdo->entries, list) { |
228 list_del(&en->list); |
230 list_del(&en->list); |
229 kfree(en); |
231 kfree(en); |
230 } |
232 } |
231 kfree(sdo); |
233 kfree(sdo); |
|
234 } |
|
235 |
|
236 // free all SDO configurations |
|
237 list_for_each_entry_safe(sdodata, next_sdodata, &slave->sdo_confs, list) { |
|
238 list_del(&sdodata->list); |
|
239 kfree(sdodata->data); |
|
240 kfree(sdodata); |
232 } |
241 } |
233 |
242 |
234 // free information about variable sized data fields |
243 // free information about variable sized data fields |
235 list_for_each_entry_safe(var, next_var, &slave->varsize_fields, list) { |
244 list_for_each_entry_safe(var, next_var, &slave->varsize_fields, list) { |
236 list_del(&var->list); |
245 list_del(&var->list); |
911 // TODO: Better bus coupler criterion |
920 // TODO: Better bus coupler criterion |
912 return slave->sii_vendor_id == 0x00000002 |
921 return slave->sii_vendor_id == 0x00000002 |
913 && slave->sii_product_code == 0x044C2C52; |
922 && slave->sii_product_code == 0x044C2C52; |
914 } |
923 } |
915 |
924 |
|
925 /*****************************************************************************/ |
|
926 |
|
927 /** |
|
928 \return 0 in case of success, else < 0 |
|
929 */ |
|
930 |
|
931 int ec_slave_conf_sdo(ec_slave_t *slave, /**< EtherCAT slave */ |
|
932 uint16_t sdo_index, /**< SDO index */ |
|
933 uint8_t sdo_subindex, /**< SDO subindex */ |
|
934 const uint8_t *data, /**< SDO data */ |
|
935 size_t size /**< SDO size in bytes */ |
|
936 ) |
|
937 { |
|
938 ec_sdo_data_t *sdodata; |
|
939 |
|
940 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
|
941 EC_ERR("Slave %i does not support CoE!\n", slave->ring_position); |
|
942 return -1; |
|
943 } |
|
944 |
|
945 if (!(sdodata = (ec_sdo_data_t *) |
|
946 kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) { |
|
947 EC_ERR("Failed to allocate memory for SDO configuration object!\n"); |
|
948 return -1; |
|
949 } |
|
950 |
|
951 if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) { |
|
952 EC_ERR("Failed to allocate memory for SDO configuration data!\n"); |
|
953 kfree(sdodata); |
|
954 return -1; |
|
955 } |
|
956 |
|
957 sdodata->index = sdo_index; |
|
958 sdodata->subindex = sdo_subindex; |
|
959 memcpy(sdodata->data, data, size); |
|
960 sdodata->size = size; |
|
961 |
|
962 list_add_tail(&sdodata->list, &slave->sdo_confs); |
|
963 return 0; |
|
964 } |
|
965 |
916 /****************************************************************************** |
966 /****************************************************************************** |
917 * Realtime interface |
967 * Realtime interface |
918 *****************************************************************************/ |
968 *****************************************************************************/ |
|
969 |
|
970 /** |
|
971 \return 0 in case of success, else < 0 |
|
972 \ingroup RealtimeInterface |
|
973 */ |
|
974 |
|
975 int ecrt_slave_conf_sdo8(ec_slave_t *slave, /**< EtherCAT slave */ |
|
976 uint16_t sdo_index, /**< SDO index */ |
|
977 uint8_t sdo_subindex, /**< SDO subindex */ |
|
978 uint8_t value /**< new SDO value */ |
|
979 ) |
|
980 { |
|
981 uint8_t data[1]; |
|
982 EC_WRITE_U8(data, value); |
|
983 return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 1); |
|
984 } |
|
985 |
|
986 /*****************************************************************************/ |
|
987 |
|
988 /** |
|
989 \return 0 in case of success, else < 0 |
|
990 \ingroup RealtimeInterface |
|
991 */ |
|
992 |
|
993 int ecrt_slave_conf_sdo16(ec_slave_t *slave, /**< EtherCAT slave */ |
|
994 uint16_t sdo_index, /**< SDO index */ |
|
995 uint8_t sdo_subindex, /**< SDO subindex */ |
|
996 uint16_t value /**< new SDO value */ |
|
997 ) |
|
998 { |
|
999 uint8_t data[2]; |
|
1000 EC_WRITE_U16(data, value); |
|
1001 return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 2); |
|
1002 } |
|
1003 |
|
1004 /*****************************************************************************/ |
|
1005 |
|
1006 /** |
|
1007 \return 0 in case of success, else < 0 |
|
1008 \ingroup RealtimeInterface |
|
1009 */ |
|
1010 |
|
1011 int ecrt_slave_conf_sdo32(ec_slave_t *slave, /**< EtherCAT slave */ |
|
1012 uint16_t sdo_index, /**< SDO index */ |
|
1013 uint8_t sdo_subindex, /**< SDO subindex */ |
|
1014 uint32_t value /**< new SDO value */ |
|
1015 ) |
|
1016 { |
|
1017 uint8_t data[4]; |
|
1018 EC_WRITE_U32(data, value); |
|
1019 return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 4); |
|
1020 } |
|
1021 |
|
1022 /*****************************************************************************/ |
919 |
1023 |
920 /** |
1024 /** |
921 \return 0 in case of success, else < 0 |
1025 \return 0 in case of success, else < 0 |
922 \ingroup RealtimeInterface |
1026 \ingroup RealtimeInterface |
923 */ |
1027 */ |