238 |
238 |
239 /** SoE state: READ REQUEST. |
239 /** SoE state: READ REQUEST. |
240 */ |
240 */ |
241 void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
241 void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
242 { |
242 { |
243 ec_datagram_t *datagram = fsm->datagram; |
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 (datagram->state == 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 (datagram->state != 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(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 (datagram->working_counter != 1) { |
260 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
261 if (!datagram->working_counter) { |
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 } |
267 fsm->state = ec_fsm_soe_error; |
267 fsm->state = ec_fsm_soe_error; |
268 EC_SLAVE_ERR(slave, "Reception of SoE read request" |
268 EC_SLAVE_ERR(slave, "Reception of SoE read request" |
269 " failed after %lu ms: ", diff_ms); |
269 " failed after %lu ms: ", diff_ms); |
270 ec_datagram_print_wc_error(datagram); |
270 ec_datagram_print_wc_error(mbox->datagram); |
271 ec_fsm_soe_print_error(fsm); |
271 ec_fsm_soe_print_error(fsm); |
272 return; |
272 return; |
273 } |
273 } |
274 |
274 |
275 fsm->jiffies_start = datagram->jiffies_sent; |
275 fsm->jiffies_start = mbox->datagram->jiffies_sent; |
276 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
276 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
277 fsm->retries = EC_FSM_RETRIES; |
277 fsm->retries = EC_FSM_RETRIES; |
278 fsm->state = ec_fsm_soe_read_check; |
278 fsm->state = ec_fsm_soe_read_check; |
279 } |
279 } |
280 |
280 |
281 /*****************************************************************************/ |
281 /*****************************************************************************/ |
282 |
282 |
283 /** CoE state: READ CHECK. |
283 /** CoE state: READ CHECK. |
284 */ |
284 */ |
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_datagram_t *datagram = fsm->datagram; |
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 (datagram->state == 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 (datagram->state != 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(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 (datagram->working_counter != 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(datagram); |
305 ec_datagram_print_wc_error(mbox->datagram); |
306 ec_fsm_soe_print_error(fsm); |
306 ec_fsm_soe_print_error(fsm); |
307 return; |
307 return; |
308 } |
308 } |
309 |
309 |
310 if (!ec_slave_mbox_check(datagram)) { |
310 if (!ec_slave_mbox_check(mbox)) { |
311 unsigned long diff_ms = |
311 unsigned long diff_ms = |
312 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
312 (mbox->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
313 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
313 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
314 fsm->state = ec_fsm_soe_error; |
314 fsm->state = ec_fsm_soe_error; |
315 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
315 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
316 " read response.\n", diff_ms); |
316 " read response.\n", diff_ms); |
317 ec_fsm_soe_print_error(fsm); |
317 ec_fsm_soe_print_error(fsm); |
318 return; |
318 return; |
319 } |
319 } |
320 |
320 |
321 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
321 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
322 fsm->retries = EC_FSM_RETRIES; |
322 fsm->retries = EC_FSM_RETRIES; |
323 return; |
323 return; |
324 } |
324 } |
325 |
325 |
326 // Fetch response |
326 // Fetch response |
327 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
327 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
328 fsm->retries = EC_FSM_RETRIES; |
328 fsm->retries = EC_FSM_RETRIES; |
329 fsm->state = ec_fsm_soe_read_response; |
329 fsm->state = ec_fsm_soe_read_response; |
330 } |
330 } |
331 |
331 |
332 /*****************************************************************************/ |
332 /*****************************************************************************/ |
333 |
333 |
334 /** SoE state: READ RESPONSE. |
334 /** SoE state: READ RESPONSE. |
335 */ |
335 */ |
336 void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
336 void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
337 { |
337 { |
338 ec_datagram_t *datagram = fsm->datagram; |
338 ec_mailbox_t *mbox = fsm->mbox; |
339 ec_slave_t *slave = fsm->slave; |
339 ec_slave_t *slave = fsm->slave; |
340 ec_master_t *master = slave->master; |
340 ec_master_t *master = slave->master; |
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 (datagram->state == 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 (datagram->state != 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(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 (datagram->working_counter != 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(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; |
363 } |
363 } |
364 |
364 |
365 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
365 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
366 if (IS_ERR(data)) { |
366 if (IS_ERR(data)) { |
367 fsm->state = ec_fsm_soe_error; |
367 fsm->state = ec_fsm_soe_error; |
368 ec_fsm_soe_print_error(fsm); |
368 ec_fsm_soe_print_error(fsm); |
369 return; |
369 return; |
370 } |
370 } |
537 |
537 |
538 /** SoE state: WRITE REQUEST. |
538 /** SoE state: WRITE REQUEST. |
539 */ |
539 */ |
540 void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
540 void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
541 { |
541 { |
542 ec_datagram_t *datagram = fsm->datagram; |
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 (datagram->state == 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 (datagram->state != 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(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 (datagram->working_counter != 1) { |
559 if (!ec_mbox_is_datagram_wc(mbox,1)) { |
560 if (!datagram->working_counter) { |
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 } |
566 fsm->state = ec_fsm_soe_error; |
566 fsm->state = ec_fsm_soe_error; |
567 EC_SLAVE_ERR(slave, "Reception of SoE write request" |
567 EC_SLAVE_ERR(slave, "Reception of SoE write request" |
568 " failed after %lu ms: ", diff_ms); |
568 " failed after %lu ms: ", diff_ms); |
569 ec_datagram_print_wc_error(datagram); |
569 ec_datagram_print_wc_error(mbox->datagram); |
570 ec_fsm_soe_print_error(fsm); |
570 ec_fsm_soe_print_error(fsm); |
571 return; |
571 return; |
572 } |
572 } |
573 |
573 |
574 fsm->jiffies_start = datagram->jiffies_sent; |
574 fsm->jiffies_start = mbox->datagram->jiffies_sent; |
575 |
575 |
576 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
576 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
577 fsm->retries = EC_FSM_RETRIES; |
577 fsm->retries = EC_FSM_RETRIES; |
578 fsm->state = ec_fsm_soe_write_check; |
578 fsm->state = ec_fsm_soe_write_check; |
579 } |
579 } |
580 |
580 |
581 /*****************************************************************************/ |
581 /*****************************************************************************/ |
582 |
582 |
583 /** CoE state: WRITE CHECK. |
583 /** CoE state: WRITE CHECK. |
584 */ |
584 */ |
585 void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
585 void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
586 { |
586 { |
587 ec_datagram_t *datagram = fsm->datagram; |
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 (datagram->state == 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 (datagram->state != 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(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 (datagram->working_counter != 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(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; |
608 } |
608 } |
609 |
609 |
610 if (fsm->offset < req->data_size) { |
610 if (fsm->offset < req->data_size) { |
611 ec_fsm_soe_write_next_fragment(fsm); |
611 ec_fsm_soe_write_next_fragment(fsm); |
612 } else { |
612 } else { |
613 if (!ec_slave_mbox_check(datagram)) { |
613 if (!ec_slave_mbox_check(mbox)) { |
614 unsigned long diff_ms = |
614 unsigned long diff_ms = |
615 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
615 (mbox->datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
616 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
616 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
617 fsm->state = ec_fsm_soe_error; |
617 fsm->state = ec_fsm_soe_error; |
618 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
618 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
619 " for write response.\n", diff_ms); |
619 " for write response.\n", diff_ms); |
620 ec_fsm_soe_print_error(fsm); |
620 ec_fsm_soe_print_error(fsm); |
621 return; |
621 return; |
622 } |
622 } |
623 |
623 |
624 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
624 ec_slave_mbox_prepare_check(slave, mbox); // can not fail. |
625 fsm->retries = EC_FSM_RETRIES; |
625 fsm->retries = EC_FSM_RETRIES; |
626 return; |
626 return; |
627 } |
627 } |
628 |
628 |
629 // Fetch response |
629 // Fetch response |
630 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
630 ec_slave_mbox_prepare_fetch(slave, mbox); // can not fail. |
631 fsm->retries = EC_FSM_RETRIES; |
631 fsm->retries = EC_FSM_RETRIES; |
632 fsm->state = ec_fsm_soe_write_response; |
632 fsm->state = ec_fsm_soe_write_response; |
633 } |
633 } |
634 } |
634 } |
635 |
635 |
637 |
637 |
638 /** SoE state: WRITE RESPONSE. |
638 /** SoE state: WRITE RESPONSE. |
639 */ |
639 */ |
640 void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
640 void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
641 { |
641 { |
642 ec_datagram_t *datagram = fsm->datagram; |
642 ec_mailbox_t *mbox = fsm->mbox; |
643 ec_slave_t *slave = fsm->slave; |
643 ec_slave_t *slave = fsm->slave; |
644 ec_master_t *master = slave->master; |
644 ec_master_t *master = slave->master; |
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 (datagram->state == 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 (datagram->state != 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(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 (datagram->working_counter != 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(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; |
668 } |
668 } |
669 |
669 |
670 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
670 data = ec_slave_mbox_fetch(slave, mbox, &mbox_prot, &rec_size); |
671 if (IS_ERR(data)) { |
671 if (IS_ERR(data)) { |
672 fsm->state = ec_fsm_soe_error; |
672 fsm->state = ec_fsm_soe_error; |
673 ec_fsm_soe_print_error(fsm); |
673 ec_fsm_soe_print_error(fsm); |
674 return; |
674 return; |
675 } |
675 } |