9 *****************************************************************************/ |
9 *****************************************************************************/ |
10 |
10 |
11 #include <linux/module.h> |
11 #include <linux/module.h> |
12 #include <linux/delay.h> |
12 #include <linux/delay.h> |
13 |
13 |
|
14 #include "../include/EtherCAT_si.h" |
14 #include "globals.h" |
15 #include "globals.h" |
15 #include "slave.h" |
16 #include "slave.h" |
16 #include "frame.h" |
17 #include "frame.h" |
17 |
18 |
18 /*****************************************************************************/ |
19 /*****************************************************************************/ |
73 printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base" |
74 printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base" |
74 " data!\n", slave->ring_position); |
75 " data!\n", slave->ring_position); |
75 return -1; |
76 return -1; |
76 } |
77 } |
77 |
78 |
78 slave->base_type = frame.data[0]; |
79 slave->base_type = EC_READ_U8 (frame.data); |
79 slave->base_revision = frame.data[1]; |
80 slave->base_revision = EC_READ_U8 (frame.data + 1); |
80 slave->base_build = frame.data[2] | (frame.data[3] << 8); |
81 slave->base_build = EC_READ_U16(frame.data + 2); |
81 slave->base_fmmu_count = frame.data[4]; |
82 slave->base_fmmu_count = EC_READ_U8 (frame.data + 4); |
82 slave->base_sync_count = frame.data[5]; |
83 slave->base_sync_count = EC_READ_U8 (frame.data + 5); |
83 |
84 |
84 if (slave->base_fmmu_count > EC_MAX_FMMUS) |
85 if (slave->base_fmmu_count > EC_MAX_FMMUS) |
85 slave->base_fmmu_count = EC_MAX_FMMUS; |
86 slave->base_fmmu_count = EC_MAX_FMMUS; |
86 |
87 |
87 // Read identification from "Slave Information Interface" (SII) |
88 // Read identification from "Slave Information Interface" (SII) |
133 unsigned char data[10]; |
134 unsigned char data[10]; |
134 unsigned int tries_left; |
135 unsigned int tries_left; |
135 |
136 |
136 // Initiate read operation |
137 // Initiate read operation |
137 |
138 |
138 data[0] = 0x00; |
139 EC_WRITE_U8 (data, 0x00); |
139 data[1] = 0x01; |
140 EC_WRITE_U8 (data + 1, 0x01); |
140 data[2] = offset & 0xFF; |
141 EC_WRITE_U16(data + 2, offset); |
141 data[3] = (offset & 0xFF00) >> 8; |
142 EC_WRITE_U16(data + 4, 0x0000); |
142 data[4] = 0x00; |
|
143 data[5] = 0x00; |
|
144 |
143 |
145 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x502, 6, |
144 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x502, 6, |
146 data); |
145 data); |
147 |
146 |
148 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
147 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
171 printk(KERN_ERR "EtherCAT: SII-read status -" |
170 printk(KERN_ERR "EtherCAT: SII-read status -" |
172 " Slave %i did not respond!\n", slave->ring_position); |
171 " Slave %i did not respond!\n", slave->ring_position); |
173 return -1; |
172 return -1; |
174 } |
173 } |
175 |
174 |
176 if (likely((frame.data[1] & 0x81) == 0)) { |
175 if (likely((EC_READ_U8(frame.data + 1) & 0x81) == 0)) { |
177 memcpy(target, frame.data + 6, 4); |
176 memcpy(target, frame.data + 6, 4); |
178 break; |
177 break; |
179 } |
178 } |
180 |
179 |
181 tries_left--; |
180 tries_left--; |
206 { |
205 { |
207 ec_frame_t frame; |
206 ec_frame_t frame; |
208 unsigned char data[2]; |
207 unsigned char data[2]; |
209 unsigned int tries_left; |
208 unsigned int tries_left; |
210 |
209 |
211 data[0] = state | EC_ACK; |
210 EC_WRITE_U16(data, state | EC_ACK); |
212 data[1] = 0x00; |
|
213 |
211 |
214 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120, |
212 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120, |
215 2, data); |
213 2, data); |
216 |
214 |
217 if (unlikely(ec_frame_send_receive(&frame) != 0)) { |
215 if (unlikely(ec_frame_send_receive(&frame) != 0)) { |
245 " %02X - Slave %i did not respond!\n", state, |
243 " %02X - Slave %i did not respond!\n", state, |
246 slave->ring_position); |
244 slave->ring_position); |
247 return; |
245 return; |
248 } |
246 } |
249 |
247 |
250 if (unlikely(frame.data[0] != state)) { |
248 if (unlikely(EC_READ_U8(frame.data) != state)) { |
251 printk(KERN_ERR "EtherCAT: Could not acknowledge state %02X on" |
249 printk(KERN_ERR "EtherCAT: Could not acknowledge state %02X on" |
252 " slave %i (code %02X)!\n", state, slave->ring_position, |
250 " slave %i (code %02X)!\n", state, slave->ring_position, |
253 frame.data[0]); |
251 EC_READ_U8(frame.data)); |
254 return; |
252 return; |
255 } |
253 } |
256 |
254 |
257 if (likely(frame.data[0] == state)) { |
255 if (likely(EC_READ_U8(frame.data) == state)) { |
258 printk(KERN_INFO "EtherCAT: Acknowleged state %02X on slave %i.\n", |
256 printk(KERN_INFO "EtherCAT: Acknowleged state %02X on slave %i.\n", |
259 state, slave->ring_position); |
257 state, slave->ring_position); |
260 return; |
258 return; |
261 } |
259 } |
262 |
260 |
287 { |
285 { |
288 ec_frame_t frame; |
286 ec_frame_t frame; |
289 unsigned char data[2]; |
287 unsigned char data[2]; |
290 unsigned int tries_left; |
288 unsigned int tries_left; |
291 |
289 |
292 data[0] = state; |
290 EC_WRITE_U16(data, state); |
293 data[1] = 0x00; |
|
294 |
291 |
295 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120, |
292 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0120, |
296 2, data); |
293 2, data); |
297 |
294 |
298 if (unlikely(ec_frame_send_receive(&frame) != 0)) { |
295 if (unlikely(ec_frame_send_receive(&frame) != 0)) { |
325 printk(KERN_ERR "EtherCAT: Could not check state %02X - Slave %i" |
322 printk(KERN_ERR "EtherCAT: Could not check state %02X - Slave %i" |
326 " did not respond!\n", state, slave->ring_position); |
323 " did not respond!\n", state, slave->ring_position); |
327 return -1; |
324 return -1; |
328 } |
325 } |
329 |
326 |
330 if (unlikely(frame.data[0] & 0x10)) { // State change error |
327 if (unlikely(EC_READ_U8(frame.data) & 0x10)) { // State change error |
331 printk(KERN_ERR "EtherCAT: Could not set state %02X - Slave %i" |
328 printk(KERN_ERR "EtherCAT: Could not set state %02X - Slave %i" |
332 " refused state change (code %02X)!\n", state, |
329 " refused state change (code %02X)!\n", state, |
333 slave->ring_position, frame.data[0]); |
330 slave->ring_position, EC_READ_U8(frame.data)); |
334 ec_slave_state_ack(slave, frame.data[0] & 0x0F); |
331 ec_slave_state_ack(slave, EC_READ_U8(frame.data) & 0x0F); |
335 return -1; |
332 return -1; |
336 } |
333 } |
337 |
334 |
338 if (likely(frame.data[0] == (state & 0x0F))) { |
335 if (likely(EC_READ_U8(frame.data) == (state & 0x0F))) { |
339 // State change successful |
336 // State change successful |
340 break; |
337 break; |
341 } |
338 } |
342 |
339 |
343 tries_left--; |
340 tries_left--; |
440 |
437 |
441 int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */) |
438 int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */) |
442 { |
439 { |
443 ec_frame_t frame; |
440 ec_frame_t frame; |
444 uint8_t data[4]; |
441 uint8_t data[4]; |
445 uint16_t crc[2]; |
|
446 |
442 |
447 ec_frame_init_nprd(&frame, slave->master, slave->station_address, 0x0300, |
443 ec_frame_init_nprd(&frame, slave->master, slave->station_address, 0x0300, |
448 4); |
444 4); |
449 |
445 |
450 if (unlikely(ec_frame_send_receive(&frame))) { |
446 if (unlikely(ec_frame_send_receive(&frame))) { |
458 printk(KERN_WARNING "EtherCAT: Reading CRC fault counters -" |
454 printk(KERN_WARNING "EtherCAT: Reading CRC fault counters -" |
459 " Slave %i did not respond!\n", slave->ring_position); |
455 " Slave %i did not respond!\n", slave->ring_position); |
460 return -1; |
456 return -1; |
461 } |
457 } |
462 |
458 |
463 crc[0] = frame.data[0] | (frame.data[1] << 8); |
|
464 crc[1] = frame.data[2] | (frame.data[3] << 8); |
|
465 |
|
466 // No CRC faults. |
459 // No CRC faults. |
467 if (!crc[0] && !crc[1]) return 0; |
460 if (!EC_READ_U16(frame.data) && !EC_READ_U16(frame.data + 2)) return 0; |
468 |
461 |
469 printk(KERN_INFO "EtherCAT: CRC faults on slave %i. A: %i, B: %i\n", |
462 printk(KERN_INFO "EtherCAT: CRC faults on slave %i. A: %i, B: %i\n", |
470 slave->ring_position, crc[0], crc[1]); |
463 slave->ring_position, EC_READ_U16(frame.data), |
|
464 EC_READ_U16(frame.data + 2)); |
471 |
465 |
472 // Reset CRC counters |
466 // Reset CRC counters |
473 memset(data, 0x00, 4); |
467 EC_WRITE_U16(data, 0x0000); |
|
468 EC_WRITE_U16(data + 2, 0x0000); |
474 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0300, |
469 ec_frame_init_npwr(&frame, slave->master, slave->station_address, 0x0300, |
475 4, data); |
470 4, data); |
476 |
471 |
477 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
472 if (unlikely(ec_frame_send_receive(&frame))) return -1; |
478 |
473 |