36 memset(data, 0x00, 0xF6); |
36 memset(data, 0x00, 0xF6); |
37 |
37 |
38 master = slave->master; |
38 master = slave->master; |
39 |
39 |
40 if (size == 0 || size > 4) { |
40 if (size == 0 || size > 4) { |
41 printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size); |
41 EC_ERR("Illegal SDO data size: %i!\n", size); |
42 return -1; |
42 return -1; |
43 } |
43 } |
44 |
44 |
45 EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
45 EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
46 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
46 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
60 data); |
60 data); |
61 |
61 |
62 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
62 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
63 |
63 |
64 if (unlikely(frame.working_counter != 1)) { |
64 if (unlikely(frame.working_counter != 1)) { |
65 printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n", |
65 EC_ERR("Mailbox send - Slave %i did not respond!\n", |
66 slave->ring_position); |
66 slave->ring_position); |
67 return -1; |
67 return -1; |
68 } |
68 } |
69 |
69 |
70 // Read "written bit" of Sync-Manager |
70 // Read "written bit" of Sync-Manager |
75 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
75 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
76 |
76 |
77 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
77 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
78 |
78 |
79 if (unlikely(frame.working_counter != 1)) { |
79 if (unlikely(frame.working_counter != 1)) { |
80 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not" |
80 EC_ERR("Mailbox check - Slave %i did not respond!\n", |
81 " respond!\n", slave->ring_position); |
81 slave->ring_position); |
82 return -1; |
82 return -1; |
83 } |
83 } |
84 |
84 |
85 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
85 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
86 break; |
86 break; |
89 udelay(1000); |
89 udelay(1000); |
90 tries_left--; |
90 tries_left--; |
91 } |
91 } |
92 |
92 |
93 if (!tries_left) { |
93 if (!tries_left) { |
94 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n", |
94 EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position); |
95 slave->ring_position); |
|
96 return -1; |
95 return -1; |
97 } |
96 } |
98 |
97 |
99 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
98 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
100 |
99 |
101 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
100 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
102 |
101 |
103 if (unlikely(frame.working_counter != 1)) { |
102 if (unlikely(frame.working_counter != 1)) { |
104 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
103 EC_ERR("Mailbox receive - Slave %i did not respond!\n", |
105 " respond!\n", slave->ring_position); |
104 slave->ring_position); |
106 return -1; |
105 return -1; |
107 } |
106 } |
108 |
107 |
109 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
108 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
110 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
109 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
111 EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response |
110 EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response |
112 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
111 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
113 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
112 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
114 { |
113 { |
115 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
114 EC_ERR("Illegal mailbox response at slave %i!\n", |
116 slave->ring_position); |
115 slave->ring_position); |
117 return -1; |
116 return -1; |
118 } |
117 } |
119 |
118 |
120 return 0; |
119 return 0; |
154 data); |
153 data); |
155 |
154 |
156 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
155 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
157 |
156 |
158 if (unlikely(frame.working_counter != 1)) { |
157 if (unlikely(frame.working_counter != 1)) { |
159 printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n", |
158 EC_ERR("Mailbox send - Slave %i did not respond!\n", |
160 slave->ring_position); |
159 slave->ring_position); |
161 return -1; |
160 return -1; |
162 } |
161 } |
163 |
162 |
164 // Read "written bit" of Sync-Manager |
163 // Read "written bit" of Sync-Manager |
169 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
168 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
170 |
169 |
171 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
170 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
172 |
171 |
173 if (unlikely(frame.working_counter != 1)) { |
172 if (unlikely(frame.working_counter != 1)) { |
174 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not" |
173 EC_ERR("Mailbox check - Slave %i did not respond!\n", |
175 " respond!\n", slave->ring_position); |
174 slave->ring_position); |
176 return -1; |
175 return -1; |
177 } |
176 } |
178 |
177 |
179 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
178 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
180 break; |
179 break; |
183 udelay(1000); |
182 udelay(1000); |
184 tries_left--; |
183 tries_left--; |
185 } |
184 } |
186 |
185 |
187 if (!tries_left) { |
186 if (!tries_left) { |
188 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n", |
187 EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position); |
189 slave->ring_position); |
|
190 return -1; |
188 return -1; |
191 } |
189 } |
192 |
190 |
193 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
191 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
194 |
192 |
195 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
193 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
196 |
194 |
197 if (unlikely(frame.working_counter != 1)) { |
195 if (unlikely(frame.working_counter != 1)) { |
198 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
196 EC_ERR("Mailbox receive - Slave %i did not respond!\n", |
199 " respond!\n", slave->ring_position); |
197 slave->ring_position); |
200 return -1; |
198 return -1; |
201 } |
199 } |
202 |
200 |
203 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
201 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
204 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
202 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
205 EC_READ_U8 (frame.data + 8) >> 5 != 0x02 || // Upload response |
203 EC_READ_U8 (frame.data + 8) >> 5 != 0x02 || // Upload response |
206 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
204 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
207 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
205 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
208 { |
206 { |
209 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
207 EC_ERR("Illegal mailbox response at slave %i!\n", |
210 slave->ring_position); |
208 slave->ring_position); |
211 return -1; |
209 return -1; |
212 } |
210 } |
213 |
211 |
214 *value = EC_READ_U32(frame.data + 12); |
212 *value = EC_READ_U32(frame.data + 12); |