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 1921 d9cf40facbc4
parent 1905 5abe24872083
child 1931 831f2d34664c
equal deleted inserted replaced
1920:d28360ee74c5 1921:d9cf40facbc4 
    83 
    83 
    84 /*****************************************************************************/
 
    84 /*****************************************************************************/
 
    85 
    85 
    86 /** Outputs an SoE error code.
 
    86 /** Outputs an SoE error code.
 
    87 */
 
    87 */
 
    88 void ec_print_soe_error(uint16_t error_code)
 
    88 void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
 
    89 {
 
    89 {
 
    90     const ec_code_msg_t *error_msg;
 
    90     const ec_code_msg_t *error_msg;
 
    91 
    91 
    92     for (error_msg = soe_error_codes; error_msg->code; error_msg++) {
 
    92     for (error_msg = soe_error_codes; error_msg->code; error_msg++) {
 
    93         if (error_msg->code == error_code) {
 
    93         if (error_msg->code == error_code) {
 
    94             EC_ERR("SoE error 0x%04X: \"%s\".\n",
 
    94             EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n",
 
    95                    error_msg->code, error_msg->message);
 
    95                    error_msg->code, error_msg->message);
 
    96             return;
 
    96             return;
 
    97         }
 
    97         }
 
    98     }
 
    98     }
 
    99 
    99 
   100     EC_ERR("Unknown SoE error 0x%04X.\n", error_code);
 
   100     EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code);
 
   101 }
 
   101 }
 
   102 
   102 
   103 /*****************************************************************************/
 
   103 /*****************************************************************************/
 
   104 
   104 
   105 /** Constructor.
 
   105 /** Constructor.
 
   174  */
 
   174  */
 
   175 void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
 
   175 void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
 
   176 {
 
   176 {
 
   177     ec_soe_request_t *request = fsm->request;
 
   177     ec_soe_request_t *request = fsm->request;
 
   178 
   178 
   179     EC_ERR("");
 
   179     EC_SLAVE_ERR(fsm->slave, "");
 
   180 
   180 
   181     if (request->dir == EC_DIR_OUTPUT) {
 
   181     if (request->dir == EC_DIR_OUTPUT) {
 
   182         printk("Writing");
 
   182         printk("Writing");
 
   183     } else {
 
   183     } else {
 
   184         printk("Reading");
 
   184         printk("Reading");
 
   185     }
 
   185     }
 
   186 
   186 
   187     printk(" IDN 0x%04X failed on slave %u.\n",
 
   187     printk(" IDN 0x%04X failed.\n", request->idn);
 
   188             request->idn, fsm->slave->ring_position);
 
       
   189 }
 
   188 }
 
   190 
   189 
   191 /******************************************************************************
 
   190 /******************************************************************************
 
   192  * SoE read state machine
 
   191  * SoE read state machine
 
   193  *****************************************************************************/
 
   192  *****************************************************************************/
 
   200     ec_slave_t *slave = fsm->slave;
 
   199     ec_slave_t *slave = fsm->slave;
 
   201     ec_master_t *master = slave->master;
 
   200     ec_master_t *master = slave->master;
 
   202     ec_soe_request_t *request = fsm->request;
 
   201     ec_soe_request_t *request = fsm->request;
 
   203     uint8_t *data;
 
   202     uint8_t *data;
 
   204 
   203 
   205     if (master->debug_level)
 
   204     EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X.\n", request->idn);
 
   206         EC_DBG("Reading IDN 0x%04X from slave %u.\n",
 
       
   207                request->idn, slave->ring_position);
 
       
   208 
   205 
   209     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
 
   206     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
 
   210         EC_ERR("Slave does not support SoE!\n");
 
   207         EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
 
   211         fsm->state = ec_fsm_soe_error;
 
   208         fsm->state = ec_fsm_soe_error;
 
   212         ec_fsm_soe_print_error(fsm);
 
   209         ec_fsm_soe_print_error(fsm);
 
   213         return;
 
   210         return;
 
   214     }
 
   211     }
 
   215 
   212 
   224     EC_WRITE_U8(data, OPCODE_READ_REQUEST);
 
   221     EC_WRITE_U8(data, OPCODE_READ_REQUEST);
 
   225     EC_WRITE_U8(data + 1, 1 << 6); // request value
 
   222     EC_WRITE_U8(data + 1, 1 << 6); // request value
 
   226     EC_WRITE_U16(data + 2, request->idn);
 
   223     EC_WRITE_U16(data + 2, request->idn);
 
   227 
   224 
   228     if (master->debug_level) {
 
   225     if (master->debug_level) {
 
   229         EC_DBG("SCC read request:\n");
 
   226         EC_SLAVE_DBG(slave, 0, "SCC read request:\n");
 
   230         ec_print_data(data, EC_SOE_SIZE);
 
   227         ec_print_data(data, EC_SOE_SIZE);
 
   231     }
 
   228     }
 
   232 
   229 
   233     fsm->request->data_size = 0;
 
   230     fsm->request->data_size = 0;
 
   234     fsm->request->jiffies_sent = jiffies;
 
   231     fsm->request->jiffies_sent = jiffies;
 
   249     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   246     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   250         return; // FIXME: check for response first?
 
   247         return; // FIXME: check for response first?
 
   251 
   248 
   252     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   249     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   253         fsm->state = ec_fsm_soe_error;
 
   250         fsm->state = ec_fsm_soe_error;
 
   254         EC_ERR("Failed to receive SoE read request: ");
 
   251         EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
 
   255         ec_datagram_print_state(datagram);
 
   252         ec_datagram_print_state(datagram);
 
   256         ec_fsm_soe_print_error(fsm);
 
   253         ec_fsm_soe_print_error(fsm);
 
   257         return;
 
   254         return;
 
   258     }
 
   255     }
 
   259 
   256 
   265                 // no response; send request datagram again
 
   262                 // no response; send request datagram again
 
   266                 return;
 
   263                 return;
 
   267             }
 
   264             }
 
   268         }
 
   265         }
 
   269         fsm->state = ec_fsm_soe_error;
 
   266         fsm->state = ec_fsm_soe_error;
 
   270         EC_ERR("Reception of SoE read request failed after %u ms: ",
 
   267         EC_SLAVE_ERR(slave, "Reception of SoE read request"
 
   271                 (u32) diff_ms);
 
   268                 " failed after %u ms: ", (u32) diff_ms);
 
   272         ec_datagram_print_wc_error(datagram);
 
   269         ec_datagram_print_wc_error(datagram);
 
   273         ec_fsm_soe_print_error(fsm);
 
   270         ec_fsm_soe_print_error(fsm);
 
   274         return;
 
   271         return;
 
   275     }
 
   272     }
 
   276 
   273 
   292     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   289     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   293         return;
 
   290         return;
 
   294 
   291 
   295     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   292     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   296         fsm->state = ec_fsm_soe_error;
 
   293         fsm->state = ec_fsm_soe_error;
 
   297         EC_ERR("Failed to receive SoE mailbox check datagram: ");
 
   294         EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
 
   298         ec_datagram_print_state(datagram);
 
   295         ec_datagram_print_state(datagram);
 
   299         ec_fsm_soe_print_error(fsm);
 
   296         ec_fsm_soe_print_error(fsm);
 
   300         return;
 
   297         return;
 
   301     }
 
   298     }
 
   302 
   299 
   303     if (datagram->working_counter != 1) {
 
   300     if (datagram->working_counter != 1) {
 
   304         fsm->state = ec_fsm_soe_error;
 
   301         fsm->state = ec_fsm_soe_error;
 
   305         EC_ERR("Reception of SoE mailbox check datagram failed: ");
 
   302         EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
 
       
   303                 " datagram failed: ");
 
   306         ec_datagram_print_wc_error(datagram);
 
   304         ec_datagram_print_wc_error(datagram);
 
   307         ec_fsm_soe_print_error(fsm);
 
   305         ec_fsm_soe_print_error(fsm);
 
   308         return;
 
   306         return;
 
   309     }
 
   307     }
 
   310 
   308 
   311     if (!ec_slave_mbox_check(datagram)) {
 
   309     if (!ec_slave_mbox_check(datagram)) {
 
   312         unsigned long diff_ms =
 
   310         unsigned long diff_ms =
 
   313             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
 
   311             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
 
   314         if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
 
   312         if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
 
   315             fsm->state = ec_fsm_soe_error;
 
   313             fsm->state = ec_fsm_soe_error;
 
   316             EC_ERR("Timeout after %u ms while waiting for read response.\n",
 
   314             EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting for"
 
   317                     (u32) diff_ms);
 
   315                     " read response.\n", (u32) diff_ms);
 
   318             ec_fsm_soe_print_error(fsm);
 
   316             ec_fsm_soe_print_error(fsm);
 
   319             return;
 
   317             return;
 
   320         }
 
   318         }
 
   321 
   319 
   322         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   320         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   347     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   345     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   348         return; // FIXME: request again?
 
   346         return; // FIXME: request again?
 
   349 
   347 
   350     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   348     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   351         fsm->state = ec_fsm_soe_error;
 
   349         fsm->state = ec_fsm_soe_error;
 
   352         EC_ERR("Failed to receive SoE read response datagram: ");
 
   350         EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
 
   353         ec_datagram_print_state(datagram);
 
   351         ec_datagram_print_state(datagram);
 
   354         ec_fsm_soe_print_error(fsm);
 
   352         ec_fsm_soe_print_error(fsm);
 
   355         return;
 
   353         return;
 
   356     }
 
   354     }
 
   357 
   355 
   358     if (datagram->working_counter != 1) {
 
   356     if (datagram->working_counter != 1) {
 
   359         fsm->state = ec_fsm_soe_error;
 
   357         fsm->state = ec_fsm_soe_error;
 
   360         EC_ERR("Reception of SoE read response failed: ");
 
   358         EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
 
   361         ec_datagram_print_wc_error(datagram);
 
   359         ec_datagram_print_wc_error(datagram);
 
   362         ec_fsm_soe_print_error(fsm);
 
   360         ec_fsm_soe_print_error(fsm);
 
   363         return;
 
   361         return;
 
   364     }
 
   362     }
 
   365 
   363 
   369         ec_fsm_soe_print_error(fsm);
 
   367         ec_fsm_soe_print_error(fsm);
 
   370         return;
 
   368         return;
 
   371     }
 
   369     }
 
   372 
   370 
   373     if (master->debug_level) {
 
   371     if (master->debug_level) {
 
   374         EC_DBG("SCC read response:\n");
 
   372         EC_SLAVE_DBG(slave, 0, "SCC read response:\n");
 
   375         ec_print_data(data, rec_size);
 
   373         ec_print_data(data, rec_size);
 
   376     }
 
   374     }
 
   377 
   375 
   378     if (mbox_prot != EC_MBOX_TYPE_SOE) {
 
   376     if (mbox_prot != EC_MBOX_TYPE_SOE) {
 
   379         fsm->state = ec_fsm_soe_error;
 
   377         fsm->state = ec_fsm_soe_error;
 
   380         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
   378         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
 
       
   379                 mbox_prot);
 
   381         ec_fsm_soe_print_error(fsm);
 
   380         ec_fsm_soe_print_error(fsm);
 
   382         return;
 
   381         return;
 
   383     }
 
   382     }
 
   384 
   383 
   385     if (rec_size < EC_SOE_SIZE) {
 
   384     if (rec_size < EC_SOE_SIZE) {
 
   386         fsm->state = ec_fsm_soe_error;
 
   385         fsm->state = ec_fsm_soe_error;
 
   387         EC_ERR("Received currupted SoE read response"
 
   386         EC_SLAVE_ERR(slave, "Received currupted SoE read response"
 
   388                 " (%zu bytes)!\n", rec_size);
 
   387                 " (%zu bytes)!\n", rec_size);
 
   389         ec_print_data(data, rec_size);
 
   388         ec_print_data(data, rec_size);
 
   390         ec_fsm_soe_print_error(fsm);
 
   389         ec_fsm_soe_print_error(fsm);
 
   391         return;
 
   390         return;
 
   392     }
 
   391     }
 
   395     opcode = header & 0x7;
 
   394     opcode = header & 0x7;
 
   396     incomplete = (header >> 3) & 1;
 
   395     incomplete = (header >> 3) & 1;
 
   397     error_flag = (header >> 4) & 1;
 
   396     error_flag = (header >> 4) & 1;
 
   398 
   397 
   399     if (opcode != OPCODE_READ_RESPONSE) {
 
   398     if (opcode != OPCODE_READ_RESPONSE) {
 
   400         EC_ERR("Received no read response (opcode %x).\n", opcode);
 
   399         EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n",
 
       
   400                 opcode);
 
   401         ec_print_data(data, rec_size);
 
   401         ec_print_data(data, rec_size);
 
   402         ec_fsm_soe_print_error(fsm);
 
   402         ec_fsm_soe_print_error(fsm);
 
   403         fsm->state = ec_fsm_soe_error;
 
   403         fsm->state = ec_fsm_soe_error;
 
   404         return;
 
   404         return;
 
   405     }
 
   405     }
 
   406 
   406 
   407     if (error_flag) {
 
   407     if (error_flag) {
 
   408         req->error_code = EC_READ_U16(data + rec_size - 2);
 
   408         req->error_code = EC_READ_U16(data + rec_size - 2);
 
   409         EC_ERR("Received error response:\n");
 
   409         EC_SLAVE_ERR(slave, "Received error response:\n");
 
   410         ec_print_soe_error(req->error_code);
 
   410         ec_print_soe_error(slave, req->error_code);
 
   411         ec_fsm_soe_print_error(fsm);
 
   411         ec_fsm_soe_print_error(fsm);
 
   412         fsm->state = ec_fsm_soe_error;
 
   412         fsm->state = ec_fsm_soe_error;
 
   413         return;
 
   413         return;
 
   414     } else {
 
   414     } else {
 
   415         req->error_code = 0x0000;
 
   415         req->error_code = 0x0000;
 
   416     }
 
   416     }
 
   417 
   417 
   418     value_included = (EC_READ_U8(data + 1) >> 6) & 1;
 
   418     value_included = (EC_READ_U8(data + 1) >> 6) & 1;
 
   419     if (!value_included) {
 
   419     if (!value_included) {
 
   420         EC_ERR("No value included!\n");
 
   420         EC_SLAVE_ERR(slave, "No value included!\n");
 
   421         ec_fsm_soe_print_error(fsm);
 
   421         ec_fsm_soe_print_error(fsm);
 
   422         fsm->state = ec_fsm_soe_error;
 
   422         fsm->state = ec_fsm_soe_error;
 
   423         return;
 
   423         return;
 
   424     }
 
   424     }
 
   425 
   425 
   430         ec_fsm_soe_print_error(fsm);
 
   430         ec_fsm_soe_print_error(fsm);
 
   431         return;
 
   431         return;
 
   432     }
 
   432     }
 
   433 
   433 
   434     if (incomplete) {
 
   434     if (incomplete) {
 
   435         if (master->debug_level) {
 
   435         EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
 
   436             EC_DBG("SoE data incomplete. Waiting for fragment"
 
   436                 " at offset %zu.\n", req->data_size);
 
   437                     " at offset %zu.\n", req->data_size);
 
       
   438         }
 
       
   439         fsm->jiffies_start = datagram->jiffies_sent;
 
   437         fsm->jiffies_start = datagram->jiffies_sent;
 
   440         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   438         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
 
   441         fsm->retries = EC_FSM_RETRIES;
 
   439         fsm->retries = EC_FSM_RETRIES;
 
   442         fsm->state = ec_fsm_soe_read_check;
 
   440         fsm->state = ec_fsm_soe_read_check;
 
   443     } else {
 
   441     } else {
 
   444         if (master->debug_level) {
 
   442         if (master->debug_level) {
 
   445             EC_DBG("IDN data:\n");
 
   443             EC_SLAVE_DBG(slave, 0, "IDN data:\n");
 
   446             ec_print_data(req->data, req->data_size);
 
   444             ec_print_data(req->data, req->data_size);
 
   447         }
 
   445         }
 
   448 
   446 
   449         fsm->state = ec_fsm_soe_end; // success
 
   447         fsm->state = ec_fsm_soe_end; // success
 
   450     }
 
   448     }
 
   468     size_t header_size, max_fragment_size, remaining_size, fragment_size;
 
   466     size_t header_size, max_fragment_size, remaining_size, fragment_size;
 
   469     uint16_t fragments_left;
 
   467     uint16_t fragments_left;
 
   470 
   468 
   471     header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
 
   469     header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
 
   472     if (slave->configured_rx_mailbox_size <= header_size) {
 
   470     if (slave->configured_rx_mailbox_size <= header_size) {
 
   473         EC_ERR("Mailbox size (%u) too small for SoE write.\n",
 
   471         EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
 
   474                 slave->configured_rx_mailbox_size);
 
   472                 slave->configured_rx_mailbox_size);
 
   475         fsm->state = ec_fsm_soe_error;
 
   473         fsm->state = ec_fsm_soe_error;
 
   476         ec_fsm_soe_print_error(fsm);
 
   474         ec_fsm_soe_print_error(fsm);
 
   477         return;
 
   475         return;
 
   478     }
 
   476     }
 
   499     EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
 
   497     EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
 
   500     memcpy(data + 4, req->data + fsm->offset, fragment_size);
 
   498     memcpy(data + 4, req->data + fsm->offset, fragment_size);
 
   501     fsm->offset += fragment_size;
 
   499     fsm->offset += fragment_size;
 
   502 
   500 
   503     if (master->debug_level) {
 
   501     if (master->debug_level) {
 
   504         EC_DBG("SCC write request:\n");
 
   502         EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
 
   505         ec_print_data(data, EC_SOE_SIZE + fragment_size);
 
   503         ec_print_data(data, EC_SOE_SIZE + fragment_size);
 
   506     }
 
   504     }
 
   507 
   505 
   508     req->jiffies_sent = jiffies;
 
   506     req->jiffies_sent = jiffies;
 
   509     fsm->retries = EC_FSM_RETRIES;
 
   507     fsm->retries = EC_FSM_RETRIES;
 
   515 /** SoE state: WRITE START.
 
   513 /** SoE state: WRITE START.
 
   516  */
 
   514  */
 
   517 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
 
   515 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
 
   518 {
 
   516 {
 
   519     ec_slave_t *slave = fsm->slave;
 
   517     ec_slave_t *slave = fsm->slave;
 
   520     ec_master_t *master = slave->master;
 
       
   521     ec_soe_request_t *req = fsm->request;
 
   518     ec_soe_request_t *req = fsm->request;
 
   522 
   519 
   523     if (master->debug_level)
 
   520     EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X (%zu byte).\n",
 
   524         EC_DBG("Writing IDN 0x%04X (%zu byte) to slave %u.\n",
 
   521             req->idn, req->data_size);
 
   525                req->idn, req->data_size, slave->ring_position);
 
       
   526 
   522 
   527     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
 
   523     if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
 
   528         EC_ERR("Slave does not support SoE!\n");
 
   524         EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
 
   529         fsm->state = ec_fsm_soe_error;
 
   525         fsm->state = ec_fsm_soe_error;
 
   530         ec_fsm_soe_print_error(fsm);
 
   526         ec_fsm_soe_print_error(fsm);
 
   531         return;
 
   527         return;
 
   532     }
 
   528     }
 
   533 
   529 
   548     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   544     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   549         return; // FIXME: check for response first?
 
   545         return; // FIXME: check for response first?
 
   550 
   546 
   551     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   547     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   552         fsm->state = ec_fsm_soe_error;
 
   548         fsm->state = ec_fsm_soe_error;
 
   553         EC_ERR("Failed to receive SoE write request: ");
 
   549         EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
 
   554         ec_datagram_print_state(datagram);
 
   550         ec_datagram_print_state(datagram);
 
   555         ec_fsm_soe_print_error(fsm);
 
   551         ec_fsm_soe_print_error(fsm);
 
   556         return;
 
   552         return;
 
   557     }
 
   553     }
 
   558 
   554 
   564                 // no response; send request datagram again
 
   560                 // no response; send request datagram again
 
   565                 return;
 
   561                 return;
 
   566             }
 
   562             }
 
   567         }
 
   563         }
 
   568         fsm->state = ec_fsm_soe_error;
 
   564         fsm->state = ec_fsm_soe_error;
 
   569         EC_ERR("Reception of SoE write request failed after %u ms: ",
 
   565         EC_SLAVE_ERR(slave, "Reception of SoE write request"
 
   570                 (u32) diff_ms);
 
   566                 " failed after %u ms: ", (u32) diff_ms);
 
   571         ec_datagram_print_wc_error(datagram);
 
   567         ec_datagram_print_wc_error(datagram);
 
   572         ec_fsm_soe_print_error(fsm);
 
   568         ec_fsm_soe_print_error(fsm);
 
   573         return;
 
   569         return;
 
   574     }
 
   570     }
 
   575 
   571 
   593     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   589     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   594         return;
 
   590         return;
 
   595 
   591 
   596     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   592     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   597         fsm->state = ec_fsm_soe_error;
 
   593         fsm->state = ec_fsm_soe_error;
 
   598         EC_ERR("Failed to receive SoE write request datagram: ");
 
   594         EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
 
   599         ec_datagram_print_state(datagram);
 
   595         ec_datagram_print_state(datagram);
 
   600         ec_fsm_soe_print_error(fsm);
 
   596         ec_fsm_soe_print_error(fsm);
 
   601         return;
 
   597         return;
 
   602     }
 
   598     }
 
   603 
   599 
   604     if (datagram->working_counter != 1) {
 
   600     if (datagram->working_counter != 1) {
 
   605         fsm->state = ec_fsm_soe_error;
 
   601         fsm->state = ec_fsm_soe_error;
 
   606         EC_ERR("Reception of SoE write request datagram: ");
 
   602         EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
 
   607         ec_datagram_print_wc_error(datagram);
 
   603         ec_datagram_print_wc_error(datagram);
 
   608         ec_fsm_soe_print_error(fsm);
 
   604         ec_fsm_soe_print_error(fsm);
 
   609         return;
 
   605         return;
 
   610     }
 
   606     }
 
   611 
   607 
   615         if (!ec_slave_mbox_check(datagram)) {
 
   611         if (!ec_slave_mbox_check(datagram)) {
 
   616             unsigned long diff_ms =
 
   612             unsigned long diff_ms =
 
   617                 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
 
   613                 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
 
   618             if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
 
   614             if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
 
   619                 fsm->state = ec_fsm_soe_error;
 
   615                 fsm->state = ec_fsm_soe_error;
 
   620                 EC_ERR("Timeout after %u ms while waiting"
 
   616                 EC_SLAVE_ERR(slave, "Timeout after %u ms while waiting"
 
   621                         " for write response.\n", (u32) diff_ms);
 
   617                         " for write response.\n", (u32) diff_ms);
 
   622                 ec_fsm_soe_print_error(fsm);
 
   618                 ec_fsm_soe_print_error(fsm);
 
   623                 return;
 
   619                 return;
 
   624             }
 
   620             }
 
   625 
   621 
   652     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   648     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
 
   653         return; // FIXME: request again?
 
   649         return; // FIXME: request again?
 
   654 
   650 
   655     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   651     if (datagram->state != EC_DATAGRAM_RECEIVED) {
 
   656         fsm->state = ec_fsm_soe_error;
 
   652         fsm->state = ec_fsm_soe_error;
 
   657         EC_ERR("Failed to receive SoE write response datagram: ");
 
   653         EC_SLAVE_ERR(slave, "Failed to receive SoE write"
 
       
   654                 " response datagram: ");
 
   658         ec_datagram_print_state(datagram);
 
   655         ec_datagram_print_state(datagram);
 
   659         ec_fsm_soe_print_error(fsm);
 
   656         ec_fsm_soe_print_error(fsm);
 
   660         return;
 
   657         return;
 
   661     }
 
   658     }
 
   662 
   659 
   663     if (datagram->working_counter != 1) {
 
   660     if (datagram->working_counter != 1) {
 
   664         fsm->state = ec_fsm_soe_error;
 
   661         fsm->state = ec_fsm_soe_error;
 
   665         EC_ERR("Reception of SoE write response failed: ");
 
   662         EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
 
   666         ec_datagram_print_wc_error(datagram);
 
   663         ec_datagram_print_wc_error(datagram);
 
   667         ec_fsm_soe_print_error(fsm);
 
   664         ec_fsm_soe_print_error(fsm);
 
   668         return;
 
   665         return;
 
   669     }
 
   666     }
 
   670 
   667 
   674         ec_fsm_soe_print_error(fsm);
 
   671         ec_fsm_soe_print_error(fsm);
 
   675         return;
 
   672         return;
 
   676     }
 
   673     }
 
   677 
   674 
   678     if (master->debug_level) {
 
   675     if (master->debug_level) {
 
   679         EC_DBG("SCC write response:\n");
 
   676         EC_SLAVE_DBG(slave, 0, "SCC write response:\n");
 
   680         ec_print_data(data, rec_size);
 
   677         ec_print_data(data, rec_size);
 
   681     }
 
   678     }
 
   682 
   679 
   683     if (mbox_prot != EC_MBOX_TYPE_SOE) {
 
   680     if (mbox_prot != EC_MBOX_TYPE_SOE) {
 
   684         fsm->state = ec_fsm_soe_error;
 
   681         fsm->state = ec_fsm_soe_error;
 
   685         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
 
   682         EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
 
       
   683                 mbox_prot);
 
   686         ec_fsm_soe_print_error(fsm);
 
   684         ec_fsm_soe_print_error(fsm);
 
   687         return;
 
   685         return;
 
   688     }
 
   686     }
 
   689 
   687 
   690     if (rec_size < EC_SOE_SIZE) {
 
   688     if (rec_size < EC_SOE_SIZE) {
 
   691         fsm->state = ec_fsm_soe_error;
 
   689         fsm->state = ec_fsm_soe_error;
 
   692         EC_ERR("Received currupted SoE write response (%zu bytes)!\n",
 
   690         EC_SLAVE_ERR(slave, "Received currupted SoE write response"
 
   693                 rec_size);
 
   691                 " (%zu bytes)!\n", rec_size);
 
   694         ec_print_data(data, rec_size);
 
   692         ec_print_data(data, rec_size);
 
   695         ec_fsm_soe_print_error(fsm);
 
   693         ec_fsm_soe_print_error(fsm);
 
   696         return;
 
   694         return;
 
   697     }
 
   695     }
 
   698 
   696 
   699     opcode = EC_READ_U8(data) & 0x7;
 
   697     opcode = EC_READ_U8(data) & 0x7;
 
   700     if (opcode != OPCODE_WRITE_RESPONSE) {
 
   698     if (opcode != OPCODE_WRITE_RESPONSE) {
 
   701         EC_ERR("Received no write response (opcode %x).\n", opcode);
 
   699         EC_SLAVE_ERR(slave, "Received no write response"
 
       
   700                 " (opcode %x).\n", opcode);
 
   702         ec_print_data(data, rec_size);
 
   701         ec_print_data(data, rec_size);
 
   703         ec_fsm_soe_print_error(fsm);
 
   702         ec_fsm_soe_print_error(fsm);
 
   704         fsm->state = ec_fsm_soe_error;
 
   703         fsm->state = ec_fsm_soe_error;
 
   705         return;
 
   704         return;
 
   706     }
 
   705     }
 
   707 
   706 
   708     idn = EC_READ_U16(data + 2);
 
   707     idn = EC_READ_U16(data + 2);
 
   709     if (idn != req->idn) {
 
   708     if (idn != req->idn) {
 
   710         EC_ERR("Received response for wrong IDN 0x%04x.\n", idn);
 
   709         EC_SLAVE_ERR(slave, "Received response for"
 
       
   710                 " wrong IDN 0x%04x.\n", idn);
 
   711         ec_print_data(data, rec_size);
 
   711         ec_print_data(data, rec_size);
 
   712         ec_fsm_soe_print_error(fsm);
 
   712         ec_fsm_soe_print_error(fsm);
 
   713         fsm->state = ec_fsm_soe_error;
 
   713         fsm->state = ec_fsm_soe_error;
 
   714         return;
 
   714         return;
 
   715     }
 
   715     }
 
   716 
   716 
   717     error_flag = (EC_READ_U8(data) >> 4) & 1;
 
   717     error_flag = (EC_READ_U8(data) >> 4) & 1;
 
   718     if (error_flag) {
 
   718     if (error_flag) {
 
   719         if (rec_size < EC_SOE_SIZE + 2) {
 
   719         if (rec_size < EC_SOE_SIZE + 2) {
 
   720             EC_ERR("Received corrupted error response - error flag set,"
 
   720             EC_SLAVE_ERR(slave, "Received corrupted error response"
 
   721                     " but received size is %zu.\n", rec_size);
 
   721                     " - error flag set, but received size is %zu.\n",
 
       
   722                     rec_size);
 
   722         } else {
 
   723         } else {
 
   723             req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
 
   724             req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
 
   724             EC_ERR("Received error response:\n");
 
   725             EC_SLAVE_ERR(slave, "Received error response:\n");
 
   725             ec_print_soe_error(req->error_code);
 
   726             ec_print_soe_error(slave, req->error_code);
 
   726         }
 
   727         }
 
   727         ec_print_data(data, rec_size);
 
   728         ec_print_data(data, rec_size);
 
   728         ec_fsm_soe_print_error(fsm);
 
   729         ec_fsm_soe_print_error(fsm);
 
   729         fsm->state = ec_fsm_soe_error;
 
   730         fsm->state = ec_fsm_soe_error;
 
   730         return;
 
   731         return;
etherlabmaster