62 |
62 |
63 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
63 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
64 |
64 |
65 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
65 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
66 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
66 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", |
67 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
68 sdo_index, sdo_subindex, slave->ring_position); |
68 sdo_index, sdo_subindex, slave->ring_position); |
69 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
69 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
70 return -1; |
70 return -1; |
71 } |
71 } |
72 |
72 |
73 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
73 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
74 EC_READ_U8 (data + 2) >> 5 != 0x2 || // Upload response |
74 EC_READ_U8 (data + 2) >> 5 != 0x2 || // Upload response |
75 EC_READ_U16(data + 3) != sdo_index || // Index |
75 EC_READ_U16(data + 3) != sdo_index || // Index |
76 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
76 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
|
77 EC_ERR("SDO upload 0x%04X:%X failed:\n", |
|
78 sdo_index, sdo_subindex, slave->ring_position); |
77 EC_ERR("Invalid SDO upload response at slave %i!\n", |
79 EC_ERR("Invalid SDO upload response at slave %i!\n", |
78 slave->ring_position); |
80 slave->ring_position); |
|
81 ec_print_data(data, rec_size); |
79 return -1; |
82 return -1; |
80 } |
83 } |
81 |
84 |
82 memcpy(target, data + 6, 4); |
85 memcpy(target, data + 6, 4); |
83 return 0; |
86 return 0; |
119 |
122 |
120 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
123 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
121 |
124 |
122 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
125 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
123 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
126 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
124 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
127 EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n", |
125 " slave %i.\n", sdo_index, sdo_subindex, size, |
128 sdo_index, sdo_subindex, size, slave->ring_position); |
126 slave->ring_position); |
|
127 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
129 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
128 return -1; |
130 return -1; |
129 } |
131 } |
130 |
132 |
131 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
133 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
132 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
134 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
133 EC_READ_U16(data + 3) != sdo_index || // Index |
135 EC_READ_U16(data + 3) != sdo_index || // Index |
134 EC_READ_U8 (data + 5) != sdo_subindex) // Subindex |
136 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
135 { |
137 EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n", |
|
138 sdo_index, sdo_subindex, size); |
136 EC_ERR("Invalid SDO download response at slave %i!\n", |
139 EC_ERR("Invalid SDO download response at slave %i!\n", |
137 slave->ring_position); |
140 slave->ring_position); |
|
141 ec_print_data(data, rec_size); |
138 return -1; |
142 return -1; |
139 } |
143 } |
140 |
144 |
141 return 0; |
145 return 0; |
142 } |
146 } |
168 |
172 |
169 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
173 if (!(data = ec_slave_mbox_simple_io(slave, &rec_size))) return -1; |
170 |
174 |
171 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
175 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
172 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
176 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
173 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
177 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
174 sdo_index, sdo_subindex, slave->ring_position); |
178 sdo_index, sdo_subindex, slave->ring_position); |
175 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
179 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
176 return -1; |
180 return -1; |
177 } |
181 } |
178 |
182 |
179 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
183 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
180 EC_READ_U8 (data + 2) >> 5 != 0x2 || // Initiate upload response |
184 EC_READ_U8 (data + 2) >> 5 != 0x2 || // Initiate upload response |
181 EC_READ_U16(data + 3) != sdo_index || // Index |
185 EC_READ_U16(data + 3) != sdo_index || // Index |
182 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
186 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
|
187 EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex); |
183 EC_ERR("Invalid SDO upload response at slave %i!\n", |
188 EC_ERR("Invalid SDO upload response at slave %i!\n", |
184 slave->ring_position); |
189 slave->ring_position); |
|
190 ec_print_data(data, rec_size); |
185 return -1; |
191 return -1; |
186 } |
192 } |
187 |
193 |
188 if (rec_size < 10) { |
194 if (rec_size < 10) { |
189 EC_ERR("Received currupted SDO upload response!\n"); |
195 EC_ERR("Received currupted SDO upload response!\n"); |
|
196 ec_print_data(data, rec_size); |
190 return -1; |
197 return -1; |
191 } |
198 } |
192 |
199 |
193 if ((complete_size = EC_READ_U32(data + 6)) > *size) { |
200 if ((complete_size = EC_READ_U32(data + 6)) > *size) { |
194 EC_ERR("SDO data does not fit into buffer (%i / %i)!\n", |
201 EC_ERR("SDO data does not fit into buffer (%i / %i)!\n", |
196 return -1; |
203 return -1; |
197 } |
204 } |
198 |
205 |
199 data_size = rec_size - 10; |
206 data_size = rec_size - 10; |
200 |
207 |
201 if (data_size == complete_size) { |
208 if (data_size != complete_size) { |
202 memcpy(target, data + 10, data_size); |
209 EC_ERR("SDO data incomplete - Fragmenting not implemented.\n"); |
203 } |
210 return -1; |
204 else { |
211 } |
205 EC_ERR("SDO data incomplete.\n"); |
212 |
206 return -1; |
213 memcpy(target, data + 10, data_size); |
207 } |
|
208 |
|
209 return 0; |
214 return 0; |
210 } |
215 } |
211 |
216 |
212 /*****************************************************************************/ |
217 /*****************************************************************************/ |
213 |
218 |
231 EC_WRITE_U8 (data + 3, 0x00); |
236 EC_WRITE_U8 (data + 3, 0x00); |
232 EC_WRITE_U16(data + 4, 0x0000); |
237 EC_WRITE_U16(data + 4, 0x0000); |
233 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
238 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
234 |
239 |
235 if (unlikely(ec_master_simple_io(slave->master, &slave->mbox_command))) { |
240 if (unlikely(ec_master_simple_io(slave->master, &slave->mbox_command))) { |
236 EC_ERR("Mailbox checking failed on slave %i!\n", |
241 EC_ERR("Mailbox checking failed on slave %i!\n", slave->ring_position); |
237 slave->ring_position); |
|
238 return -1; |
242 return -1; |
239 } |
243 } |
240 |
244 |
241 do { |
245 do { |
242 if (!(data = ec_slave_mbox_simple_receive(slave, 0x03, &rec_size))) |
246 if (!(data = ec_slave_mbox_simple_receive(slave, 0x03, &rec_size))) |
252 |
256 |
253 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
257 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
254 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
258 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
255 EC_ERR("Invalid SDO list response at slave %i!\n", |
259 EC_ERR("Invalid SDO list response at slave %i!\n", |
256 slave->ring_position); |
260 slave->ring_position); |
|
261 ec_print_data(data, rec_size); |
257 return -1; |
262 return -1; |
258 } |
263 } |
259 |
264 |
260 if (rec_size < 8) { |
265 if (rec_size < 8) { |
261 EC_ERR("Invalid data size!\n"); |
266 EC_ERR("Invalid data size!\n"); |
|
267 ec_print_data(data, rec_size); |
262 return -1; |
268 return -1; |
263 } |
269 } |
264 |
270 |
265 sdo_count = (rec_size - 8) / 2; |
271 sdo_count = (rec_size - 8) / 2; |
266 for (i = 0; i < sdo_count; i++) { |
272 for (i = 0; i < sdo_count; i++) { |
269 |
275 |
270 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) { |
276 if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_KERNEL))) { |
271 EC_ERR("Failed to allocate memory for SDO!\n"); |
277 EC_ERR("Failed to allocate memory for SDO!\n"); |
272 return -1; |
278 return -1; |
273 } |
279 } |
|
280 |
|
281 // Initialize SDO object |
274 sdo->index = sdo_index; |
282 sdo->index = sdo_index; |
|
283 sdo->type = 0x0000; |
|
284 sdo->features = 0x00; |
275 sdo->name = NULL; |
285 sdo->name = NULL; |
|
286 INIT_LIST_HEAD(&sdo->entries); |
|
287 |
276 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
288 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
277 } |
289 } |
278 } while (EC_READ_U8(data + 2) & 0x80); |
290 } |
|
291 while (EC_READ_U8(data + 2) & 0x80); |
279 |
292 |
280 // Alle Beschreibungen laden |
293 // Alle Beschreibungen laden |
281 if (ec_slave_fetch_sdo_descriptions(slave)) return -1; |
294 if (ec_slave_fetch_sdo_descriptions(slave)) return -1; |
282 |
295 |
283 return 0; |
296 return 0; |
319 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
332 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
320 EC_READ_U16(data + 6) != sdo->index) { // SDO index |
333 EC_READ_U16(data + 6) != sdo->index) { // SDO index |
321 EC_ERR("Invalid object description response at slave %i while" |
334 EC_ERR("Invalid object description response at slave %i while" |
322 " fetching SDO 0x%04X!\n", slave->ring_position, |
335 " fetching SDO 0x%04X!\n", slave->ring_position, |
323 sdo->index); |
336 sdo->index); |
|
337 ec_print_data(data, rec_size); |
324 return -1; |
338 return -1; |
325 } |
339 } |
326 |
340 |
327 if (rec_size < 12) { |
341 if (rec_size < 12) { |
328 EC_ERR("Invalid data size!\n"); |
342 EC_ERR("Invalid data size!\n"); |
|
343 ec_print_data(data, rec_size); |
329 return -1; |
344 return -1; |
330 } |
345 } |
331 |
346 |
332 sdo->type = EC_READ_U16(data + 8); |
347 sdo->type = EC_READ_U16(data + 8); |
333 sdo->features = EC_READ_U8(data + 11); |
348 sdo->features = EC_READ_U8(data + 11); |
334 INIT_LIST_HEAD(&sdo->entries); |
|
335 |
349 |
336 name_size = rec_size - 12; |
350 name_size = rec_size - 12; |
337 if (name_size) { |
351 if (name_size) { |
338 if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) { |
352 if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) { |
339 EC_ERR("Failed to allocate SDO name!\n"); |
353 EC_ERR("Failed to allocate SDO name!\n"); |
402 EC_READ_U16(data + 6) != sdo->index || // SDO index |
416 EC_READ_U16(data + 6) != sdo->index || // SDO index |
403 EC_READ_U8(data + 8) != i) { // SDO subindex |
417 EC_READ_U8(data + 8) != i) { // SDO subindex |
404 EC_ERR("Invalid entry description response at slave %i while" |
418 EC_ERR("Invalid entry description response at slave %i while" |
405 " fetching SDO 0x%04X:%i!\n", slave->ring_position, |
419 " fetching SDO 0x%04X:%i!\n", slave->ring_position, |
406 sdo->index, i); |
420 sdo->index, i); |
|
421 ec_print_data(data, rec_size); |
407 return -1; |
422 return -1; |
408 } |
423 } |
409 |
424 |
410 if (rec_size < 16) { |
425 if (rec_size < 16) { |
411 EC_ERR("Invalid data size!\n"); |
426 EC_ERR("Invalid data size!\n"); |
|
427 ec_print_data(data, rec_size); |
412 return -1; |
428 return -1; |
413 } |
429 } |
414 |
430 |
415 if (!EC_READ_U16(data + 12)) // bit length = 0 |
431 if (!EC_READ_U16(data + 12)) // bit length = 0 |
416 continue; |
432 continue; |
456 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
472 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
457 abort_msg->code, abort_msg->message); |
473 abort_msg->code, abort_msg->message); |
458 return; |
474 return; |
459 } |
475 } |
460 } |
476 } |
|
477 |
461 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
478 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
462 } |
479 } |
463 |
480 |
464 /*****************************************************************************/ |
481 /*****************************************************************************/ |
465 |
482 |