66 */ |
64 */ |
67 #define DEBUG_LONG 0 |
65 #define DEBUG_LONG 0 |
68 |
66 |
69 /*****************************************************************************/ |
67 /*****************************************************************************/ |
70 |
68 |
71 void ec_fsm_coe_dict_start(ec_fsm_coe_t *); |
69 void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *); |
72 void ec_fsm_coe_dict_request(ec_fsm_coe_t *); |
70 void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *); |
73 void ec_fsm_coe_dict_check(ec_fsm_coe_t *); |
71 void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *); |
74 void ec_fsm_coe_dict_response(ec_fsm_coe_t *); |
72 void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *); |
75 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *); |
73 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *); |
76 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *); |
74 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *); |
77 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *); |
75 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *); |
78 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *); |
76 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *); |
79 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *); |
77 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *); |
80 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *); |
78 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *); |
81 |
79 |
82 void ec_fsm_coe_down_start(ec_fsm_coe_t *); |
80 void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *); |
83 void ec_fsm_coe_down_request(ec_fsm_coe_t *); |
81 void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *); |
84 void ec_fsm_coe_down_check(ec_fsm_coe_t *); |
82 void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *); |
85 void ec_fsm_coe_down_response(ec_fsm_coe_t *); |
83 void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *); |
86 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *); |
84 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *); |
87 void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *); |
85 void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *); |
88 |
86 |
89 void ec_fsm_coe_up_start(ec_fsm_coe_t *); |
87 void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *); |
90 void ec_fsm_coe_up_request(ec_fsm_coe_t *); |
88 void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *); |
91 void ec_fsm_coe_up_check(ec_fsm_coe_t *); |
89 void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *); |
92 void ec_fsm_coe_up_response(ec_fsm_coe_t *); |
90 void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *); |
93 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *); |
91 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *); |
94 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *); |
92 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *); |
95 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *); |
93 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *); |
96 |
94 |
97 void ec_fsm_coe_end(ec_fsm_coe_t *); |
95 void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *); |
98 void ec_fsm_coe_error(ec_fsm_coe_t *); |
96 void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *); |
99 |
97 |
100 /*****************************************************************************/ |
98 /*****************************************************************************/ |
101 |
99 |
102 /** |
100 /** SDO abort messages. |
103 SDO abort messages. |
101 * |
104 The "abort SDO transfer request" supplies an abort code, |
102 * The "abort SDO transfer request" supplies an abort code, which can be |
105 which can be translated to clear text. This table does |
103 * translated to clear text. This table does the mapping of the codes and |
106 the mapping of the codes and messages. |
104 * messages. |
107 */ |
105 */ |
108 |
|
109 const ec_code_msg_t sdo_abort_messages[] = { |
106 const ec_code_msg_t sdo_abort_messages[] = { |
110 {0x05030000, "Toggle bit not changed"}, |
107 {0x05030000, "Toggle bit not changed"}, |
111 {0x05040000, "SDO protocol timeout"}, |
108 {0x05040000, "SDO protocol timeout"}, |
112 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
109 {0x05040001, "Client/Server command specifier not valid or unknown"}, |
113 {0x05040005, "Out of memory"}, |
110 {0x05040005, "Out of memory"}, |
162 EC_SLAVE_ERR(slave, "Unknown SDO abort code 0x%08X.\n", abort_code); |
162 EC_SLAVE_ERR(slave, "Unknown SDO abort code 0x%08X.\n", abort_code); |
163 } |
163 } |
164 |
164 |
165 /*****************************************************************************/ |
165 /*****************************************************************************/ |
166 |
166 |
167 /** |
167 /** Constructor. |
168 Constructor. |
168 */ |
169 */ |
169 void ec_fsm_coe_init( |
170 |
170 ec_fsm_coe_t *fsm /**< Finite state machine */ |
171 void ec_fsm_coe_init(ec_fsm_coe_t *fsm, /**< finite state machine */ |
171 ) |
172 ec_datagram_t *datagram /**< datagram */ |
|
173 ) |
|
174 { |
172 { |
175 fsm->state = NULL; |
173 fsm->state = NULL; |
176 fsm->datagram = datagram; |
174 fsm->datagram = NULL; |
177 } |
175 } |
178 |
176 |
179 /*****************************************************************************/ |
177 /*****************************************************************************/ |
180 |
178 |
181 /** |
179 /** Destructor. |
182 Destructor. |
180 */ |
183 */ |
181 void ec_fsm_coe_clear( |
184 |
182 ec_fsm_coe_t *fsm /**< Finite state machine */ |
185 void ec_fsm_coe_clear(ec_fsm_coe_t *fsm /**< finite state machine */) |
183 ) |
186 { |
184 { |
187 } |
185 } |
188 |
186 |
189 /*****************************************************************************/ |
187 /*****************************************************************************/ |
190 |
188 |
191 /** |
189 /** Starts reading a slaves' SDO dictionary. |
192 Starts reading a slaves' SDO dictionary. |
190 */ |
193 */ |
191 void ec_fsm_coe_dictionary( |
194 |
192 ec_fsm_coe_t *fsm, /**< Finite state machine */ |
195 void ec_fsm_coe_dictionary(ec_fsm_coe_t *fsm, /**< finite state machine */ |
193 ec_slave_t *slave /**< EtherCAT slave */ |
196 ec_slave_t *slave /**< EtherCAT slave */ |
194 ) |
197 ) |
|
198 { |
195 { |
199 fsm->slave = slave; |
196 fsm->slave = slave; |
200 fsm->state = ec_fsm_coe_dict_start; |
197 fsm->state = ec_fsm_coe_dict_start; |
201 } |
198 } |
202 |
199 |
203 /*****************************************************************************/ |
200 /*****************************************************************************/ |
204 |
201 |
205 /** |
202 /** Starts to transfer an SDO to/from a slave. |
206 Starts to transfer an SDO to/from a slave. |
203 */ |
207 */ |
|
208 |
|
209 void ec_fsm_coe_transfer( |
204 void ec_fsm_coe_transfer( |
210 ec_fsm_coe_t *fsm, /**< State machine. */ |
205 ec_fsm_coe_t *fsm, /**< State machine. */ |
211 ec_slave_t *slave, /**< EtherCAT slave. */ |
206 ec_slave_t *slave, /**< EtherCAT slave. */ |
212 ec_sdo_request_t *request /**< SDO request. */ |
207 ec_sdo_request_t *request /**< SDO request. */ |
213 ) |
208 ) |
214 { |
209 { |
215 fsm->slave = slave; |
210 fsm->slave = slave; |
216 fsm->request = request; |
211 fsm->request = request; |
217 if (request->dir == EC_DIR_OUTPUT) |
212 |
|
213 if (request->dir == EC_DIR_OUTPUT) { |
218 fsm->state = ec_fsm_coe_down_start; |
214 fsm->state = ec_fsm_coe_down_start; |
219 else |
215 } |
|
216 else { |
220 fsm->state = ec_fsm_coe_up_start; |
217 fsm->state = ec_fsm_coe_up_start; |
221 } |
218 } |
222 |
219 } |
223 /*****************************************************************************/ |
220 |
224 |
221 /*****************************************************************************/ |
225 /** |
222 |
226 Executes the current state of the state machine. |
223 /** Executes the current state of the state machine. |
227 \return false, if state machine has terminated |
224 * |
228 */ |
225 * \return 1 if the datagram was used, else 0. |
229 |
226 */ |
230 int ec_fsm_coe_exec(ec_fsm_coe_t *fsm /**< finite state machine */) |
227 int ec_fsm_coe_exec( |
231 { |
228 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
232 fsm->state(fsm); |
229 ec_datagram_t *datagram /**< Datagram to use. */ |
233 |
230 ) |
234 return fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error; |
231 { |
235 } |
232 int datagram_used = 0; |
236 |
233 |
237 /*****************************************************************************/ |
234 if (fsm->datagram && |
238 |
235 (fsm->datagram->state == EC_DATAGRAM_INIT || |
239 /** |
236 fsm->datagram->state == EC_DATAGRAM_QUEUED || |
240 Returns, if the state machine terminated with success. |
237 fsm->datagram->state == EC_DATAGRAM_SENT)) { |
241 \return non-zero if successful. |
238 // datagram not received yet |
242 */ |
239 return datagram_used; |
243 |
240 } |
244 int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */) |
241 |
|
242 fsm->state(fsm, datagram); |
|
243 |
|
244 datagram_used = |
|
245 fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error; |
|
246 |
|
247 if (datagram_used) { |
|
248 fsm->datagram = datagram; |
|
249 } else { |
|
250 fsm->datagram = NULL; |
|
251 } |
|
252 |
|
253 return datagram_used; |
|
254 } |
|
255 |
|
256 /*****************************************************************************/ |
|
257 |
|
258 /** Returns, if the state machine terminated with success. |
|
259 * \return non-zero if successful. |
|
260 */ |
|
261 int ec_fsm_coe_success( |
|
262 const ec_fsm_coe_t *fsm /**< Finite state machine */ |
|
263 ) |
245 { |
264 { |
246 return fsm->state == ec_fsm_coe_end; |
265 return fsm->state == ec_fsm_coe_end; |
247 } |
266 } |
248 |
267 |
249 /*****************************************************************************/ |
268 /*****************************************************************************/ |
286 |
305 |
287 /****************************************************************************** |
306 /****************************************************************************** |
288 * CoE dictionary state machine |
307 * CoE dictionary state machine |
289 *****************************************************************************/ |
308 *****************************************************************************/ |
290 |
309 |
291 /** |
310 int ec_fsm_coe_prepare_dict( |
292 CoE state: DICT START. |
311 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
293 */ |
312 ec_datagram_t *datagram /**< Datagram to use. */ |
294 |
313 ) |
295 void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */) |
314 { |
296 { |
315 ec_slave_t *slave = fsm->slave; |
297 ec_datagram_t *datagram = fsm->datagram; |
316 uint8_t *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); |
298 ec_slave_t *slave = fsm->slave; |
|
299 uint8_t *data; |
|
300 |
|
301 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
|
302 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
|
303 fsm->state = ec_fsm_coe_error; |
|
304 return; |
|
305 } |
|
306 |
|
307 if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) { |
|
308 EC_SLAVE_ERR(slave, "Slave does not support" |
|
309 " SDO information service!\n"); |
|
310 fsm->state = ec_fsm_coe_error; |
|
311 return; |
|
312 } |
|
313 |
|
314 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); |
|
315 if (IS_ERR(data)) { |
317 if (IS_ERR(data)) { |
316 fsm->state = ec_fsm_coe_error; |
318 return PTR_ERR(data); |
317 return; |
|
318 } |
319 } |
319 |
320 |
320 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
321 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
321 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
322 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
322 EC_WRITE_U8 (data + 3, 0x00); |
323 EC_WRITE_U8 (data + 3, 0x00); |
323 EC_WRITE_U16(data + 4, 0x0000); |
324 EC_WRITE_U16(data + 4, 0x0000); |
324 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
325 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
325 |
326 |
|
327 fsm->state = ec_fsm_coe_dict_request; |
|
328 return 0; |
|
329 } |
|
330 |
|
331 /*****************************************************************************/ |
|
332 |
|
333 /** CoE state: DICT START. |
|
334 */ |
|
335 void ec_fsm_coe_dict_start( |
|
336 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
|
337 ec_datagram_t *datagram /**< Datagram to use. */ |
|
338 ) |
|
339 { |
|
340 ec_slave_t *slave = fsm->slave; |
|
341 |
|
342 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
|
343 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
|
344 fsm->state = ec_fsm_coe_error; |
|
345 return; |
|
346 } |
|
347 |
|
348 if (slave->sii.has_general && !slave->sii.coe_details.enable_sdo_info) { |
|
349 EC_SLAVE_ERR(slave, "Slave does not support" |
|
350 " SDO information service!\n"); |
|
351 fsm->state = ec_fsm_coe_error; |
|
352 return; |
|
353 } |
|
354 |
326 fsm->retries = EC_FSM_RETRIES; |
355 fsm->retries = EC_FSM_RETRIES; |
327 fsm->state = ec_fsm_coe_dict_request; |
356 |
328 } |
357 if (ec_fsm_coe_prepare_dict(fsm, datagram)) { |
329 |
358 fsm->state = ec_fsm_coe_error; |
330 /*****************************************************************************/ |
359 } |
331 |
360 } |
332 /** |
361 |
333 CoE state: DICT REQUEST. |
362 /*****************************************************************************/ |
334 \todo Timeout behavior |
363 |
335 */ |
364 /** CoE state: DICT REQUEST. |
336 |
365 * \todo Timeout behavior |
337 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
366 */ |
338 { |
367 void ec_fsm_coe_dict_request( |
339 ec_datagram_t *datagram = fsm->datagram; |
368 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
340 ec_slave_t *slave = fsm->slave; |
369 ec_datagram_t *datagram /**< Datagram to use. */ |
341 |
370 ) |
342 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
371 { |
343 return; // FIXME: request again? |
372 ec_slave_t *slave = fsm->slave; |
344 |
373 |
345 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
374 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
|
375 if (ec_fsm_coe_prepare_dict(fsm, datagram)) { |
|
376 fsm->state = ec_fsm_coe_error; |
|
377 } |
|
378 return; |
|
379 } |
|
380 |
|
381 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
346 fsm->state = ec_fsm_coe_error; |
382 fsm->state = ec_fsm_coe_error; |
347 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
383 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
348 " request datagram: "); |
384 " request datagram: "); |
349 ec_datagram_print_state(datagram); |
385 ec_datagram_print_state(fsm->datagram); |
350 return; |
386 return; |
351 } |
387 } |
352 |
388 |
353 if (datagram->working_counter != 1) { |
389 if (fsm->datagram->working_counter != 1) { |
354 fsm->state = ec_fsm_coe_error; |
390 fsm->state = ec_fsm_coe_error; |
355 EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: "); |
391 EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: "); |
356 ec_datagram_print_wc_error(datagram); |
392 ec_datagram_print_wc_error(fsm->datagram); |
357 return; |
393 return; |
358 } |
394 } |
359 |
395 |
360 fsm->jiffies_start = datagram->jiffies_sent; |
396 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
361 |
397 |
362 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
398 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
363 fsm->retries = EC_FSM_RETRIES; |
399 fsm->retries = EC_FSM_RETRIES; |
364 fsm->state = ec_fsm_coe_dict_check; |
400 fsm->state = ec_fsm_coe_dict_check; |
365 } |
401 } |
366 |
402 |
367 /*****************************************************************************/ |
403 /*****************************************************************************/ |
368 |
404 |
369 /** |
405 /** CoE state: DICT CHECK. |
370 CoE state: DICT CHECK. |
406 */ |
371 */ |
407 void ec_fsm_coe_dict_check( |
372 |
408 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
373 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
409 ec_datagram_t *datagram /**< Datagram to use. */ |
374 { |
410 ) |
375 ec_datagram_t *datagram = fsm->datagram; |
411 { |
376 ec_slave_t *slave = fsm->slave; |
412 ec_slave_t *slave = fsm->slave; |
377 |
413 |
378 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
414 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
379 return; |
415 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
380 |
416 return; |
381 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
417 } |
|
418 |
|
419 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
382 fsm->state = ec_fsm_coe_error; |
420 fsm->state = ec_fsm_coe_error; |
383 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
421 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
384 ec_datagram_print_state(datagram); |
422 ec_datagram_print_state(fsm->datagram); |
385 return; |
423 return; |
386 } |
424 } |
387 |
425 |
388 if (datagram->working_counter != 1) { |
426 if (fsm->datagram->working_counter != 1) { |
389 fsm->state = ec_fsm_coe_error; |
427 fsm->state = ec_fsm_coe_error; |
390 EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" |
428 EC_SLAVE_ERR(slave,"Reception of CoE mailbox check" |
391 " datagram failed: "); |
429 " datagram failed: "); |
392 ec_datagram_print_wc_error(datagram); |
430 ec_datagram_print_wc_error(fsm->datagram); |
393 return; |
431 return; |
394 } |
432 } |
395 |
433 |
396 if (!ec_slave_mbox_check(datagram)) { |
434 if (!ec_slave_mbox_check(fsm->datagram)) { |
397 unsigned long diff_ms = |
435 unsigned long diff_ms = |
398 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
436 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
437 1000 / HZ; |
399 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
438 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
400 fsm->state = ec_fsm_coe_error; |
439 fsm->state = ec_fsm_coe_error; |
401 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
440 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
402 " SDO dictionary list response.\n"); |
441 " SDO dictionary list response.\n"); |
403 return; |
442 return; |
414 fsm->state = ec_fsm_coe_dict_response; |
453 fsm->state = ec_fsm_coe_dict_response; |
415 } |
454 } |
416 |
455 |
417 /*****************************************************************************/ |
456 /*****************************************************************************/ |
418 |
457 |
|
458 int ec_fsm_coe_dict_prepare_desc( |
|
459 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
|
460 ec_datagram_t *datagram /**< Datagram to use. */ |
|
461 ) |
|
462 { |
|
463 ec_slave_t *slave = fsm->slave; |
|
464 u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); |
|
465 if (IS_ERR(data)) { |
|
466 return PTR_ERR(data); |
|
467 } |
|
468 |
|
469 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
470 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
|
471 EC_WRITE_U8 (data + 3, 0x00); |
|
472 EC_WRITE_U16(data + 4, 0x0000); |
|
473 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
|
474 |
|
475 fsm->state = ec_fsm_coe_dict_desc_request; |
|
476 return 0; |
|
477 } |
|
478 |
|
479 /*****************************************************************************/ |
|
480 |
419 /** |
481 /** |
420 CoE state: DICT RESPONSE. |
482 CoE state: DICT RESPONSE. |
421 \todo Timeout behavior |
483 \todo Timeout behavior |
422 */ |
484 */ |
423 |
485 |
424 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
486 void ec_fsm_coe_dict_response( |
425 { |
487 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
426 ec_datagram_t *datagram = fsm->datagram; |
488 ec_datagram_t *datagram /**< Datagram to use. */ |
|
489 ) |
|
490 { |
427 ec_slave_t *slave = fsm->slave; |
491 ec_slave_t *slave = fsm->slave; |
428 uint8_t *data, mbox_prot; |
492 uint8_t *data, mbox_prot; |
429 size_t rec_size; |
493 size_t rec_size; |
430 unsigned int sdo_count, i; |
494 unsigned int sdo_count, i; |
431 uint16_t sdo_index, fragments_left; |
495 uint16_t sdo_index, fragments_left; |
432 ec_sdo_t *sdo; |
496 ec_sdo_t *sdo; |
433 bool first_segment; |
497 bool first_segment; |
434 size_t index_list_offset; |
498 size_t index_list_offset; |
435 |
499 |
436 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
500 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
437 return; // FIXME: request again? |
501 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
438 |
502 return; |
439 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
503 } |
|
504 |
|
505 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
440 fsm->state = ec_fsm_coe_error; |
506 fsm->state = ec_fsm_coe_error; |
441 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
507 EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary" |
442 " response datagram: "); |
508 " response datagram: "); |
443 ec_datagram_print_state(datagram); |
509 ec_datagram_print_state(fsm->datagram); |
444 return; |
510 return; |
445 } |
511 } |
446 |
512 |
447 if (datagram->working_counter != 1) { |
513 if (fsm->datagram->working_counter != 1) { |
448 fsm->state = ec_fsm_coe_error; |
514 fsm->state = ec_fsm_coe_error; |
449 EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); |
515 EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: "); |
450 ec_datagram_print_wc_error(datagram); |
516 ec_datagram_print_wc_error(fsm->datagram); |
451 return; |
517 return; |
452 } |
518 } |
453 |
519 |
454 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
520 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
455 if (IS_ERR(data)) { |
521 if (IS_ERR(data)) { |
456 fsm->state = ec_fsm_coe_error; |
522 fsm->state = ec_fsm_coe_error; |
457 return; |
523 return; |
458 } |
524 } |
459 |
525 |
557 } |
623 } |
558 |
624 |
559 // fetch SDO descriptions |
625 // fetch SDO descriptions |
560 fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); |
626 fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list); |
561 |
627 |
562 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); |
|
563 if (IS_ERR(data)) { |
|
564 fsm->state = ec_fsm_coe_error; |
|
565 return; |
|
566 } |
|
567 |
|
568 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
569 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
|
570 EC_WRITE_U8 (data + 3, 0x00); |
|
571 EC_WRITE_U16(data + 4, 0x0000); |
|
572 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
|
573 |
|
574 fsm->retries = EC_FSM_RETRIES; |
628 fsm->retries = EC_FSM_RETRIES; |
575 fsm->state = ec_fsm_coe_dict_desc_request; |
629 if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) { |
|
630 fsm->state = ec_fsm_coe_error; |
|
631 } |
576 } |
632 } |
577 |
633 |
578 /*****************************************************************************/ |
634 /*****************************************************************************/ |
579 |
635 |
580 /** |
636 /** |
581 CoE state: DICT DESC REQUEST. |
637 CoE state: DICT DESC REQUEST. |
582 \todo Timeout behavior |
638 \todo Timeout behavior |
583 */ |
639 */ |
584 |
640 |
585 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
641 void ec_fsm_coe_dict_desc_request( |
586 { |
642 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
587 ec_datagram_t *datagram = fsm->datagram; |
643 ec_datagram_t *datagram /**< Datagram to use. */ |
588 ec_slave_t *slave = fsm->slave; |
644 ) |
589 |
645 { |
590 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
646 ec_slave_t *slave = fsm->slave; |
591 return; // FIXME: check for response first? |
647 |
592 |
648 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
593 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
649 if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) { |
|
650 fsm->state = ec_fsm_coe_error; |
|
651 } |
|
652 return; |
|
653 } |
|
654 |
|
655 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
594 fsm->state = ec_fsm_coe_error; |
656 fsm->state = ec_fsm_coe_error; |
595 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
657 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
596 " description request datagram: "); |
658 " description request datagram: "); |
597 ec_datagram_print_state(datagram); |
659 ec_datagram_print_state(fsm->datagram); |
598 return; |
660 return; |
599 } |
661 } |
600 |
662 |
601 if (datagram->working_counter != 1) { |
663 if (fsm->datagram->working_counter != 1) { |
602 fsm->state = ec_fsm_coe_error; |
664 fsm->state = ec_fsm_coe_error; |
603 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
665 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
604 " request failed: "); |
666 " request failed: "); |
605 ec_datagram_print_wc_error(datagram); |
667 ec_datagram_print_wc_error(fsm->datagram); |
606 return; |
668 return; |
607 } |
669 } |
608 |
670 |
609 fsm->jiffies_start = datagram->jiffies_sent; |
671 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
610 |
672 |
611 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
673 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
612 fsm->retries = EC_FSM_RETRIES; |
674 fsm->retries = EC_FSM_RETRIES; |
613 fsm->state = ec_fsm_coe_dict_desc_check; |
675 fsm->state = ec_fsm_coe_dict_desc_check; |
614 } |
676 } |
617 |
679 |
618 /** |
680 /** |
619 CoE state: DICT DESC CHECK. |
681 CoE state: DICT DESC CHECK. |
620 */ |
682 */ |
621 |
683 |
622 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
684 void ec_fsm_coe_dict_desc_check( |
623 { |
685 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
624 ec_datagram_t *datagram = fsm->datagram; |
686 ec_datagram_t *datagram /**< Datagram to use. */ |
625 ec_slave_t *slave = fsm->slave; |
687 ) |
626 |
688 { |
627 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
689 ec_slave_t *slave = fsm->slave; |
628 return; |
690 |
629 |
691 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
630 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
692 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
693 return; |
|
694 } |
|
695 |
|
696 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
631 fsm->state = ec_fsm_coe_error; |
697 fsm->state = ec_fsm_coe_error; |
632 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
698 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
633 ec_datagram_print_state(datagram); |
699 ec_datagram_print_state(fsm->datagram); |
634 return; |
700 return; |
635 } |
701 } |
636 |
702 |
637 if (datagram->working_counter != 1) { |
703 if (fsm->datagram->working_counter != 1) { |
638 fsm->state = ec_fsm_coe_error; |
704 fsm->state = ec_fsm_coe_error; |
639 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
705 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
640 " datagram failed: "); |
706 " datagram failed: "); |
641 ec_datagram_print_wc_error(datagram); |
707 ec_datagram_print_wc_error(fsm->datagram); |
642 return; |
708 return; |
643 } |
709 } |
644 |
710 |
645 if (!ec_slave_mbox_check(datagram)) { |
711 if (!ec_slave_mbox_check(fsm->datagram)) { |
646 unsigned long diff_ms = |
712 unsigned long diff_ms = |
647 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
713 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
714 1000 / HZ; |
648 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
715 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
649 fsm->state = ec_fsm_coe_error; |
716 fsm->state = ec_fsm_coe_error; |
650 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
717 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
651 " SDO 0x%04x object description response.\n", |
718 " SDO 0x%04x object description response.\n", |
652 fsm->sdo->index); |
719 fsm->sdo->index); |
664 fsm->state = ec_fsm_coe_dict_desc_response; |
731 fsm->state = ec_fsm_coe_dict_desc_response; |
665 } |
732 } |
666 |
733 |
667 /*****************************************************************************/ |
734 /*****************************************************************************/ |
668 |
735 |
|
736 int ec_fsm_coe_dict_prepare_entry( |
|
737 ec_fsm_coe_t *fsm, /**< Finite state machine */ |
|
738 ec_datagram_t *datagram /**< Datagram to use. */ |
|
739 ) |
|
740 { |
|
741 ec_slave_t *slave = fsm->slave; |
|
742 u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); |
|
743 if (IS_ERR(data)) { |
|
744 return PTR_ERR(data); |
|
745 } |
|
746 |
|
747 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
748 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
|
749 EC_WRITE_U8 (data + 3, 0x00); |
|
750 EC_WRITE_U16(data + 4, 0x0000); |
|
751 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
|
752 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
|
753 EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only) |
|
754 |
|
755 fsm->state = ec_fsm_coe_dict_entry_request; |
|
756 return 0; |
|
757 } |
|
758 |
|
759 /*****************************************************************************/ |
|
760 |
669 /** |
761 /** |
670 CoE state: DICT DESC RESPONSE. |
762 CoE state: DICT DESC RESPONSE. |
671 \todo Timeout behavior |
763 \todo Timeout behavior |
672 */ |
764 */ |
673 |
765 |
674 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm |
766 void ec_fsm_coe_dict_desc_response( |
675 /**< finite state machine */) |
767 ec_fsm_coe_t *fsm, /**< Finite state machine */ |
676 { |
768 ec_datagram_t *datagram /**< Datagram to use. */ |
677 ec_datagram_t *datagram = fsm->datagram; |
769 ) |
|
770 { |
678 ec_slave_t *slave = fsm->slave; |
771 ec_slave_t *slave = fsm->slave; |
679 ec_sdo_t *sdo = fsm->sdo; |
772 ec_sdo_t *sdo = fsm->sdo; |
680 uint8_t *data, mbox_prot; |
773 uint8_t *data, mbox_prot; |
681 size_t rec_size, name_size; |
774 size_t rec_size, name_size; |
682 |
775 |
683 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
776 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
684 return; // FIXME: request again? |
777 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
685 |
778 return; |
686 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
779 } |
|
780 |
|
781 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
687 fsm->state = ec_fsm_coe_error; |
782 fsm->state = ec_fsm_coe_error; |
688 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" |
783 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description" |
689 " response datagram: "); |
784 " response datagram: "); |
690 ec_datagram_print_state(datagram); |
785 ec_datagram_print_state(fsm->datagram); |
691 return; |
786 return; |
692 } |
787 } |
693 |
788 |
694 if (datagram->working_counter != 1) { |
789 if (fsm->datagram->working_counter != 1) { |
695 fsm->state = ec_fsm_coe_error; |
790 fsm->state = ec_fsm_coe_error; |
696 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
791 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
697 " response failed: "); |
792 " response failed: "); |
698 ec_datagram_print_wc_error(datagram); |
793 ec_datagram_print_wc_error(fsm->datagram); |
699 return; |
794 return; |
700 } |
795 } |
701 |
796 |
702 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
797 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
703 if (IS_ERR(data)) { |
798 if (IS_ERR(data)) { |
704 fsm->state = ec_fsm_coe_error; |
799 fsm->state = ec_fsm_coe_error; |
705 return; |
800 return; |
706 } |
801 } |
707 |
802 |
787 } |
882 } |
788 |
883 |
789 // start fetching entries |
884 // start fetching entries |
790 |
885 |
791 fsm->subindex = 0; |
886 fsm->subindex = 0; |
792 |
|
793 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); |
|
794 if (IS_ERR(data)) { |
|
795 fsm->state = ec_fsm_coe_error; |
|
796 return; |
|
797 } |
|
798 |
|
799 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
800 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
|
801 EC_WRITE_U8 (data + 3, 0x00); |
|
802 EC_WRITE_U16(data + 4, 0x0000); |
|
803 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
|
804 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
|
805 EC_WRITE_U8 (data + 9, 0x01); // value info (access rights only) |
|
806 |
|
807 fsm->retries = EC_FSM_RETRIES; |
887 fsm->retries = EC_FSM_RETRIES; |
808 fsm->state = ec_fsm_coe_dict_entry_request; |
888 |
|
889 if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) { |
|
890 fsm->state = ec_fsm_coe_error; |
|
891 } |
809 } |
892 } |
810 |
893 |
811 /*****************************************************************************/ |
894 /*****************************************************************************/ |
812 |
895 |
813 /** |
896 /** |
814 CoE state: DICT ENTRY REQUEST. |
897 CoE state: DICT ENTRY REQUEST. |
815 \todo Timeout behavior |
898 \todo Timeout behavior |
816 */ |
899 */ |
817 |
900 |
818 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm |
901 void ec_fsm_coe_dict_entry_request( |
819 /**< finite state machine */) |
902 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
820 { |
903 ec_datagram_t *datagram /**< Datagram to use. */ |
821 ec_datagram_t *datagram = fsm->datagram; |
904 ) |
822 ec_slave_t *slave = fsm->slave; |
905 { |
823 |
906 ec_slave_t *slave = fsm->slave; |
824 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
907 |
825 return; // FIXME: check for response first? |
908 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
826 |
909 if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) { |
827 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
910 fsm->state = ec_fsm_coe_error; |
|
911 } |
|
912 return; |
|
913 } |
|
914 |
|
915 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
828 fsm->state = ec_fsm_coe_error; |
916 fsm->state = ec_fsm_coe_error; |
829 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry" |
917 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry" |
830 " request datagram: "); |
918 " request datagram: "); |
831 ec_datagram_print_state(datagram); |
919 ec_datagram_print_state(fsm->datagram); |
832 return; |
920 return; |
833 } |
921 } |
834 |
922 |
835 if (datagram->working_counter != 1) { |
923 if (fsm->datagram->working_counter != 1) { |
836 fsm->state = ec_fsm_coe_error; |
924 fsm->state = ec_fsm_coe_error; |
837 EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: "); |
925 EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: "); |
838 ec_datagram_print_wc_error(datagram); |
926 ec_datagram_print_wc_error(fsm->datagram); |
839 return; |
927 return; |
840 } |
928 } |
841 |
929 |
842 fsm->jiffies_start = datagram->jiffies_sent; |
930 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
843 |
931 |
844 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
932 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
845 fsm->retries = EC_FSM_RETRIES; |
933 fsm->retries = EC_FSM_RETRIES; |
846 fsm->state = ec_fsm_coe_dict_entry_check; |
934 fsm->state = ec_fsm_coe_dict_entry_check; |
847 } |
935 } |
850 |
938 |
851 /** |
939 /** |
852 CoE state: DICT ENTRY CHECK. |
940 CoE state: DICT ENTRY CHECK. |
853 */ |
941 */ |
854 |
942 |
855 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm |
943 void ec_fsm_coe_dict_entry_check( |
856 /**< finite state machine */) |
944 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
857 { |
945 ec_datagram_t *datagram /**< Datagram to use. */ |
858 ec_datagram_t *datagram = fsm->datagram; |
946 ) |
859 ec_slave_t *slave = fsm->slave; |
947 { |
860 |
948 ec_slave_t *slave = fsm->slave; |
861 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
949 |
862 return; |
950 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
863 |
951 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
864 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
952 return; |
|
953 } |
|
954 |
|
955 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
865 fsm->state = ec_fsm_coe_error; |
956 fsm->state = ec_fsm_coe_error; |
866 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
957 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
867 ec_datagram_print_state(datagram); |
958 ec_datagram_print_state(fsm->datagram); |
868 return; |
959 return; |
869 } |
960 } |
870 |
961 |
871 if (datagram->working_counter != 1) { |
962 if (fsm->datagram->working_counter != 1) { |
872 fsm->state = ec_fsm_coe_error; |
963 fsm->state = ec_fsm_coe_error; |
873 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
964 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
874 " datagram failed: "); |
965 " datagram failed: "); |
875 ec_datagram_print_wc_error(datagram); |
966 ec_datagram_print_wc_error(fsm->datagram); |
876 return; |
967 return; |
877 } |
968 } |
878 |
969 |
879 if (!ec_slave_mbox_check(datagram)) { |
970 if (!ec_slave_mbox_check(fsm->datagram)) { |
880 unsigned long diff_ms = |
971 unsigned long diff_ms = |
881 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
972 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
973 1000 / HZ; |
882 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
974 if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) { |
883 fsm->state = ec_fsm_coe_error; |
975 fsm->state = ec_fsm_coe_error; |
884 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
976 EC_SLAVE_ERR(slave, "Timeout while waiting for" |
885 " SDO entry 0x%04x:%x description response.\n", |
977 " SDO entry 0x%04x:%x description response.\n", |
886 fsm->sdo->index, fsm->subindex); |
978 fsm->sdo->index, fsm->subindex); |
903 /** |
995 /** |
904 CoE state: DICT ENTRY RESPONSE. |
996 CoE state: DICT ENTRY RESPONSE. |
905 \todo Timeout behavior |
997 \todo Timeout behavior |
906 */ |
998 */ |
907 |
999 |
908 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm |
1000 void ec_fsm_coe_dict_entry_response( |
909 /**< finite state machine */) |
1001 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
910 { |
1002 ec_datagram_t *datagram /**< Datagram to use. */ |
911 ec_datagram_t *datagram = fsm->datagram; |
1003 ) |
|
1004 { |
912 ec_slave_t *slave = fsm->slave; |
1005 ec_slave_t *slave = fsm->slave; |
913 ec_sdo_t *sdo = fsm->sdo; |
1006 ec_sdo_t *sdo = fsm->sdo; |
914 uint8_t *data, mbox_prot; |
1007 uint8_t *data, mbox_prot; |
915 size_t rec_size, data_size; |
1008 size_t rec_size, data_size; |
916 ec_sdo_entry_t *entry; |
1009 ec_sdo_entry_t *entry; |
917 u16 word; |
1010 u16 word; |
918 |
1011 |
919 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1012 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
920 return; // FIXME: request again? |
1013 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
921 |
1014 return; |
922 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1015 } |
|
1016 |
|
1017 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
923 fsm->state = ec_fsm_coe_error; |
1018 fsm->state = ec_fsm_coe_error; |
924 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
1019 EC_SLAVE_ERR(slave, "Failed to receive CoE SDO" |
925 " description response datagram: "); |
1020 " description response datagram: "); |
926 ec_datagram_print_state(datagram); |
1021 ec_datagram_print_state(fsm->datagram); |
927 return; |
1022 return; |
928 } |
1023 } |
929 |
1024 |
930 if (datagram->working_counter != 1) { |
1025 if (fsm->datagram->working_counter != 1) { |
931 fsm->state = ec_fsm_coe_error; |
1026 fsm->state = ec_fsm_coe_error; |
932 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
1027 EC_SLAVE_ERR(slave, "Reception of CoE SDO description" |
933 " response failed: "); |
1028 " response failed: "); |
934 ec_datagram_print_wc_error(datagram); |
1029 ec_datagram_print_wc_error(fsm->datagram); |
935 return; |
1030 return; |
936 } |
1031 } |
937 |
1032 |
938 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1033 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
939 if (IS_ERR(data)) { |
1034 if (IS_ERR(data)) { |
940 fsm->state = ec_fsm_coe_error; |
1035 fsm->state = ec_fsm_coe_error; |
941 return; |
1036 return; |
942 } |
1037 } |
943 |
1038 |
1044 |
1139 |
1045 list_add_tail(&entry->list, &sdo->entries); |
1140 list_add_tail(&entry->list, &sdo->entries); |
1046 } |
1141 } |
1047 |
1142 |
1048 if (fsm->subindex < sdo->max_subindex) { |
1143 if (fsm->subindex < sdo->max_subindex) { |
|
1144 |
1049 fsm->subindex++; |
1145 fsm->subindex++; |
1050 |
1146 fsm->retries = EC_FSM_RETRIES; |
1051 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); |
1147 |
1052 if (IS_ERR(data)) { |
1148 if (ec_fsm_coe_dict_prepare_entry(fsm, datagram)) { |
1053 fsm->state = ec_fsm_coe_error; |
1149 fsm->state = ec_fsm_coe_error; |
1054 return; |
1150 } |
1055 } |
1151 |
1056 |
|
1057 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
1058 EC_WRITE_U8 (data + 2, 0x05); // Get entry description request |
|
1059 EC_WRITE_U8 (data + 3, 0x00); |
|
1060 EC_WRITE_U16(data + 4, 0x0000); |
|
1061 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
|
1062 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
|
1063 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
|
1064 |
|
1065 fsm->retries = EC_FSM_RETRIES; |
|
1066 fsm->state = ec_fsm_coe_dict_entry_request; |
|
1067 return; |
1152 return; |
1068 } |
1153 } |
1069 |
1154 |
1070 // another SDO description to fetch? |
1155 // another SDO description to fetch? |
1071 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
1156 if (fsm->sdo->list.next != &slave->sdo_dictionary) { |
|
1157 |
1072 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
1158 fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list); |
1073 |
1159 fsm->retries = EC_FSM_RETRIES; |
1074 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8); |
1160 |
1075 if (IS_ERR(data)) { |
1161 if (ec_fsm_coe_dict_prepare_desc(fsm, datagram)) { |
1076 fsm->state = ec_fsm_coe_error; |
1162 fsm->state = ec_fsm_coe_error; |
1077 return; |
1163 } |
1078 } |
1164 |
1079 |
|
1080 EC_WRITE_U16(data, 0x8 << 12); // SDO information |
|
1081 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
|
1082 EC_WRITE_U8 (data + 3, 0x00); |
|
1083 EC_WRITE_U16(data + 4, 0x0000); |
|
1084 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
|
1085 |
|
1086 fsm->retries = EC_FSM_RETRIES; |
|
1087 fsm->state = ec_fsm_coe_dict_desc_request; |
|
1088 return; |
1165 return; |
1089 } |
1166 } |
1090 |
1167 |
1091 fsm->state = ec_fsm_coe_end; |
1168 fsm->state = ec_fsm_coe_end; |
1092 } |
1169 } |
1093 |
1170 |
1094 /****************************************************************************** |
1171 /****************************************************************************** |
1095 * CoE state machine |
1172 * CoE state machine |
1096 *****************************************************************************/ |
1173 *****************************************************************************/ |
1097 |
1174 |
1098 /** CoE state: DOWN START. |
1175 int ec_fsm_coe_prepare_down_start( |
1099 */ |
1176 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1100 void ec_fsm_coe_down_start( |
1177 ec_datagram_t *datagram /**< Datagram to use. */ |
1101 ec_fsm_coe_t *fsm /**< finite state machine */ |
1178 ) |
1102 ) |
1179 { |
1103 { |
1180 u8 *data; |
1104 ec_datagram_t *datagram = fsm->datagram; |
|
1105 ec_slave_t *slave = fsm->slave; |
1181 ec_slave_t *slave = fsm->slave; |
1106 ec_sdo_request_t *request = fsm->request; |
1182 ec_sdo_request_t *request = fsm->request; |
1107 uint8_t *data; |
|
1108 uint8_t data_set_size; |
1183 uint8_t data_set_size; |
1109 |
|
1110 if (fsm->slave->master->debug_level) { |
|
1111 char subidxstr[10]; |
|
1112 if (request->complete_access) { |
|
1113 subidxstr[0] = 0x00; |
|
1114 } else { |
|
1115 sprintf(subidxstr, ":%02X", request->subindex); |
|
1116 } |
|
1117 EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n", |
|
1118 request->index, subidxstr); |
|
1119 ec_print_data(request->data, request->data_size); |
|
1120 } |
|
1121 |
|
1122 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
|
1123 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
|
1124 request->errno = EPROTONOSUPPORT; |
|
1125 fsm->state = ec_fsm_coe_error; |
|
1126 return; |
|
1127 } |
|
1128 |
|
1129 if (slave->configured_rx_mailbox_size < |
|
1130 EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { |
|
1131 EC_SLAVE_ERR(slave, "Mailbox too small!\n"); |
|
1132 request->errno = EOVERFLOW; |
|
1133 fsm->state = ec_fsm_coe_error; |
|
1134 return; |
|
1135 } |
|
1136 |
1184 |
1137 if (request->data_size <= 4) { // use expedited transfer type |
1185 if (request->data_size <= 4) { // use expedited transfer type |
1138 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1186 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1139 EC_COE_DOWN_REQ_HEADER_SIZE); |
1187 EC_COE_DOWN_REQ_HEADER_SIZE); |
1140 if (IS_ERR(data)) { |
1188 if (IS_ERR(data)) { |
1141 request->errno = PTR_ERR(data); |
1189 request->errno = PTR_ERR(data); |
1142 fsm->state = ec_fsm_coe_error; |
1190 return PTR_ERR(data); |
1143 return; |
|
1144 } |
1191 } |
1145 |
1192 |
1146 fsm->remaining = 0; |
1193 fsm->remaining = 0; |
1147 |
1194 |
1148 data_set_size = 4 - request->data_size; |
1195 data_set_size = 4 - request->data_size; |
1210 EC_SLAVE_DBG(slave, 1, "Normal download request:\n"); |
1256 EC_SLAVE_DBG(slave, 1, "Normal download request:\n"); |
1211 ec_print_data(data, data_size); |
1257 ec_print_data(data, data_size); |
1212 } |
1258 } |
1213 } |
1259 } |
1214 |
1260 |
|
1261 fsm->state = ec_fsm_coe_down_request; |
|
1262 return 0; |
|
1263 } |
|
1264 |
|
1265 /****************************************************************************/ |
|
1266 |
|
1267 /** CoE state: DOWN START. |
|
1268 */ |
|
1269 void ec_fsm_coe_down_start( |
|
1270 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
|
1271 ec_datagram_t *datagram /**< Datagram to use. */ |
|
1272 ) |
|
1273 { |
|
1274 ec_slave_t *slave = fsm->slave; |
|
1275 ec_sdo_request_t *request = fsm->request; |
|
1276 |
|
1277 if (fsm->slave->master->debug_level) { |
|
1278 char subidxstr[10]; |
|
1279 if (request->complete_access) { |
|
1280 subidxstr[0] = 0x00; |
|
1281 } else { |
|
1282 sprintf(subidxstr, ":%02X", request->subindex); |
|
1283 } |
|
1284 EC_SLAVE_DBG(slave, 1, "Downloading SDO 0x%04X%s.\n", |
|
1285 request->index, subidxstr); |
|
1286 ec_print_data(request->data, request->data_size); |
|
1287 } |
|
1288 |
|
1289 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
|
1290 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
|
1291 request->errno = EPROTONOSUPPORT; |
|
1292 fsm->state = ec_fsm_coe_error; |
|
1293 return; |
|
1294 } |
|
1295 |
|
1296 if (slave->configured_rx_mailbox_size < |
|
1297 EC_MBOX_HEADER_SIZE + EC_COE_DOWN_REQ_HEADER_SIZE) { |
|
1298 EC_SLAVE_ERR(slave, "Mailbox too small!\n"); |
|
1299 request->errno = EOVERFLOW; |
|
1300 fsm->state = ec_fsm_coe_error; |
|
1301 return; |
|
1302 } |
|
1303 |
|
1304 |
1215 fsm->request->jiffies_sent = jiffies; |
1305 fsm->request->jiffies_sent = jiffies; |
1216 fsm->retries = EC_FSM_RETRIES; |
1306 fsm->retries = EC_FSM_RETRIES; |
1217 fsm->state = ec_fsm_coe_down_request; |
1307 |
|
1308 if (ec_fsm_coe_prepare_down_start(fsm, datagram)) { |
|
1309 fsm->state = ec_fsm_coe_error; |
|
1310 } |
1218 } |
1311 } |
1219 |
1312 |
1220 /*****************************************************************************/ |
1313 /*****************************************************************************/ |
1221 |
1314 |
1222 /** |
1315 /** |
1223 CoE state: DOWN REQUEST. |
1316 CoE state: DOWN REQUEST. |
1224 \todo Timeout behavior |
1317 \todo Timeout behavior |
1225 */ |
1318 */ |
1226 |
1319 |
1227 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1320 void ec_fsm_coe_down_request( |
1228 { |
1321 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1229 ec_datagram_t *datagram = fsm->datagram; |
1322 ec_datagram_t *datagram /**< Datagram to use. */ |
|
1323 ) |
|
1324 { |
1230 ec_slave_t *slave = fsm->slave; |
1325 ec_slave_t *slave = fsm->slave; |
1231 unsigned long diff_ms; |
1326 unsigned long diff_ms; |
1232 |
1327 |
1233 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1328 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1234 return; // FIXME: check for response first? |
1329 if (ec_fsm_coe_prepare_down_start(fsm, datagram)) { |
1235 |
1330 fsm->state = ec_fsm_coe_error; |
1236 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1331 } |
|
1332 return; |
|
1333 } |
|
1334 |
|
1335 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1237 fsm->request->errno = EIO; |
1336 fsm->request->errno = EIO; |
1238 fsm->state = ec_fsm_coe_error; |
1337 fsm->state = ec_fsm_coe_error; |
1239 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1338 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1240 " request datagram: "); |
1339 " request datagram: "); |
1241 ec_datagram_print_state(datagram); |
1340 ec_datagram_print_state(fsm->datagram); |
1242 return; |
1341 return; |
1243 } |
1342 } |
1244 |
1343 |
1245 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1344 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1246 |
1345 |
1247 if (datagram->working_counter != 1) { |
1346 if (fsm->datagram->working_counter != 1) { |
1248 if (!datagram->working_counter) { |
1347 if (!fsm->datagram->working_counter) { |
1249 if (diff_ms < fsm->request->response_timeout) { |
1348 if (diff_ms < fsm->request->response_timeout) { |
1250 #if DEBUG_RETRIES |
1349 #if DEBUG_RETRIES |
1251 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO" |
1350 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO" |
1252 " download request. Retrying after %lu ms...\n", |
1351 " download request. Retrying after %lu ms...\n", |
1253 diff_ms); |
1352 diff_ms); |
1254 #endif |
1353 #endif |
1255 // no response; send request datagram again |
1354 // no response; send request datagram again |
|
1355 if (ec_fsm_coe_prepare_down_start(fsm, datagram)) { |
|
1356 fsm->state = ec_fsm_coe_error; |
|
1357 } |
1256 return; |
1358 return; |
1257 } |
1359 } |
1258 } |
1360 } |
1259 fsm->request->errno = EIO; |
1361 fsm->request->errno = EIO; |
1260 fsm->state = ec_fsm_coe_error; |
1362 fsm->state = ec_fsm_coe_error; |
1261 EC_SLAVE_ERR(slave, "Reception of CoE download request" |
1363 EC_SLAVE_ERR(slave, "Reception of CoE download request" |
1262 " for SDO 0x%04x:%x failed with timeout after %lu ms: ", |
1364 " for SDO 0x%04x:%x failed with timeout after %lu ms: ", |
1263 fsm->request->index, fsm->request->subindex, diff_ms); |
1365 fsm->request->index, fsm->request->subindex, diff_ms); |
1264 ec_datagram_print_wc_error(datagram); |
1366 ec_datagram_print_wc_error(fsm->datagram); |
1265 return; |
1367 return; |
1266 } |
1368 } |
1267 |
1369 |
1268 #if DEBUG_LONG |
1370 #if DEBUG_LONG |
1269 if (diff_ms > 200) { |
1371 if (diff_ms > 200) { |
1270 EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %lu ms.\n", |
1372 EC_SLAVE_WARN(slave, "SDO 0x%04x:%x download took %lu ms.\n", |
1271 fsm->request->index, fsm->request->subindex, diff_ms); |
1373 fsm->request->index, fsm->request->subindex, diff_ms); |
1272 } |
1374 } |
1273 #endif |
1375 #endif |
1274 |
1376 |
1275 fsm->jiffies_start = datagram->jiffies_sent; |
1377 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
1276 |
1378 |
1277 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1379 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1278 fsm->retries = EC_FSM_RETRIES; |
1380 fsm->retries = EC_FSM_RETRIES; |
1279 fsm->state = ec_fsm_coe_down_check; |
1381 fsm->state = ec_fsm_coe_down_check; |
1280 } |
1382 } |
1281 |
1383 |
1282 /*****************************************************************************/ |
1384 /*****************************************************************************/ |
1283 |
1385 |
1284 /** CoE state: DOWN CHECK. |
1386 /** CoE state: DOWN CHECK. |
1285 */ |
1387 */ |
1286 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1388 void ec_fsm_coe_down_check( |
1287 { |
1389 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1288 ec_datagram_t *datagram = fsm->datagram; |
1390 ec_datagram_t *datagram /**< Datagram to use. */ |
1289 ec_slave_t *slave = fsm->slave; |
1391 ) |
1290 |
1392 { |
1291 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1393 ec_slave_t *slave = fsm->slave; |
1292 return; |
1394 |
1293 |
1395 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1294 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1396 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1397 return; |
|
1398 } |
|
1399 |
|
1400 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1295 fsm->request->errno = EIO; |
1401 fsm->request->errno = EIO; |
1296 fsm->state = ec_fsm_coe_error; |
1402 fsm->state = ec_fsm_coe_error; |
1297 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
1403 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
1298 " datagram: "); |
1404 " datagram: "); |
1299 ec_datagram_print_state(datagram); |
1405 ec_datagram_print_state(fsm->datagram); |
1300 return; |
1406 return; |
1301 } |
1407 } |
1302 |
1408 |
1303 if (datagram->working_counter != 1) { |
1409 if (fsm->datagram->working_counter != 1) { |
1304 fsm->request->errno = EIO; |
1410 fsm->request->errno = EIO; |
1305 fsm->state = ec_fsm_coe_error; |
1411 fsm->state = ec_fsm_coe_error; |
1306 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1412 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1307 " datagram failed: "); |
1413 " datagram failed: "); |
1308 ec_datagram_print_wc_error(datagram); |
1414 ec_datagram_print_wc_error(fsm->datagram); |
1309 return; |
1415 return; |
1310 } |
1416 } |
1311 |
1417 |
1312 if (!ec_slave_mbox_check(datagram)) { |
1418 if (!ec_slave_mbox_check(fsm->datagram)) { |
1313 unsigned long diff_ms = |
1419 unsigned long diff_ms = |
1314 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1420 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
1421 1000 / HZ; |
1315 if (diff_ms >= fsm->request->response_timeout) { |
1422 if (diff_ms >= fsm->request->response_timeout) { |
1316 fsm->request->errno = EIO; |
1423 fsm->request->errno = EIO; |
1317 fsm->state = ec_fsm_coe_error; |
1424 fsm->state = ec_fsm_coe_error; |
1318 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
1425 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
1319 " for SDO 0x%04x:%x download response.\n", diff_ms, |
1426 " for SDO 0x%04x:%x download response.\n", diff_ms, |
1335 /*****************************************************************************/ |
1442 /*****************************************************************************/ |
1336 |
1443 |
1337 /** Prepare a download segment request. |
1444 /** Prepare a download segment request. |
1338 */ |
1445 */ |
1339 void ec_fsm_coe_down_prepare_segment_request( |
1446 void ec_fsm_coe_down_prepare_segment_request( |
1340 ec_fsm_coe_t *fsm /**< finite state machine */ |
1447 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1341 ) |
1448 ec_datagram_t *datagram /**< Datagram to use. */ |
1342 { |
1449 ) |
1343 ec_datagram_t *datagram = fsm->datagram; |
1450 { |
1344 ec_slave_t *slave = fsm->slave; |
1451 ec_slave_t *slave = fsm->slave; |
1345 ec_sdo_request_t *request = fsm->request; |
1452 ec_sdo_request_t *request = fsm->request; |
1346 size_t max_segment_size = |
1453 size_t max_segment_size = |
1347 slave->configured_rx_mailbox_size |
1454 slave->configured_rx_mailbox_size |
1348 - EC_MBOX_HEADER_SIZE |
1455 - EC_MBOX_HEADER_SIZE |
1349 - EC_COE_DOWN_SEG_REQ_HEADER_SIZE; |
1456 - EC_COE_DOWN_SEG_REQ_HEADER_SIZE; |
1350 size_t segment_size, data_size; |
1457 size_t data_size; |
1351 uint8_t last_segment, seg_data_size, *data; |
1458 uint8_t last_segment, seg_data_size, *data; |
1352 |
1459 |
1353 if (fsm->remaining > max_segment_size) { |
1460 if (fsm->remaining > max_segment_size) { |
1354 segment_size = max_segment_size; |
1461 fsm->segment_size = max_segment_size; |
1355 last_segment = 0; |
1462 last_segment = 0; |
1356 } else { |
1463 } else { |
1357 segment_size = fsm->remaining; |
1464 fsm->segment_size = fsm->remaining; |
1358 last_segment = 1; |
1465 last_segment = 1; |
1359 } |
1466 } |
1360 |
1467 |
1361 if (segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) { |
1468 if (fsm->segment_size > EC_COE_DOWN_SEG_MIN_DATA_SIZE) { |
1362 seg_data_size = 0x00; |
1469 seg_data_size = 0x00; |
1363 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + segment_size; |
1470 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE + fsm->segment_size; |
1364 } else { |
1471 } else { |
1365 seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size; |
1472 seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - fsm->segment_size; |
1366 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE |
1473 data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE |
1367 + EC_COE_DOWN_SEG_MIN_DATA_SIZE; |
1474 + EC_COE_DOWN_SEG_MIN_DATA_SIZE; |
1368 } |
1475 } |
1369 |
1476 |
1370 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1477 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, |
1403 /** |
1507 /** |
1404 CoE state: DOWN RESPONSE. |
1508 CoE state: DOWN RESPONSE. |
1405 \todo Timeout behavior |
1509 \todo Timeout behavior |
1406 */ |
1510 */ |
1407 |
1511 |
1408 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1512 void ec_fsm_coe_down_response( |
1409 { |
1513 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1410 ec_datagram_t *datagram = fsm->datagram; |
1514 ec_datagram_t *datagram /**< Datagram to use. */ |
|
1515 ) |
|
1516 { |
1411 ec_slave_t *slave = fsm->slave; |
1517 ec_slave_t *slave = fsm->slave; |
1412 uint8_t *data, mbox_prot; |
1518 uint8_t *data, mbox_prot; |
1413 size_t rec_size; |
1519 size_t rec_size; |
1414 ec_sdo_request_t *request = fsm->request; |
1520 ec_sdo_request_t *request = fsm->request; |
1415 |
1521 |
1416 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1522 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1417 return; // FIXME: request again? |
1523 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1418 |
1524 return; |
1419 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1525 } |
|
1526 |
|
1527 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1420 request->errno = EIO; |
1528 request->errno = EIO; |
1421 fsm->state = ec_fsm_coe_error; |
1529 fsm->state = ec_fsm_coe_error; |
1422 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1530 EC_SLAVE_ERR(slave, "Failed to receive CoE download" |
1423 " response datagram: "); |
1531 " response datagram: "); |
1424 ec_datagram_print_state(datagram); |
1532 ec_datagram_print_state(fsm->datagram); |
1425 return; |
1533 return; |
1426 } |
1534 } |
1427 |
1535 |
1428 if (datagram->working_counter != 1) { |
1536 if (fsm->datagram->working_counter != 1) { |
1429 request->errno = EIO; |
1537 request->errno = EIO; |
1430 fsm->state = ec_fsm_coe_error; |
1538 fsm->state = ec_fsm_coe_error; |
1431 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1539 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1432 ec_datagram_print_wc_error(datagram); |
1540 ec_datagram_print_wc_error(fsm->datagram); |
1433 return; |
1541 return; |
1434 } |
1542 } |
1435 |
1543 |
1436 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1544 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
1437 if (IS_ERR(data)) { |
1545 if (IS_ERR(data)) { |
1438 request->errno = PTR_ERR(data); |
1546 request->errno = PTR_ERR(data); |
1439 fsm->state = ec_fsm_coe_error; |
1547 fsm->state = ec_fsm_coe_error; |
1440 return; |
1548 return; |
1441 } |
1549 } |
1520 |
1628 |
1521 /** |
1629 /** |
1522 CoE state: DOWN SEG CHECK. |
1630 CoE state: DOWN SEG CHECK. |
1523 */ |
1631 */ |
1524 |
1632 |
1525 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1633 void ec_fsm_coe_down_seg_check( |
1526 { |
1634 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1527 ec_datagram_t *datagram = fsm->datagram; |
1635 ec_datagram_t *datagram /**< Datagram to use. */ |
1528 ec_slave_t *slave = fsm->slave; |
1636 ) |
1529 |
1637 { |
1530 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1638 ec_slave_t *slave = fsm->slave; |
1531 return; |
1639 |
1532 |
1640 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1533 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1641 return; |
|
1642 |
|
1643 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1534 fsm->request->errno = EIO; |
1644 fsm->request->errno = EIO; |
1535 fsm->state = ec_fsm_coe_error; |
1645 fsm->state = ec_fsm_coe_error; |
1536 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1646 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1537 ec_datagram_print_state(datagram); |
1647 ec_datagram_print_state(fsm->datagram); |
1538 return; |
1648 return; |
1539 } |
1649 } |
1540 |
1650 |
1541 if (datagram->working_counter != 1) { |
1651 if (fsm->datagram->working_counter != 1) { |
1542 fsm->request->errno = EIO; |
1652 fsm->request->errno = EIO; |
1543 fsm->state = ec_fsm_coe_error; |
1653 fsm->state = ec_fsm_coe_error; |
1544 EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" |
1654 EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check" |
1545 " datagram failed: "); |
1655 " datagram failed: "); |
1546 ec_datagram_print_wc_error(datagram); |
1656 ec_datagram_print_wc_error(fsm->datagram); |
1547 return; |
1657 return; |
1548 } |
1658 } |
1549 |
1659 |
1550 if (!ec_slave_mbox_check(datagram)) { |
1660 if (!ec_slave_mbox_check(fsm->datagram)) { |
1551 unsigned long diff_ms = |
1661 unsigned long diff_ms = |
1552 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1662 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
1663 1000 / HZ; |
1553 if (diff_ms >= fsm->request->response_timeout) { |
1664 if (diff_ms >= fsm->request->response_timeout) { |
1554 fsm->request->errno = EIO; |
1665 fsm->request->errno = EIO; |
1555 fsm->state = ec_fsm_coe_error; |
1666 fsm->state = ec_fsm_coe_error; |
1556 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" |
1667 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download" |
1557 " segment response.\n"); |
1668 " segment response.\n"); |
1575 CoE state: DOWN SEG RESPONSE. |
1686 CoE state: DOWN SEG RESPONSE. |
1576 \todo Timeout behavior |
1687 \todo Timeout behavior |
1577 */ |
1688 */ |
1578 |
1689 |
1579 void ec_fsm_coe_down_seg_response( |
1690 void ec_fsm_coe_down_seg_response( |
1580 ec_fsm_coe_t *fsm /**< finite state machine */ |
1691 ec_fsm_coe_t *fsm, /**< Finite state machine */ |
1581 ) |
1692 ec_datagram_t *datagram /**< Datagram to use. */ |
1582 { |
1693 ) |
1583 ec_datagram_t *datagram = fsm->datagram; |
1694 { |
1584 ec_slave_t *slave = fsm->slave; |
1695 ec_slave_t *slave = fsm->slave; |
1585 uint8_t *data, mbox_prot; |
1696 uint8_t *data, mbox_prot; |
1586 size_t rec_size; |
1697 size_t rec_size; |
1587 ec_sdo_request_t *request = fsm->request; |
1698 ec_sdo_request_t *request = fsm->request; |
1588 |
1699 |
1589 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1700 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1590 return; // FIXME: request again? |
1701 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1591 |
1702 return; |
1592 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1703 } |
|
1704 |
|
1705 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1593 request->errno = EIO; |
1706 request->errno = EIO; |
1594 fsm->state = ec_fsm_coe_error; |
1707 fsm->state = ec_fsm_coe_error; |
1595 EC_SLAVE_ERR(slave, "Failed to receive CoE download response" |
1708 EC_SLAVE_ERR(slave, "Failed to receive CoE download response" |
1596 " datagram: "); |
1709 " datagram: "); |
1597 ec_datagram_print_state(datagram); |
1710 ec_datagram_print_state(fsm->datagram); |
1598 return; |
1711 return; |
1599 } |
1712 } |
1600 |
1713 |
1601 if (datagram->working_counter != 1) { |
1714 if (fsm->datagram->working_counter != 1) { |
1602 request->errno = EIO; |
1715 request->errno = EIO; |
1603 fsm->state = ec_fsm_coe_error; |
1716 fsm->state = ec_fsm_coe_error; |
1604 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1717 EC_SLAVE_ERR(slave, "Reception of CoE download response failed: "); |
1605 ec_datagram_print_wc_error(datagram); |
1718 ec_datagram_print_wc_error(fsm->datagram); |
1606 return; |
1719 return; |
1607 } |
1720 } |
1608 |
1721 |
1609 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
1722 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
1610 if (IS_ERR(data)) { |
1723 if (IS_ERR(data)) { |
1611 request->errno = PTR_ERR(data); |
1724 request->errno = PTR_ERR(data); |
1612 fsm->state = ec_fsm_coe_error; |
1725 fsm->state = ec_fsm_coe_error; |
1613 return; |
1726 return; |
1614 } |
1727 } |
1686 request->errno = EIO; |
1799 request->errno = EIO; |
1687 fsm->state = ec_fsm_coe_error; |
1800 fsm->state = ec_fsm_coe_error; |
1688 return; |
1801 return; |
1689 } |
1802 } |
1690 |
1803 |
|
1804 fsm->offset += fsm->segment_size; |
|
1805 fsm->remaining -= fsm->segment_size; |
|
1806 |
1691 if (fsm->remaining) { // more segments to download |
1807 if (fsm->remaining) { // more segments to download |
1692 fsm->toggle = !fsm->toggle; |
1808 fsm->toggle = !fsm->toggle; |
1693 ec_fsm_coe_down_prepare_segment_request(fsm); |
1809 ec_fsm_coe_down_prepare_segment_request(fsm, datagram); |
1694 } else { |
1810 } else { |
1695 fsm->state = ec_fsm_coe_end; // success |
1811 fsm->state = ec_fsm_coe_end; // success |
1696 } |
1812 } |
1697 } |
1813 } |
1698 |
1814 |
1699 /*****************************************************************************/ |
1815 /*****************************************************************************/ |
1700 |
1816 |
1701 /** |
1817 int ec_fsm_coe_prepare_up( |
1702 CoE state: UP START. |
1818 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1703 */ |
1819 ec_datagram_t *datagram /**< Datagram to use. */ |
1704 |
1820 ) |
1705 void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */) |
1821 { |
1706 { |
1822 ec_slave_t *slave = fsm->slave; |
1707 ec_datagram_t *datagram = fsm->datagram; |
1823 ec_sdo_request_t *request = fsm->request; |
1708 ec_slave_t *slave = fsm->slave; |
|
1709 ec_master_t *master = slave->master; |
1824 ec_master_t *master = slave->master; |
1710 ec_sdo_request_t *request = fsm->request; |
1825 |
1711 uint8_t *data; |
1826 u8 *data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); |
1712 |
|
1713 EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n", |
|
1714 request->index, request->subindex); |
|
1715 |
|
1716 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
|
1717 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
|
1718 request->errno = EPROTONOSUPPORT; |
|
1719 fsm->state = ec_fsm_coe_error; |
|
1720 return; |
|
1721 } |
|
1722 |
|
1723 data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10); |
|
1724 if (IS_ERR(data)) { |
1827 if (IS_ERR(data)) { |
1725 request->errno = PTR_ERR(data); |
1828 request->errno = PTR_ERR(data); |
1726 fsm->state = ec_fsm_coe_error; |
1829 return PTR_ERR(data); |
1727 return; |
|
1728 } |
1830 } |
1729 |
1831 |
1730 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1832 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
1731 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1833 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
1732 EC_WRITE_U16(data + 3, request->index); |
1834 EC_WRITE_U16(data + 3, request->index); |
1736 if (master->debug_level) { |
1838 if (master->debug_level) { |
1737 EC_SLAVE_DBG(slave, 1, "Upload request:\n"); |
1839 EC_SLAVE_DBG(slave, 1, "Upload request:\n"); |
1738 ec_print_data(data, 10); |
1840 ec_print_data(data, 10); |
1739 } |
1841 } |
1740 |
1842 |
|
1843 fsm->state = ec_fsm_coe_up_request; |
|
1844 return 0; |
|
1845 } |
|
1846 |
|
1847 /*****************************************************************************/ |
|
1848 |
|
1849 /** |
|
1850 CoE state: UP START. |
|
1851 */ |
|
1852 |
|
1853 void ec_fsm_coe_up_start( |
|
1854 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
|
1855 ec_datagram_t *datagram /**< Datagram to use. */ |
|
1856 ) |
|
1857 { |
|
1858 ec_slave_t *slave = fsm->slave; |
|
1859 ec_sdo_request_t *request = fsm->request; |
|
1860 |
|
1861 EC_SLAVE_DBG(slave, 1, "Uploading SDO 0x%04X:%02X.\n", |
|
1862 request->index, request->subindex); |
|
1863 |
|
1864 if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
|
1865 EC_SLAVE_ERR(slave, "Slave does not support CoE!\n"); |
|
1866 request->errno = EPROTONOSUPPORT; |
|
1867 fsm->state = ec_fsm_coe_error; |
|
1868 return; |
|
1869 } |
|
1870 |
|
1871 fsm->retries = EC_FSM_RETRIES; |
1741 fsm->request->jiffies_sent = jiffies; |
1872 fsm->request->jiffies_sent = jiffies; |
1742 fsm->retries = EC_FSM_RETRIES; |
1873 |
1743 fsm->state = ec_fsm_coe_up_request; |
1874 if (ec_fsm_coe_prepare_up(fsm, datagram)) { |
1744 } |
1875 fsm->state = ec_fsm_coe_error; |
1745 |
1876 } |
1746 /*****************************************************************************/ |
1877 } |
1747 |
1878 |
|
1879 /*****************************************************************************/ |
1748 /** |
1880 /** |
1749 CoE state: UP REQUEST. |
1881 CoE state: UP REQUEST. |
1750 \todo Timeout behavior |
1882 \todo Timeout behavior |
1751 */ |
1883 */ |
1752 |
1884 |
1753 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1885 void ec_fsm_coe_up_request( |
1754 { |
1886 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1755 ec_datagram_t *datagram = fsm->datagram; |
1887 ec_datagram_t *datagram /**< Datagram to use. */ |
|
1888 ) |
|
1889 { |
1756 ec_slave_t *slave = fsm->slave; |
1890 ec_slave_t *slave = fsm->slave; |
1757 unsigned long diff_ms; |
1891 unsigned long diff_ms; |
1758 |
1892 |
1759 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1893 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1760 return; // FIXME: check for response first? |
1894 if (ec_fsm_coe_prepare_up(fsm, datagram)) { |
1761 |
1895 fsm->state = ec_fsm_coe_error; |
1762 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1896 } |
|
1897 return; |
|
1898 } |
|
1899 |
|
1900 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1763 fsm->request->errno = EIO; |
1901 fsm->request->errno = EIO; |
1764 fsm->state = ec_fsm_coe_error; |
1902 fsm->state = ec_fsm_coe_error; |
1765 EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: "); |
1903 EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: "); |
1766 ec_datagram_print_state(datagram); |
1904 ec_datagram_print_state(fsm->datagram); |
1767 return; |
1905 return; |
1768 } |
1906 } |
1769 |
1907 |
1770 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1908 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
1771 |
1909 |
1772 if (datagram->working_counter != 1) { |
1910 if (fsm->datagram->working_counter != 1) { |
1773 if (!datagram->working_counter) { |
1911 if (!fsm->datagram->working_counter) { |
1774 if (diff_ms < fsm->request->response_timeout) { |
1912 if (diff_ms < fsm->request->response_timeout) { |
1775 #if DEBUG_RETRIES |
1913 #if DEBUG_RETRIES |
1776 EC_SLAVE_DBG(slave, 1, "Slave did not respond to" |
1914 EC_SLAVE_DBG(slave, 1, "Slave did not respond to" |
1777 " SDO upload request. Retrying after %lu ms...\n", |
1915 " SDO upload request. Retrying after %lu ms...\n", |
1778 diff_ms); |
1916 diff_ms); |
1779 #endif |
1917 #endif |
1780 // no response; send request datagram again |
1918 // no response; send request datagram again |
|
1919 if (ec_fsm_coe_prepare_up(fsm, datagram)) { |
|
1920 fsm->state = ec_fsm_coe_error; |
|
1921 } |
1781 return; |
1922 return; |
1782 } |
1923 } |
1783 } |
1924 } |
1784 fsm->request->errno = EIO; |
1925 fsm->request->errno = EIO; |
1785 fsm->state = ec_fsm_coe_error; |
1926 fsm->state = ec_fsm_coe_error; |
1786 EC_SLAVE_ERR(slave, "Reception of CoE upload request for" |
1927 EC_SLAVE_ERR(slave, "Reception of CoE upload request for" |
1787 " SDO 0x%04x:%x failed with timeout after %lu ms: ", |
1928 " SDO 0x%04x:%x failed with timeout after %lu ms: ", |
1788 fsm->request->index, fsm->request->subindex, diff_ms); |
1929 fsm->request->index, fsm->request->subindex, diff_ms); |
1789 ec_datagram_print_wc_error(datagram); |
1930 ec_datagram_print_wc_error(fsm->datagram); |
1790 return; |
1931 return; |
1791 } |
1932 } |
1792 |
1933 |
1793 #if DEBUG_LONG |
1934 #if DEBUG_LONG |
1794 if (diff_ms > 200) { |
1935 if (diff_ms > 200) { |
1795 EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %lu ms.\n", |
1936 EC_SLAVE_WARN(slave, "SDO 0x%04x:%x upload took %lu ms.\n", |
1796 fsm->request->index, fsm->request->subindex, diff_ms); |
1937 fsm->request->index, fsm->request->subindex, diff_ms); |
1797 } |
1938 } |
1798 #endif |
1939 #endif |
1799 |
1940 |
1800 fsm->jiffies_start = datagram->jiffies_sent; |
1941 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
1801 |
1942 |
1802 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1943 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1803 fsm->retries = EC_FSM_RETRIES; |
1944 fsm->retries = EC_FSM_RETRIES; |
1804 fsm->state = ec_fsm_coe_up_check; |
1945 fsm->state = ec_fsm_coe_up_check; |
1805 } |
1946 } |
1808 |
1949 |
1809 /** |
1950 /** |
1810 CoE state: UP CHECK. |
1951 CoE state: UP CHECK. |
1811 */ |
1952 */ |
1812 |
1953 |
1813 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1954 void ec_fsm_coe_up_check( |
1814 { |
1955 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1815 ec_datagram_t *datagram = fsm->datagram; |
1956 ec_datagram_t *datagram /**< Datagram to use. */ |
1816 ec_slave_t *slave = fsm->slave; |
1957 ) |
1817 |
1958 { |
1818 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1959 ec_slave_t *slave = fsm->slave; |
1819 return; |
1960 |
1820 |
1961 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1821 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1962 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
1963 return; |
|
1964 } |
|
1965 |
|
1966 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1822 fsm->request->errno = EIO; |
1967 fsm->request->errno = EIO; |
1823 fsm->state = ec_fsm_coe_error; |
1968 fsm->state = ec_fsm_coe_error; |
1824 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1969 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: "); |
1825 ec_datagram_print_state(datagram); |
1970 ec_datagram_print_state(fsm->datagram); |
1826 return; |
1971 return; |
1827 } |
1972 } |
1828 |
1973 |
1829 if (datagram->working_counter != 1) { |
1974 if (fsm->datagram->working_counter != 1) { |
1830 fsm->request->errno = EIO; |
1975 fsm->request->errno = EIO; |
1831 fsm->state = ec_fsm_coe_error; |
1976 fsm->state = ec_fsm_coe_error; |
1832 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1977 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check" |
1833 " datagram failed: "); |
1978 " datagram failed: "); |
1834 ec_datagram_print_wc_error(datagram); |
1979 ec_datagram_print_wc_error(fsm->datagram); |
1835 return; |
1980 return; |
1836 } |
1981 } |
1837 |
1982 |
1838 if (!ec_slave_mbox_check(datagram)) { |
1983 if (!ec_slave_mbox_check(fsm->datagram)) { |
1839 unsigned long diff_ms = |
1984 unsigned long diff_ms = |
1840 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
1985 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
1986 1000 / HZ; |
1841 if (diff_ms >= fsm->request->response_timeout) { |
1987 if (diff_ms >= fsm->request->response_timeout) { |
1842 fsm->request->errno = EIO; |
1988 fsm->request->errno = EIO; |
1843 fsm->state = ec_fsm_coe_error; |
1989 fsm->state = ec_fsm_coe_error; |
1844 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
1990 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
1845 " SDO 0x%04x:%x upload response.\n", diff_ms, |
1991 " SDO 0x%04x:%x upload response.\n", diff_ms, |
1890 /** |
2037 /** |
1891 CoE state: UP RESPONSE. |
2038 CoE state: UP RESPONSE. |
1892 \todo Timeout behavior |
2039 \todo Timeout behavior |
1893 */ |
2040 */ |
1894 |
2041 |
1895 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
2042 void ec_fsm_coe_up_response( |
1896 { |
2043 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
1897 ec_datagram_t *datagram = fsm->datagram; |
2044 ec_datagram_t *datagram /**< Datagram to use. */ |
|
2045 ) |
|
2046 { |
1898 ec_slave_t *slave = fsm->slave; |
2047 ec_slave_t *slave = fsm->slave; |
1899 ec_master_t *master = slave->master; |
2048 ec_master_t *master = slave->master; |
1900 uint16_t rec_index; |
2049 uint16_t rec_index; |
1901 uint8_t *data, mbox_prot, rec_subindex; |
2050 uint8_t *data, mbox_prot, rec_subindex; |
1902 size_t rec_size, data_size; |
2051 size_t rec_size, data_size; |
1903 ec_sdo_request_t *request = fsm->request; |
2052 ec_sdo_request_t *request = fsm->request; |
1904 unsigned int expedited, size_specified; |
2053 unsigned int expedited, size_specified; |
1905 int ret; |
2054 int ret; |
1906 |
2055 |
1907 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2056 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1908 return; // FIXME: request again? |
2057 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1909 |
2058 return; |
1910 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2059 } |
|
2060 |
|
2061 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
1911 request->errno = EIO; |
2062 request->errno = EIO; |
1912 fsm->state = ec_fsm_coe_error; |
2063 fsm->state = ec_fsm_coe_error; |
1913 EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" |
2064 EC_SLAVE_ERR(slave, "Failed to receive CoE upload response" |
1914 " datagram: "); |
2065 " datagram: "); |
1915 ec_datagram_print_state(datagram); |
2066 ec_datagram_print_state(fsm->datagram); |
1916 return; |
2067 return; |
1917 } |
2068 } |
1918 |
2069 |
1919 if (datagram->working_counter != 1) { |
2070 if (fsm->datagram->working_counter != 1) { |
1920 request->errno = EIO; |
2071 request->errno = EIO; |
1921 fsm->state = ec_fsm_coe_error; |
2072 fsm->state = ec_fsm_coe_error; |
1922 EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); |
2073 EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: "); |
1923 ec_datagram_print_wc_error(datagram); |
2074 ec_datagram_print_wc_error(fsm->datagram); |
1924 return; |
2075 return; |
1925 } |
2076 } |
1926 |
2077 |
1927 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
2078 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
1928 if (IS_ERR(data)) { |
2079 if (IS_ERR(data)) { |
1929 request->errno = PTR_ERR(data); |
2080 request->errno = PTR_ERR(data); |
1930 fsm->state = ec_fsm_coe_error; |
2081 fsm->state = ec_fsm_coe_error; |
1931 return; |
2082 return; |
1932 } |
2083 } |
2089 /** |
2240 /** |
2090 CoE state: UP REQUEST. |
2241 CoE state: UP REQUEST. |
2091 \todo Timeout behavior |
2242 \todo Timeout behavior |
2092 */ |
2243 */ |
2093 |
2244 |
2094 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
2245 void ec_fsm_coe_up_seg_request( |
2095 { |
2246 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
2096 ec_datagram_t *datagram = fsm->datagram; |
2247 ec_datagram_t *datagram /**< Datagram to use. */ |
2097 ec_slave_t *slave = fsm->slave; |
2248 ) |
2098 |
2249 { |
2099 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2250 ec_slave_t *slave = fsm->slave; |
2100 return; // FIXME: check for response first? |
2251 |
2101 |
2252 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
2102 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2253 ec_fsm_coe_up_prepare_segment_request(fsm, datagram); |
|
2254 return; |
|
2255 } |
|
2256 |
|
2257 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
2103 fsm->request->errno = EIO; |
2258 fsm->request->errno = EIO; |
2104 fsm->state = ec_fsm_coe_error; |
2259 fsm->state = ec_fsm_coe_error; |
2105 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2260 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2106 " request datagram: "); |
2261 " request datagram: "); |
2107 ec_datagram_print_state(datagram); |
2262 ec_datagram_print_state(fsm->datagram); |
2108 return; |
2263 return; |
2109 } |
2264 } |
2110 |
2265 |
2111 if (datagram->working_counter != 1) { |
2266 if (fsm->datagram->working_counter != 1) { |
2112 fsm->request->errno = EIO; |
2267 fsm->request->errno = EIO; |
2113 fsm->state = ec_fsm_coe_error; |
2268 fsm->state = ec_fsm_coe_error; |
2114 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2269 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2115 " request failed: "); |
2270 " request failed: "); |
2116 ec_datagram_print_wc_error(datagram); |
2271 ec_datagram_print_wc_error(fsm->datagram); |
2117 return; |
2272 return; |
2118 } |
2273 } |
2119 |
2274 |
2120 fsm->jiffies_start = datagram->jiffies_sent; |
2275 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
2121 |
2276 |
2122 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2277 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
2123 fsm->retries = EC_FSM_RETRIES; |
2278 fsm->retries = EC_FSM_RETRIES; |
2124 fsm->state = ec_fsm_coe_up_seg_check; |
2279 fsm->state = ec_fsm_coe_up_seg_check; |
2125 } |
2280 } |
2128 |
2283 |
2129 /** |
2284 /** |
2130 CoE state: UP CHECK. |
2285 CoE state: UP CHECK. |
2131 */ |
2286 */ |
2132 |
2287 |
2133 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
2288 void ec_fsm_coe_up_seg_check( |
2134 { |
2289 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
2135 ec_datagram_t *datagram = fsm->datagram; |
2290 ec_datagram_t *datagram /**< Datagram to use. */ |
2136 ec_slave_t *slave = fsm->slave; |
2291 ) |
2137 |
2292 { |
2138 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2293 ec_slave_t *slave = fsm->slave; |
2139 return; |
2294 |
2140 |
2295 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
2141 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2296 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
2297 return; |
|
2298 } |
|
2299 |
|
2300 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
2142 fsm->request->errno = EIO; |
2301 fsm->request->errno = EIO; |
2143 fsm->state = ec_fsm_coe_error; |
2302 fsm->state = ec_fsm_coe_error; |
2144 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
2303 EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check" |
2145 " datagram: "); |
2304 " datagram: "); |
2146 ec_datagram_print_state(datagram); |
2305 ec_datagram_print_state(fsm->datagram); |
2147 return; |
2306 return; |
2148 } |
2307 } |
2149 |
2308 |
2150 if (datagram->working_counter != 1) { |
2309 if (fsm->datagram->working_counter != 1) { |
2151 fsm->request->errno = EIO; |
2310 fsm->request->errno = EIO; |
2152 fsm->state = ec_fsm_coe_error; |
2311 fsm->state = ec_fsm_coe_error; |
2153 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" |
2312 EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram" |
2154 " failed: "); |
2313 " failed: "); |
2155 ec_datagram_print_wc_error(datagram); |
2314 ec_datagram_print_wc_error(fsm->datagram); |
2156 return; |
2315 return; |
2157 } |
2316 } |
2158 |
2317 |
2159 if (!ec_slave_mbox_check(datagram)) { |
2318 if (!ec_slave_mbox_check(fsm->datagram)) { |
2160 unsigned long diff_ms = |
2319 unsigned long diff_ms = |
2161 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
2320 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
2321 1000 / HZ; |
2162 if (diff_ms >= fsm->request->response_timeout) { |
2322 if (diff_ms >= fsm->request->response_timeout) { |
2163 fsm->request->errno = EIO; |
2323 fsm->request->errno = EIO; |
2164 fsm->state = ec_fsm_coe_error; |
2324 fsm->state = ec_fsm_coe_error; |
2165 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" |
2325 EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload" |
2166 " segment response.\n"); |
2326 " segment response.\n"); |
2183 /** |
2343 /** |
2184 CoE state: UP RESPONSE. |
2344 CoE state: UP RESPONSE. |
2185 \todo Timeout behavior |
2345 \todo Timeout behavior |
2186 */ |
2346 */ |
2187 |
2347 |
2188 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
2348 void ec_fsm_coe_up_seg_response( |
2189 { |
2349 ec_fsm_coe_t *fsm, /**< Finite state machine. */ |
2190 ec_datagram_t *datagram = fsm->datagram; |
2350 ec_datagram_t *datagram /**< Datagram to use. */ |
|
2351 ) |
|
2352 { |
2191 ec_slave_t *slave = fsm->slave; |
2353 ec_slave_t *slave = fsm->slave; |
2192 ec_master_t *master = slave->master; |
2354 ec_master_t *master = slave->master; |
2193 uint8_t *data, mbox_prot; |
2355 uint8_t *data, mbox_prot; |
2194 size_t rec_size, data_size; |
2356 size_t rec_size, data_size; |
2195 ec_sdo_request_t *request = fsm->request; |
2357 ec_sdo_request_t *request = fsm->request; |
2196 unsigned int last_segment; |
2358 unsigned int last_segment; |
2197 |
2359 |
2198 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
2360 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
2199 return; // FIXME: request again? |
2361 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
2200 |
2362 return; |
2201 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
2363 } |
|
2364 |
|
2365 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
2202 request->errno = EIO; |
2366 request->errno = EIO; |
2203 fsm->state = ec_fsm_coe_error; |
2367 fsm->state = ec_fsm_coe_error; |
2204 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2368 EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment" |
2205 " response datagram: "); |
2369 " response datagram: "); |
2206 ec_datagram_print_state(datagram); |
2370 ec_datagram_print_state(fsm->datagram); |
2207 return; |
2371 return; |
2208 } |
2372 } |
2209 |
2373 |
2210 if (datagram->working_counter != 1) { |
2374 if (fsm->datagram->working_counter != 1) { |
2211 request->errno = EIO; |
2375 request->errno = EIO; |
2212 fsm->state = ec_fsm_coe_error; |
2376 fsm->state = ec_fsm_coe_error; |
2213 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2377 EC_SLAVE_ERR(slave, "Reception of CoE upload segment" |
2214 " response failed: "); |
2378 " response failed: "); |
2215 ec_datagram_print_wc_error(datagram); |
2379 ec_datagram_print_wc_error(fsm->datagram); |
2216 return; |
2380 return; |
2217 } |
2381 } |
2218 |
2382 |
2219 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
2383 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
2220 if (IS_ERR(data)) { |
2384 if (IS_ERR(data)) { |
2221 request->errno = PTR_ERR(data); |
2385 request->errno = PTR_ERR(data); |
2222 fsm->state = ec_fsm_coe_error; |
2386 fsm->state = ec_fsm_coe_error; |
2223 return; |
2387 return; |
2224 } |
2388 } |