242 { |
242 { |
243 ec_mailbox_t *mbox = fsm->mbox; |
243 ec_mailbox_t *mbox = fsm->mbox; |
244 ec_slave_t *slave = fsm->slave; |
244 ec_slave_t *slave = fsm->slave; |
245 unsigned long diff_ms; |
245 unsigned long diff_ms; |
246 |
246 |
247 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
247 if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
248 return; // FIXME: check for response first? |
248 return; // FIXME: check for response first? |
249 |
249 |
250 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
250 if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) { |
251 fsm->state = ec_fsm_soe_error; |
251 fsm->state = ec_fsm_soe_error; |
252 EC_SLAVE_ERR(slave, "Failed to receive SoE read request: "); |
252 EC_SLAVE_ERR(slave, "Failed to receive SoE read request: "); |
253 ec_datagram_print_state(mbox->datagram); |
253 ec_datagram_print_state(mbox->datagram); |
254 ec_fsm_soe_print_error(fsm); |
254 ec_fsm_soe_print_error(fsm); |
255 return; |
255 return; |
256 } |
256 } |
257 |
257 |
258 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
258 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
259 |
259 |
260 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
260 if (!ec_mbox_is_datagram_wc(mbox, 1)) { |
261 if (ec_mbox_is_datagram_wc(mbox,0)) { |
261 if (ec_mbox_is_datagram_wc(mbox, 0)) { |
262 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
262 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
263 // no response; send request datagram again |
263 // no response; send request datagram again |
264 return; |
264 return; |
265 } |
265 } |
266 } |
266 } |
285 void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
285 void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
286 { |
286 { |
287 ec_mailbox_t *mbox = fsm->mbox; |
287 ec_mailbox_t *mbox = fsm->mbox; |
288 ec_slave_t *slave = fsm->slave; |
288 ec_slave_t *slave = fsm->slave; |
289 |
289 |
290 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
290 if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
291 return; |
291 return; |
292 |
292 |
293 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
293 if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) { |
294 fsm->state = ec_fsm_soe_error; |
294 fsm->state = ec_fsm_soe_error; |
295 EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); |
295 EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); |
296 ec_datagram_print_state(mbox->datagram); |
296 ec_datagram_print_state(mbox->datagram); |
297 ec_fsm_soe_print_error(fsm); |
297 ec_fsm_soe_print_error(fsm); |
298 return; |
298 return; |
299 } |
299 } |
300 |
300 |
301 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
301 if (!ec_mbox_is_datagram_wc(mbox, 1)) { |
302 fsm->state = ec_fsm_soe_error; |
302 fsm->state = ec_fsm_soe_error; |
303 EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" |
303 EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" |
304 " datagram failed: "); |
304 " datagram failed: "); |
305 ec_datagram_print_wc_error(mbox->datagram); |
305 ec_datagram_print_wc_error(mbox->datagram); |
306 ec_fsm_soe_print_error(fsm); |
306 ec_fsm_soe_print_error(fsm); |
341 uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, |
341 uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, |
342 value_included; |
342 value_included; |
343 size_t rec_size, data_size; |
343 size_t rec_size, data_size; |
344 ec_soe_request_t *req = fsm->request; |
344 ec_soe_request_t *req = fsm->request; |
345 |
345 |
346 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
346 if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
347 return; // FIXME: request again? |
347 return; // FIXME: request again? |
348 |
348 |
349 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
349 if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) { |
350 fsm->state = ec_fsm_soe_error; |
350 fsm->state = ec_fsm_soe_error; |
351 EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); |
351 EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); |
352 ec_datagram_print_state(mbox->datagram); |
352 ec_datagram_print_state(mbox->datagram); |
353 ec_fsm_soe_print_error(fsm); |
353 ec_fsm_soe_print_error(fsm); |
354 return; |
354 return; |
355 } |
355 } |
356 |
356 |
357 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
357 if (!ec_mbox_is_datagram_wc(mbox, 1)) { |
358 fsm->state = ec_fsm_soe_error; |
358 fsm->state = ec_fsm_soe_error; |
359 EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); |
359 EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); |
360 ec_datagram_print_wc_error(mbox->datagram); |
360 ec_datagram_print_wc_error(mbox->datagram); |
361 ec_fsm_soe_print_error(fsm); |
361 ec_fsm_soe_print_error(fsm); |
362 return; |
362 return; |
541 { |
541 { |
542 ec_mailbox_t *mbox = fsm->mbox; |
542 ec_mailbox_t *mbox = fsm->mbox; |
543 ec_slave_t *slave = fsm->slave; |
543 ec_slave_t *slave = fsm->slave; |
544 unsigned long diff_ms; |
544 unsigned long diff_ms; |
545 |
545 |
546 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
546 if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
547 return; // FIXME: check for response first? |
547 return; // FIXME: check for response first? |
548 |
548 |
549 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
549 if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) { |
550 fsm->state = ec_fsm_soe_error; |
550 fsm->state = ec_fsm_soe_error; |
551 EC_SLAVE_ERR(slave, "Failed to receive SoE write request: "); |
551 EC_SLAVE_ERR(slave, "Failed to receive SoE write request: "); |
552 ec_datagram_print_state(mbox->datagram); |
552 ec_datagram_print_state(mbox->datagram); |
553 ec_fsm_soe_print_error(fsm); |
553 ec_fsm_soe_print_error(fsm); |
554 return; |
554 return; |
555 } |
555 } |
556 |
556 |
557 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
557 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
558 |
558 |
559 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
559 if (!ec_mbox_is_datagram_wc(mbox, 1)) { |
560 if (ec_mbox_is_datagram_wc(mbox,0)) { |
560 if (ec_mbox_is_datagram_wc(mbox, 0)) { |
561 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
561 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
562 // no response; send request datagram again |
562 // no response; send request datagram again |
563 return; |
563 return; |
564 } |
564 } |
565 } |
565 } |
586 { |
586 { |
587 ec_mailbox_t *mbox = fsm->mbox; |
587 ec_mailbox_t *mbox = fsm->mbox; |
588 ec_slave_t *slave = fsm->slave; |
588 ec_slave_t *slave = fsm->slave; |
589 ec_soe_request_t *req = fsm->request; |
589 ec_soe_request_t *req = fsm->request; |
590 |
590 |
591 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
591 if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
592 return; |
592 return; |
593 |
593 |
594 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
594 if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) { |
595 fsm->state = ec_fsm_soe_error; |
595 fsm->state = ec_fsm_soe_error; |
596 EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); |
596 EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); |
597 ec_datagram_print_state(mbox->datagram); |
597 ec_datagram_print_state(mbox->datagram); |
598 ec_fsm_soe_print_error(fsm); |
598 ec_fsm_soe_print_error(fsm); |
599 return; |
599 return; |
600 } |
600 } |
601 |
601 |
602 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
602 if (!ec_mbox_is_datagram_wc(mbox, 1)) { |
603 fsm->state = ec_fsm_soe_error; |
603 fsm->state = ec_fsm_soe_error; |
604 EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); |
604 EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); |
605 ec_datagram_print_wc_error(mbox->datagram); |
605 ec_datagram_print_wc_error(mbox->datagram); |
606 ec_fsm_soe_print_error(fsm); |
606 ec_fsm_soe_print_error(fsm); |
607 return; |
607 return; |
645 ec_soe_request_t *req = fsm->request; |
645 ec_soe_request_t *req = fsm->request; |
646 uint8_t *data, mbox_prot, opcode, error_flag; |
646 uint8_t *data, mbox_prot, opcode, error_flag; |
647 uint16_t idn; |
647 uint16_t idn; |
648 size_t rec_size; |
648 size_t rec_size; |
649 |
649 |
650 if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
650 if (ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_TIMED_OUT) && fsm->retries--) |
651 return; // FIXME: request again? |
651 return; // FIXME: request again? |
652 |
652 |
653 if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) { |
653 if (!ec_mbox_is_datagram_state(mbox, EC_DATAGRAM_RECEIVED)) { |
654 fsm->state = ec_fsm_soe_error; |
654 fsm->state = ec_fsm_soe_error; |
655 EC_SLAVE_ERR(slave, "Failed to receive SoE write" |
655 EC_SLAVE_ERR(slave, "Failed to receive SoE write" |
656 " response datagram: "); |
656 " response datagram: "); |
657 ec_datagram_print_state(mbox->datagram); |
657 ec_datagram_print_state(mbox->datagram); |
658 ec_fsm_soe_print_error(fsm); |
658 ec_fsm_soe_print_error(fsm); |
659 return; |
659 return; |
660 } |
660 } |
661 |
661 |
662 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
662 if (!ec_mbox_is_datagram_wc(mbox, 1)) { |
663 fsm->state = ec_fsm_soe_error; |
663 fsm->state = ec_fsm_soe_error; |
664 EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); |
664 EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); |
665 ec_datagram_print_wc_error(mbox->datagram); |
665 ec_datagram_print_wc_error(mbox->datagram); |
666 ec_fsm_soe_print_error(fsm); |
666 ec_fsm_soe_print_error(fsm); |
667 return; |
667 return; |