|
1 /****************************************************************************** |
|
2 * |
|
3 * f r a m e . c |
|
4 * |
|
5 * Methoden für einen EtherCAT-Frame. |
|
6 * |
|
7 * $Id$ |
|
8 * |
|
9 *****************************************************************************/ |
|
10 |
|
11 #include <linux/slab.h> |
|
12 #include <linux/delay.h> |
|
13 |
|
14 #include "frame.h" |
|
15 #include "master.h" |
|
16 |
|
17 /*****************************************************************************/ |
|
18 |
|
19 #define EC_FUNC_HEADER \ |
|
20 frame->master = master; \ |
|
21 frame->state = ec_frame_ready; \ |
|
22 frame->index = 0; \ |
|
23 frame->working_counter = 0; |
|
24 |
|
25 #define EC_FUNC_WRITE_FOOTER \ |
|
26 frame->data_length = length; \ |
|
27 memcpy(frame->data, data, length); |
|
28 |
|
29 #define EC_FUNC_READ_FOOTER \ |
|
30 frame->data_length = length; |
|
31 |
|
32 /*****************************************************************************/ |
|
33 |
|
34 /** |
|
35 Initialisiert ein EtherCAT-NPRD-Kommando. |
|
36 |
|
37 Node-adressed physical read. |
|
38 */ |
|
39 |
|
40 void ec_frame_init_nprd(ec_frame_t *frame, |
|
41 /**< EtherCAT-Rahmen */ |
|
42 ec_master_t *master, |
|
43 /**< EtherCAT-Master */ |
|
44 uint16_t node_address, |
|
45 /**< Adresse des Knotens (Slaves) */ |
|
46 uint16_t offset, |
|
47 /**< Physikalische Speicheradresse im Slave */ |
|
48 unsigned int length |
|
49 /**< Länge der zu lesenden Daten */ |
|
50 ) |
|
51 { |
|
52 if (unlikely(node_address == 0x0000)) |
|
53 printk(KERN_WARNING "EtherCAT: Warning - Using node address 0x0000!\n"); |
|
54 |
|
55 EC_FUNC_HEADER; |
|
56 |
|
57 frame->type = ec_frame_type_nprd; |
|
58 frame->address.physical.slave = node_address; |
|
59 frame->address.physical.mem = offset; |
|
60 |
|
61 EC_FUNC_READ_FOOTER; |
|
62 } |
|
63 |
|
64 /*****************************************************************************/ |
|
65 |
|
66 /** |
|
67 Initialisiert ein EtherCAT-NPWR-Kommando. |
|
68 |
|
69 Node-adressed physical write. |
|
70 */ |
|
71 |
|
72 void ec_frame_init_npwr(ec_frame_t *frame, |
|
73 /**< EtherCAT-Rahmen */ |
|
74 ec_master_t *master, |
|
75 /**< EtherCAT-Master */ |
|
76 uint16_t node_address, |
|
77 /**< Adresse des Knotens (Slaves) */ |
|
78 uint16_t offset, |
|
79 /**< Physikalische Speicheradresse im Slave */ |
|
80 unsigned int length, |
|
81 /**< Länge der zu schreibenden Daten */ |
|
82 const uint8_t *data |
|
83 /**< Zeiger auf Speicher mit zu schreibenden Daten */ |
|
84 ) |
|
85 { |
|
86 if (unlikely(node_address == 0x0000)) |
|
87 printk(KERN_WARNING "EtherCAT: Warning - Using node address 0x0000!\n"); |
|
88 |
|
89 EC_FUNC_HEADER; |
|
90 |
|
91 frame->type = ec_frame_type_npwr; |
|
92 frame->address.physical.slave = node_address; |
|
93 frame->address.physical.mem = offset; |
|
94 |
|
95 EC_FUNC_WRITE_FOOTER; |
|
96 } |
|
97 |
|
98 /*****************************************************************************/ |
|
99 |
|
100 /** |
|
101 Initialisiert ein EtherCAT-APRD-Kommando. |
|
102 |
|
103 Autoincrement physical read. |
|
104 */ |
|
105 |
|
106 void ec_frame_init_aprd(ec_frame_t *frame, |
|
107 /**< EtherCAT-Rahmen */ |
|
108 ec_master_t *master, |
|
109 /**< EtherCAT-Master */ |
|
110 uint16_t ring_position, |
|
111 /**< Position des Slaves im Bus */ |
|
112 uint16_t offset, |
|
113 /**< Physikalische Speicheradresse im Slave */ |
|
114 unsigned int length |
|
115 /**< Länge der zu lesenden Daten */ |
|
116 ) |
|
117 { |
|
118 EC_FUNC_HEADER; |
|
119 |
|
120 frame->type = ec_frame_type_aprd; |
|
121 frame->address.physical.slave = (int16_t) ring_position * (-1); |
|
122 frame->address.physical.mem = offset; |
|
123 |
|
124 EC_FUNC_READ_FOOTER; |
|
125 } |
|
126 |
|
127 /*****************************************************************************/ |
|
128 |
|
129 /** |
|
130 Initialisiert ein EtherCAT-APWR-Kommando. |
|
131 |
|
132 Autoincrement physical write. |
|
133 */ |
|
134 |
|
135 void ec_frame_init_apwr(ec_frame_t *frame, |
|
136 /**< EtherCAT-Rahmen */ |
|
137 ec_master_t *master, |
|
138 /**< EtherCAT-Master */ |
|
139 uint16_t ring_position, |
|
140 /**< Position des Slaves im Bus */ |
|
141 uint16_t offset, |
|
142 /**< Physikalische Speicheradresse im Slave */ |
|
143 unsigned int length, |
|
144 /**< Länge der zu schreibenden Daten */ |
|
145 const uint8_t *data |
|
146 /**< Zeiger auf Speicher mit zu schreibenden Daten */ |
|
147 ) |
|
148 { |
|
149 EC_FUNC_HEADER; |
|
150 |
|
151 frame->type = ec_frame_type_apwr; |
|
152 frame->address.physical.slave = (int16_t) ring_position * (-1); |
|
153 frame->address.physical.mem = offset; |
|
154 |
|
155 EC_FUNC_WRITE_FOOTER; |
|
156 } |
|
157 |
|
158 /*****************************************************************************/ |
|
159 |
|
160 /** |
|
161 Initialisiert ein EtherCAT-BRD-Kommando. |
|
162 |
|
163 Broadcast read. |
|
164 */ |
|
165 |
|
166 void ec_frame_init_brd(ec_frame_t *frame, |
|
167 /**< EtherCAT-Rahmen */ |
|
168 ec_master_t *master, |
|
169 /**< EtherCAT-Master */ |
|
170 uint16_t offset, |
|
171 /**< Physikalische Speicheradresse im Slave */ |
|
172 unsigned int length |
|
173 /**< Länge der zu lesenden Daten */ |
|
174 ) |
|
175 { |
|
176 EC_FUNC_HEADER; |
|
177 |
|
178 frame->type = ec_frame_type_brd; |
|
179 frame->address.physical.slave = 0x0000; |
|
180 frame->address.physical.mem = offset; |
|
181 |
|
182 EC_FUNC_READ_FOOTER; |
|
183 } |
|
184 |
|
185 /*****************************************************************************/ |
|
186 |
|
187 /** |
|
188 Initialisiert ein EtherCAT-BWR-Kommando. |
|
189 |
|
190 Broadcast write. |
|
191 */ |
|
192 |
|
193 void ec_frame_init_bwr(ec_frame_t *frame, |
|
194 /**< EtherCAT-Rahmen */ |
|
195 ec_master_t *master, |
|
196 /**< EtherCAT-Master */ |
|
197 uint16_t offset, |
|
198 /**< Physikalische Speicheradresse im Slave */ |
|
199 unsigned int length, |
|
200 /**< Länge der zu schreibenden Daten */ |
|
201 const uint8_t *data |
|
202 /**< Zeiger auf Speicher mit zu schreibenden Daten */ |
|
203 ) |
|
204 { |
|
205 EC_FUNC_HEADER; |
|
206 |
|
207 frame->type = ec_frame_type_bwr; |
|
208 frame->address.physical.slave = 0x0000; |
|
209 frame->address.physical.mem = offset; |
|
210 |
|
211 EC_FUNC_WRITE_FOOTER; |
|
212 } |
|
213 |
|
214 /*****************************************************************************/ |
|
215 |
|
216 /** |
|
217 Initialisiert ein EtherCAT-LRW-Kommando. |
|
218 |
|
219 Logical read write. |
|
220 */ |
|
221 |
|
222 void ec_frame_init_lrw(ec_frame_t *frame, |
|
223 /**< EtherCAT-Rahmen */ |
|
224 ec_master_t *master, |
|
225 /**< EtherCAT-Master */ |
|
226 uint32_t offset, |
|
227 /**< Logische Startadresse */ |
|
228 unsigned int length, |
|
229 /**< Länge der zu lesenden/schreibenden Daten */ |
|
230 uint8_t *data |
|
231 /**< Zeiger auf die Daten */ |
|
232 ) |
|
233 { |
|
234 EC_FUNC_HEADER; |
|
235 |
|
236 frame->type = ec_frame_type_lrw; |
|
237 frame->address.logical = offset; |
|
238 |
|
239 EC_FUNC_WRITE_FOOTER; |
|
240 } |
|
241 |
|
242 /*****************************************************************************/ |
|
243 |
|
244 /** |
|
245 Sendet einen einzelnen EtherCAT-Rahmen. |
|
246 |
|
247 \return 0 bei Erfolg, sonst < 0 |
|
248 */ |
|
249 |
|
250 int ec_frame_send(ec_frame_t *frame /**< Rahmen zum Senden */) |
|
251 { |
|
252 unsigned int command_size, frame_size, i; |
|
253 uint8_t *data; |
|
254 |
|
255 if (unlikely(frame->master->debug_level > 0)) { |
|
256 printk(KERN_DEBUG "EtherCAT: ec_frame_send\n"); |
|
257 } |
|
258 |
|
259 if (unlikely(frame->state != ec_frame_ready)) { |
|
260 printk(KERN_WARNING "EtherCAT: Frame not in \"ready\" state!\n"); |
|
261 } |
|
262 |
|
263 command_size = frame->data_length + EC_COMMAND_HEADER_SIZE |
|
264 + EC_COMMAND_FOOTER_SIZE; |
|
265 frame_size = command_size + EC_FRAME_HEADER_SIZE; |
|
266 |
|
267 if (unlikely(frame_size > EC_MAX_FRAME_SIZE)) { |
|
268 printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", frame_size); |
|
269 return -1; |
|
270 } |
|
271 |
|
272 if (frame_size < EC_MIN_FRAME_SIZE) frame_size = EC_MIN_FRAME_SIZE; |
|
273 |
|
274 if (unlikely(frame->master->debug_level > 0)) { |
|
275 printk(KERN_DEBUG "EtherCAT: Frame length: %i\n", frame_size); |
|
276 } |
|
277 |
|
278 frame->index = frame->master->command_index; |
|
279 frame->master->command_index = (frame->master->command_index + 1) % 0x0100; |
|
280 |
|
281 if (unlikely(frame->master->debug_level > 0)) { |
|
282 printk(KERN_DEBUG "EtherCAT: Sending command index 0x%X\n", |
|
283 frame->index); |
|
284 } |
|
285 |
|
286 frame->state = ec_frame_sent; |
|
287 |
|
288 // Zeiger auf Socket-Buffer holen |
|
289 data = ec_device_prepare(&frame->master->device); |
|
290 |
|
291 // EtherCAT frame header |
|
292 data[0] = command_size & 0xFF; |
|
293 data[1] = ((command_size & 0x700) >> 8) | 0x10; |
|
294 data += EC_FRAME_HEADER_SIZE; |
|
295 |
|
296 // EtherCAT command header |
|
297 data[0] = frame->type; |
|
298 data[1] = frame->index; |
|
299 data[2] = frame->address.raw[0]; |
|
300 data[3] = frame->address.raw[1]; |
|
301 data[4] = frame->address.raw[2]; |
|
302 data[5] = frame->address.raw[3]; |
|
303 data[6] = frame->data_length & 0xFF; |
|
304 data[7] = (frame->data_length & 0x700) >> 8; |
|
305 data[8] = 0x00; |
|
306 data[9] = 0x00; |
|
307 data += EC_COMMAND_HEADER_SIZE; |
|
308 |
|
309 if (likely(frame->type == ec_frame_type_apwr // Write commands |
|
310 || frame->type == ec_frame_type_npwr |
|
311 || frame->type == ec_frame_type_bwr |
|
312 || frame->type == ec_frame_type_lrw)) { |
|
313 memcpy(data, frame->data, frame->data_length); |
|
314 } |
|
315 else { // Read commands |
|
316 memset(data, 0x00, frame->data_length); |
|
317 } |
|
318 |
|
319 // EtherCAT command footer |
|
320 data += frame->data_length; |
|
321 data[0] = frame->working_counter & 0xFF; |
|
322 data[1] = (frame->working_counter & 0xFF00) >> 8; |
|
323 data += EC_COMMAND_FOOTER_SIZE; |
|
324 |
|
325 // Pad with zeros |
|
326 for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE |
|
327 + frame->data_length + EC_COMMAND_FOOTER_SIZE; |
|
328 i < EC_MIN_FRAME_SIZE; i++) { |
|
329 *data++ = 0x00; |
|
330 } |
|
331 |
|
332 // Send frame |
|
333 ec_device_send(&frame->master->device, frame_size); |
|
334 |
|
335 return 0; |
|
336 } |
|
337 |
|
338 /*****************************************************************************/ |
|
339 |
|
340 /** |
|
341 Empfängt einen gesendeten Rahmen. |
|
342 |
|
343 \return 0 bei Erfolg, sonst < 0 |
|
344 */ |
|
345 |
|
346 int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */) |
|
347 { |
|
348 unsigned int received_length, frame_length, data_length; |
|
349 uint8_t *data; |
|
350 uint8_t command_type, command_index; |
|
351 ec_device_t *device; |
|
352 |
|
353 if (unlikely(frame->state != ec_frame_sent)) { |
|
354 printk(KERN_ERR "EtherCAT: Frame was not sent!\n"); |
|
355 return -1; |
|
356 } |
|
357 |
|
358 device = &frame->master->device; |
|
359 |
|
360 if (!(received_length = ec_device_received(device))) return -1; |
|
361 |
|
362 device->state = EC_DEVICE_STATE_READY; |
|
363 |
|
364 if (unlikely(received_length < EC_FRAME_HEADER_SIZE)) { |
|
365 printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT" |
|
366 " frame header!\n"); |
|
367 ec_frame_print(frame); |
|
368 return -1; |
|
369 } |
|
370 |
|
371 data = ec_device_data(device); |
|
372 |
|
373 // Länge des gesamten Frames prüfen |
|
374 frame_length = (data[0] & 0xFF) | ((data[1] & 0x07) << 8); |
|
375 |
|
376 if (unlikely(frame_length > received_length)) { |
|
377 printk(KERN_ERR "EtherCAT: Received corrupted frame (length does" |
|
378 " not match)!\n"); |
|
379 ec_frame_print(frame); |
|
380 return -1; |
|
381 } |
|
382 |
|
383 // Command header |
|
384 data += EC_FRAME_HEADER_SIZE; |
|
385 command_type = data[0]; |
|
386 command_index = data[1]; |
|
387 data_length = (data[6] & 0xFF) | ((data[7] & 0x07) << 8); |
|
388 |
|
389 if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE |
|
390 + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) { |
|
391 printk(KERN_ERR "EtherCAT: Received frame with incomplete command" |
|
392 " data!\n"); |
|
393 ec_frame_print(frame); |
|
394 return -1; |
|
395 } |
|
396 |
|
397 if (unlikely(frame->type != command_type |
|
398 || frame->index != command_index |
|
399 || frame->data_length != data_length)) |
|
400 { |
|
401 printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
|
402 ec_frame_print(frame); |
|
403 ec_device_call_isr(device); // Empfangenes "vergessen" |
|
404 return -1; |
|
405 } |
|
406 |
|
407 frame->state = ec_frame_received; |
|
408 |
|
409 // Empfangene Daten in Kommandodatenspeicher kopieren |
|
410 data += EC_COMMAND_HEADER_SIZE; |
|
411 memcpy(frame->data, data, data_length); |
|
412 data += data_length; |
|
413 |
|
414 // Working-Counter setzen |
|
415 frame->working_counter = (data[0] & 0xFF) | ((data[1] & 0xFF) << 8); |
|
416 |
|
417 if (unlikely(frame->master->debug_level > 1)) { |
|
418 ec_frame_print(frame); |
|
419 } |
|
420 |
|
421 return 0; |
|
422 } |
|
423 |
|
424 /*****************************************************************************/ |
|
425 |
|
426 /** |
|
427 Sendet einen einzeln Rahmen und wartet auf dessen Empfang. |
|
428 |
|
429 \return 0 bei Erfolg, sonst < 0 |
|
430 */ |
|
431 |
|
432 int ec_frame_send_receive(ec_frame_t *frame |
|
433 /**< Rahmen zum Senden/Empfangen */ |
|
434 ) |
|
435 { |
|
436 unsigned int tries_left; |
|
437 |
|
438 if (unlikely(ec_frame_send(frame) < 0)) { |
|
439 printk(KERN_ERR "EtherCAT: Frame sending failed!\n"); |
|
440 return -1; |
|
441 } |
|
442 |
|
443 tries_left = 20; |
|
444 do |
|
445 { |
|
446 udelay(1); |
|
447 ec_device_call_isr(&frame->master->device); |
|
448 tries_left--; |
|
449 } |
|
450 while (unlikely(!ec_device_received(&frame->master->device) |
|
451 && tries_left)); |
|
452 |
|
453 if (unlikely(!tries_left)) { |
|
454 printk(KERN_ERR "EtherCAT: Frame timeout!\n"); |
|
455 return -1; |
|
456 } |
|
457 |
|
458 if (unlikely(ec_frame_receive(frame) < 0)) { |
|
459 printk(KERN_ERR "EtherCAT: Frame receiving failed!\n"); |
|
460 return -1; |
|
461 } |
|
462 |
|
463 return 0; |
|
464 } |
|
465 |
|
466 /*****************************************************************************/ |
|
467 |
|
468 /** |
|
469 Gibt Frame-Inhalte zwecks Debugging aus. |
|
470 */ |
|
471 |
|
472 void ec_frame_print(const ec_frame_t *frame /**< EtherCAT-Frame */) |
|
473 { |
|
474 unsigned int i; |
|
475 |
|
476 printk(KERN_DEBUG "EtherCAT: Frame contents (%i Bytes):\n", |
|
477 frame->data_length); |
|
478 |
|
479 printk(KERN_DEBUG); |
|
480 for (i = 0; i < frame->data_length; i++) |
|
481 { |
|
482 printk("%02X ", frame->data[i]); |
|
483 if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG); |
|
484 } |
|
485 printk("\n"); |
|
486 } |
|
487 |
|
488 /*****************************************************************************/ |
|
489 |
|
490 /* Emacs-Konfiguration |
|
491 ;;; Local Variables: *** |
|
492 ;;; c-basic-offset:4 *** |
|
493 ;;; End: *** |
|
494 */ |