15 #include <linux/slab.h> |
15 #include <linux/slab.h> |
16 #include <linux/delay.h> |
16 #include <linux/delay.h> |
17 |
17 |
18 #include "ec_globals.h" |
18 #include "ec_globals.h" |
19 #include "ec_master.h" |
19 #include "ec_master.h" |
20 #include "ec_dbg.h" |
|
21 |
20 |
22 /***************************************************************/ |
21 /***************************************************************/ |
23 |
22 |
24 /** |
23 /** |
25 Konstruktor des EtherCAT-Masters. |
24 Konstruktor des EtherCAT-Masters. |
35 int EtherCAT_master_init(EtherCAT_master_t *master, |
34 int EtherCAT_master_init(EtherCAT_master_t *master, |
36 EtherCAT_device_t *dev) |
35 EtherCAT_device_t *dev) |
37 { |
36 { |
38 if (!dev) |
37 if (!dev) |
39 { |
38 { |
40 EC_DBG(KERN_ERR "EtherCAT: Master init without device!\n"); |
39 printk(KERN_ERR "EtherCAT: Master init without device!\n"); |
41 return -1; |
40 return -1; |
42 } |
41 } |
43 |
42 |
44 master->slaves = NULL; |
43 master->slaves = NULL; |
45 master->slave_count = 0; |
44 master->slave_count = 0; |
93 int EtherCAT_simple_send_receive(EtherCAT_master_t *master, |
92 int EtherCAT_simple_send_receive(EtherCAT_master_t *master, |
94 EtherCAT_command_t *cmd) |
93 EtherCAT_command_t *cmd) |
95 { |
94 { |
96 unsigned int tries_left; |
95 unsigned int tries_left; |
97 |
96 |
98 // EC_DBG("ECAT send...\n"); //HM |
|
99 |
|
100 if (EtherCAT_simple_send(master, cmd) < 0) return -1; |
97 if (EtherCAT_simple_send(master, cmd) < 0) return -1; |
101 |
98 |
102 // EC_DBG("ECAT call isr \n"); //HM |
99 udelay(3); |
103 udelay(3); //FIXME nur zum Test HM |
|
104 |
100 |
105 EtherCAT_device_call_isr(master->dev); |
101 EtherCAT_device_call_isr(master->dev); |
106 |
102 |
107 tries_left = 100; |
103 tries_left = 100; |
108 while (master->dev->state == ECAT_DS_SENT && tries_left) |
104 while (master->dev->state == ECAT_DS_SENT && tries_left) |
109 { |
105 { |
110 udelay(1); |
106 udelay(1); |
111 // EC_DBG("ECAT call isr \n"); //HM |
|
112 EtherCAT_device_call_isr(master->dev); |
107 EtherCAT_device_call_isr(master->dev); |
113 tries_left--; |
108 tries_left--; |
114 } |
109 } |
115 |
110 |
116 // EC_DBG("ECAT recieve \n"); //HM |
|
117 |
|
118 if (EtherCAT_simple_receive(master, cmd) < 0) return -1; |
111 if (EtherCAT_simple_receive(master, cmd) < 0) return -1; |
119 |
|
120 // EC_DBG("ECAT recieve done\n"); //HM |
|
121 |
112 |
122 return 0; |
113 return 0; |
123 } |
114 } |
124 |
115 |
125 /***************************************************************/ |
116 /***************************************************************/ |
138 { |
129 { |
139 unsigned int length, framelength, i; |
130 unsigned int length, framelength, i; |
140 |
131 |
141 if (master->debug_level > 0) |
132 if (master->debug_level > 0) |
142 { |
133 { |
143 EC_DBG(KERN_DEBUG "EtherCAT_send_receive_command\n"); |
134 printk(KERN_DEBUG "EtherCAT_send_receive_command\n"); |
144 } |
135 } |
145 |
136 |
146 if (cmd->state != ECAT_CS_READY) |
137 if (cmd->state != ECAT_CS_READY) |
147 { |
138 { |
148 EC_DBG(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n"); |
139 printk(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n"); |
149 } |
140 } |
150 |
141 |
151 length = cmd->data_length + 12; |
142 length = cmd->data_length + 12; |
152 framelength = length + 2; |
143 framelength = length + 2; |
153 |
144 |
154 if (framelength > ECAT_FRAME_BUFFER_SIZE) |
145 if (framelength > ECAT_FRAME_BUFFER_SIZE) |
155 { |
146 { |
156 EC_DBG(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength); |
147 printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength); |
157 return -1; |
148 return -1; |
158 } |
149 } |
159 |
150 |
160 if (framelength < 46) framelength = 46; |
151 if (framelength < 46) framelength = 46; |
161 |
152 |
162 if (master->debug_level > 0) |
153 if (master->debug_level > 0) |
163 { |
154 { |
164 EC_DBG(KERN_DEBUG "Frame length: %i\n", framelength); |
155 printk(KERN_DEBUG "Frame length: %i\n", framelength); |
165 } |
156 } |
166 |
157 |
167 master->tx_data[0] = length & 0xFF; |
158 master->tx_data[0] = length & 0xFF; |
168 master->tx_data[1] = ((length & 0x700) >> 8) | 0x10; |
159 master->tx_data[1] = ((length & 0x700) >> 8) | 0x10; |
169 |
160 |
170 cmd->index = master->command_index; |
161 cmd->index = master->command_index; |
171 master->command_index = (master->command_index + 1) % 0x0100; |
162 master->command_index = (master->command_index + 1) % 0x0100; |
172 |
163 |
173 if (master->debug_level > 0) |
164 if (master->debug_level > 0) |
174 { |
165 { |
175 EC_DBG(KERN_DEBUG "Sending command index %i\n", cmd->index); |
166 printk(KERN_DEBUG "Sending command index %i\n", cmd->index); |
176 } |
167 } |
177 |
168 |
178 cmd->state = ECAT_CS_SENT; |
169 cmd->state = ECAT_CS_SENT; |
179 |
170 |
180 master->tx_data[2 + 0] = cmd->type; |
171 master->tx_data[2 + 0] = cmd->type; |
208 |
199 |
209 master->tx_data_length = framelength; |
200 master->tx_data_length = framelength; |
210 |
201 |
211 if (master->debug_level > 0) |
202 if (master->debug_level > 0) |
212 { |
203 { |
213 EC_DBG(KERN_DEBUG "device send...\n"); |
204 printk(KERN_DEBUG "device send...\n"); |
214 } |
205 } |
215 |
206 |
216 // Send frame |
207 // Send frame |
217 if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0) |
208 if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0) |
218 { |
209 { |
219 EC_DBG(KERN_ERR "EtherCAT: Could not send!\n"); |
210 printk(KERN_ERR "EtherCAT: Could not send!\n"); |
220 return -1; |
211 return -1; |
221 } |
212 } |
222 |
213 |
223 if (master->debug_level > 0) |
214 if (master->debug_level > 0) |
224 { |
215 { |
225 EC_DBG(KERN_DEBUG "EtherCAT_send done.\n"); |
216 printk(KERN_DEBUG "EtherCAT_send done.\n"); |
226 } |
217 } |
227 |
218 |
228 return 0; |
219 return 0; |
229 } |
220 } |
230 |
221 |
255 |
246 |
256 master->rx_data_length = (unsigned int) receive_ret; |
247 master->rx_data_length = (unsigned int) receive_ret; |
257 |
248 |
258 if (master->rx_data_length < 2) |
249 if (master->rx_data_length < 2) |
259 { |
250 { |
260 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n"); |
251 printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n"); |
261 output_debug_data(master); |
252 output_debug_data(master); |
262 return -1; |
253 return -1; |
263 } |
254 } |
264 |
255 |
265 // Länge des gesamten Frames prüfen |
256 // Länge des gesamten Frames prüfen |
266 length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); |
257 length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); |
267 |
258 |
268 if (length > master->rx_data_length) |
259 if (length > master->rx_data_length) |
269 { |
260 { |
270 EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); |
261 printk(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); |
271 output_debug_data(master); |
262 output_debug_data(master); |
272 return -1; |
263 return -1; |
273 } |
264 } |
274 |
265 |
275 command_type = master->rx_data[2]; |
266 command_type = master->rx_data[2]; |
276 command_index = master->rx_data[2 + 1]; |
267 command_index = master->rx_data[2 + 1]; |
277 length = (master->rx_data[2 + 6] & 0xFF) | ((master->rx_data[2 + 7] & 0x07) << 8); |
268 length = (master->rx_data[2 + 6] & 0xFF) | ((master->rx_data[2 + 7] & 0x07) << 8); |
278 |
269 |
279 if (master->rx_data_length - 2 < length + 12) |
270 if (master->rx_data_length - 2 < length + 12) |
280 { |
271 { |
281 EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); |
272 printk(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); |
282 output_debug_data(master); |
273 output_debug_data(master); |
283 return -1; |
274 return -1; |
284 } |
275 } |
285 |
276 |
286 if (cmd->state == ECAT_CS_SENT |
277 if (cmd->state == ECAT_CS_SENT |
297 cmd->working_counter = ((master->rx_data[length + 2 + 10] & 0xFF) |
288 cmd->working_counter = ((master->rx_data[length + 2 + 10] & 0xFF) |
298 | ((master->rx_data[length + 2 + 11] & 0xFF) << 8)); |
289 | ((master->rx_data[length + 2 + 11] & 0xFF) << 8)); |
299 } |
290 } |
300 else |
291 else |
301 { |
292 { |
302 EC_DBG(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
293 printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); |
303 output_debug_data(master); |
294 output_debug_data(master); |
304 } |
295 } |
305 |
296 |
306 master->dev->state = ECAT_DS_READY; |
297 master->dev->state = ECAT_DS_READY; |
307 |
298 |
335 unsigned char data[2]; |
326 unsigned char data[2]; |
336 |
327 |
337 // EtherCAT_clear_slaves() must be called before! |
328 // EtherCAT_clear_slaves() must be called before! |
338 if (master->slaves || master->slave_count) |
329 if (master->slaves || master->slave_count) |
339 { |
330 { |
340 EC_DBG(KERN_ERR "EtherCAT duplicate slave check!"); |
331 printk(KERN_ERR "EtherCAT duplicate slave check!"); |
341 return -1; |
332 return -1; |
342 } |
333 } |
343 |
334 |
344 // No slaves. |
335 // No slaves. |
345 if (slave_count == 0) |
336 if (slave_count == 0) |
346 { |
337 { |
347 EC_DBG(KERN_ERR "EtherCAT: No slaves in list!"); |
338 printk(KERN_ERR "EtherCAT: No slaves in list!"); |
348 return -1; |
339 return -1; |
349 } |
340 } |
350 |
341 |
351 // Determine number of slaves on bus |
342 // Determine number of slaves on bus |
352 |
343 |
354 |
345 |
355 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
346 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
356 |
347 |
357 if (cmd.working_counter != slave_count) |
348 if (cmd.working_counter != slave_count) |
358 { |
349 { |
359 EC_DBG(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n", |
350 printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n", |
360 cmd.working_counter, slave_count); |
351 cmd.working_counter, slave_count); |
361 return -1; |
352 return -1; |
362 } |
353 } |
363 else |
354 else |
364 { |
355 { |
365 EC_DBG("EtherCAT: Found all %i slaves.\n", slave_count); |
356 printk("EtherCAT: Found all %i slaves.\n", slave_count); |
366 } |
357 } |
367 |
358 |
368 // For every slave in the list |
359 // For every slave in the list |
369 for (i = 0; i < slave_count; i++) |
360 for (i = 0; i < slave_count; i++) |
370 { |
361 { |
371 cur = &slaves[i]; |
362 cur = &slaves[i]; |
372 |
363 |
373 if (!cur->desc) |
364 if (!cur->desc) |
374 { |
365 { |
375 EC_DBG(KERN_ERR "EtherCAT: Slave %i has no description.\n", i); |
366 printk(KERN_ERR "EtherCAT: Slave %i has no description.\n", i); |
376 return -1; |
367 return -1; |
377 } |
368 } |
378 |
369 |
379 // Set ring position |
370 // Set ring position |
380 cur->ring_position = -i; |
371 cur->ring_position = -i; |
388 EtherCAT_command_position_write(&cmd, cur->ring_position, 0x0010, 2, data); |
379 EtherCAT_command_position_write(&cmd, cur->ring_position, 0x0010, 2, data); |
389 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
380 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
390 |
381 |
391 if (cmd.working_counter != 1) |
382 if (cmd.working_counter != 1) |
392 { |
383 { |
393 EC_DBG(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i); |
384 printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i); |
394 return -1; |
385 return -1; |
395 } |
386 } |
396 |
387 |
397 // Read base data |
388 // Read base data |
398 |
389 |
399 EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4); |
390 EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4); |
400 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
391 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
401 |
392 |
402 if (cmd.working_counter != 1) |
393 if (cmd.working_counter != 1) |
403 { |
394 { |
404 EC_DBG(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i); |
395 printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i); |
405 return -1; |
396 return -1; |
406 } |
397 } |
407 |
398 |
408 // Get base data |
399 // Get base data |
409 cur->type = cmd.data[0]; |
400 cur->type = cmd.data[0]; |
413 // Read identification from "Slave Information Interface" (SII) |
404 // Read identification from "Slave Information Interface" (SII) |
414 |
405 |
415 if (EtherCAT_read_slave_information(master, cur->station_address, |
406 if (EtherCAT_read_slave_information(master, cur->station_address, |
416 0x0008, &cur->vendor_id) != 0) |
407 0x0008, &cur->vendor_id) != 0) |
417 { |
408 { |
418 EC_DBG(KERN_ERR "EtherCAT: Could not read SII vendor id!\n"); |
409 printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n"); |
419 return -1; |
410 return -1; |
420 } |
411 } |
421 |
412 |
422 if (EtherCAT_read_slave_information(master, cur->station_address, |
413 if (EtherCAT_read_slave_information(master, cur->station_address, |
423 0x000A, &cur->product_code) != 0) |
414 0x000A, &cur->product_code) != 0) |
424 { |
415 { |
425 EC_DBG(KERN_ERR "EtherCAT: Could not read SII product code!\n"); |
416 printk(KERN_ERR "EtherCAT: Could not read SII product code!\n"); |
426 return -1; |
417 return -1; |
427 } |
418 } |
428 |
419 |
429 if (EtherCAT_read_slave_information(master, cur->station_address, |
420 if (EtherCAT_read_slave_information(master, cur->station_address, |
430 0x000C, &cur->revision_number) != 0) |
421 0x000C, &cur->revision_number) != 0) |
431 { |
422 { |
432 EC_DBG(KERN_ERR "EtherCAT: Could not read SII revision number!\n"); |
423 printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n"); |
433 return -1; |
424 return -1; |
434 } |
425 } |
435 |
426 |
436 if (EtherCAT_read_slave_information(master, cur->station_address, |
427 if (EtherCAT_read_slave_information(master, cur->station_address, |
437 0x000E, &cur->serial_number) != 0) |
428 0x000E, &cur->serial_number) != 0) |
438 { |
429 { |
439 EC_DBG(KERN_ERR "EtherCAT: Could not read SII serial number!\n"); |
430 printk(KERN_ERR "EtherCAT: Could not read SII serial number!\n"); |
440 return -1; |
431 return -1; |
441 } |
432 } |
442 |
433 |
443 // Search for identification in "database" |
434 // Search for identification in "database" |
444 |
435 |
451 { |
442 { |
452 found = 1; |
443 found = 1; |
453 |
444 |
454 if (cur->desc != slave_idents[j].desc) |
445 if (cur->desc != slave_idents[j].desc) |
455 { |
446 { |
456 EC_DBG(KERN_ERR "EtherCAT: Unexpected slave device \"%s %s\"" |
447 printk(KERN_ERR "EtherCAT: Unexpected slave device \"%s %s\"" |
457 " at position %i. Expected: \"%s %s\"\n", |
448 " at position %i. Expected: \"%s %s\"\n", |
458 slave_idents[j].desc->vendor_name, |
449 slave_idents[j].desc->vendor_name, |
459 slave_idents[j].desc->product_name, i, |
450 slave_idents[j].desc->product_name, i, |
460 cur->desc->vendor_name, cur->desc->product_name); |
451 cur->desc->vendor_name, cur->desc->product_name); |
461 return -1; |
452 return -1; |
481 } |
472 } |
482 |
473 |
483 if ((master->process_data = (unsigned char *) |
474 if ((master->process_data = (unsigned char *) |
484 kmalloc(sizeof(unsigned char) * length, GFP_KERNEL)) == NULL) |
475 kmalloc(sizeof(unsigned char) * length, GFP_KERNEL)) == NULL) |
485 { |
476 { |
486 EC_DBG(KERN_ERR "EtherCAT: Could not allocate %i bytes for process data.\n", length); |
477 printk(KERN_ERR "EtherCAT: Could not allocate %i bytes for process data.\n", length); |
487 return -1; |
478 return -1; |
488 } |
479 } |
489 |
480 |
490 master->process_data_length = length; |
481 master->process_data_length = length; |
491 memset(master->process_data, 0x00, length); |
482 memset(master->process_data, 0x00, length); |
494 for (i = 0; i < slave_count; i++) |
485 for (i = 0; i < slave_count; i++) |
495 { |
486 { |
496 slaves[i].process_data = master->process_data + pos; |
487 slaves[i].process_data = master->process_data + pos; |
497 slaves[i].logical_address0 = pos; |
488 slaves[i].logical_address0 = pos; |
498 |
489 |
499 EC_DBG(KERN_DEBUG "EtherCAT: Slave %i - Address 0x%X, \"%s %s\", s/n %u\n", |
490 printk(KERN_DEBUG "EtherCAT: Slave %i - Address 0x%X, \"%s %s\", s/n %u\n", |
500 i, pos, slaves[i].desc->vendor_name, slaves[i].desc->product_name, |
491 i, pos, slaves[i].desc->vendor_name, slaves[i].desc->product_name, |
501 slaves[i].serial_number); |
492 slaves[i].serial_number); |
502 |
493 |
503 pos += slaves[i].desc->data_length; |
494 pos += slaves[i].desc->data_length; |
504 } |
495 } |
559 EtherCAT_command_write(&cmd, node_address, 0x502, 6, data); |
550 EtherCAT_command_write(&cmd, node_address, 0x502, 6, data); |
560 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -3; |
551 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -3; |
561 |
552 |
562 if (cmd.working_counter != 1) |
553 if (cmd.working_counter != 1) |
563 { |
554 { |
564 EC_DBG(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n", |
555 printk(KERN_ERR "EtherCAT: SII-read - Slave %04X did not respond!\n", |
565 node_address); |
556 node_address); |
566 return -4; |
557 return -4; |
567 } |
558 } |
568 |
559 |
569 // Der Slave legt die Informationen des Slave-Information-Interface |
560 // Der Slave legt die Informationen des Slave-Information-Interface |
578 EtherCAT_command_read(&cmd, node_address, 0x502, 10); |
569 EtherCAT_command_read(&cmd, node_address, 0x502, 10); |
579 if (EtherCAT_simple_send_receive(master, &cmd) != 0) return -3; |
570 if (EtherCAT_simple_send_receive(master, &cmd) != 0) return -3; |
580 |
571 |
581 if (cmd.working_counter != 1) |
572 if (cmd.working_counter != 1) |
582 { |
573 { |
583 EC_DBG(KERN_ERR "EtherCAT: SII-read status - Slave %04X did not respond!\n", |
574 printk(KERN_ERR "EtherCAT: SII-read status - Slave %04X did not respond!\n", |
584 node_address); |
575 node_address); |
585 return -4; |
576 return -4; |
586 } |
577 } |
587 |
578 |
588 if ((cmd.data[1] & 0x81) == 0) |
579 if ((cmd.data[1] & 0x81) == 0) |
632 |
623 |
633 EtherCAT_command_write(&cmd, slave->station_address, 0x0120, 2, data); |
624 EtherCAT_command_write(&cmd, slave->station_address, 0x0120, 2, data); |
634 |
625 |
635 if (EtherCAT_simple_send_receive(master, &cmd) != 0) |
626 if (EtherCAT_simple_send_receive(master, &cmd) != 0) |
636 { |
627 { |
637 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", state_and_ack); |
628 printk(KERN_ERR "EtherCAT: Could not set state %02X - Unable to send!\n", state_and_ack); |
638 return -2; |
629 return -2; |
639 } |
630 } |
640 |
631 |
641 if (cmd.working_counter != 1) |
632 if (cmd.working_counter != 1) |
642 { |
633 { |
643 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device \"%s %s\" (%d) did not respond!\n", |
634 printk(KERN_ERR "EtherCAT: Could not set state %02X - Device \"%s %s\" (%d) did not respond!\n", |
644 state_and_ack, |
635 state_and_ack, |
645 slave->desc->vendor_name, |
636 slave->desc->vendor_name, |
646 slave->desc->product_name, |
637 slave->desc->product_name, |
647 slave->ring_position*(-1)); |
638 slave->ring_position*(-1)); |
648 return -3; |
639 return -3; |
657 |
648 |
658 EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2); |
649 EtherCAT_command_read(&cmd, slave->station_address, 0x0130, 2); |
659 |
650 |
660 if (EtherCAT_simple_send_receive(master, &cmd) != 0) |
651 if (EtherCAT_simple_send_receive(master, &cmd) != 0) |
661 { |
652 { |
662 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Unable to send!\n", state_and_ack); |
653 printk(KERN_ERR "EtherCAT: Could not check state %02X - Unable to send!\n", state_and_ack); |
663 return -2; |
654 return -2; |
664 } |
655 } |
665 |
656 |
666 if (cmd.working_counter != 1) |
657 if (cmd.working_counter != 1) |
667 { |
658 { |
668 EC_DBG(KERN_ERR "EtherCAT: Could not check state %02X - Device did not respond!\n", state_and_ack); |
659 printk(KERN_ERR "EtherCAT: Could not check state %02X - Device did not respond!\n", state_and_ack); |
669 return -3; |
660 return -3; |
670 } |
661 } |
671 |
662 |
672 if (cmd.data[0] & 0x10) // State change error |
663 if (cmd.data[0] & 0x10) // State change error |
673 { |
664 { |
674 EC_DBG(KERN_ERR "EtherCAT: Could not set state %02X - Device refused state change (code %02X)!\n", state_and_ack, cmd.data[0]); |
665 printk(KERN_ERR "EtherCAT: Could not set state %02X - Device refused state change (code %02X)!\n", state_and_ack, cmd.data[0]); |
675 return -4; |
666 return -4; |
676 } |
667 } |
677 |
668 |
678 if (cmd.data[0] == (state_and_ack & 0x0F)) // State change successful |
669 if (cmd.data[0] == (state_and_ack & 0x0F)) // State change successful |
679 { |
670 { |
732 |
723 |
733 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
724 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
734 |
725 |
735 if (cmd.working_counter != 1) |
726 if (cmd.working_counter != 1) |
736 { |
727 { |
737 EC_DBG(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not respond!\n", |
728 printk(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %04X did not respond!\n", |
738 slave->station_address); |
729 slave->station_address); |
739 return -2; |
730 return -2; |
740 } |
731 } |
741 |
732 |
742 // Resetting Sync Manager channels |
733 // Resetting Sync Manager channels |
748 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 256, data); |
739 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 256, data); |
749 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
740 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
750 |
741 |
751 if (cmd.working_counter != 1) |
742 if (cmd.working_counter != 1) |
752 { |
743 { |
753 EC_DBG(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not respond!\n", |
744 printk(KERN_ERR "EtherCAT: Resetting SMs - Slave %04X did not respond!\n", |
754 slave->station_address); |
745 slave->station_address); |
755 return -2; |
746 return -2; |
756 } |
747 } |
757 } |
748 } |
758 |
749 |
765 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0); |
756 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0); |
766 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
757 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
767 |
758 |
768 if (cmd.working_counter != 1) |
759 if (cmd.working_counter != 1) |
769 { |
760 { |
770 EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
761 printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
771 slave->station_address); |
762 slave->station_address); |
772 return -3; |
763 return -3; |
773 } |
764 } |
774 } |
765 } |
775 |
766 |
778 EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1); |
769 EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1); |
779 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
770 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
780 |
771 |
781 if (cmd.working_counter != 1) |
772 if (cmd.working_counter != 1) |
782 { |
773 { |
783 EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
774 printk(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
784 slave->station_address); |
775 slave->station_address); |
785 return -2; |
776 return -2; |
786 } |
777 } |
787 } |
778 } |
788 } |
779 } |
808 EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu); |
799 EtherCAT_command_write(&cmd, slave->station_address, 0x0600, 16, fmmu); |
809 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
800 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
810 |
801 |
811 if (cmd.working_counter != 1) |
802 if (cmd.working_counter != 1) |
812 { |
803 { |
813 EC_DBG(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not respond!\n", |
804 printk(KERN_ERR "EtherCAT: Setting FMMU0 - Slave %04X did not respond!\n", |
814 slave->station_address); |
805 slave->station_address); |
815 return -2; |
806 return -2; |
816 } |
807 } |
817 } |
808 } |
818 |
809 |
825 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0); |
816 EtherCAT_command_write(&cmd, slave->station_address, 0x0800, 8, desc->sm0); |
826 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
817 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
827 |
818 |
828 if (cmd.working_counter != 1) |
819 if (cmd.working_counter != 1) |
829 { |
820 { |
830 EC_DBG(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
821 printk(KERN_ERR "EtherCAT: Setting SM0 - Slave %04X did not respond!\n", |
831 slave->station_address); |
822 slave->station_address); |
832 return -3; |
823 return -3; |
833 } |
824 } |
834 } |
825 } |
835 |
826 |
838 EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1); |
829 EtherCAT_command_write(&cmd, slave->station_address, 0x0808, 8, desc->sm1); |
839 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
830 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
840 |
831 |
841 if (cmd.working_counter != 1) |
832 if (cmd.working_counter != 1) |
842 { |
833 { |
843 EC_DBG(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
834 printk(KERN_ERR "EtherCAT: Setting SM1 - Slave %04X did not respond!\n", |
844 slave->station_address); |
835 slave->station_address); |
845 return -3; |
836 return -3; |
846 } |
837 } |
847 } |
838 } |
848 } |
839 } |
852 EtherCAT_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2); |
843 EtherCAT_command_write(&cmd, slave->station_address, 0x0810, 8, desc->sm2); |
853 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
844 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
854 |
845 |
855 if (cmd.working_counter != 1) |
846 if (cmd.working_counter != 1) |
856 { |
847 { |
857 EC_DBG(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n", |
848 printk(KERN_ERR "EtherCAT: Setting SM2 - Slave %04X did not respond!\n", |
858 slave->station_address); |
849 slave->station_address); |
859 return -3; |
850 return -3; |
860 } |
851 } |
861 } |
852 } |
862 |
853 |
865 EtherCAT_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3); |
856 EtherCAT_command_write(&cmd, slave->station_address, 0x0818, 8, desc->sm3); |
866 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
857 if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1; |
867 |
858 |
868 if (cmd.working_counter != 1) |
859 if (cmd.working_counter != 1) |
869 { |
860 { |
870 EC_DBG(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n", |
861 printk(KERN_ERR "EtherCAT: Setting SM3 - Slave %04X did not respond!\n", |
871 slave->station_address); |
862 slave->station_address); |
872 return -3; |
863 return -3; |
873 } |
864 } |
874 } |
865 } |
875 |
866 |
984 0, master->process_data_length, |
975 0, master->process_data_length, |
985 master->process_data); |
976 master->process_data); |
986 |
977 |
987 if (EtherCAT_simple_send(master, &master->process_data_command) < 0) |
978 if (EtherCAT_simple_send(master, &master->process_data_command) < 0) |
988 { |
979 { |
989 EC_DBG(KERN_ERR "EtherCAT: Could not send process data command!\n"); |
980 printk(KERN_ERR "EtherCAT: Could not send process data command!\n"); |
990 return -1; |
981 return -1; |
991 } |
982 } |
992 |
983 |
993 return 0; |
984 return 0; |
994 } |
985 } |
1021 tries_left--; |
1012 tries_left--; |
1022 } |
1013 } |
1023 |
1014 |
1024 if (!tries_left) |
1015 if (!tries_left) |
1025 { |
1016 { |
1026 EC_DBG(KERN_ERR "EtherCAT: Timeout while receiving process data!\n"); |
1017 printk(KERN_ERR "EtherCAT: Timeout while receiving process data!\n"); |
1027 return -1; |
1018 return -1; |
1028 } |
1019 } |
1029 |
1020 |
1030 if (EtherCAT_simple_receive(master, &master->process_data_command) < 0) |
1021 if (EtherCAT_simple_receive(master, &master->process_data_command) < 0) |
1031 { |
1022 { |
1032 EC_DBG(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); |
1023 printk(KERN_ERR "EtherCAT: Could not receive cyclic command!\n"); |
1033 return -1; |
1024 return -1; |
1034 } |
1025 } |
1035 |
1026 |
1036 if (master->process_data_command.state != ECAT_CS_RECEIVED) |
1027 if (master->process_data_command.state != ECAT_CS_RECEIVED) |
1037 { |
1028 { |
1038 EC_DBG(KERN_WARNING "EtherCAT: Process data command not received!\n"); |
1029 printk(KERN_WARNING "EtherCAT: Process data command not received!\n"); |
1039 return -1; |
1030 return -1; |
1040 } |
1031 } |
1041 |
1032 |
1042 // Daten von Kommando in Prozessdaten des Master kopieren |
1033 // Daten von Kommando in Prozessdaten des Master kopieren |
1043 memcpy(master->process_data, master->process_data_command.data, |
1034 memcpy(master->process_data, master->process_data_command.data, |
1070 |
1061 |
1071 void output_debug_data(const EtherCAT_master_t *master) |
1062 void output_debug_data(const EtherCAT_master_t *master) |
1072 { |
1063 { |
1073 unsigned int i; |
1064 unsigned int i; |
1074 |
1065 |
1075 EC_DBG(KERN_DEBUG "EtherCAT: tx_data content (%i Bytes):\n", |
1066 printk(KERN_DEBUG "EtherCAT: tx_data content (%i Bytes):\n", |
1076 master->tx_data_length); |
1067 master->tx_data_length); |
1077 |
1068 |
1078 EC_DBG(KERN_DEBUG); |
1069 printk(KERN_DEBUG); |
1079 for (i = 0; i < master->tx_data_length; i++) |
1070 for (i = 0; i < master->tx_data_length; i++) |
1080 { |
1071 { |
1081 EC_DBG("%02X ", master->tx_data[i]); |
1072 printk("%02X ", master->tx_data[i]); |
1082 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
1073 if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG); |
1083 } |
1074 } |
1084 EC_DBG("\n"); |
1075 printk("\n"); |
1085 |
1076 |
1086 EC_DBG(KERN_DEBUG "EtherCAT: rx_data content (%i Bytes):\n", |
1077 printk(KERN_DEBUG "EtherCAT: rx_data content (%i Bytes):\n", |
1087 master->rx_data_length); |
1078 master->rx_data_length); |
1088 |
1079 |
1089 EC_DBG(KERN_DEBUG); |
1080 printk(KERN_DEBUG); |
1090 for (i = 0; i < master->rx_data_length; i++) |
1081 for (i = 0; i < master->rx_data_length; i++) |
1091 { |
1082 { |
1092 EC_DBG("%02X ", master->rx_data[i]); |
1083 printk("%02X ", master->rx_data[i]); |
1093 if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); |
1084 if ((i + 1) % 16 == 0) printk("\n" KERN_DEBUG); |
1094 } |
1085 } |
1095 EC_DBG("\n"); |
1086 printk("\n"); |
1096 } |
1087 } |
1097 |
1088 |
1098 /***************************************************************/ |
1089 /***************************************************************/ |
1099 |
1090 |
1100 EXPORT_SYMBOL(EtherCAT_master_init); |
1091 EXPORT_SYMBOL(EtherCAT_master_init); |