46 |
44 |
47 EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
45 EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
48 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
46 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
49 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
47 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
50 EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
48 EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
51 EC_WRITE_U16(data + 6, 0x2000); // Number & Service |
49 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
52 EC_WRITE_U8 (data + 8, 0x13 | ((4 - size) << 2)); // Spec., exp., init. |
50 EC_WRITE_U8 (data + 8, 0x13 | ((4 - size) << 2)); // Spec., exp., init. |
53 EC_WRITE_U16(data + 9, sdo_index); |
51 EC_WRITE_U16(data + 9, sdo_index); |
54 EC_WRITE_U8 (data + 11, sdo_subindex); |
52 EC_WRITE_U8 (data + 11, sdo_subindex); |
55 |
53 |
56 for (i = 0; i < size; i++) { |
54 for (i = 0; i < size; i++) { |
122 return 0; |
120 return 0; |
123 } |
121 } |
124 |
122 |
125 /*****************************************************************************/ |
123 /*****************************************************************************/ |
126 |
124 |
|
125 /** |
|
126 Schreibt ein CANopen-SDO (service data object). |
|
127 */ |
|
128 |
|
129 int EtherCAT_rt_canopen_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */ |
|
130 uint16_t sdo_index, /**< SDO-Index */ |
|
131 uint8_t sdo_subindex, /**< SDO-Subindex */ |
|
132 uint32_t *value /**< Speicher für gel. Wert */ |
|
133 ) |
|
134 { |
|
135 unsigned char data[0xF6]; |
|
136 ec_frame_t frame; |
|
137 unsigned int tries_left; |
|
138 ec_master_t *master; |
|
139 |
|
140 memset(data, 0x00, 0xF6); |
|
141 |
|
142 master = slave->master; |
|
143 |
|
144 EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data |
|
145 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
|
146 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
|
147 EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
|
148 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
|
149 EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
|
150 EC_WRITE_U16(data + 9, sdo_index); |
|
151 EC_WRITE_U8 (data + 11, sdo_subindex); |
|
152 |
|
153 ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
|
154 data); |
|
155 |
|
156 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
|
157 |
|
158 if (unlikely(frame.working_counter != 1)) { |
|
159 printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n", |
|
160 slave->ring_position); |
|
161 return -1; |
|
162 } |
|
163 |
|
164 // Read "written bit" of Sync-Manager |
|
165 |
|
166 tries_left = 10; |
|
167 while (tries_left) |
|
168 { |
|
169 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
|
170 |
|
171 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
|
172 |
|
173 if (unlikely(frame.working_counter != 1)) { |
|
174 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not" |
|
175 " respond!\n", slave->ring_position); |
|
176 return -1; |
|
177 } |
|
178 |
|
179 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
|
180 break; |
|
181 } |
|
182 |
|
183 udelay(1000); |
|
184 tries_left--; |
|
185 } |
|
186 |
|
187 if (!tries_left) { |
|
188 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n", |
|
189 slave->ring_position); |
|
190 return -1; |
|
191 } |
|
192 |
|
193 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
|
194 |
|
195 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
|
196 |
|
197 if (unlikely(frame.working_counter != 1)) { |
|
198 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
|
199 " respond!\n", slave->ring_position); |
|
200 return -1; |
|
201 } |
|
202 |
|
203 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
|
204 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
|
205 EC_READ_U8 (frame.data + 8) >> 5 != 0x02 || // Upload response |
|
206 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
|
207 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
|
208 { |
|
209 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
|
210 slave->ring_position); |
|
211 return -1; |
|
212 } |
|
213 |
|
214 *value = EC_READ_U32(frame.data + 12); |
|
215 |
|
216 return 0; |
|
217 } |
|
218 |
|
219 /*****************************************************************************/ |
|
220 |
|
221 int EtherCAT_rt_canopen_sdo_addr_write(ec_master_t *master, |
|
222 /**< EtherCAT-Master */ |
|
223 const char *addr, |
|
224 /**< Addresse, siehe ec_address() */ |
|
225 uint16_t index, |
|
226 /**< SDO-Index */ |
|
227 uint8_t subindex, |
|
228 /**< SDO-Subindex */ |
|
229 uint32_t value, |
|
230 /**< Neuer Wert */ |
|
231 size_t size |
|
232 /**< Größe des Datenfeldes */ |
|
233 ) |
|
234 { |
|
235 ec_slave_t *slave; |
|
236 if (!(slave = ec_address(master, addr))) return -1; |
|
237 return EtherCAT_rt_canopen_sdo_write(slave, index, subindex, value, size); |
|
238 } |
|
239 |
|
240 /*****************************************************************************/ |
|
241 |
|
242 int EtherCAT_rt_canopen_sdo_addr_read(ec_master_t *master, |
|
243 /**< EtherCAT-Slave */ |
|
244 const char *addr, |
|
245 /**< Addresse, siehe ec_address() */ |
|
246 uint16_t index, |
|
247 /**< SDO-Index */ |
|
248 uint8_t subindex, |
|
249 /**< SDO-Subindex */ |
|
250 uint32_t *value |
|
251 /**< Speicher für gel. Wert */ |
|
252 ) |
|
253 { |
|
254 ec_slave_t *slave; |
|
255 if (!(slave = ec_address(master, addr))) return -1; |
|
256 return EtherCAT_rt_canopen_sdo_read(slave, index, subindex, value); |
|
257 } |
|
258 |
|
259 /*****************************************************************************/ |
|
260 |
127 EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write); |
261 EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write); |
|
262 EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_read); |
|
263 EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_addr_write); |
|
264 EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_addr_read); |
128 |
265 |
129 /*****************************************************************************/ |
266 /*****************************************************************************/ |
130 |
267 |
131 /* Emacs-Konfiguration |
268 /* Emacs-Konfiguration |
132 ;;; Local Variables: *** |
269 ;;; Local Variables: *** |