40 uint8_t sdo_subindex, /**< SDO-Subindex */ |
42 uint8_t sdo_subindex, /**< SDO-Subindex */ |
41 uint32_t value, /**< Neuer Wert */ |
43 uint32_t value, /**< Neuer Wert */ |
42 size_t size /**< Größe des Datenfeldes */ |
44 size_t size /**< Größe des Datenfeldes */ |
43 ) |
45 ) |
44 { |
46 { |
45 uint8_t data[0xF6]; |
47 uint8_t data[0x0A]; |
46 ec_command_t command; |
|
47 unsigned int i; |
48 unsigned int i; |
48 ec_master_t *master; |
49 size_t rec_size; |
49 cycles_t start, end, timeout; |
|
50 uint32_t abort_code; |
|
51 const ec_sdo_abort_message_t *abort_msg; |
|
52 |
|
53 memset(data, 0x00, 0xF6); |
|
54 |
|
55 master = slave->master; |
|
56 |
50 |
57 if (size == 0 || size > 4) { |
51 if (size == 0 || size > 4) { |
58 EC_ERR("Invalid SDO data size: %i!\n", size); |
52 EC_ERR("Invalid SDO data size: %i!\n", size); |
59 return -1; |
53 return -1; |
60 } |
54 } |
61 |
55 |
62 EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
56 EC_WRITE_U16(data, 0x02 << 12); // Number (0), Service (SDO request) |
63 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
57 EC_WRITE_U8 (data + 2, 0x23 | ((4 - size) << 2)); // Spec., exp., init. |
64 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
58 EC_WRITE_U16(data + 3, sdo_index); |
65 EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
59 EC_WRITE_U8 (data + 5, sdo_subindex); |
66 EC_WRITE_U16(data + 6, 0x02 << 12); // Number (0), Service (SDO request) |
|
67 EC_WRITE_U8 (data + 8, 0x23 | ((4 - size) << 2)); // Spec., exp., init. |
|
68 EC_WRITE_U16(data + 9, sdo_index); |
|
69 EC_WRITE_U8 (data + 11, sdo_subindex); |
|
70 |
60 |
71 for (i = 0; i < size; i++) { |
61 for (i = 0; i < size; i++) { |
72 EC_WRITE_U8(data + 12 + i, value & 0xFF); |
62 EC_WRITE_U8(data + 6 + i, value & 0xFF); |
73 value >>= 8; |
63 value >>= 8; |
74 } |
64 } |
75 |
65 |
76 ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); |
66 // Mailox senden und empfangen |
77 if (unlikely(ec_master_simple_io(master, &command))) { |
67 if (ec_slave_mailbox_send(slave, 0x03, data, 0x0A)) return -1; |
78 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
68 |
79 return -1; |
69 rec_size = 0x0A; |
80 } |
70 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
81 |
71 |
82 // Read "written bit" of Sync-Manager |
72 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
83 start = get_cycles(); |
73 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. |
84 timeout = (cycles_t) 10 * cpu_khz; // 10ms |
|
85 |
|
86 while (1) |
|
87 { |
|
88 udelay(100); |
|
89 |
|
90 ec_command_init_nprd(&command, slave->station_address, 0x808, 8); |
|
91 if (unlikely(ec_master_simple_io(master, &command))) { |
|
92 EC_ERR("Mailbox checking failed on slave %i!\n", |
|
93 slave->ring_position); |
|
94 return -1; |
|
95 } |
|
96 |
|
97 end = get_cycles(); |
|
98 |
|
99 if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high |
|
100 |
|
101 if ((end - start) >= timeout) { |
|
102 EC_ERR("Mailbox check - Slave %i timed out.\n", |
|
103 slave->ring_position); |
|
104 return -1; |
|
105 } |
|
106 } |
|
107 |
|
108 if (unlikely(slave->master->debug_level) > 1) |
|
109 EC_DBG("SDO download took %ius.\n", ((u32) (end - start) * 1000 |
|
110 / cpu_khz)); |
|
111 |
|
112 ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); |
|
113 if (unlikely(ec_master_simple_io(master, &command))) { |
|
114 EC_ERR("Mailbox receiving failed on slave %i!\n", |
|
115 slave->ring_position); |
|
116 return -1; |
|
117 } |
|
118 |
|
119 if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE |
|
120 EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", |
|
121 slave->ring_position); |
|
122 return -1; |
|
123 } |
|
124 |
|
125 if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request |
|
126 EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req. |
|
127 EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave" |
74 EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave" |
128 " %i.\n", sdo_index, sdo_subindex, value, size, |
75 " %i.\n", sdo_index, sdo_subindex, value, size, |
129 slave->ring_position); |
76 slave->ring_position); |
130 abort_code = EC_READ_U32(command.data + 12); |
77 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
131 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
78 return -1; |
132 if (abort_msg->code == abort_code) { |
79 } |
133 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
80 |
134 abort_msg->code, abort_msg->message); |
81 if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response |
135 return -1; |
82 EC_READ_U8 (data + 2) >> 5 != 0x03 || // Download response |
136 } |
83 EC_READ_U16(data + 3) != sdo_index || // Index |
137 } |
84 EC_READ_U8 (data + 5) != sdo_subindex) // Subindex |
138 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
|
139 return -1; |
|
140 } |
|
141 |
|
142 if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response |
|
143 EC_READ_U8 (command.data + 8) >> 5 != 0x03 || // Download response |
|
144 EC_READ_U16(command.data + 9) != sdo_index || // Index |
|
145 EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex |
|
146 { |
85 { |
147 EC_ERR("Invalid SDO download response at slave %i!\n", |
86 EC_ERR("Invalid SDO download response at slave %i!\n", |
148 slave->ring_position); |
87 slave->ring_position); |
149 return -1; |
88 return -1; |
150 } |
89 } |
162 uint16_t sdo_index, /**< SDO-Index */ |
101 uint16_t sdo_index, /**< SDO-Index */ |
163 uint8_t sdo_subindex, /**< SDO-Subindex */ |
102 uint8_t sdo_subindex, /**< SDO-Subindex */ |
164 uint32_t *value /**< Speicher für gel. Wert */ |
103 uint32_t *value /**< Speicher für gel. Wert */ |
165 ) |
104 ) |
166 { |
105 { |
167 uint8_t data[0xF6]; |
106 uint8_t data[0x0A]; |
168 ec_command_t command; |
107 size_t rec_size; |
169 ec_master_t *master; |
108 |
170 cycles_t start, end, timeout; |
109 EC_WRITE_U16(data, 0x2000); // Number (0), Service (SDO request) |
171 uint32_t abort_code; |
110 EC_WRITE_U8 (data + 2, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
172 const ec_sdo_abort_message_t *abort_msg; |
111 EC_WRITE_U16(data + 3, sdo_index); |
173 |
112 EC_WRITE_U8 (data + 5, sdo_subindex); |
174 memset(data, 0x00, 0xF6); |
113 |
175 master = slave->master; |
114 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
176 |
115 |
177 EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data |
116 rec_size = 6; |
178 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
117 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
179 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
118 |
180 EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
119 if (EC_READ_U16(data ) >> 12 == 0x02 && // SDO request |
181 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
120 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. |
182 EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
|
183 EC_WRITE_U16(data + 9, sdo_index); |
|
184 EC_WRITE_U8 (data + 11, sdo_subindex); |
|
185 |
|
186 ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); |
|
187 if (unlikely(ec_master_simple_io(master, &command))) { |
|
188 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
|
189 return -1; |
|
190 } |
|
191 |
|
192 // Read "written bit" of Sync-Manager |
|
193 |
|
194 start = get_cycles(); |
|
195 timeout = cpu_khz; // 1ms |
|
196 |
|
197 while (1) |
|
198 { |
|
199 udelay(10); |
|
200 |
|
201 ec_command_init_nprd(&command, slave->station_address, 0x808, 8); |
|
202 if (unlikely(ec_master_simple_io(master, &command))) { |
|
203 EC_ERR("Mailbox checking failed on slave %i!\n", |
|
204 slave->ring_position); |
|
205 return -1; |
|
206 } |
|
207 |
|
208 end = get_cycles(); |
|
209 |
|
210 if (EC_READ_U8(command.data + 5) & 8) { // Written bit is high |
|
211 break; |
|
212 } |
|
213 |
|
214 if (unlikely((end - start) >= timeout)) { |
|
215 EC_ERR("Mailbox check on slave %i timed out.\n", |
|
216 slave->ring_position); |
|
217 return -1; |
|
218 } |
|
219 } |
|
220 |
|
221 ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); |
|
222 if (unlikely(ec_master_simple_io(master, &command))) { |
|
223 EC_ERR("Mailbox receiving failed on slave %i!\n", |
|
224 slave->ring_position); |
|
225 return -1; |
|
226 } |
|
227 |
|
228 if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE |
|
229 EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", |
|
230 slave->ring_position); |
|
231 return -1; |
|
232 } |
|
233 |
|
234 if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request |
|
235 EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req. |
|
236 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
121 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
237 sdo_index, sdo_subindex, slave->ring_position); |
122 sdo_index, sdo_subindex, slave->ring_position); |
238 abort_code = EC_READ_U32(command.data + 12); |
123 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
239 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
124 return -1; |
240 if (abort_msg->code == abort_code) { |
125 } |
241 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
126 |
242 abort_msg->code, abort_msg->message); |
127 if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response |
243 return -1; |
128 EC_READ_U8 (data + 2) >> 5 != 0x02 || // Upload response |
244 } |
129 EC_READ_U16(data + 3) != sdo_index || // Index |
245 } |
130 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
246 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
|
247 return -1; |
|
248 } |
|
249 |
|
250 if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response |
|
251 EC_READ_U8 (command.data + 8) >> 5 != 0x02 || // Upload response |
|
252 EC_READ_U16(command.data + 9) != sdo_index || // Index |
|
253 EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex |
|
254 { |
|
255 EC_ERR("Invalid SDO upload response at slave %i!\n", |
131 EC_ERR("Invalid SDO upload response at slave %i!\n", |
256 slave->ring_position); |
132 slave->ring_position); |
257 return -1; |
133 return -1; |
258 } |
134 } |
259 |
135 |
260 *value = EC_READ_U32(command.data + 12); |
136 *value = EC_READ_U32(data + 6); |
261 |
|
262 return 0; |
137 return 0; |
263 } |
138 } |
264 |
139 |
265 /*****************************************************************************/ |
140 /*****************************************************************************/ |
266 |
141 |
314 ) |
189 ) |
315 { |
190 { |
316 ec_slave_t *slave; |
191 ec_slave_t *slave; |
317 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
192 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
318 return ecrt_slave_sdo_read(slave, index, subindex, value); |
193 return ecrt_slave_sdo_read(slave, index, subindex, value); |
|
194 } |
|
195 |
|
196 /*****************************************************************************/ |
|
197 |
|
198 /** |
|
199 Holt das Object-Dictionary aus dem Slave. |
|
200 |
|
201 \return 0, wenn alles ok, sonst < 0 |
|
202 */ |
|
203 |
|
204 int ec_slave_fetch_sdo_list(ec_slave_t *slave /**< EtherCAT-Slave */) |
|
205 { |
|
206 uint8_t data[0xF0]; |
|
207 size_t rec_size; |
|
208 |
|
209 //EC_DBG("Fetching SDO list for slave %i...\n", slave->ring_position); |
|
210 |
|
211 EC_WRITE_U16(data, 0x8000); // Number (0), Service (get OD request) |
|
212 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
|
213 EC_WRITE_U8 (data + 3, 0x00); // res. |
|
214 EC_WRITE_U16(data + 4, 0x0000); // fragments left |
|
215 EC_WRITE_U16(data + 6, 0x0001); // Deliver all SDOs! |
|
216 |
|
217 if (ec_slave_mailbox_send(slave, 0x03, data, 8)) return -1; |
|
218 |
|
219 do |
|
220 { |
|
221 rec_size = 0xF0; |
|
222 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
|
223 |
|
224 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
|
225 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. |
|
226 EC_ERR("SDO list download aborted on slave %i.\n", |
|
227 slave->ring_position); |
|
228 ec_canopen_abort_msg(EC_READ_U32(data + 12)); |
|
229 return -1; |
|
230 } |
|
231 |
|
232 if (EC_READ_U16(data) >> 12 == 0x08 && // SDO information |
|
233 (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // Get OD List response |
|
234 EC_ERR("SDO information error response at slave %i!\n", |
|
235 slave->ring_position); |
|
236 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
237 return -1; |
|
238 } |
|
239 |
|
240 if (EC_READ_U16(data) >> 12 != 0x08 || // SDO information |
|
241 (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response |
|
242 EC_ERR("Invalid SDO list response at slave %i!\n", |
|
243 slave->ring_position); |
|
244 return -1; |
|
245 } |
|
246 |
|
247 if (rec_size < 8) { |
|
248 EC_ERR("Invalid data size!\n"); |
|
249 return -1; |
|
250 } |
|
251 |
|
252 #if 0 |
|
253 for (i = 0; i < (rec_size - 8) / 2; i++) |
|
254 EC_INFO("Object 0x%04X\n", EC_READ_U16(data + 8 + i * 2)); |
|
255 #endif |
|
256 } |
|
257 while (EC_READ_U8(data + 2) & 0x80); |
|
258 |
|
259 return 0; |
|
260 } |
|
261 |
|
262 /*****************************************************************************/ |
|
263 |
|
264 /** |
|
265 Gibt eine SDO-Abort-Meldung aus. |
|
266 */ |
|
267 |
|
268 void ec_canopen_abort_msg(uint32_t abort_code) |
|
269 { |
|
270 const ec_sdo_abort_message_t *abort_msg; |
|
271 |
|
272 for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { |
|
273 if (abort_msg->code == abort_code) { |
|
274 EC_ERR("SDO abort message 0x%08X: \"%s\".\n", |
|
275 abort_msg->code, abort_msg->message); |
|
276 return; |
|
277 } |
|
278 } |
|
279 EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); |
319 } |
280 } |
320 |
281 |
321 /*****************************************************************************/ |
282 /*****************************************************************************/ |
322 |
283 |
323 const ec_sdo_abort_message_t sdo_abort_messages[] = { |
284 const ec_sdo_abort_message_t sdo_abort_messages[] = { |