166 /**

   166 /**

   167    Constructor.

   167    Constructor.

   168 */

   168 */

   169 

   169 

   170 void ec_fsm_coe_init(ec_fsm_coe_t *fsm, /**< finite state machine */

   170 void ec_fsm_coe_init(ec_fsm_coe_t *fsm, /**< finite state machine */

   172                      )

   172                      )

   173 {

   173 {

   174     fsm->state = NULL;

   174     fsm->state = NULL;

   176 }

   176 }

   177 

   177 

   178 /*****************************************************************************/

   178 /*****************************************************************************/

   179 

   179 

   180 /**

   180 /**

   284    CoE state: DICT START.

   284    CoE state: DICT START.

   285 */

   285 */

   286 

   286 

   287 void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)

   287 void ec_fsm_coe_dict_start(ec_fsm_coe_t *fsm /**< finite state machine */)

   288 {

   288 {

   290     ec_slave_t *slave = fsm->slave;

   290     ec_slave_t *slave = fsm->slave;

   291     uint8_t *data;

   291     uint8_t *data;

   292 

   292 

   293     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {

   293     if (!(slave->sii.mailbox_protocols & EC_MBOX_COE)) {

   294         EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");

   294         EC_SLAVE_ERR(slave, "Slave does not support CoE!\n");

   301                 " SDO information service!\n");

   301                 " SDO information service!\n");

   302         fsm->state = ec_fsm_coe_error;

   302         fsm->state = ec_fsm_coe_error;

   303         return;

   303         return;

   304     }

   304     }

   305 

   305 

   307     if (IS_ERR(data)) {

   307     if (IS_ERR(data)) {

   308         fsm->state = ec_fsm_coe_error;

   308         fsm->state = ec_fsm_coe_error;

   309         return;

   309         return;

   310     }

   310     }

   311 

   311 

   326    \todo Timeout behavior

   326    \todo Timeout behavior

   327 */

   327 */

   328 

   328 

   329 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)

   329 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */)

   330 {

   330 {

   332     ec_slave_t *slave = fsm->slave;

   333     ec_slave_t *slave = fsm->slave;

   333 

   334 

   335         return; // FIXME: request again?

   336         return; // FIXME: request again?

   336 

   337 

   338         fsm->state = ec_fsm_coe_error;

   339         fsm->state = ec_fsm_coe_error;

   339         EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   340         EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   340                 " request datagram: ");

   341                 " request datagram: ");

   341         ec_datagram_print_state(datagram);

   342         ec_datagram_print_state(datagram);

   342         return;

   343         return;

   343     }

   344     }

   344 

   345 

   346         fsm->state = ec_fsm_coe_error;

   347         fsm->state = ec_fsm_coe_error;

   347         EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");

   348         EC_SLAVE_ERR(slave, "Reception of CoE dictionary request failed: ");

   348         ec_datagram_print_wc_error(datagram);

   349         ec_datagram_print_wc_error(datagram);

   349         return;

   350         return;

   350     }

   351     }

   351 

   352 

   352     fsm->jiffies_start = datagram->jiffies_sent;

   353     fsm->jiffies_start = datagram->jiffies_sent;

   353 

   354 

   355     fsm->retries = EC_FSM_RETRIES;

   356     fsm->retries = EC_FSM_RETRIES;

   356     fsm->state = ec_fsm_coe_dict_check;

   357     fsm->state = ec_fsm_coe_dict_check;

   357 }

   358 }

   358 

   359 

   359 /*****************************************************************************/

   360 /*****************************************************************************/

   362    CoE state: DICT CHECK.

   363    CoE state: DICT CHECK.

   363 */

   364 */

   364 

   365 

   365 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)

   366 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */)

   366 {

   367 {

   368     ec_slave_t *slave = fsm->slave;

   370     ec_slave_t *slave = fsm->slave;

   369 

   371 

   374         fsm->state = ec_fsm_coe_error;

   376         fsm->state = ec_fsm_coe_error;

   375         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   377         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   376         ec_datagram_print_state(datagram);

   378         ec_datagram_print_state(datagram);

   377         return;

   379         return;

   378     }

   380     }

   379 

   381 

   381         fsm->state = ec_fsm_coe_error;

   383         fsm->state = ec_fsm_coe_error;

   382         EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"

   384         EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"

   383                 " datagram failed: ");

   385                 " datagram failed: ");

   384         ec_datagram_print_wc_error(datagram);

   386         ec_datagram_print_wc_error(datagram);

   385         return;

   387         return;

   386     }

   388     }

   387 

   389 

   389         unsigned long diff_ms =

   391         unsigned long diff_ms =

   390             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   392             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   391         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   393         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   392             fsm->state = ec_fsm_coe_error;

   394             fsm->state = ec_fsm_coe_error;

   393             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   395             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   394                     " SDO dictionary list response.\n");

   396                     " SDO dictionary list response.\n");

   395             return;

   397             return;

   396         }

   398         }

   397 

   399 

   399         fsm->retries = EC_FSM_RETRIES;

   401         fsm->retries = EC_FSM_RETRIES;

   400         return;

   402         return;

   401     }

   403     }

   402 

   404 

   403     // Fetch response

   405     // Fetch response

   405     fsm->retries = EC_FSM_RETRIES;

   407     fsm->retries = EC_FSM_RETRIES;

   406     fsm->state = ec_fsm_coe_dict_response;

   408     fsm->state = ec_fsm_coe_dict_response;

   407 }

   409 }

   408 

   410 

   409 /*****************************************************************************/

   411 /*****************************************************************************/

   413    \todo Timeout behavior

   415    \todo Timeout behavior

   414 */

   416 */

   415 

   417 

   416 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)

   418 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */)

   417 {

   419 {

   419     ec_slave_t *slave = fsm->slave;

   422     ec_slave_t *slave = fsm->slave;

   420     uint8_t *data, mbox_prot;

   423     uint8_t *data, mbox_prot;

   421     size_t rec_size;

   424     size_t rec_size;

   422     unsigned int sdo_count, i;

   425     unsigned int sdo_count, i;

   423     uint16_t sdo_index, fragments_left;

   426     uint16_t sdo_index, fragments_left;

   424     ec_sdo_t *sdo;

   427     ec_sdo_t *sdo;

   427         return; // FIXME: request again?

   432         return; // FIXME: request again?

   428 

   433 

   430         fsm->state = ec_fsm_coe_error;

   435         fsm->state = ec_fsm_coe_error;

   431         EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   436         EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"

   432                 " response datagram: ");

   437                 " response datagram: ");

   433         ec_datagram_print_state(datagram);

   438         ec_datagram_print_state(datagram);

   434         return;

   439         return;

   435     }

   440     }

   436 

   441 

   438         fsm->state = ec_fsm_coe_error;

   443         fsm->state = ec_fsm_coe_error;

   439         EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");

   444         EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");

   440         ec_datagram_print_wc_error(datagram);

   445         ec_datagram_print_wc_error(datagram);

   441         return;

   446         return;

   442     }

   447     }

   443 

   448 

   445     if (IS_ERR(data)) {

   450     if (IS_ERR(data)) {

   446         fsm->state = ec_fsm_coe_error;

   451         fsm->state = ec_fsm_coe_error;

   447         return;

   452         return;

   448     }

   453     }

   449 

   454 

   454         return;

   459         return;

   455     }

   460     }

   456 

   461 

   457     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   462     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   458         // check for CoE response again

   463         // check for CoE response again

   460         fsm->retries = EC_FSM_RETRIES;

   465         fsm->retries = EC_FSM_RETRIES;

   461         fsm->state = ec_fsm_coe_dict_check;

   466         fsm->state = ec_fsm_coe_dict_check;

   462         return;

   467         return;

   463     }

   468     }

   464 

   469 

   488         if (fsm->slave->master->debug_level) {

   493         if (fsm->slave->master->debug_level) {

   489             EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!"

   494             EC_SLAVE_DBG(slave, 1, "Invalid SDO list response!"

   490                     " Retrying...\n");

   495                     " Retrying...\n");

   491             ec_print_data(data, rec_size);

   496             ec_print_data(data, rec_size);

   492         }

   497         }

   494         fsm->retries = EC_FSM_RETRIES;

   499         fsm->retries = EC_FSM_RETRIES;

   495         fsm->state = ec_fsm_coe_dict_check;

   500         fsm->state = ec_fsm_coe_dict_check;

   496         return;

   501         return;

   497     }

   502     }

   498 

   503 

   501         ec_print_data(data, rec_size);

   506         ec_print_data(data, rec_size);

   502         fsm->state = ec_fsm_coe_error;

   507         fsm->state = ec_fsm_coe_error;

   503         return;

   508         return;

   504     }

   509     }

   505 

   510 

   507 

   515 

   508     for (i = 0; i < sdo_count; i++) {

   516     for (i = 0; i < sdo_count; i++) {

   510         if (!sdo_index) {

   518         if (!sdo_index) {

   511             EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n");

   519             EC_SLAVE_DBG(slave, 1, "SDO dictionary contains index 0x0000.\n");

   512             continue;

   520             continue;

   513         }

   521         }

   514 

   522 

   529     }

   537     }

   530 

   538 

   531     if (EC_READ_U8(data + 2) & 0x80 || fragments_left) {

   539     if (EC_READ_U8(data + 2) & 0x80 || fragments_left) {

   532         // more messages waiting. check again.

   540         // more messages waiting. check again.

   533         fsm->jiffies_start = datagram->jiffies_sent;

   541         fsm->jiffies_start = datagram->jiffies_sent;

   535         fsm->retries = EC_FSM_RETRIES;

   543         fsm->retries = EC_FSM_RETRIES;

   536         fsm->state = ec_fsm_coe_dict_check;

   544         fsm->state = ec_fsm_coe_dict_check;

   537         return;

   545         return;

   538     }

   546     }

   539 

   547 

   544     }

   552     }

   545 

   553 

   546     // fetch SDO descriptions

   554     // fetch SDO descriptions

   547     fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);

   555     fsm->sdo = list_entry(slave->sdo_dictionary.next, ec_sdo_t, list);

   548 

   556 

   550     if (IS_ERR(data)) {

   558     if (IS_ERR(data)) {

   551         fsm->state = ec_fsm_coe_error;

   559         fsm->state = ec_fsm_coe_error;

   552         return;

   560         return;

   553     }

   561     }

   554 

   562 

   569    \todo Timeout behavior

   577    \todo Timeout behavior

   570 */

   578 */

   571 

   579 

   572 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */)

   580 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */)

   573 {

   581 {

   575     ec_slave_t *slave = fsm->slave;

   584     ec_slave_t *slave = fsm->slave;

   576 

   585 

   578         return; // FIXME: check for response first?

   587         return; // FIXME: check for response first?

   579 

   588 

   581         fsm->state = ec_fsm_coe_error;

   590         fsm->state = ec_fsm_coe_error;

   582         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"

   591         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"

   583                 " description request datagram: ");

   592                 " description request datagram: ");

   584         ec_datagram_print_state(datagram);

   593         ec_datagram_print_state(datagram);

   585         return;

   594         return;

   586     }

   595     }

   587 

   596 

   589         fsm->state = ec_fsm_coe_error;

   598         fsm->state = ec_fsm_coe_error;

   590         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"

   599         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"

   591                 " request failed: ");

   600                 " request failed: ");

   592         ec_datagram_print_wc_error(datagram);

   601         ec_datagram_print_wc_error(datagram);

   593         return;

   602         return;

   594     }

   603     }

   595 

   604 

   596     fsm->jiffies_start = datagram->jiffies_sent;

   605     fsm->jiffies_start = datagram->jiffies_sent;

   597 

   606 

   599     fsm->retries = EC_FSM_RETRIES;

   608     fsm->retries = EC_FSM_RETRIES;

   600     fsm->state = ec_fsm_coe_dict_desc_check;

   609     fsm->state = ec_fsm_coe_dict_desc_check;

   601 }

   610 }

   602 

   611 

   603 /*****************************************************************************/

   612 /*****************************************************************************/

   606    CoE state: DICT DESC CHECK.

   615    CoE state: DICT DESC CHECK.

   607 */

   616 */

   608 

   617 

   609 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)

   618 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */)

   610 {

   619 {

   612     ec_slave_t *slave = fsm->slave;

   622     ec_slave_t *slave = fsm->slave;

   613 

   623 

   618         fsm->state = ec_fsm_coe_error;

   628         fsm->state = ec_fsm_coe_error;

   619         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   629         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   620         ec_datagram_print_state(datagram);

   630         ec_datagram_print_state(datagram);

   621         return;

   631         return;

   622     }

   632     }

   623 

   633 

   625         fsm->state = ec_fsm_coe_error;

   635         fsm->state = ec_fsm_coe_error;

   626         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

   636         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

   627                 " datagram failed: ");

   637                 " datagram failed: ");

   628         ec_datagram_print_wc_error(datagram);

   638         ec_datagram_print_wc_error(datagram);

   629         return;

   639         return;

   630     }

   640     }

   631 

   641 

   633         unsigned long diff_ms =

   643         unsigned long diff_ms =

   634             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   644             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   635         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   645         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   636             fsm->state = ec_fsm_coe_error;

   646             fsm->state = ec_fsm_coe_error;

   637             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   647             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   638                     " SDO 0x%04x object description response.\n",

   648                     " SDO 0x%04x object description response.\n",

   639                     fsm->sdo->index);

   649                     fsm->sdo->index);

   640             return;

   650             return;

   641         }

   651         }

   642 

   652 

   644         fsm->retries = EC_FSM_RETRIES;

   654         fsm->retries = EC_FSM_RETRIES;

   645         return;

   655         return;

   646     }

   656     }

   647 

   657 

   648     // Fetch response

   658     // Fetch response

   650     fsm->retries = EC_FSM_RETRIES;

   660     fsm->retries = EC_FSM_RETRIES;

   651     fsm->state = ec_fsm_coe_dict_desc_response;

   661     fsm->state = ec_fsm_coe_dict_desc_response;

   652 }

   662 }

   653 

   663 

   654 /*****************************************************************************/

   664 /*****************************************************************************/

   659 */

   669 */

   660 

   670 

   661 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm

   671 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm

   662                                    /**< finite state machine */)

   672                                    /**< finite state machine */)

   663 {

   673 {

   665     ec_slave_t *slave = fsm->slave;

   676     ec_slave_t *slave = fsm->slave;

   666     ec_sdo_t *sdo = fsm->sdo;

   677     ec_sdo_t *sdo = fsm->sdo;

   667     uint8_t *data, mbox_prot;

   678     uint8_t *data, mbox_prot;

   668     size_t rec_size, name_size;

   679     size_t rec_size, name_size;

   669 

   680 

   671         return; // FIXME: request again?

   682         return; // FIXME: request again?

   672 

   683 

   674         fsm->state = ec_fsm_coe_error;

   685         fsm->state = ec_fsm_coe_error;

   675         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"

   686         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"

   676                 " response datagram: ");

   687                 " response datagram: ");

   677         ec_datagram_print_state(datagram);

   688         ec_datagram_print_state(datagram);

   678         return;

   689         return;

   679     }

   690     }

   680 

   691 

   682         fsm->state = ec_fsm_coe_error;

   693         fsm->state = ec_fsm_coe_error;

   683         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"

   694         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"

   684                 " response failed: ");

   695                 " response failed: ");

   685         ec_datagram_print_wc_error(datagram);

   696         ec_datagram_print_wc_error(datagram);

   686         return;

   697         return;

   687     }

   698     }

   688 

   699 

   690     if (IS_ERR(data)) {

   701     if (IS_ERR(data)) {

   691         fsm->state = ec_fsm_coe_error;

   702         fsm->state = ec_fsm_coe_error;

   692         return;

   703         return;

   693     }

   704     }

   694 

   705 

   699         return;

   710         return;

   700     }

   711     }

   701 

   712 

   702     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   713     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   703         // check for CoE response again

   714         // check for CoE response again

   705         fsm->retries = EC_FSM_RETRIES;

   716         fsm->retries = EC_FSM_RETRIES;

   706         fsm->state = ec_fsm_coe_dict_desc_check;

   717         fsm->state = ec_fsm_coe_dict_desc_check;

   707         return;

   718         return;

   708     }

   719     }

   709 

   720 

   737             EC_SLAVE_DBG(slave, 1, "Invalid object description response while"

   748             EC_SLAVE_DBG(slave, 1, "Invalid object description response while"

   738                     " fetching SDO 0x%04X!\n", sdo->index);

   749                     " fetching SDO 0x%04X!\n", sdo->index);

   739             ec_print_data(data, rec_size);

   750             ec_print_data(data, rec_size);

   740         }

   751         }

   741         // check for CoE response again

   752         // check for CoE response again

   743         fsm->retries = EC_FSM_RETRIES;

   754         fsm->retries = EC_FSM_RETRIES;

   744         fsm->state = ec_fsm_coe_dict_desc_check;

   755         fsm->state = ec_fsm_coe_dict_desc_check;

   745         return;

   756         return;

   746     }

   757     }

   747 

   758 

   775 

   786 

   776     // start fetching entries

   787     // start fetching entries

   777 

   788 

   778     fsm->subindex = 0;

   789     fsm->subindex = 0;

   779 

   790 

   781     if (IS_ERR(data)) {

   792     if (IS_ERR(data)) {

   782         fsm->state = ec_fsm_coe_error;

   793         fsm->state = ec_fsm_coe_error;

   783         return;

   794         return;

   784     }

   795     }

   785 

   796 

   803 */

   814 */

   804 

   815 

   805 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm

   816 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm

   806                                    /**< finite state machine */)

   817                                    /**< finite state machine */)

   807 {

   818 {

   809     ec_slave_t *slave = fsm->slave;

   821     ec_slave_t *slave = fsm->slave;

   810 

   822 

   812         return; // FIXME: check for response first?

   824         return; // FIXME: check for response first?

   813 

   825 

   815         fsm->state = ec_fsm_coe_error;

   827         fsm->state = ec_fsm_coe_error;

   816         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry"

   828         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO entry"

   817                 " request datagram: ");

   829                 " request datagram: ");

   818         ec_datagram_print_state(datagram);

   830         ec_datagram_print_state(datagram);

   819         return;

   831         return;

   820     }

   832     }

   821 

   833 

   823         fsm->state = ec_fsm_coe_error;

   835         fsm->state = ec_fsm_coe_error;

   824         EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: ");

   836         EC_SLAVE_ERR(slave, "Reception of CoE SDO entry request failed: ");

   825         ec_datagram_print_wc_error(datagram);

   837         ec_datagram_print_wc_error(datagram);

   826         return;

   838         return;

   827     }

   839     }

   828 

   840 

   829     fsm->jiffies_start = datagram->jiffies_sent;

   841     fsm->jiffies_start = datagram->jiffies_sent;

   830 

   842 

   832     fsm->retries = EC_FSM_RETRIES;

   844     fsm->retries = EC_FSM_RETRIES;

   833     fsm->state = ec_fsm_coe_dict_entry_check;

   845     fsm->state = ec_fsm_coe_dict_entry_check;

   834 }

   846 }

   835 

   847 

   836 /*****************************************************************************/

   848 /*****************************************************************************/

   840 */

   852 */

   841 

   853 

   842 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm

   854 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm

   843                                  /**< finite state machine */)

   855                                  /**< finite state machine */)

   844 {

   856 {

   846     ec_slave_t *slave = fsm->slave;

   859     ec_slave_t *slave = fsm->slave;

   847 

   860 

   852         fsm->state = ec_fsm_coe_error;

   865         fsm->state = ec_fsm_coe_error;

   853         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   866         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

   854         ec_datagram_print_state(datagram);

   867         ec_datagram_print_state(datagram);

   855         return;

   868         return;

   856     }

   869     }

   857 

   870 

   859         fsm->state = ec_fsm_coe_error;

   872         fsm->state = ec_fsm_coe_error;

   860         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

   873         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

   861                 " datagram failed: ");

   874                 " datagram failed: ");

   862         ec_datagram_print_wc_error(datagram);

   875         ec_datagram_print_wc_error(datagram);

   863         return;

   876         return;

   864     }

   877     }

   865 

   878 

   867         unsigned long diff_ms =

   880         unsigned long diff_ms =

   868             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   881             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   869         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   882         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   870             fsm->state = ec_fsm_coe_error;

   883             fsm->state = ec_fsm_coe_error;

   871             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   884             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   872                     " SDO entry 0x%04x:%x description response.\n",

   885                     " SDO entry 0x%04x:%x description response.\n",

   873                     fsm->sdo->index, fsm->subindex);

   886                     fsm->sdo->index, fsm->subindex);

   874             return;

   887             return;

   875         }

   888         }

   876 

   889 

   878         fsm->retries = EC_FSM_RETRIES;

   891         fsm->retries = EC_FSM_RETRIES;

   879         return;

   892         return;

   880     }

   893     }

   881 

   894 

   882     // Fetch response

   895     // Fetch response

   884     fsm->retries = EC_FSM_RETRIES;

   897     fsm->retries = EC_FSM_RETRIES;

   885     fsm->state = ec_fsm_coe_dict_entry_response;

   898     fsm->state = ec_fsm_coe_dict_entry_response;

   886 }

   899 }

   887 

   900 

   888 /*****************************************************************************/

   901 /*****************************************************************************/

   893 */

   906 */

   894 

   907 

   895 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm

   908 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm

   896                                     /**< finite state machine */)

   909                                     /**< finite state machine */)

   897 {

   910 {

   899     ec_slave_t *slave = fsm->slave;

   913     ec_slave_t *slave = fsm->slave;

   900     ec_sdo_t *sdo = fsm->sdo;

   914     ec_sdo_t *sdo = fsm->sdo;

   901     uint8_t *data, mbox_prot;

   915     uint8_t *data, mbox_prot;

   902     size_t rec_size, data_size;

   916     size_t rec_size, data_size;

   903     ec_sdo_entry_t *entry;

   917     ec_sdo_entry_t *entry;

   904     u16 word;

   918     u16 word;

   905 

   919 

   907         return; // FIXME: request again?

   921         return; // FIXME: request again?

   908 

   922 

   910         fsm->state = ec_fsm_coe_error;

   924         fsm->state = ec_fsm_coe_error;

   911         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"

   925         EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"

   912                 " description response datagram: ");

   926                 " description response datagram: ");

   913         ec_datagram_print_state(datagram);

   927         ec_datagram_print_state(datagram);

   914         return;

   928         return;

   915     }

   929     }

   916 

   930 

   918         fsm->state = ec_fsm_coe_error;

   932         fsm->state = ec_fsm_coe_error;

   919         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"

   933         EC_SLAVE_ERR(slave, "Reception of CoE SDO description"

   920                 " response failed: ");

   934                 " response failed: ");

   921         ec_datagram_print_wc_error(datagram);

   935         ec_datagram_print_wc_error(datagram);

   922         return;

   936         return;

   923     }

   937     }

   924 

   938 

   926     if (IS_ERR(data)) {

   940     if (IS_ERR(data)) {

   927         fsm->state = ec_fsm_coe_error;

   941         fsm->state = ec_fsm_coe_error;

   928         return;

   942         return;

   929     }

   943     }

   930 

   944 

   935         return;

   949         return;

   936     }

   950     }

   937 

   951 

   938     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   952     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

   939         // check for CoE response again

   953         // check for CoE response again

   941         fsm->retries = EC_FSM_RETRIES;

   955         fsm->retries = EC_FSM_RETRIES;

   942         fsm->state = ec_fsm_coe_dict_entry_check;

   956         fsm->state = ec_fsm_coe_dict_entry_check;

   943         return;

   957         return;

   944     }

   958     }

   945 

   959 

   976                     " fetching SDO entry 0x%04X:%02X!\n",

   990                     " fetching SDO entry 0x%04X:%02X!\n",

   977                     sdo->index, fsm->subindex);

   991                     sdo->index, fsm->subindex);

   978             ec_print_data(data, rec_size);

   992             ec_print_data(data, rec_size);

   979         }

   993         }

   980         // check for CoE response again

   994         // check for CoE response again

   982         fsm->retries = EC_FSM_RETRIES;

   996         fsm->retries = EC_FSM_RETRIES;

   983         fsm->state = ec_fsm_coe_dict_entry_check;

   997         fsm->state = ec_fsm_coe_dict_entry_check;

   984         return;

   998         return;

   985     }

   999     }

   986 

  1000 

  1028     list_add_tail(&entry->list, &sdo->entries);

  1042     list_add_tail(&entry->list, &sdo->entries);

  1029 

  1043 

  1030     if (fsm->subindex < sdo->max_subindex) {

  1044     if (fsm->subindex < sdo->max_subindex) {

  1031         fsm->subindex++;

  1045         fsm->subindex++;

  1032 

  1046 

  1034         if (IS_ERR(data)) {

  1048         if (IS_ERR(data)) {

  1035             fsm->state = ec_fsm_coe_error;

  1049             fsm->state = ec_fsm_coe_error;

  1036             return;

  1050             return;

  1037         }

  1051         }

  1038 

  1052 

  1051 

  1065 

  1052     // another SDO description to fetch?

  1066     // another SDO description to fetch?

  1053     if (fsm->sdo->list.next != &slave->sdo_dictionary) {

  1067     if (fsm->sdo->list.next != &slave->sdo_dictionary) {

  1054         fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list);

  1068         fsm->sdo = list_entry(fsm->sdo->list.next, ec_sdo_t, list);

  1055 

  1069 

  1057         if (IS_ERR(data)) {

  1071         if (IS_ERR(data)) {

  1058             fsm->state = ec_fsm_coe_error;

  1072             fsm->state = ec_fsm_coe_error;

  1059             return;

  1073             return;

  1060         }

  1074         }

  1061 

  1075 

  1081  */

  1095  */

  1082 void ec_fsm_coe_down_start(

  1096 void ec_fsm_coe_down_start(

  1083         ec_fsm_coe_t *fsm /**< finite state machine */

  1097         ec_fsm_coe_t *fsm /**< finite state machine */

  1084         )

  1098         )

  1085 {

  1099 {

  1087     ec_slave_t *slave = fsm->slave;

  1101     ec_slave_t *slave = fsm->slave;

  1088     ec_sdo_request_t *request = fsm->request;

  1102     ec_sdo_request_t *request = fsm->request;

  1089     uint8_t *data;

  1103     uint8_t *data;

  1090     uint8_t data_set_size;

  1104     uint8_t data_set_size;

  1091 

  1105 

  1115         fsm->state = ec_fsm_coe_error;

  1129         fsm->state = ec_fsm_coe_error;

  1116         return;

  1130         return;

  1117     }

  1131     }

  1118 

  1132 

  1119     if (request->data_size <= 4) { // use expedited transfer type

  1133     if (request->data_size <= 4) { // use expedited transfer type

  1121                 EC_COE_DOWN_REQ_HEADER_SIZE);

  1135                 EC_COE_DOWN_REQ_HEADER_SIZE);

  1122         if (IS_ERR(data)) {

  1136         if (IS_ERR(data)) {

  1123             request->errno = PTR_ERR(data);

  1137             request->errno = PTR_ERR(data);

  1124             fsm->state = ec_fsm_coe_error;

  1138             fsm->state = ec_fsm_coe_error;

  1125             return;

  1139             return;

  1157             data_size = max_data_size;

  1171             data_size = max_data_size;

  1158         } else {

  1172         } else {

  1159             data_size = required_data_size;

  1173             data_size = required_data_size;

  1160         }

  1174         }

  1161 

  1175 

  1163                 data_size);

  1177                 data_size);

  1164         if (IS_ERR(data)) {

  1178         if (IS_ERR(data)) {

  1165             request->errno = PTR_ERR(data);

  1179             request->errno = PTR_ERR(data);

  1166             fsm->state = ec_fsm_coe_error;

  1180             fsm->state = ec_fsm_coe_error;

  1167             return;

  1181             return;

  1206    \todo Timeout behavior

  1220    \todo Timeout behavior

  1207 */

  1221 */

  1208 

  1222 

  1209 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)

  1223 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */)

  1210 {

  1224 {

  1212     ec_slave_t *slave = fsm->slave;

  1227     ec_slave_t *slave = fsm->slave;

  1213     unsigned long diff_ms;

  1228     unsigned long diff_ms;

  1214 

  1229 

  1216         return; // FIXME: check for response first?

  1231         return; // FIXME: check for response first?

  1217 

  1232 

  1219         fsm->request->errno = EIO;

  1234         fsm->request->errno = EIO;

  1220         fsm->state = ec_fsm_coe_error;

  1235         fsm->state = ec_fsm_coe_error;

  1221         EC_SLAVE_ERR(slave, "Failed to receive CoE download"

  1236         EC_SLAVE_ERR(slave, "Failed to receive CoE download"

  1222                 " request datagram: ");

  1237                 " request datagram: ");

  1223         ec_datagram_print_state(datagram);

  1238         ec_datagram_print_state(datagram);

  1224         return;

  1239         return;

  1225     }

  1240     }

  1226 

  1241 

  1227     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

  1242     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

  1228 

  1243 

  1231             if (diff_ms < fsm->request->response_timeout) {

  1246             if (diff_ms < fsm->request->response_timeout) {

  1232 #if DEBUG_RETRIES

  1247 #if DEBUG_RETRIES

  1233                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO"

  1248                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to SDO"

  1234                         " download request. Retrying after %lu ms...\n",

  1249                         " download request. Retrying after %lu ms...\n",

  1235                         diff_ms);

  1250                         diff_ms);

  1254     }

  1269     }

  1255 #endif

  1270 #endif

  1256 

  1271 

  1257     fsm->jiffies_start = datagram->jiffies_sent;

  1272     fsm->jiffies_start = datagram->jiffies_sent;

  1258 

  1273 

  1260     fsm->retries = EC_FSM_RETRIES;

  1275     fsm->retries = EC_FSM_RETRIES;

  1261     fsm->state = ec_fsm_coe_down_check;

  1276     fsm->state = ec_fsm_coe_down_check;

  1262 }

  1277 }

  1263 

  1278 

  1264 /*****************************************************************************/

  1279 /*****************************************************************************/

  1265 

  1280 

  1266 /** CoE state: DOWN CHECK.

  1281 /** CoE state: DOWN CHECK.

  1267  */

  1282  */

  1268 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1283 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1269 {

  1284 {

  1271     ec_slave_t *slave = fsm->slave;

  1287     ec_slave_t *slave = fsm->slave;

  1272 

  1288 

  1277         fsm->request->errno = EIO;

  1293         fsm->request->errno = EIO;

  1278         fsm->state = ec_fsm_coe_error;

  1294         fsm->state = ec_fsm_coe_error;

  1279         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"

  1295         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"

  1280                 " datagram: ");

  1296                 " datagram: ");

  1281         ec_datagram_print_state(datagram);

  1297         ec_datagram_print_state(datagram);

  1282         return;

  1298         return;

  1283     }

  1299     }

  1284 

  1300 

  1286         fsm->request->errno = EIO;

  1302         fsm->request->errno = EIO;

  1287         fsm->state = ec_fsm_coe_error;

  1303         fsm->state = ec_fsm_coe_error;

  1288         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

  1304         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

  1289                 " datagram failed: ");

  1305                 " datagram failed: ");

  1290         ec_datagram_print_wc_error(datagram);

  1306         ec_datagram_print_wc_error(datagram);

  1291         return;

  1307         return;

  1292     }

  1308     }

  1293 

  1309 

  1295         unsigned long diff_ms =

  1311         unsigned long diff_ms =

  1296             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1312             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1297         if (diff_ms >= fsm->request->response_timeout) {

  1313         if (diff_ms >= fsm->request->response_timeout) {

  1298             fsm->request->errno = EIO;

  1314             fsm->request->errno = EIO;

  1299             fsm->state = ec_fsm_coe_error;

  1315             fsm->state = ec_fsm_coe_error;

  1301                     " for SDO 0x%04x:%x download response.\n", diff_ms,

  1317                     " for SDO 0x%04x:%x download response.\n", diff_ms,

  1302                     fsm->request->index, fsm->request->subindex);

  1318                     fsm->request->index, fsm->request->subindex);

  1303             return;

  1319             return;

  1304         }

  1320         }

  1305 

  1321 

  1307         fsm->retries = EC_FSM_RETRIES;

  1323         fsm->retries = EC_FSM_RETRIES;

  1308         return;

  1324         return;

  1309     }

  1325     }

  1310 

  1326 

  1311     // Fetch response

  1327     // Fetch response

  1313     fsm->retries = EC_FSM_RETRIES;

  1329     fsm->retries = EC_FSM_RETRIES;

  1314     fsm->state = ec_fsm_coe_down_response;

  1330     fsm->state = ec_fsm_coe_down_response;

  1315 }

  1331 }

  1316 

  1332 

  1317 /*****************************************************************************/

  1333 /*****************************************************************************/

  1320  */

  1336  */

  1321 void ec_fsm_coe_down_prepare_segment_request(

  1337 void ec_fsm_coe_down_prepare_segment_request(

  1322         ec_fsm_coe_t *fsm /**< finite state machine */

  1338         ec_fsm_coe_t *fsm /**< finite state machine */

  1323         )

  1339         )

  1324 {

  1340 {

  1326     ec_slave_t *slave = fsm->slave;

  1342     ec_slave_t *slave = fsm->slave;

  1327     ec_sdo_request_t *request = fsm->request;

  1343     ec_sdo_request_t *request = fsm->request;

  1328     size_t max_segment_size =

  1344     size_t max_segment_size =

  1329         slave->configured_rx_mailbox_size

  1345         slave->configured_rx_mailbox_size

  1330         - EC_MBOX_HEADER_SIZE

  1346         - EC_MBOX_HEADER_SIZE

  1347         seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size;

  1363         seg_data_size = EC_COE_DOWN_SEG_MIN_DATA_SIZE - segment_size;

  1348         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE

  1364         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE

  1349             + EC_COE_DOWN_SEG_MIN_DATA_SIZE;

  1365             + EC_COE_DOWN_SEG_MIN_DATA_SIZE;

  1350     }

  1366     }

  1351 

  1367 

  1353             data_size);

  1369             data_size);

  1354     if (IS_ERR(data)) {

  1370     if (IS_ERR(data)) {

  1355         request->errno = PTR_ERR(data);

  1371         request->errno = PTR_ERR(data);

  1356         fsm->state = ec_fsm_coe_error;

  1372         fsm->state = ec_fsm_coe_error;

  1357         return;

  1373         return;

  1387    \todo Timeout behavior

  1403    \todo Timeout behavior

  1388 */

  1404 */

  1389 

  1405 

  1390 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)

  1406 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */)

  1391 {

  1407 {

  1393     ec_slave_t *slave = fsm->slave;

  1410     ec_slave_t *slave = fsm->slave;

  1394     uint8_t *data, mbox_prot;

  1411     uint8_t *data, mbox_prot;

  1395     size_t rec_size;

  1412     size_t rec_size;

  1396     ec_sdo_request_t *request = fsm->request;

  1413     ec_sdo_request_t *request = fsm->request;

  1397 

  1414 

  1399         return; // FIXME: request again?

  1416         return; // FIXME: request again?

  1400 

  1417 

  1402         request->errno = EIO;

  1419         request->errno = EIO;

  1403         fsm->state = ec_fsm_coe_error;

  1420         fsm->state = ec_fsm_coe_error;

  1404         EC_SLAVE_ERR(slave, "Failed to receive CoE download"

  1421         EC_SLAVE_ERR(slave, "Failed to receive CoE download"

  1405                 " response datagram: ");

  1422                 " response datagram: ");

  1406         ec_datagram_print_state(datagram);

  1423         ec_datagram_print_state(datagram);

  1407         return;

  1424         return;

  1408     }

  1425     }

  1409 

  1426 

  1411         request->errno = EIO;

  1428         request->errno = EIO;

  1412         fsm->state = ec_fsm_coe_error;

  1429         fsm->state = ec_fsm_coe_error;

  1413         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");

  1430         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");

  1414         ec_datagram_print_wc_error(datagram);

  1431         ec_datagram_print_wc_error(datagram);

  1415         return;

  1432         return;

  1416     }

  1433     }

  1417 

  1434 

  1419     if (IS_ERR(data)) {

  1436     if (IS_ERR(data)) {

  1420         request->errno = PTR_ERR(data);

  1437         request->errno = PTR_ERR(data);

  1421         fsm->state = ec_fsm_coe_error;

  1438         fsm->state = ec_fsm_coe_error;

  1422         return;

  1439         return;

  1423     }

  1440     }

  1430         return;

  1447         return;

  1431     }

  1448     }

  1432 

  1449 

  1433     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1450     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1434         // check for CoE response again

  1451         // check for CoE response again

  1436         fsm->retries = EC_FSM_RETRIES;

  1453         fsm->retries = EC_FSM_RETRIES;

  1437         fsm->state = ec_fsm_coe_down_check;

  1454         fsm->state = ec_fsm_coe_down_check;

  1438         return;

  1455         return;

  1439     }

  1456     }

  1440 

  1457 

  1482             EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!"

  1499             EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!"

  1483                     " Retrying...\n");

  1500                     " Retrying...\n");

  1484             ec_print_data(data, rec_size);

  1501             ec_print_data(data, rec_size);

  1485         }

  1502         }

  1486         // check for CoE response again

  1503         // check for CoE response again

  1488         fsm->retries = EC_FSM_RETRIES;

  1505         fsm->retries = EC_FSM_RETRIES;

  1489         fsm->state = ec_fsm_coe_down_check;

  1506         fsm->state = ec_fsm_coe_down_check;

  1490         return;

  1507         return;

  1491     }

  1508     }

  1492 

  1509 

  1504    CoE state: DOWN SEG CHECK.

  1521    CoE state: DOWN SEG CHECK.

  1505 */

  1522 */

  1506 

  1523 

  1507 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1524 void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1508 {

  1525 {

  1510     ec_slave_t *slave = fsm->slave;

  1528     ec_slave_t *slave = fsm->slave;

  1511 

  1529 

  1516         fsm->request->errno = EIO;

  1534         fsm->request->errno = EIO;

  1517         fsm->state = ec_fsm_coe_error;

  1535         fsm->state = ec_fsm_coe_error;

  1518         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

  1536         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

  1519         ec_datagram_print_state(datagram);

  1537         ec_datagram_print_state(datagram);

  1520         return;

  1538         return;

  1521     }

  1539     }

  1522 

  1540 

  1524         fsm->request->errno = EIO;

  1542         fsm->request->errno = EIO;

  1525         fsm->state = ec_fsm_coe_error;

  1543         fsm->state = ec_fsm_coe_error;

  1526         EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"

  1544         EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"

  1527                 " datagram failed: ");

  1545                 " datagram failed: ");

  1528         ec_datagram_print_wc_error(datagram);

  1546         ec_datagram_print_wc_error(datagram);

  1529         return;

  1547         return;

  1530     }

  1548     }

  1531 

  1549 

  1533         unsigned long diff_ms =

  1551         unsigned long diff_ms =

  1534             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1552             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1535         if (diff_ms >= fsm->request->response_timeout) {

  1553         if (diff_ms >= fsm->request->response_timeout) {

  1536             fsm->request->errno = EIO;

  1554             fsm->request->errno = EIO;

  1537             fsm->state = ec_fsm_coe_error;

  1555             fsm->state = ec_fsm_coe_error;

  1538             EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download"

  1556             EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download"

  1539                     " segment response.\n");

  1557                     " segment response.\n");

  1540             return;

  1558             return;

  1541         }

  1559         }

  1542 

  1560 

  1544         fsm->retries = EC_FSM_RETRIES;

  1562         fsm->retries = EC_FSM_RETRIES;

  1545         return;

  1563         return;

  1546     }

  1564     }

  1547 

  1565 

  1548     // Fetch response

  1566     // Fetch response

  1550     fsm->retries = EC_FSM_RETRIES;

  1568     fsm->retries = EC_FSM_RETRIES;

  1551     fsm->state = ec_fsm_coe_down_seg_response;

  1569     fsm->state = ec_fsm_coe_down_seg_response;

  1552 }

  1570 }

  1553 

  1571 

  1554 /*****************************************************************************/

  1572 /*****************************************************************************/

  1560 

  1578 

  1561 void ec_fsm_coe_down_seg_response(

  1579 void ec_fsm_coe_down_seg_response(

  1562         ec_fsm_coe_t *fsm /**< finite state machine */

  1580         ec_fsm_coe_t *fsm /**< finite state machine */

  1563         )

  1581         )

  1564 {

  1582 {

  1566     ec_slave_t *slave = fsm->slave;

  1585     ec_slave_t *slave = fsm->slave;

  1567     uint8_t *data, mbox_prot;

  1586     uint8_t *data, mbox_prot;

  1568     size_t rec_size;

  1587     size_t rec_size;

  1569     ec_sdo_request_t *request = fsm->request;

  1588     ec_sdo_request_t *request = fsm->request;

  1570 

  1589 

  1572         return; // FIXME: request again?

  1591         return; // FIXME: request again?

  1573 

  1592 

  1575         request->errno = EIO;

  1594         request->errno = EIO;

  1576         fsm->state = ec_fsm_coe_error;

  1595         fsm->state = ec_fsm_coe_error;

  1577         EC_SLAVE_ERR(slave, "Failed to receive CoE download response"

  1596         EC_SLAVE_ERR(slave, "Failed to receive CoE download response"

  1578                 " datagram: ");

  1597                 " datagram: ");

  1579         ec_datagram_print_state(datagram);

  1598         ec_datagram_print_state(datagram);

  1580         return;

  1599         return;

  1581     }

  1600     }

  1582 

  1601 

  1584         request->errno = EIO;

  1603         request->errno = EIO;

  1585         fsm->state = ec_fsm_coe_error;

  1604         fsm->state = ec_fsm_coe_error;

  1586         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");

  1605         EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");

  1587         ec_datagram_print_wc_error(datagram);

  1606         ec_datagram_print_wc_error(datagram);

  1588         return;

  1607         return;

  1589     }

  1608     }

  1590 

  1609 

  1592     if (IS_ERR(data)) {

  1611     if (IS_ERR(data)) {

  1593         request->errno = PTR_ERR(data);

  1612         request->errno = PTR_ERR(data);

  1594         fsm->state = ec_fsm_coe_error;

  1613         fsm->state = ec_fsm_coe_error;

  1595         return;

  1614         return;

  1596     }

  1615     }

  1603         return;

  1622         return;

  1604     }

  1623     }

  1605 

  1624 

  1606     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1625     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1607         // check for CoE response again

  1626         // check for CoE response again

  1609         fsm->retries = EC_FSM_RETRIES;

  1628         fsm->retries = EC_FSM_RETRIES;

  1610         fsm->state = ec_fsm_coe_down_check;

  1629         fsm->state = ec_fsm_coe_down_check;

  1611         return;

  1630         return;

  1612     }

  1631     }

  1613 

  1632 

  1653             EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!"

  1672             EC_SLAVE_DBG(slave, 1, "Invalid SDO download response!"

  1654                     " Retrying...\n");

  1673                     " Retrying...\n");

  1655             ec_print_data(data, rec_size);

  1674             ec_print_data(data, rec_size);

  1656         }

  1675         }

  1657         // check for CoE response again

  1676         // check for CoE response again

  1659         fsm->retries = EC_FSM_RETRIES;

  1678         fsm->retries = EC_FSM_RETRIES;

  1660         fsm->state = ec_fsm_coe_down_seg_check;

  1679         fsm->state = ec_fsm_coe_down_seg_check;

  1661         return;

  1680         return;

  1662     }

  1681     }

  1663 

  1682 

  1684    CoE state: UP START.

  1703    CoE state: UP START.

  1685 */

  1704 */

  1686 

  1705 

  1687 void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)

  1706 void ec_fsm_coe_up_start(ec_fsm_coe_t *fsm /**< finite state machine */)

  1688 {

  1707 {

  1690     ec_slave_t *slave = fsm->slave;

  1709     ec_slave_t *slave = fsm->slave;

  1691     ec_master_t *master = slave->master;

  1710     ec_master_t *master = slave->master;

  1692     ec_sdo_request_t *request = fsm->request;

  1711     ec_sdo_request_t *request = fsm->request;

  1693     uint8_t *data;

  1712     uint8_t *data;

  1694 

  1713 

  1700         request->errno = EPROTONOSUPPORT;

  1719         request->errno = EPROTONOSUPPORT;

  1701         fsm->state = ec_fsm_coe_error;

  1720         fsm->state = ec_fsm_coe_error;

  1702         return;

  1721         return;

  1703     }

  1722     }

  1704 

  1723 

  1706     if (IS_ERR(data)) {

  1725     if (IS_ERR(data)) {

  1707         request->errno = PTR_ERR(data);

  1726         request->errno = PTR_ERR(data);

  1708         fsm->state = ec_fsm_coe_error;

  1727         fsm->state = ec_fsm_coe_error;

  1709         return;

  1728         return;

  1710     }

  1729     }

  1732    \todo Timeout behavior

  1751    \todo Timeout behavior

  1733 */

  1752 */

  1734 

  1753 

  1735 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)

  1754 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */)

  1736 {

  1755 {

  1738     ec_slave_t *slave = fsm->slave;

  1758     ec_slave_t *slave = fsm->slave;

  1739     unsigned long diff_ms;

  1759     unsigned long diff_ms;

  1740 

  1760 

  1742         return; // FIXME: check for response first?

  1762         return; // FIXME: check for response first?

  1743 

  1763 

  1745         fsm->request->errno = EIO;

  1765         fsm->request->errno = EIO;

  1746         fsm->state = ec_fsm_coe_error;

  1766         fsm->state = ec_fsm_coe_error;

  1747         EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: ");

  1767         EC_SLAVE_ERR(slave, "Failed to receive CoE upload request: ");

  1748         ec_datagram_print_state(datagram);

  1768         ec_datagram_print_state(datagram);

  1749         return;

  1769         return;

  1750     }

  1770     }

  1751 

  1771 

  1752     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

  1772     diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

  1753 

  1773 

  1756             if (diff_ms < fsm->request->response_timeout) {

  1776             if (diff_ms < fsm->request->response_timeout) {

  1757 #if DEBUG_RETRIES

  1777 #if DEBUG_RETRIES

  1758                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to"

  1778                 EC_SLAVE_DBG(slave, 1, "Slave did not respond to"

  1759                         " SDO upload request. Retrying after %lu ms...\n",

  1779                         " SDO upload request. Retrying after %lu ms...\n",

  1760                         diff_ms);

  1780                         diff_ms);

  1779     }

  1799     }

  1780 #endif

  1800 #endif

  1781 

  1801 

  1782     fsm->jiffies_start = datagram->jiffies_sent;

  1802     fsm->jiffies_start = datagram->jiffies_sent;

  1783 

  1803 

  1785     fsm->retries = EC_FSM_RETRIES;

  1805     fsm->retries = EC_FSM_RETRIES;

  1786     fsm->state = ec_fsm_coe_up_check;

  1806     fsm->state = ec_fsm_coe_up_check;

  1787 }

  1807 }

  1788 

  1808 

  1789 /*****************************************************************************/

  1809 /*****************************************************************************/

  1792    CoE state: UP CHECK.

  1812    CoE state: UP CHECK.

  1793 */

  1813 */

  1794 

  1814 

  1795 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1815 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  1796 {

  1816 {

  1798     ec_slave_t *slave = fsm->slave;

  1819     ec_slave_t *slave = fsm->slave;

  1799 

  1820 

  1804         fsm->request->errno = EIO;

  1825         fsm->request->errno = EIO;

  1805         fsm->state = ec_fsm_coe_error;

  1826         fsm->state = ec_fsm_coe_error;

  1806         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

  1827         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");

  1807         ec_datagram_print_state(datagram);

  1828         ec_datagram_print_state(datagram);

  1808         return;

  1829         return;

  1809     }

  1830     }

  1810 

  1831 

  1812         fsm->request->errno = EIO;

  1833         fsm->request->errno = EIO;

  1813         fsm->state = ec_fsm_coe_error;

  1834         fsm->state = ec_fsm_coe_error;

  1814         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

  1835         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"

  1815                 " datagram failed: ");

  1836                 " datagram failed: ");

  1816         ec_datagram_print_wc_error(datagram);

  1837         ec_datagram_print_wc_error(datagram);

  1817         return;

  1838         return;

  1818     }

  1839     }

  1819 

  1840 

  1821         unsigned long diff_ms =

  1842         unsigned long diff_ms =

  1822             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1843             (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

  1823         if (diff_ms >= fsm->request->response_timeout) {

  1844         if (diff_ms >= fsm->request->response_timeout) {

  1824             fsm->request->errno = EIO;

  1845             fsm->request->errno = EIO;

  1825             fsm->state = ec_fsm_coe_error;

  1846             fsm->state = ec_fsm_coe_error;

  1827                     " SDO 0x%04x:%x upload response.\n", diff_ms,

  1848                     " SDO 0x%04x:%x upload response.\n", diff_ms,

  1828                     fsm->request->index, fsm->request->subindex);

  1849                     fsm->request->index, fsm->request->subindex);

  1829             return;

  1850             return;

  1830         }

  1851         }

  1831 

  1852 

  1833         fsm->retries = EC_FSM_RETRIES;

  1854         fsm->retries = EC_FSM_RETRIES;

  1834         return;

  1855         return;

  1835     }

  1856     }

  1836 

  1857 

  1837     // Fetch response

  1858     // Fetch response

  1839     fsm->retries = EC_FSM_RETRIES;

  1860     fsm->retries = EC_FSM_RETRIES;

  1840     fsm->state = ec_fsm_coe_up_response;

  1861     fsm->state = ec_fsm_coe_up_response;

  1841 }

  1862 }

  1842 

  1863 

  1843 /*****************************************************************************/

  1864 /*****************************************************************************/

  1847 void ec_fsm_coe_up_prepare_segment_request(

  1868 void ec_fsm_coe_up_prepare_segment_request(

  1848         ec_fsm_coe_t *fsm /**< Finite state machine */

  1869         ec_fsm_coe_t *fsm /**< Finite state machine */

  1849         )

  1870         )

  1850 {

  1871 {

  1851     uint8_t *data =

  1872     uint8_t *data =

  1853     if (IS_ERR(data)) {

  1874     if (IS_ERR(data)) {

  1854         fsm->request->errno = PTR_ERR(data);

  1875         fsm->request->errno = PTR_ERR(data);

  1855         fsm->state = ec_fsm_coe_error;

  1876         fsm->state = ec_fsm_coe_error;

  1856         return;

  1877         return;

  1857     }

  1878     }

  1874    \todo Timeout behavior

  1895    \todo Timeout behavior

  1875 */

  1896 */

  1876 

  1897 

  1877 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)

  1898 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */)

  1878 {

  1899 {

  1880     ec_slave_t *slave = fsm->slave;

  1902     ec_slave_t *slave = fsm->slave;

  1881     ec_master_t *master = slave->master;

  1903     ec_master_t *master = slave->master;

  1882     uint16_t rec_index;

  1904     uint16_t rec_index;

  1883     uint8_t *data, mbox_prot, rec_subindex;

  1905     uint8_t *data, mbox_prot, rec_subindex;

  1884     size_t rec_size, data_size;

  1906     size_t rec_size, data_size;

  1885     ec_sdo_request_t *request = fsm->request;

  1907     ec_sdo_request_t *request = fsm->request;

  1886     unsigned int expedited, size_specified;

  1908     unsigned int expedited, size_specified;

  1887     int ret;

  1909     int ret;

  1888 

  1910 

  1890         return; // FIXME: request again?

  1912         return; // FIXME: request again?

  1891 

  1913 

  1893         request->errno = EIO;

  1915         request->errno = EIO;

  1894         fsm->state = ec_fsm_coe_error;

  1916         fsm->state = ec_fsm_coe_error;

  1895         EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"

  1917         EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"

  1896                 " datagram: ");

  1918                 " datagram: ");

  1897         ec_datagram_print_state(datagram);

  1919         ec_datagram_print_state(datagram);

  1898         return;

  1920         return;

  1899     }

  1921     }

  1900 

  1922 

  1902         request->errno = EIO;

  1924         request->errno = EIO;

  1903         fsm->state = ec_fsm_coe_error;

  1925         fsm->state = ec_fsm_coe_error;

  1904         EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");

  1926         EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");

  1905         ec_datagram_print_wc_error(datagram);

  1927         ec_datagram_print_wc_error(datagram);

  1906         return;

  1928         return;

  1907     }

  1929     }

  1908 

  1930 

  1910     if (IS_ERR(data)) {

  1932     if (IS_ERR(data)) {

  1911         request->errno = PTR_ERR(data);

  1933         request->errno = PTR_ERR(data);

  1912         fsm->state = ec_fsm_coe_error;

  1934         fsm->state = ec_fsm_coe_error;

  1913         return;

  1935         return;

  1914     }

  1936     }

  1926         return;

  1948         return;

  1927     }

  1949     }

  1928 

  1950 

  1929     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1951     if (ec_fsm_coe_check_emergency(fsm, data, rec_size)) {

  1930         // check for CoE response again

  1952         // check for CoE response again

  1932         fsm->retries = EC_FSM_RETRIES;

  1954         fsm->retries = EC_FSM_RETRIES;

  1933         fsm->state = ec_fsm_coe_up_check;

  1955         fsm->state = ec_fsm_coe_up_check;

  1934         return;

  1956         return;

  1935     }

  1957     }

  1936 

  1958 

  1977                 " (0x%04X:%02X, requested: 0x%04X:%02X).\n",

  1999                 " (0x%04X:%02X, requested: 0x%04X:%02X).\n",

  1978                 rec_index, rec_subindex, request->index, request->subindex);

  2000                 rec_index, rec_subindex, request->index, request->subindex);

  1979         ec_print_data(data, rec_size);

  2001         ec_print_data(data, rec_size);

  1980 

  2002 

  1981         // check for CoE response again

  2003         // check for CoE response again

  1983         fsm->retries = EC_FSM_RETRIES;

  2005         fsm->retries = EC_FSM_RETRIES;

  1984         fsm->state = ec_fsm_coe_up_check;

  2006         fsm->state = ec_fsm_coe_up_check;

  1985         return;

  2007         return;

  1986     }

  2008     }

  1987 

  2009 

  2073    \todo Timeout behavior

  2095    \todo Timeout behavior

  2074 */

  2096 */

  2075 

  2097 

  2076 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)

  2098 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */)

  2077 {

  2099 {

  2079     ec_slave_t *slave = fsm->slave;

  2102     ec_slave_t *slave = fsm->slave;

  2080 

  2103 

  2082         return; // FIXME: check for response first?

  2105         return; // FIXME: check for response first?

  2083 

  2106 

  2085         fsm->request->errno = EIO;

  2108         fsm->request->errno = EIO;

  2086         fsm->state = ec_fsm_coe_error;

  2109         fsm->state = ec_fsm_coe_error;

  2087         EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"

  2110         EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"

  2088                 " request datagram: ");

  2111                 " request datagram: ");

  2089         ec_datagram_print_state(datagram);

  2112         ec_datagram_print_state(datagram);

  2090         return;

  2113         return;

  2091     }

  2114     }

  2092 

  2115 

  2094         fsm->request->errno = EIO;

  2117         fsm->request->errno = EIO;

  2095         fsm->state = ec_fsm_coe_error;

  2118         fsm->state = ec_fsm_coe_error;

  2096         EC_SLAVE_ERR(slave, "Reception of CoE upload segment"

  2119         EC_SLAVE_ERR(slave, "Reception of CoE upload segment"

  2097                 " request failed: ");

  2120                 " request failed: ");

  2098         ec_datagram_print_wc_error(datagram);

  2121         ec_datagram_print_wc_error(datagram);

  2099         return;

  2122         return;

  2100     }

  2123     }

  2101 

  2124 

  2102     fsm->jiffies_start = datagram->jiffies_sent;

  2125     fsm->jiffies_start = datagram->jiffies_sent;

  2103 

  2126 

  2105     fsm->retries = EC_FSM_RETRIES;

  2128     fsm->retries = EC_FSM_RETRIES;

  2106     fsm->state = ec_fsm_coe_up_seg_check;

  2129     fsm->state = ec_fsm_coe_up_seg_check;

  2107 }

  2130 }

  2108 

  2131 

  2109 /*****************************************************************************/

  2132 /*****************************************************************************/

  2112    CoE state: UP CHECK.

  2135    CoE state: UP CHECK.

  2113 */

  2136 */

  2114 

  2137 

  2115 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  2138 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */)

  2116 {

  2139 {

  2118     ec_slave_t *slave = fsm->slave;

  2142     ec_slave_t *slave = fsm->slave;

  2119 

  2143 

  2124         fsm->request->errno = EIO;

  2148         fsm->request->errno = EIO;

  2125         fsm->state = ec_fsm_coe_error;

  2149         fsm->state = ec_fsm_coe_error;

  2126         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"

  2150         EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"

  2127                 " datagram: ");

  2151                 " datagram: ");

  2128         ec_datagram_print_state(datagram);

  2152         ec_datagram_print_state(datagram);

  2129         return;

  2153         return;

  2130     }

  2154     }

  2131 

  2155 

  2133         fsm->request->errno = EIO;

  2157         fsm->request->errno = EIO;

  2134         fsm->state = ec_fsm_coe_error;

  2158         fsm->state = ec_fsm_coe_error;

  2135         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"

  2159         EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"

  2136                 " failed: ");

  2160                 " failed: ");

  2137         ec_datagram_print_wc_error(datagram);

  2161         ec_datagram_print_wc_error(datagram);

  2138         return;

  2162         return;

  2139     }

  2163     }

  2140 

  2164 

  2142         unsigned long diff_ms =

  2166         unsigned long diff_ms =