31 const char *message; |
32 const char *message; |
32 } |
33 } |
33 ec_sdo_abort_message_t; |
34 ec_sdo_abort_message_t; |
34 |
35 |
35 const ec_sdo_abort_message_t sdo_abort_messages[]; |
36 const ec_sdo_abort_message_t sdo_abort_messages[]; |
|
37 |
|
38 /*****************************************************************************/ |
|
39 |
|
40 /** |
|
41 Liest 32 Bit eines CANopen-SDOs im Expedited-Modus aus einem Slave. |
|
42 \return 0 wenn alles ok, < 0 bei Fehler |
|
43 */ |
|
44 |
|
45 int ec_slave_sdo_read_exp(ec_slave_t *slave, /**< EtherCAT-Slave */ |
|
46 uint16_t sdo_index, /**< SDO-Index */ |
|
47 uint8_t sdo_subindex, /**< SDO-Subindex */ |
|
48 uint8_t *target /**< Speicher für 4 Bytes */ |
|
49 ) |
|
50 { |
|
51 uint8_t data[0x20]; |
|
52 size_t rec_size; |
|
53 |
|
54 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
55 EC_WRITE_U8 (data + 2, (0x1 << 1 // expedited transfer |
|
56 | 0x2 << 5)); // initiate upload request |
|
57 EC_WRITE_U16(data + 3, sdo_index); |
|
58 EC_WRITE_U8 (data + 5, sdo_subindex); |
|
59 |
|
60 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
|
61 |
|
62 rec_size = 0x20; |
|
63 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
|
64 |
|
65 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO 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", |
|
68 sdo_index, sdo_subindex, slave->ring_position); |
|
69 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
70 return -1; |
|
71 } |
|
72 |
|
73 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
|
74 EC_READ_U8 (data + 2) >> 5 != 0x2 || // Upload response |
|
75 EC_READ_U16(data + 3) != sdo_index || // Index |
|
76 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
|
77 EC_ERR("Invalid SDO upload response at slave %i!\n", |
|
78 slave->ring_position); |
|
79 return -1; |
|
80 } |
|
81 |
|
82 memcpy(target, data + 6, 4); |
|
83 return 0; |
|
84 } |
36 |
85 |
37 /*****************************************************************************/ |
86 /*****************************************************************************/ |
38 |
87 |
39 /** |
88 /** |
40 Beschreibt ein CANopen-SDO eines Slaves im Expedited-Modus. |
89 Beschreibt ein CANopen-SDO eines Slaves im Expedited-Modus. |
54 if (size == 0 || size > 4) { |
103 if (size == 0 || size > 4) { |
55 EC_ERR("Invalid data size!\n"); |
104 EC_ERR("Invalid data size!\n"); |
56 return -1; |
105 return -1; |
57 } |
106 } |
58 |
107 |
59 EC_WRITE_U16(data, 0x2000); // SDO request |
108 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
60 EC_WRITE_U8 (data + 2, 0x23 | (4 - size) << 2); // expedited, size specif. |
109 EC_WRITE_U8 (data + 2, (0x1 // size specified |
|
110 | 0x1 << 1 // expedited transfer |
|
111 | (4 - size) << 2 // data set size |
|
112 | 0x1 << 5)); // initiate download request |
61 EC_WRITE_U16(data + 3, sdo_index); |
113 EC_WRITE_U16(data + 3, sdo_index); |
62 EC_WRITE_U8 (data + 5, sdo_subindex); |
114 EC_WRITE_U8 (data + 5, sdo_subindex); |
63 |
115 |
64 memcpy(data + 6, sdo_data, size); |
116 memcpy(data + 6, sdo_data, size); |
65 if (size < 4) memset(data + 6 + size, 0x00, 4 - size); |
117 if (size < 4) memset(data + 6 + size, 0x00, 4 - size); |
68 if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1; |
120 if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1; |
69 |
121 |
70 rec_size = 0x0A; |
122 rec_size = 0x0A; |
71 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
123 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
72 |
124 |
73 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
125 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
74 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request |
126 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
75 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
127 EC_ERR("SDO download of 0x%04X:%X (%i bytes) aborted on!" |
76 " slave %i.\n", sdo_index, sdo_subindex, size, |
128 " slave %i.\n", sdo_index, sdo_subindex, size, |
77 slave->ring_position); |
129 slave->ring_position); |
78 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
130 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
79 return -1; |
131 return -1; |
80 } |
132 } |
81 |
133 |
82 if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response |
134 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
83 EC_READ_U8 (data + 2) >> 5 != 0x03 || // Download response |
135 EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response |
84 EC_READ_U16(data + 3) != sdo_index || // Index |
136 EC_READ_U16(data + 3) != sdo_index || // Index |
85 EC_READ_U8 (data + 5) != sdo_subindex) // Subindex |
137 EC_READ_U8 (data + 5) != sdo_subindex) // Subindex |
86 { |
138 { |
87 EC_ERR("Invalid SDO download response at slave %i!\n", |
139 EC_ERR("Invalid SDO download response at slave %i!\n", |
88 slave->ring_position); |
140 slave->ring_position); |
93 } |
145 } |
94 |
146 |
95 /*****************************************************************************/ |
147 /*****************************************************************************/ |
96 |
148 |
97 /** |
149 /** |
98 Liest 32-Bit eines CANopen-SDOs im Expedited-Modus aus einem Slave. |
150 Liest ein CANopen-SDO im "Normal-Modus" aus einem Slave. |
99 \return 0 wenn alles ok, < 0 bei Fehler |
|
100 */ |
|
101 |
|
102 int ec_slave_sdo_read_exp(ec_slave_t *slave, /**< EtherCAT-Slave */ |
|
103 uint16_t sdo_index, /**< SDO-Index */ |
|
104 uint8_t sdo_subindex, /**< SDO-Subindex */ |
|
105 uint8_t *target /**< Speicher für 4 Bytes */ |
|
106 ) |
|
107 { |
|
108 uint8_t data[0x20]; |
|
109 size_t rec_size; |
|
110 |
|
111 EC_WRITE_U16(data, 0x2000); // SDO request |
|
112 EC_WRITE_U8 (data + 2, 0x42); // Upload request, expedited |
|
113 EC_WRITE_U16(data + 3, sdo_index); |
|
114 EC_WRITE_U8 (data + 5, sdo_subindex); |
|
115 |
|
116 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
|
117 |
|
118 rec_size = 0x20; |
|
119 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
|
120 |
|
121 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
|
122 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request |
|
123 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
|
124 sdo_index, sdo_subindex, slave->ring_position); |
|
125 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
126 return -1; |
|
127 } |
|
128 |
|
129 if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response |
|
130 EC_READ_U8 (data + 2) >> 5 != 0x02 || // Upload response |
|
131 EC_READ_U16(data + 3) != sdo_index || // Index |
|
132 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
|
133 EC_ERR("Invalid SDO upload response at slave %i!\n", |
|
134 slave->ring_position); |
|
135 return -1; |
|
136 } |
|
137 |
|
138 memcpy(target, data + 6, 4); |
|
139 return 0; |
|
140 } |
|
141 |
|
142 /*****************************************************************************/ |
|
143 |
|
144 /** |
|
145 Liest ein CANopen-SDO aus einem Slave. |
|
146 \return 0 wenn alles ok, < 0 bei Fehler |
151 \return 0 wenn alles ok, < 0 bei Fehler |
147 */ |
152 */ |
148 |
153 |
149 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */ |
154 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */ |
150 uint16_t sdo_index, /**< SDO-Index */ |
155 uint16_t sdo_index, /**< SDO-Index */ |
155 { |
160 { |
156 uint8_t data[0x20]; |
161 uint8_t data[0x20]; |
157 size_t rec_size, data_size; |
162 size_t rec_size, data_size; |
158 uint32_t complete_size; |
163 uint32_t complete_size; |
159 |
164 |
160 EC_WRITE_U16(data, 0x2000); // SDO request |
165 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
161 EC_WRITE_U8 (data + 2, 0x2 << 5); // Initiate upload request |
166 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
162 EC_WRITE_U16(data + 3, sdo_index); |
167 EC_WRITE_U16(data + 3, sdo_index); |
163 EC_WRITE_U8 (data + 5, sdo_subindex); |
168 EC_WRITE_U8 (data + 5, sdo_subindex); |
164 |
169 |
165 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
170 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
166 |
171 |
167 rec_size = 0x20; |
172 rec_size = 0x20; |
168 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
173 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
169 |
174 |
170 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
175 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
171 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request |
176 EC_READ_U8 (data + 2) >> 5 == 0x4) { // Abort SDO transfer request |
172 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
177 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
173 sdo_index, sdo_subindex, slave->ring_position); |
178 sdo_index, sdo_subindex, slave->ring_position); |
174 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
179 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
175 return -1; |
180 return -1; |
176 } |
181 } |
177 |
182 |
178 if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response |
183 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
179 EC_READ_U8 (data + 2) >> 5 != 0x02 || // Initiate upload response |
184 EC_READ_U8 (data + 2) >> 5 != 0x2 || // Initiate upload response |
180 EC_READ_U16(data + 3) != sdo_index || // Index |
185 EC_READ_U16(data + 3) != sdo_index || // Index |
181 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
186 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
182 EC_ERR("Invalid SDO upload response at slave %i!\n", |
187 EC_ERR("Invalid SDO upload response at slave %i!\n", |
183 slave->ring_position); |
188 slave->ring_position); |
184 return -1; |
189 return -1; |
221 size_t rec_size; |
226 size_t rec_size; |
222 unsigned int i, sdo_count; |
227 unsigned int i, sdo_count; |
223 ec_sdo_t *sdo; |
228 ec_sdo_t *sdo; |
224 uint16_t sdo_index; |
229 uint16_t sdo_index; |
225 |
230 |
226 EC_WRITE_U16(data, 0x8000); // SDO information |
231 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
227 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
232 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
228 EC_WRITE_U8 (data + 3, 0x00); |
233 EC_WRITE_U8 (data + 3, 0x00); |
229 EC_WRITE_U16(data + 4, 0x0000); |
234 EC_WRITE_U16(data + 4, 0x0000); |
230 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
235 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
231 |
236 |
232 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
237 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
233 |
238 |
234 do { |
239 do { |
235 rec_size = 0xF0; |
240 rec_size = 0xF0; |
236 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
241 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
237 |
242 |
238 if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information |
243 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
239 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response |
244 (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // Error response |
240 EC_ERR("SDO information error response at slave %i!\n", |
245 EC_ERR("SDO information error response at slave %i!\n", |
241 slave->ring_position); |
246 slave->ring_position); |
242 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
247 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
243 return -1; |
248 return -1; |
244 } |
249 } |
245 |
250 |
246 if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information |
251 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
247 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
252 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
248 EC_ERR("Invalid SDO list response at slave %i!\n", |
253 EC_ERR("Invalid SDO list response at slave %i!\n", |
249 slave->ring_position); |
254 slave->ring_position); |
250 return -1; |
255 return -1; |
251 } |
256 } |
268 sdo->name = NULL; |
273 sdo->name = NULL; |
269 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
274 list_add_tail(&sdo->list, &slave->sdo_dictionary); |
270 } |
275 } |
271 } while (EC_READ_U8(data + 2) & 0x80); |
276 } while (EC_READ_U8(data + 2) & 0x80); |
272 |
277 |
273 // Jetzt alle Beschreibungen holen |
278 // Alle Beschreibungen laden |
274 if (ec_slave_fetch_sdo_descriptions(slave)) return -1; |
279 if (ec_slave_fetch_sdo_descriptions(slave)) return -1; |
275 |
280 |
276 return 0; |
281 return 0; |
277 } |
282 } |
278 |
283 |
279 /*****************************************************************************/ |
284 /*****************************************************************************/ |
280 |
285 |
281 /** |
286 /** |
282 Holt die Beschreibungen zu allen bereits bekannten SDOs. |
287 Holt die Beschreibungen zu allen bereits bekannten SDOs. |
283 |
|
284 \return 0, wenn alles ok, sonst < 0 |
288 \return 0, wenn alles ok, sonst < 0 |
285 */ |
289 */ |
286 |
290 |
287 int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave /**< EtherCAT-Slave */) |
291 int ec_slave_fetch_sdo_descriptions(ec_slave_t *slave /**< EtherCAT-Slave */) |
288 { |
292 { |
289 uint8_t data[0xF0]; |
293 uint8_t data[0xF0]; |
290 size_t rec_size, name_size; |
294 size_t rec_size, name_size; |
291 ec_sdo_t *sdo; |
295 ec_sdo_t *sdo; |
292 |
296 |
293 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
297 list_for_each_entry(sdo, &slave->sdo_dictionary, list) { |
294 EC_WRITE_U16(data, 0x8000); // SDO information |
298 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
295 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
299 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
296 EC_WRITE_U8 (data + 3, 0x00); |
300 EC_WRITE_U8 (data + 3, 0x00); |
297 EC_WRITE_U16(data + 4, 0x0000); |
301 EC_WRITE_U16(data + 4, 0x0000); |
298 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
302 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
299 |
303 |
300 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
304 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
301 |
305 |
302 rec_size = 0xF0; |
306 rec_size = 0xF0; |
303 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) |
307 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) |
304 return -1; |
308 return -1; |
305 |
309 |
306 if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information |
310 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
307 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
311 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
308 EC_ERR("SDO information error response at slave %i while" |
312 EC_ERR("SDO information error response at slave %i while" |
309 " fetching SDO 0x%04X!\n", slave->ring_position, |
313 " fetching SDO 0x%04X!\n", slave->ring_position, |
310 sdo->index); |
314 sdo->index); |
311 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
315 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
312 return -1; |
316 return -1; |
313 } |
317 } |
314 |
318 |
315 if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information |
319 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
316 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Obj. desc. resp. |
320 (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response |
317 EC_READ_U16(data + 6) != sdo->index) { // SDO index |
321 EC_READ_U16(data + 6) != sdo->index) { // SDO index |
318 EC_ERR("Invalid object description response at slave %i while" |
322 EC_ERR("Invalid object description response at slave %i while" |
319 " fetching SDO 0x%04X!\n", slave->ring_position, |
323 " fetching SDO 0x%04X!\n", slave->ring_position, |
320 sdo->index); |
324 sdo->index); |
321 return -1; |
325 return -1; |
325 EC_ERR("Invalid data size!\n"); |
329 EC_ERR("Invalid data size!\n"); |
326 return -1; |
330 return -1; |
327 } |
331 } |
328 |
332 |
329 sdo->type = EC_READ_U16(data + 8); |
333 sdo->type = EC_READ_U16(data + 8); |
330 sdo->max_subindex = EC_READ_U8(data + 10); |
|
331 sdo->features = EC_READ_U8(data + 11); |
334 sdo->features = EC_READ_U8(data + 11); |
|
335 INIT_LIST_HEAD(&sdo->entries); |
332 |
336 |
333 name_size = rec_size - 12; |
337 name_size = rec_size - 12; |
334 if (!name_size) { |
338 if (name_size) { |
335 EC_WARN("Object 0x%04X name size is 0...", sdo->index); |
339 if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) { |
|
340 EC_ERR("Failed to allocate SDO name!\n"); |
|
341 return -1; |
|
342 } |
|
343 |
|
344 memcpy(sdo->name, data + 12, name_size); |
|
345 sdo->name[name_size] = 0; |
|
346 } |
|
347 |
|
348 if (EC_READ_U8(data + 2) & 0x80) { |
|
349 EC_ERR("Fragment follows (not implemented)!\n"); |
|
350 return -1; |
|
351 } |
|
352 |
|
353 // Alle Entries (Subindizes) laden |
|
354 if (ec_slave_fetch_sdo_entries(slave, sdo, EC_READ_U8(data + 10))) |
|
355 return -1; |
|
356 } |
|
357 |
|
358 return 0; |
|
359 } |
|
360 |
|
361 /*****************************************************************************/ |
|
362 |
|
363 /** |
|
364 Lädt alle Entries (Subindizes) zu einem SDO. |
|
365 \return 0, wenn alles ok, sonst < 0 |
|
366 */ |
|
367 |
|
368 int ec_slave_fetch_sdo_entries(ec_slave_t *slave, /**< EtherCAT-Slave */ |
|
369 ec_sdo_t *sdo, /**< SDO */ |
|
370 uint8_t subindices /**< Anzahl Subindizes */ |
|
371 ) |
|
372 { |
|
373 uint8_t data[0xF0]; |
|
374 size_t rec_size, data_size; |
|
375 uint8_t i; |
|
376 ec_sdo_entry_t *entry; |
|
377 |
|
378 for (i = 1; i <= subindices; i++) { |
|
379 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
380 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
|
381 EC_WRITE_U8 (data + 3, 0x00); |
|
382 EC_WRITE_U16(data + 4, 0x0000); |
|
383 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
|
384 EC_WRITE_U8 (data + 8, i); // SDO subindex |
|
385 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
|
386 |
|
387 if (ec_slave_mailbox_send(slave, 0x03, data, 10)) return -1; |
|
388 |
|
389 rec_size = 0xF0; |
|
390 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) |
|
391 return -1; |
|
392 |
|
393 if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information |
|
394 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Error response |
|
395 EC_ERR("SDO information error response at slave %i while" |
|
396 " fetching SDO entry 0x%04X:%i!\n", slave->ring_position, |
|
397 sdo->index, i); |
|
398 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
399 return -1; |
|
400 } |
|
401 |
|
402 if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information |
|
403 (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // entry descr. response |
|
404 EC_READ_U16(data + 6) != sdo->index || // SDO index |
|
405 EC_READ_U8(data + 8) != i) { // SDO subindex |
|
406 EC_ERR("Invalid entry description response at slave %i while" |
|
407 " fetching SDO 0x%04X:%i!\n", slave->ring_position, |
|
408 sdo->index, i); |
|
409 return -1; |
|
410 } |
|
411 |
|
412 if (rec_size < 16) { |
|
413 EC_ERR("Invalid data size!\n"); |
|
414 return -1; |
|
415 } |
|
416 |
|
417 if (!EC_READ_U16(data + 12)) // bit length = 0 |
336 continue; |
418 continue; |
337 } |
419 |
338 |
420 data_size = rec_size - 16; |
339 if (!(sdo->name = kmalloc(name_size + 1, GFP_KERNEL))) { |
421 |
340 EC_ERR("Failed to allocate SDO name!\n"); |
422 if (!(entry = (ec_sdo_entry_t *) |
341 return -1; |
423 kmalloc(sizeof(ec_sdo_entry_t) + data_size + 1, GFP_KERNEL))) { |
342 } |
424 EC_ERR("Failed to allocate entry memory!\n"); |
343 |
425 return -1; |
344 memcpy(sdo->name, data + 12, name_size); |
426 } |
345 sdo->name[name_size] = 0; |
427 |
346 |
428 entry->subindex = i; |
347 if (EC_READ_U8(data + 2) & 0x80) |
429 entry->data_type = EC_READ_U16(data + 10); |
348 EC_WARN("Fragment follows in object description!\n"); |
430 entry->bit_length = EC_READ_U16(data + 12); |
|
431 if (!data_size) { |
|
432 entry->name = NULL; |
|
433 } |
|
434 else { |
|
435 entry->name = (uint8_t *) entry + sizeof(ec_sdo_entry_t); |
|
436 memcpy(entry->name, data + 16, data_size); |
|
437 entry->name[data_size] = 0; |
|
438 } |
|
439 |
|
440 list_add_tail(&entry->list, &sdo->entries); |
349 } |
441 } |
350 |
442 |
351 return 0; |
443 return 0; |
352 } |
444 } |
353 |
445 |