422 |
422 |
423 remaining_size = req->data_size - fsm->offset; |
423 remaining_size = req->data_size - fsm->offset; |
424 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
424 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
425 incomplete = remaining_size > max_fragment_size; |
425 incomplete = remaining_size > max_fragment_size; |
426 fragment_size = incomplete ? max_fragment_size : remaining_size; |
426 fragment_size = incomplete ? max_fragment_size : remaining_size; |
427 fragments_left = remaining_size / fragment_size; |
427 fragments_left = remaining_size / fragment_size - 1; |
428 if (remaining_size % fragment_size) { |
428 if (remaining_size % fragment_size) { |
429 fragments_left++; |
429 fragments_left++; |
430 } |
430 } |
431 |
431 |
432 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
432 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
436 return; |
436 return; |
437 } |
437 } |
438 |
438 |
439 EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST | incomplete << 3); |
439 EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST | incomplete << 3); |
440 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
440 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
441 EC_WRITE_U16(data + 2, fsm->offset ? fragments_left : req->idn); |
441 EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn); |
442 memcpy(data + 4, req->data + fsm->offset, fragment_size); |
442 memcpy(data + 4, req->data + fsm->offset, fragment_size); |
443 fsm->offset += fragment_size; |
443 fsm->offset += fragment_size; |
444 |
444 |
445 if (master->debug_level) { |
445 if (master->debug_level) { |
446 EC_DBG("SCC write request:\n"); |
446 EC_DBG("SCC write request:\n"); |
461 ec_slave_t *slave = fsm->slave; |
461 ec_slave_t *slave = fsm->slave; |
462 ec_master_t *master = slave->master; |
462 ec_master_t *master = slave->master; |
463 ec_soe_request_t *req = fsm->request; |
463 ec_soe_request_t *req = fsm->request; |
464 |
464 |
465 if (master->debug_level) |
465 if (master->debug_level) |
466 EC_DBG("Writing IDN 0x%04X to slave %u.\n", |
466 EC_DBG("Writing IDN 0x%04X (%zu byte) to slave %u.\n", |
467 req->idn, slave->ring_position); |
467 req->idn, req->data_size, slave->ring_position); |
468 |
468 |
469 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
469 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
470 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
470 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
471 fsm->state = ec_fsm_soe_error; |
471 fsm->state = ec_fsm_soe_error; |
472 return; |
472 return; |
528 */ |
528 */ |
529 void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
529 void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
530 { |
530 { |
531 ec_datagram_t *datagram = fsm->datagram; |
531 ec_datagram_t *datagram = fsm->datagram; |
532 ec_slave_t *slave = fsm->slave; |
532 ec_slave_t *slave = fsm->slave; |
|
533 ec_soe_request_t *req = fsm->request; |
533 |
534 |
534 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
535 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
535 return; |
536 return; |
536 |
537 |
537 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
538 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
538 fsm->state = ec_fsm_soe_error; |
539 fsm->state = ec_fsm_soe_error; |
539 EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ", |
540 EC_ERR("Failed to receive SoE write request datagram from slave %u: ", |
540 slave->ring_position); |
541 slave->ring_position); |
541 ec_datagram_print_state(datagram); |
542 ec_datagram_print_state(datagram); |
542 return; |
543 return; |
543 } |
544 } |
544 |
545 |
545 if (datagram->working_counter != 1) { |
546 if (datagram->working_counter != 1) { |
546 fsm->state = ec_fsm_soe_error; |
547 fsm->state = ec_fsm_soe_error; |
547 EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ", |
548 EC_ERR("Reception of SoE write request datagram failed on slave %u: ", |
548 slave->ring_position); |
549 slave->ring_position); |
549 ec_datagram_print_wc_error(datagram); |
550 ec_datagram_print_wc_error(datagram); |
550 return; |
551 return; |
551 } |
552 } |
552 |
553 |
553 if (!ec_slave_mbox_check(datagram)) { |
554 if (fsm->offset < req->data_size) { |
554 unsigned long diff_ms = |
555 ec_fsm_soe_write_next_fragment(fsm); |
555 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
556 } else { |
556 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
557 if (!ec_slave_mbox_check(datagram)) { |
557 fsm->state = ec_fsm_soe_error; |
558 unsigned long diff_ms = |
558 EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x" |
559 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
559 " write response on slave %u.\n", (u32) diff_ms, |
560 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
560 fsm->request->idn, slave->ring_position); |
561 fsm->state = ec_fsm_soe_error; |
|
562 EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x" |
|
563 " write response on slave %u.\n", (u32) diff_ms, |
|
564 fsm->request->idn, slave->ring_position); |
|
565 return; |
|
566 } |
|
567 |
|
568 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
569 fsm->retries = EC_FSM_RETRIES; |
561 return; |
570 return; |
562 } |
571 } |
563 |
572 |
564 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
573 // Fetch response |
|
574 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
565 fsm->retries = EC_FSM_RETRIES; |
575 fsm->retries = EC_FSM_RETRIES; |
566 return; |
576 fsm->state = ec_fsm_soe_write_response; |
567 } |
577 } |
568 |
|
569 // Fetch response |
|
570 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
|
571 fsm->retries = EC_FSM_RETRIES; |
|
572 fsm->state = ec_fsm_soe_write_response; |
|
573 } |
578 } |
574 |
579 |
575 /*****************************************************************************/ |
580 /*****************************************************************************/ |
576 |
581 |
577 /** SoE state: WRITE RESPONSE. |
582 /** SoE state: WRITE RESPONSE. |
579 void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
584 void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
580 { |
585 { |
581 ec_datagram_t *datagram = fsm->datagram; |
586 ec_datagram_t *datagram = fsm->datagram; |
582 ec_slave_t *slave = fsm->slave; |
587 ec_slave_t *slave = fsm->slave; |
583 ec_master_t *master = slave->master; |
588 ec_master_t *master = slave->master; |
|
589 ec_soe_request_t *req = fsm->request; |
584 uint8_t *data, mbox_prot, opcode, error_flag; |
590 uint8_t *data, mbox_prot, opcode, error_flag; |
585 uint16_t idn; |
591 uint16_t idn; |
586 size_t rec_size; |
592 size_t rec_size; |
587 ec_soe_request_t *req = fsm->request; |
|
588 |
593 |
589 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
594 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
590 return; // FIXME: request again? |
595 return; // FIXME: request again? |
591 |
596 |
592 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
597 if (datagram->state != EC_DATAGRAM_RECEIVED) { |