Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_soe.c
changeset 2080 42fbd117c3e3
parent 2045 ff2a13a4603c
child 2094 83e9160319ec
equal deleted inserted replaced
2079:56993027a2d0 2080:42fbd117c3e3 
   104 
   104 
   105 /** Constructor.
 
   105 /** Constructor.
 
   106  */
 
   106  */
 
   107 void ec_fsm_soe_init(
 
   107 void ec_fsm_soe_init(
 
   108         ec_fsm_soe_t *fsm, /**< finite state machine */
 
   108         ec_fsm_soe_t *fsm, /**< finite state machine */
 
   109         ec_datagram_t *datagram /**< datagram */
 
   109         ec_mailbox_t *mbox /**< mailbox */
 
   110         )
 
   110         )
 
   111 {
 
   111 {
 
   112     fsm->state = NULL;
 
   112     fsm->state = NULL;
 
   113     fsm->datagram = datagram;
 
   113     fsm->mbox = mbox;
 
   114 }
 
   114 }
 
   115 
   115 
   116 /*****************************************************************************/
 
   116 /*****************************************************************************/
 
   117 
   117 
   118 /** Destructor.
 
   118 /** Destructor.
 
   193 
   193 
   194 /** SoE state: READ START.
 
   194 /** SoE state: READ START.
 
   195  */
 
   195  */
 
   196 void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
 
   196 void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
 
   197 {
 
   197 {
 
   198     ec_datagram_t *datagram = fsm->datagram;
 
   198     ec_mailbox_t *mbox = fsm->mbox;
 
   199     ec_slave_t *slave = fsm->slave;
 
   199     ec_slave_t *slave = fsm->slave;
 
   200     ec_master_t *master = slave->master;
 
   200     ec_master_t *master = slave->master;
 
   201     ec_soe_request_t *request = fsm->request;
 
   201     ec_soe_request_t *request = fsm->request;
 
   202     uint8_t *data;
 
   202     uint8_t *data;
 
   203 
   203 
   209         fsm->state = ec_fsm_soe_error;
 
   209         fsm->state = ec_fsm_soe_error;
 
   210         ec_fsm_soe_print_error(fsm);
 
   210         ec_fsm_soe_print_error(fsm);
 
   211         return;
 
   211         return;
 
   212     }
 
   212     }
 
   213 
   213 
   214     data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
 
   214     data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,
 
   215             EC_SOE_SIZE);
 
   215             EC_SOE_SIZE);
 
   216     if (IS_ERR(data)) {
 
   216     if (IS_ERR(data)) {
 
   217         fsm->state = ec_fsm_soe_error;
 
   217         fsm->state = ec_fsm_soe_error;
 
   218         ec_fsm_soe_print_error(fsm);
 
   218         ec_fsm_soe_print_error(fsm);
 
   219         return;
 
   219         return;
 
   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     }
 
   433     }
 
   433     }
 
   434 
   434 
   435     if (incomplete) {
 
   435     if (incomplete) {
 
   436         EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
 
   436         EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
 
   437                 " at offset %zu.\n", req->data_size);
 
   437                 " at offset %zu.\n", req->data_size);
 
   438         fsm->jiffies_start = datagram->jiffies_sent;
 
   438         fsm->jiffies_start = mbox->datagram->jiffies_sent;
 
   439         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   439         ec_slave_mbox_prepare_check(slave, mbox); // can not fail.
 
   440         fsm->retries = EC_FSM_RETRIES;
 
   440         fsm->retries = EC_FSM_RETRIES;
 
   441         fsm->state = ec_fsm_soe_read_check;
 
   441         fsm->state = ec_fsm_soe_read_check;
 
   442     } else {
 
   442     } else {
 
   443         if (master->debug_level) {
 
   443         if (master->debug_level) {
 
   444             EC_SLAVE_DBG(slave, 0, "IDN data:\n");
 
   444             EC_SLAVE_DBG(slave, 0, "IDN data:\n");
 
   457  */
 
   457  */
 
   458 void ec_fsm_soe_write_next_fragment(
 
   458 void ec_fsm_soe_write_next_fragment(
 
   459         ec_fsm_soe_t *fsm /**< finite state machine */
 
   459         ec_fsm_soe_t *fsm /**< finite state machine */
 
   460         )
 
   460         )
 
   461 {
 
   461 {
 
   462     ec_datagram_t *datagram = fsm->datagram;
 
   462     ec_mailbox_t *mbox = fsm->mbox;
 
   463     ec_slave_t *slave = fsm->slave;
 
   463     ec_slave_t *slave = fsm->slave;
 
   464     ec_master_t *master = slave->master;
 
   464     ec_master_t *master = slave->master;
 
   465     ec_soe_request_t *req = fsm->request;
 
   465     ec_soe_request_t *req = fsm->request;
 
   466     uint8_t incomplete, *data;
 
   466     uint8_t incomplete, *data;
 
   467     size_t header_size, max_fragment_size, remaining_size, fragment_size;
 
   467     size_t header_size, max_fragment_size, remaining_size, fragment_size;
 
   483     fragments_left = remaining_size / fragment_size - 1;
 
   483     fragments_left = remaining_size / fragment_size - 1;
 
   484     if (remaining_size % fragment_size) {
 
   484     if (remaining_size % fragment_size) {
 
   485         fragments_left++;
 
   485         fragments_left++;
 
   486     }
 
   486     }
 
   487 
   487 
   488     data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
 
   488     data = ec_slave_mbox_prepare_send(slave, mbox, EC_MBOX_TYPE_SOE,
 
   489             EC_SOE_SIZE + fragment_size);
 
   489             EC_SOE_SIZE + fragment_size);
 
   490     if (IS_ERR(data)) {
 
   490     if (IS_ERR(data)) {
 
   491         fsm->state = ec_fsm_soe_error;
 
   491         fsm->state = ec_fsm_soe_error;
 
   492         ec_fsm_soe_print_error(fsm);
 
   492         ec_fsm_soe_print_error(fsm);
 
   493         return;
 
   493         return;
 
   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     }
etherlabmaster