11 #include <linux/delay.h> |
11 #include <linux/delay.h> |
12 #include <linux/slab.h> |
12 #include <linux/slab.h> |
13 #include <linux/module.h> |
13 #include <linux/module.h> |
14 |
14 |
15 #include "master.h" |
15 #include "master.h" |
|
16 #include "mailbox.h" |
16 |
17 |
17 /*****************************************************************************/ |
18 /*****************************************************************************/ |
18 |
19 |
19 void ec_canopen_abort_msg(uint32_t); |
20 void ec_canopen_abort_msg(uint32_t); |
20 int ec_slave_fetch_sdo_descriptions(ec_slave_t *); |
21 int ec_slave_fetch_sdo_descriptions(ec_slave_t *); |
49 ) |
50 ) |
50 { |
51 { |
51 size_t rec_size; |
52 size_t rec_size; |
52 uint8_t *data; |
53 uint8_t *data; |
53 |
54 |
54 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 6))) return -1; |
55 if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 6))) return -1; |
|
56 |
55 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
57 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
56 EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer |
58 EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer |
57 | 0x2 << 5)); // initiate upload request |
59 | 0x2 << 5)); // initiate upload request |
58 EC_WRITE_U16(data + 3, sdo_index); |
60 EC_WRITE_U16(data + 3, sdo_index); |
59 EC_WRITE_U8 (data + 5, sdo_subindex); |
61 EC_WRITE_U8 (data + 5, sdo_subindex); |
60 if (unlikely(ec_master_simple_io(slave->master))) { |
62 |
61 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
63 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
62 return -1; |
|
63 } |
|
64 |
|
65 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) return -1; |
|
66 |
64 |
67 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
65 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
68 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
66 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
69 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
67 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
70 sdo_index, sdo_subindex, slave->ring_position); |
68 sdo_index, sdo_subindex, slave->ring_position); |
105 if (size == 0 || size > 4) { |
103 if (size == 0 || size > 4) { |
106 EC_ERR("Invalid data size!\n"); |
104 EC_ERR("Invalid data size!\n"); |
107 return -1; |
105 return -1; |
108 } |
106 } |
109 |
107 |
110 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 0x0A))) return -1; |
108 if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 0x0A))) return -1; |
|
109 |
111 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
110 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
112 EC_WRITE_U8 (data + 2, (0x1 // size specified |
111 EC_WRITE_U8 (data + 2, (0x1 // size specified |
113 | 0x1 << 1 // expedited transfer |
112 | 0x1 << 1 // expedited transfer |
114 | (4 - size) << 2 // data set size |
113 | (4 - size) << 2 // data set size |
115 | 0x1 << 5)); // initiate download request |
114 | 0x1 << 5)); // initiate download request |
116 EC_WRITE_U16(data + 3, sdo_index); |
115 EC_WRITE_U16(data + 3, sdo_index); |
117 EC_WRITE_U8 (data + 5, sdo_subindex); |
116 EC_WRITE_U8 (data + 5, sdo_subindex); |
118 memcpy(data + 6, sdo_data, size); |
117 memcpy(data + 6, sdo_data, size); |
119 if (size < 4) memset(data + 6 + size, 0x00, 4 - size); |
118 if (size < 4) memset(data + 6 + size, 0x00, 4 - size); |
120 if (unlikely(ec_master_simple_io(slave->master))) { |
119 |
121 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
120 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
122 return -1; |
|
123 } |
|
124 |
|
125 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) return -1; |
|
126 |
121 |
127 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
122 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
128 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
123 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
129 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
124 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
130 " slave %i.\n", sdo_index, sdo_subindex, size, |
125 " slave %i.\n", sdo_index, sdo_subindex, size, |
162 { |
157 { |
163 uint8_t *data; |
158 uint8_t *data; |
164 size_t rec_size, data_size; |
159 size_t rec_size, data_size; |
165 uint32_t complete_size; |
160 uint32_t complete_size; |
166 |
161 |
167 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 6))) return -1; |
162 if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 6))) return -1; |
|
163 |
168 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
164 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
169 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
165 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
170 EC_WRITE_U16(data + 3, sdo_index); |
166 EC_WRITE_U16(data + 3, sdo_index); |
171 EC_WRITE_U8 (data + 5, sdo_subindex); |
167 EC_WRITE_U8 (data + 5, sdo_subindex); |
172 if (unlikely(ec_master_simple_io(slave->master))) { |
168 |
173 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
169 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
174 return -1; |
|
175 } |
|
176 |
|
177 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) return -1; |
|
178 |
170 |
179 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
171 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
180 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
172 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
181 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
173 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
182 sdo_index, sdo_subindex, slave->ring_position); |
174 sdo_index, sdo_subindex, slave->ring_position); |
230 size_t rec_size; |
222 size_t rec_size; |
231 unsigned int i, sdo_count; |
223 unsigned int i, sdo_count; |
232 ec_sdo_t *sdo; |
224 ec_sdo_t *sdo; |
233 uint16_t sdo_index; |
225 uint16_t sdo_index; |
234 |
226 |
235 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 8))) return -1; |
227 if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 8))) return -1; |
|
228 |
236 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
229 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
237 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
230 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
238 EC_WRITE_U8 (data + 3, 0x00); |
231 EC_WRITE_U8 (data + 3, 0x00); |
239 EC_WRITE_U16(data + 4, 0x0000); |
232 EC_WRITE_U16(data + 4, 0x0000); |
240 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
233 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
241 if (unlikely(ec_master_simple_io(slave->master))) { |
234 |
242 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
235 if (unlikely(ec_master_simple_io(slave->master, &slave->mbox_command))) { |
|
236 EC_ERR("Mailbox checking failed on slave %i!\n", |
|
237 slave->ring_position); |
243 return -1; |
238 return -1; |
244 } |
239 } |
245 |
240 |
246 do { |
241 do { |
247 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) |
242 if (!(data = ec_slave_mbox_simple_receive(slave, 0x03, &rec_size))) |
248 return -1; |
243 return -1; |
249 |
244 |
250 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
245 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
251 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response |
246 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response |
252 EC_ERR("SDO information error response at slave %i!\n", |
247 EC_ERR("SDO information error response at slave %i!\n", |
300 uint8_t *data; |
295 uint8_t *data; |
301 size_t rec_size, name_size; |
296 size_t rec_size, name_size; |
302 ec_sdo_t *sdo; |
297 ec_sdo_t *sdo; |
303 |
298 |
304 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
299 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
305 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 8))) return -1; |
300 if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 8))) return -1; |
306 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
301 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
307 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
302 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
308 EC_WRITE_U8 (data + 3, 0x00); |
303 EC_WRITE_U8 (data + 3, 0x00); |
309 EC_WRITE_U16(data + 4, 0x0000); |
304 EC_WRITE_U16(data + 4, 0x0000); |
310 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
305 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
311 if (unlikely(ec_master_simple_io(slave->master))) { |
306 |
312 EC_ERR("Mailbox sending failed on slave %i!\n", |
307 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
313 slave->ring_position); |
|
314 return -1; |
|
315 } |
|
316 |
|
317 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) |
|
318 return -1; |
|
319 |
308 |
320 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
309 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
321 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
310 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
322 EC_ERR("SDO information error response at slave %i while" |
311 EC_ERR("SDO information error response at slave %i while" |
323 " fetching SDO 0x%04X!\n", slave->ring_position, |
312 " fetching SDO 0x%04X!\n", slave->ring_position, |
384 size_t rec_size, data_size; |
373 size_t rec_size, data_size; |
385 uint8_t i; |
374 uint8_t i; |
386 ec_sdo_entry_t *entry; |
375 ec_sdo_entry_t *entry; |
387 |
376 |
388 for (i = 1; i <= subindices; i++) { |
377 for (i = 1; i <= subindices; i++) { |
389 if (!(data = ec_slave_prepare_mailbox_send(slave, 0x03, 10))) |
378 if (!(data = ec_slave_mbox_prepare_send(slave, 0x03, 10))) |
390 return -1; |
379 return -1; |
|
380 |
391 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
381 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
392 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
382 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
393 EC_WRITE_U8 (data + 3, 0x00); |
383 EC_WRITE_U8 (data + 3, 0x00); |
394 EC_WRITE_U16(data + 4, 0x0000); |
384 EC_WRITE_U16(data + 4, 0x0000); |
395 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
385 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
396 EC_WRITE_U8 (data + 8, i); // SDO subindex |
386 EC_WRITE_U8 (data + 8, i); // SDO subindex |
397 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
387 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
398 if (unlikely(ec_master_simple_io(slave->master))) { |
388 |
399 EC_ERR("Mailbox sending failed on slave %i!\n", |
389 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
400 slave->ring_position); |
|
401 return -1; |
|
402 } |
|
403 |
|
404 if (!(data = ec_slave_mailbox_receive(slave, 0x03, &rec_size))) |
|
405 return -1; |
|
406 |
390 |
407 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
391 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
408 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
392 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
409 EC_ERR("SDO information error response at slave %i while" |
393 EC_ERR("SDO information error response at slave %i while" |
410 " fetching SDO entry 0x%04X:%i!\n", slave->ring_position, |
394 " fetching SDO entry 0x%04X:%i!\n", slave->ring_position, |