46 uint16_t sdo_index, /**< SDO-Index */ |
46 uint16_t sdo_index, /**< SDO-Index */ |
47 uint8_t sdo_subindex, /**< SDO-Subindex */ |
47 uint8_t sdo_subindex, /**< SDO-Subindex */ |
48 uint8_t *target /**< Speicher für 4 Bytes */ |
48 uint8_t *target /**< Speicher für 4 Bytes */ |
49 ) |
49 ) |
50 { |
50 { |
51 uint8_t data[0x20]; |
|
52 size_t rec_size; |
51 size_t rec_size; |
53 |
52 uint8_t *data; |
|
53 |
|
54 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 6))) return -1; |
54 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
55 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
55 EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer |
56 EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer |
56 | 0x2 << 5)); // initiate upload request |
57 | 0x2 << 5)); // initiate upload request |
57 EC_WRITE_U16(data + 3, sdo_index); |
58 EC_WRITE_U16(data + 3, sdo_index); |
58 EC_WRITE_U8 (data + 5, sdo_subindex); |
59 EC_WRITE_U8 (data + 5, sdo_subindex); |
59 |
60 if (unlikely(ec_master_simple_io(slave->master))) { |
60 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
61 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
61 |
62 return -1; |
62 rec_size = 0x20; |
63 } |
63 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
64 |
|
65 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) return -1; |
64 |
66 |
65 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
67 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
66 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
68 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
67 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
69 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
68 sdo_index, sdo_subindex, slave->ring_position); |
70 sdo_index, sdo_subindex, slave->ring_position); |
95 uint8_t sdo_subindex, /**< SDO-Subindex */ |
97 uint8_t sdo_subindex, /**< SDO-Subindex */ |
96 const uint8_t *sdo_data, /**< Neuer Wert */ |
98 const uint8_t *sdo_data, /**< Neuer Wert */ |
97 size_t size |
99 size_t size |
98 ) |
100 ) |
99 { |
101 { |
100 uint8_t data[0x0A]; |
102 uint8_t *data; |
101 size_t rec_size; |
103 size_t rec_size; |
102 |
104 |
103 if (size == 0 || size > 4) { |
105 if (size == 0 || size > 4) { |
104 EC_ERR("Invalid data size!\n"); |
106 EC_ERR("Invalid data size!\n"); |
105 return -1; |
107 return -1; |
106 } |
108 } |
107 |
109 |
|
110 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 0x0A))) return -1; |
108 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
111 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
109 EC_WRITE_U8 (data + 2, (0x1 // size specified |
112 EC_WRITE_U8 (data + 2, (0x1 // size specified |
110 | 0x1 << 1 // expedited transfer |
113 | 0x1 << 1 // expedited transfer |
111 | (4 - size) << 2 // data set size |
114 | (4 - size) << 2 // data set size |
112 | 0x1 << 5)); // initiate download request |
115 | 0x1 << 5)); // initiate download request |
113 EC_WRITE_U16(data + 3, sdo_index); |
116 EC_WRITE_U16(data + 3, sdo_index); |
114 EC_WRITE_U8 (data + 5, sdo_subindex); |
117 EC_WRITE_U8 (data + 5, sdo_subindex); |
115 |
|
116 memcpy(data + 6, sdo_data, size); |
118 memcpy(data + 6, sdo_data, size); |
117 if (size < 4) memset(data + 6 + size, 0x00, 4 - size); |
119 if (size < 4) memset(data + 6 + size, 0x00, 4 - size); |
118 |
120 if (unlikely(ec_master_simple_io(slave->master))) { |
119 // Mailox senden und empfangen |
121 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
120 if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1; |
122 return -1; |
121 |
123 } |
122 rec_size = 0x0A; |
124 |
123 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
125 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) return -1; |
124 |
126 |
125 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
127 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
126 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
128 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
127 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
129 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
128 " slave %i.\n", sdo_index, sdo_subindex, size, |
130 " slave %i.\n", sdo_index, sdo_subindex, size, |
156 uint8_t sdo_subindex, /**< SDO-Subindex */ |
158 uint8_t sdo_subindex, /**< SDO-Subindex */ |
157 uint8_t *target, /**< Speicher für gel. Wert */ |
159 uint8_t *target, /**< Speicher für gel. Wert */ |
158 size_t *size /**< Größe des Speichers */ |
160 size_t *size /**< Größe des Speichers */ |
159 ) |
161 ) |
160 { |
162 { |
161 uint8_t data[0x20]; |
163 uint8_t *data; |
162 size_t rec_size, data_size; |
164 size_t rec_size, data_size; |
163 uint32_t complete_size; |
165 uint32_t complete_size; |
164 |
166 |
|
167 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 6))) return -1; |
165 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
168 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
166 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
169 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
167 EC_WRITE_U16(data + 3, sdo_index); |
170 EC_WRITE_U16(data + 3, sdo_index); |
168 EC_WRITE_U8 (data + 5, sdo_subindex); |
171 EC_WRITE_U8 (data + 5, sdo_subindex); |
169 |
172 if (unlikely(ec_master_simple_io(slave->master))) { |
170 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
173 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
171 |
174 return -1; |
172 rec_size = 0x20; |
175 } |
173 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
176 |
|
177 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) return -1; |
174 |
178 |
175 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
179 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
176 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
180 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
177 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
181 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
178 sdo_index, sdo_subindex, slave->ring_position); |
182 sdo_index, sdo_subindex, slave->ring_position); |
220 \return 0, wenn alles ok, sonst < 0 |
224 \return 0, wenn alles ok, sonst < 0 |
221 */ |
225 */ |
222 |
226 |
223 int ec_slave_fetch_sdo_list(ec_slave_t *slave /**< EtherCAT-Slave */) |
227 int ec_slave_fetch_sdo_list(ec_slave_t *slave /**< EtherCAT-Slave */) |
224 { |
228 { |
225 uint8_t data[0xF0]; |
229 uint8_t *data; |
226 size_t rec_size; |
230 size_t rec_size; |
227 unsigned int i, sdo_count; |
231 unsigned int i, sdo_count; |
228 ec_sdo_t *sdo; |
232 ec_sdo_t *sdo; |
229 uint16_t sdo_index; |
233 uint16_t sdo_index; |
230 |
234 |
|
235 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 8))) return -1; |
231 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
236 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
232 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
237 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
233 EC_WRITE_U8 (data + 3, 0x00); |
238 EC_WRITE_U8 (data + 3, 0x00); |
234 EC_WRITE_U16(data + 4, 0x0000); |
239 EC_WRITE_U16(data + 4, 0x0000); |
235 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
240 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
236 |
241 if (unlikely(ec_master_simple_io(slave->master))) { |
237 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
242 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
|
243 return -1; |
|
244 } |
238 |
245 |
239 do { |
246 do { |
240 rec_size = 0xF0; |
247 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) |
241 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
248 return -1; |
242 |
249 |
243 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
250 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
244 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response |
251 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response |
245 EC_ERR("SDO information error response at slave %i!\n", |
252 EC_ERR("SDO information error response at slave %i!\n", |
246 slave->ring_position); |
253 slave->ring_position); |
288 \return 0, wenn alles ok, sonst < 0 |
295 \return 0, wenn alles ok, sonst < 0 |
289 */ |
296 */ |
290 |
297 |
291 int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave /**< EtherCAT-Slave */) |
298 int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave /**< EtherCAT-Slave */) |
292 { |
299 { |
293 uint8_t data[0xF0]; |
300 uint8_t *data; |
294 size_t rec_size, name_size; |
301 size_t rec_size, name_size; |
295 ec_sdo_t *sdo; |
302 ec_sdo_t *sdo; |
296 |
303 |
297 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
304 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
|
305 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 8))) return -1; |
298 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
306 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
299 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
307 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
300 EC_WRITE_U8 (data + 3, 0x00); |
308 EC_WRITE_U8 (data + 3, 0x00); |
301 EC_WRITE_U16(data + 4, 0x0000); |
309 EC_WRITE_U16(data + 4, 0x0000); |
302 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
310 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
303 |
311 if (unlikely(ec_master_simple_io(slave->master))) { |
304 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
312 EC_ERR("Mailbox sending failed on slave %i!\n", |
305 |
313 slave->ring_position); |
306 rec_size = 0xF0; |
314 return -1; |
307 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) |
315 } |
|
316 |
|
317 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) |
308 return -1; |
318 return -1; |
309 |
319 |
310 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
320 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
311 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
321 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
312 EC_ERR("SDO information error response at slave %i while" |
322 EC_ERR("SDO information error response at slave %i while" |
368 int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT-Slave */ |
378 int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT-Slave */ |
369 ec_sdo_t *sdo, /**< SDO */ |
379 ec_sdo_t *sdo, /**< SDO */ |
370 uint8_t subindices /**< Anzahl Subindizes */ |
380 uint8_t subindices /**< Anzahl Subindizes */ |
371 ) |
381 ) |
372 { |
382 { |
373 uint8_t data[0xF0]; |
383 uint8_t *data; |
374 size_t rec_size, data_size; |
384 size_t rec_size, data_size; |
375 uint8_t i; |
385 uint8_t i; |
376 ec_sdo_entry_t *entry; |
386 ec_sdo_entry_t *entry; |
377 |
387 |
378 for (i = 1; i <= subindices; i++) { |
388 for (i = 1; i <= subindices; i++) { |
|
389 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 10))) |
|
390 return -1; |
379 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
391 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
380 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
392 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
381 EC_WRITE_U8 (data + 3, 0x00); |
393 EC_WRITE_U8 (data + 3, 0x00); |
382 EC_WRITE_U16(data + 4, 0x0000); |
394 EC_WRITE_U16(data + 4, 0x0000); |
383 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
395 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
384 EC_WRITE_U8 (data + 8, i); // SDO subindex |
396 EC_WRITE_U8 (data + 8, i); // SDO subindex |
385 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
397 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
386 |
398 if (unlikely(ec_master_simple_io(slave->master))) { |
387 if (ec_slave_mailbox_send(slave, 0x03, data, 10)) return -1; |
399 EC_ERR("Mailbox sending failed on slave %i!\n", |
388 |
400 slave->ring_position); |
389 rec_size = 0xF0; |
401 return -1; |
390 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) |
402 } |
|
403 |
|
404 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) |
391 return -1; |
405 return -1; |
392 |
406 |
393 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
407 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
394 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
408 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
395 EC_ERR("SDO information error response at slave %i while" |
409 EC_ERR("SDO information error response at slave %i while" |