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 

   508 

   513 

   509     sdo_count = (rec_size - index_list_offset) / 2;

   514     sdo_count = (rec_size - index_list_offset) / 2;

   510 

   515 

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

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

   512         sdo_index = EC_READ_U16(data + index_list_offset + i * 2);

   517         sdo_index = EC_READ_U16(data + index_list_offset + i * 2);

   532     }

   537     }

   533 

   538 

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

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

   535         // more messages waiting. check again.

   540         // more messages waiting. check again.

   536         fsm->jiffies_start = datagram->jiffies_sent;

   541         fsm->jiffies_start = datagram->jiffies_sent;

   538         fsm->retries = EC_FSM_RETRIES;

   543         fsm->retries = EC_FSM_RETRIES;

   539         fsm->state = ec_fsm_coe_dict_check;

   544         fsm->state = ec_fsm_coe_dict_check;

   540         return;

   545         return;

   541     }

   546     }

   542 

   547 

   547     }

   552     }

   548 

   553 

   549     // fetch SDO descriptions

   554     // fetch SDO descriptions

   550     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);

   551 

   556 

   553     if (IS_ERR(data)) {

   558     if (IS_ERR(data)) {

   554         fsm->state = ec_fsm_coe_error;

   559         fsm->state = ec_fsm_coe_error;

   555         return;

   560         return;

   556     }

   561     }

   557 

   562 

   572    \todo Timeout behavior

   577    \todo Timeout behavior

   573 */

   578 */

   574 

   579 

   575 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 */)

   576 {

   581 {

   578     ec_slave_t *slave = fsm->slave;

   584     ec_slave_t *slave = fsm->slave;

   579 

   585 

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

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

   582 

   588 

   584         fsm->state = ec_fsm_coe_error;

   590         fsm->state = ec_fsm_coe_error;

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

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

   586                 " description request datagram: ");

   592                 " description request datagram: ");

   587         ec_datagram_print_state(datagram);

   593         ec_datagram_print_state(datagram);

   588         return;

   594         return;

   589     }

   595     }

   590 

   596 

   592         fsm->state = ec_fsm_coe_error;

   598         fsm->state = ec_fsm_coe_error;

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

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

   594                 " request failed: ");

   600                 " request failed: ");

   595         ec_datagram_print_wc_error(datagram);

   601         ec_datagram_print_wc_error(datagram);

   596         return;

   602         return;

   597     }

   603     }

   598 

   604 

   599     fsm->jiffies_start = datagram->jiffies_sent;

   605     fsm->jiffies_start = datagram->jiffies_sent;

   600 

   606 

   602     fsm->retries = EC_FSM_RETRIES;

   608     fsm->retries = EC_FSM_RETRIES;

   603     fsm->state = ec_fsm_coe_dict_desc_check;

   609     fsm->state = ec_fsm_coe_dict_desc_check;

   604 }

   610 }

   605 

   611 

   606 /*****************************************************************************/

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

   609    CoE state: DICT DESC CHECK.

   615    CoE state: DICT DESC CHECK.

   610 */

   616 */

   611 

   617 

   612 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 */)

   613 {

   619 {

   615     ec_slave_t *slave = fsm->slave;

   622     ec_slave_t *slave = fsm->slave;

   616 

   623 

   621         fsm->state = ec_fsm_coe_error;

   628         fsm->state = ec_fsm_coe_error;

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

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

   623         ec_datagram_print_state(datagram);

   630         ec_datagram_print_state(datagram);

   624         return;

   631         return;

   625     }

   632     }

   626 

   633 

   628         fsm->state = ec_fsm_coe_error;

   635         fsm->state = ec_fsm_coe_error;

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

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

   630                 " datagram failed: ");

   637                 " datagram failed: ");

   631         ec_datagram_print_wc_error(datagram);

   638         ec_datagram_print_wc_error(datagram);

   632         return;

   639         return;

   633     }

   640     }

   634 

   641 

   636         unsigned long diff_ms =

   643         unsigned long diff_ms =

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

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

   638         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   645         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   639             fsm->state = ec_fsm_coe_error;

   646             fsm->state = ec_fsm_coe_error;

   640             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   647             EC_SLAVE_ERR(slave, "Timeout while waiting for"

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

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

   642                     fsm->sdo->index);

   649                     fsm->sdo->index);

   643             return;

   650             return;

   644         }

   651         }

   645 

   652 

   647         fsm->retries = EC_FSM_RETRIES;

   654         fsm->retries = EC_FSM_RETRIES;

   648         return;

   655         return;

   649     }

   656     }

   650 

   657 

   651     // Fetch response

   658     // Fetch response

   653     fsm->retries = EC_FSM_RETRIES;

   660     fsm->retries = EC_FSM_RETRIES;

   654     fsm->state = ec_fsm_coe_dict_desc_response;

   661     fsm->state = ec_fsm_coe_dict_desc_response;

   655 }

   662 }

   656 

   663 

   657 /*****************************************************************************/

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

   662 */

   669 */

   663 

   670 

   664 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

   665                                    /**< finite state machine */)

   672                                    /**< finite state machine */)

   666 {

   673 {

   668     ec_slave_t *slave = fsm->slave;

   676     ec_slave_t *slave = fsm->slave;

   669     ec_sdo_t *sdo = fsm->sdo;

   677     ec_sdo_t *sdo = fsm->sdo;

   670     uint8_t *data, mbox_prot;

   678     uint8_t *data, mbox_prot;

   671     size_t rec_size, name_size;

   679     size_t rec_size, name_size;

   672 

   680 

   674         return; // FIXME: request again?

   682         return; // FIXME: request again?

   675 

   683 

   677         fsm->state = ec_fsm_coe_error;

   685         fsm->state = ec_fsm_coe_error;

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

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

   679                 " response datagram: ");

   687                 " response datagram: ");

   680         ec_datagram_print_state(datagram);

   688         ec_datagram_print_state(datagram);

   681         return;

   689         return;

   682     }

   690     }

   683 

   691 

   685         fsm->state = ec_fsm_coe_error;

   693         fsm->state = ec_fsm_coe_error;

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

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

   687                 " response failed: ");

   695                 " response failed: ");

   688         ec_datagram_print_wc_error(datagram);

   696         ec_datagram_print_wc_error(datagram);

   689         return;

   697         return;

   690     }

   698     }

   691 

   699 

   693     if (IS_ERR(data)) {

   701     if (IS_ERR(data)) {

   694         fsm->state = ec_fsm_coe_error;

   702         fsm->state = ec_fsm_coe_error;

   695         return;

   703         return;

   696     }

   704     }

   697 

   705 

   702         return;

   710         return;

   703     }

   711     }

   704 

   712 

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

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

   706         // check for CoE response again

   714         // check for CoE response again

   708         fsm->retries = EC_FSM_RETRIES;

   716         fsm->retries = EC_FSM_RETRIES;

   709         fsm->state = ec_fsm_coe_dict_desc_check;

   717         fsm->state = ec_fsm_coe_dict_desc_check;

   710         return;

   718         return;

   711     }

   719     }

   712 

   720 

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

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

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

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

   742             ec_print_data(data, rec_size);

   750             ec_print_data(data, rec_size);

   743         }

   751         }

   744         // check for CoE response again

   752         // check for CoE response again

   746         fsm->retries = EC_FSM_RETRIES;

   754         fsm->retries = EC_FSM_RETRIES;

   747         fsm->state = ec_fsm_coe_dict_desc_check;

   755         fsm->state = ec_fsm_coe_dict_desc_check;

   748         return;

   756         return;

   749     }

   757     }

   750 

   758 

   778 

   786 

   779     // start fetching entries

   787     // start fetching entries

   780 

   788 

   781     fsm->subindex = 0;

   789     fsm->subindex = 0;

   782 

   790 

   784     if (IS_ERR(data)) {

   792     if (IS_ERR(data)) {

   785         fsm->state = ec_fsm_coe_error;

   793         fsm->state = ec_fsm_coe_error;

   786         return;

   794         return;

   787     }

   795     }

   788 

   796 

   806 */

   814 */

   807 

   815 

   808 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

   809                                    /**< finite state machine */)

   817                                    /**< finite state machine */)

   810 {

   818 {

   812     ec_slave_t *slave = fsm->slave;

   821     ec_slave_t *slave = fsm->slave;

   813 

   822 

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

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

   816 

   825 

   818         fsm->state = ec_fsm_coe_error;

   827         fsm->state = ec_fsm_coe_error;

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

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

   820                 " request datagram: ");

   829                 " request datagram: ");

   821         ec_datagram_print_state(datagram);

   830         ec_datagram_print_state(datagram);

   822         return;

   831         return;

   823     }

   832     }

   824 

   833 

   826         fsm->state = ec_fsm_coe_error;

   835         fsm->state = ec_fsm_coe_error;

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

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

   828         ec_datagram_print_wc_error(datagram);

   837         ec_datagram_print_wc_error(datagram);

   829         return;

   838         return;

   830     }

   839     }

   831 

   840 

   832     fsm->jiffies_start = datagram->jiffies_sent;

   841     fsm->jiffies_start = datagram->jiffies_sent;

   833 

   842 

   835     fsm->retries = EC_FSM_RETRIES;

   844     fsm->retries = EC_FSM_RETRIES;

   836     fsm->state = ec_fsm_coe_dict_entry_check;

   845     fsm->state = ec_fsm_coe_dict_entry_check;

   837 }

   846 }

   838 

   847 

   839 /*****************************************************************************/

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

   843 */

   852 */

   844 

   853 

   845 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

   846                                  /**< finite state machine */)

   855                                  /**< finite state machine */)

   847 {

   856 {

   849     ec_slave_t *slave = fsm->slave;

   859     ec_slave_t *slave = fsm->slave;

   850 

   860 

   855         fsm->state = ec_fsm_coe_error;

   865         fsm->state = ec_fsm_coe_error;

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

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

   857         ec_datagram_print_state(datagram);

   867         ec_datagram_print_state(datagram);

   858         return;

   868         return;

   859     }

   869     }

   860 

   870 

   862         fsm->state = ec_fsm_coe_error;

   872         fsm->state = ec_fsm_coe_error;

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

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

   864                 " datagram failed: ");

   874                 " datagram failed: ");

   865         ec_datagram_print_wc_error(datagram);

   875         ec_datagram_print_wc_error(datagram);

   866         return;

   876         return;

   867     }

   877     }

   868 

   878 

   870         unsigned long diff_ms =

   880         unsigned long diff_ms =

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

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

   872         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   882         if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {

   873             fsm->state = ec_fsm_coe_error;

   883             fsm->state = ec_fsm_coe_error;

   874             EC_SLAVE_ERR(slave, "Timeout while waiting for"

   884             EC_SLAVE_ERR(slave, "Timeout while waiting for"

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

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

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

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

   877             return;

   887             return;

   878         }

   888         }

   879 

   889 

   881         fsm->retries = EC_FSM_RETRIES;

   891         fsm->retries = EC_FSM_RETRIES;

   882         return;

   892         return;

   883     }

   893     }

   884 

   894 

   885     // Fetch response

   895     // Fetch response

   887     fsm->retries = EC_FSM_RETRIES;

   897     fsm->retries = EC_FSM_RETRIES;

   888     fsm->state = ec_fsm_coe_dict_entry_response;

   898     fsm->state = ec_fsm_coe_dict_entry_response;

   889 }

   899 }

   890 

   900 

   891 /*****************************************************************************/

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

   896 */

   906 */

   897 

   907 

   898 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

   899                                     /**< finite state machine */)

   909                                     /**< finite state machine */)

   900 {

   910 {

   902     ec_slave_t *slave = fsm->slave;

   913     ec_slave_t *slave = fsm->slave;

   903     ec_sdo_t *sdo = fsm->sdo;

   914     ec_sdo_t *sdo = fsm->sdo;

   904     uint8_t *data, mbox_prot;

   915     uint8_t *data, mbox_prot;

   905     size_t rec_size, data_size;

   916     size_t rec_size, data_size;

   906     ec_sdo_entry_t *entry;

   917     ec_sdo_entry_t *entry;

   907     u16 word;

   918     u16 word;

   908 

   919 

   910         return; // FIXME: request again?

   921         return; // FIXME: request again?

   911 

   922 

   913         fsm->state = ec_fsm_coe_error;

   924         fsm->state = ec_fsm_coe_error;

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

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

   915                 " description response datagram: ");

   926                 " description response datagram: ");

   916         ec_datagram_print_state(datagram);

   927         ec_datagram_print_state(datagram);

   917         return;

   928         return;

   918     }

   929     }

   919 

   930 

   921         fsm->state = ec_fsm_coe_error;

   932         fsm->state = ec_fsm_coe_error;

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

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

   923                 " response failed: ");

   934                 " response failed: ");

   924         ec_datagram_print_wc_error(datagram);

   935         ec_datagram_print_wc_error(datagram);

   925         return;

   936         return;

   926     }

   937     }

   927 

   938 

   929     if (IS_ERR(data)) {

   940     if (IS_ERR(data)) {

   930         fsm->state = ec_fsm_coe_error;

   941         fsm->state = ec_fsm_coe_error;

   931         return;

   942         return;

   932     }

   943     }

   933 

   944 

   938         return;

   949         return;

   939     }

   950     }

   940 

   951 

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

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

   942         // check for CoE response again

   953         // check for CoE response again

   944         fsm->retries = EC_FSM_RETRIES;

   955         fsm->retries = EC_FSM_RETRIES;

   945         fsm->state = ec_fsm_coe_dict_entry_check;

   956         fsm->state = ec_fsm_coe_dict_entry_check;

   946         return;

   957         return;

   947     }

   958     }

   948 

   959 

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

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

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

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

   981             ec_print_data(data, rec_size);

   992             ec_print_data(data, rec_size);

   982         }

   993         }

   983         // check for CoE response again

   994         // check for CoE response again

   985         fsm->retries = EC_FSM_RETRIES;

   996         fsm->retries = EC_FSM_RETRIES;

   986         fsm->state = ec_fsm_coe_dict_entry_check;

   997         fsm->state = ec_fsm_coe_dict_entry_check;

   987         return;

   998         return;

   988     }

   999     }

   989 

  1000 

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

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

  1032 

  1043 

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

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

  1034         fsm->subindex++;

  1045         fsm->subindex++;

  1035 

  1046 

  1037         if (IS_ERR(data)) {

  1048         if (IS_ERR(data)) {

  1038             fsm->state = ec_fsm_coe_error;

  1049             fsm->state = ec_fsm_coe_error;

  1039             return;

  1050             return;

  1040         }

  1051         }

  1041 

  1052 

  1054 

  1065 

  1055     // another SDO description to fetch?

  1066     // another SDO description to fetch?

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

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

  1057         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);

  1058 

  1069 

  1060         if (IS_ERR(data)) {

  1071         if (IS_ERR(data)) {

  1061             fsm->state = ec_fsm_coe_error;

  1072             fsm->state = ec_fsm_coe_error;

  1062             return;

  1073             return;

  1063         }

  1074         }

  1064 

  1075 

  1084  */

  1095  */

  1085 void ec_fsm_coe_down_start(

  1096 void ec_fsm_coe_down_start(

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

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

  1087         )

  1098         )

  1088 {

  1099 {

  1090     ec_slave_t *slave = fsm->slave;

  1101     ec_slave_t *slave = fsm->slave;

  1091     ec_sdo_request_t *request = fsm->request;

  1102     ec_sdo_request_t *request = fsm->request;

  1092     uint8_t *data;

  1103     uint8_t *data;

  1093     uint8_t data_set_size;

  1104     uint8_t data_set_size;

  1094 

  1105 

  1118         fsm->state = ec_fsm_coe_error;

  1129         fsm->state = ec_fsm_coe_error;

  1119         return;

  1130         return;

  1120     }

  1131     }

  1121 

  1132 

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

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

  1124                 EC_COE_DOWN_REQ_HEADER_SIZE);

  1135                 EC_COE_DOWN_REQ_HEADER_SIZE);

  1125         if (IS_ERR(data)) {

  1136         if (IS_ERR(data)) {

  1126             request->errno = PTR_ERR(data);

  1137             request->errno = PTR_ERR(data);

  1127             fsm->state = ec_fsm_coe_error;

  1138             fsm->state = ec_fsm_coe_error;

  1128             return;

  1139             return;

  1160             data_size = max_data_size;

  1171             data_size = max_data_size;

  1161         } else {

  1172         } else {

  1162             data_size = required_data_size;

  1173             data_size = required_data_size;

  1163         }

  1174         }

  1164 

  1175 

  1166                 data_size);

  1177                 data_size);

  1167         if (IS_ERR(data)) {

  1178         if (IS_ERR(data)) {

  1168             request->errno = PTR_ERR(data);

  1179             request->errno = PTR_ERR(data);

  1169             fsm->state = ec_fsm_coe_error;

  1180             fsm->state = ec_fsm_coe_error;

  1170             return;

  1181             return;

  1209    \todo Timeout behavior

  1220    \todo Timeout behavior

  1210 */

  1221 */

  1211 

  1222 

  1212 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 */)

  1213 {

  1224 {

  1215     ec_slave_t *slave = fsm->slave;

  1227     ec_slave_t *slave = fsm->slave;

  1216     unsigned long diff_ms;

  1228     unsigned long diff_ms;

  1217 

  1229 

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

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

  1220 

  1232 

  1222         fsm->request->errno = EIO;

  1234         fsm->request->errno = EIO;

  1223         fsm->state = ec_fsm_coe_error;

  1235         fsm->state = ec_fsm_coe_error;

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

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

  1225                 " request datagram: ");

  1237                 " request datagram: ");

  1226         ec_datagram_print_state(datagram);

  1238         ec_datagram_print_state(datagram);

  1227         return;

  1239         return;

  1228     }

  1240     }

  1229 

  1241 

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

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

  1231 

  1243 

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

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

  1235 #if DEBUG_RETRIES

  1247 #if DEBUG_RETRIES

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

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

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

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

  1238                         diff_ms);

  1250                         diff_ms);

  1257     }

  1269     }

  1258 #endif

  1270 #endif

  1259 

  1271 

  1260     fsm->jiffies_start = datagram->jiffies_sent;

  1272     fsm->jiffies_start = datagram->jiffies_sent;

  1261 

  1273 

  1263     fsm->retries = EC_FSM_RETRIES;

  1275     fsm->retries = EC_FSM_RETRIES;

  1264     fsm->state = ec_fsm_coe_down_check;

  1276     fsm->state = ec_fsm_coe_down_check;

  1265 }

  1277 }

  1266 

  1278 

  1267 /*****************************************************************************/

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

  1268 

  1280 

  1269 /** CoE state: DOWN CHECK.

  1281 /** CoE state: DOWN CHECK.

  1270  */

  1282  */

  1271 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 */)

  1272 {

  1284 {

  1274     ec_slave_t *slave = fsm->slave;

  1287     ec_slave_t *slave = fsm->slave;

  1275 

  1288 

  1280         fsm->request->errno = EIO;

  1293         fsm->request->errno = EIO;

  1281         fsm->state = ec_fsm_coe_error;

  1294         fsm->state = ec_fsm_coe_error;

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

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

  1283                 " datagram: ");

  1296                 " datagram: ");

  1284         ec_datagram_print_state(datagram);

  1297         ec_datagram_print_state(datagram);

  1285         return;

  1298         return;

  1286     }

  1299     }

  1287 

  1300 

  1289         fsm->request->errno = EIO;

  1302         fsm->request->errno = EIO;

  1290         fsm->state = ec_fsm_coe_error;

  1303         fsm->state = ec_fsm_coe_error;

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

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

  1292                 " datagram failed: ");

  1305                 " datagram failed: ");

  1293         ec_datagram_print_wc_error(datagram);

  1306         ec_datagram_print_wc_error(datagram);

  1294         return;

  1307         return;

  1295     }

  1308     }

  1296 

  1309 

  1298         unsigned long diff_ms =

  1311         unsigned long diff_ms =

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

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

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

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

  1301             fsm->request->errno = EIO;

  1314             fsm->request->errno = EIO;

  1302             fsm->state = ec_fsm_coe_error;

  1315             fsm->state = ec_fsm_coe_error;

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

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

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

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

  1306             return;

  1319             return;

  1307         }

  1320         }

  1308 

  1321 

  1310         fsm->retries = EC_FSM_RETRIES;

  1323         fsm->retries = EC_FSM_RETRIES;

  1311         return;

  1324         return;

  1312     }

  1325     }

  1313 

  1326 

  1314     // Fetch response

  1327     // Fetch response

  1316     fsm->retries = EC_FSM_RETRIES;

  1329     fsm->retries = EC_FSM_RETRIES;

  1317     fsm->state = ec_fsm_coe_down_response;

  1330     fsm->state = ec_fsm_coe_down_response;

  1318 }

  1331 }

  1319 

  1332 

  1320 /*****************************************************************************/

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

  1323  */

  1336  */

  1324 void ec_fsm_coe_down_prepare_segment_request(

  1337 void ec_fsm_coe_down_prepare_segment_request(

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

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

  1326         )

  1339         )

  1327 {

  1340 {

  1329     ec_slave_t *slave = fsm->slave;

  1342     ec_slave_t *slave = fsm->slave;

  1330     ec_sdo_request_t *request = fsm->request;

  1343     ec_sdo_request_t *request = fsm->request;

  1331     size_t max_segment_size =

  1344     size_t max_segment_size =

  1332         slave->configured_rx_mailbox_size

  1345         slave->configured_rx_mailbox_size

  1333         - EC_MBOX_HEADER_SIZE

  1346         - EC_MBOX_HEADER_SIZE

  1350         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;

  1351         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE

  1364         data_size = EC_COE_DOWN_SEG_REQ_HEADER_SIZE

  1352             + EC_COE_DOWN_SEG_MIN_DATA_SIZE;

  1365             + EC_COE_DOWN_SEG_MIN_DATA_SIZE;

  1353     }

  1366     }

  1354 

  1367 

  1356             data_size);

  1369             data_size);

  1357     if (IS_ERR(data)) {

  1370     if (IS_ERR(data)) {

  1358         request->errno = PTR_ERR(data);

  1371         request->errno = PTR_ERR(data);

  1359         fsm->state = ec_fsm_coe_error;

  1372         fsm->state = ec_fsm_coe_error;

  1360         return;

  1373         return;

  1390    \todo Timeout behavior

  1403    \todo Timeout behavior

  1391 */

  1404 */

  1392 

  1405 

  1393 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 */)

  1394 {

  1407 {

  1396     ec_slave_t *slave = fsm->slave;

  1410     ec_slave_t *slave = fsm->slave;

  1397     uint8_t *data, mbox_prot;

  1411     uint8_t *data, mbox_prot;

  1398     size_t rec_size;

  1412     size_t rec_size;

  1399     ec_sdo_request_t *request = fsm->request;

  1413     ec_sdo_request_t *request = fsm->request;

  1400 

  1414 

  1402         return; // FIXME: request again?

  1416         return; // FIXME: request again?

  1403 

  1417 

  1405         request->errno = EIO;

  1419         request->errno = EIO;

  1406         fsm->state = ec_fsm_coe_error;

  1420         fsm->state = ec_fsm_coe_error;

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

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

  1408                 " response datagram: ");

  1422                 " response datagram: ");

  1409         ec_datagram_print_state(datagram);

  1423         ec_datagram_print_state(datagram);

  1410         return;

  1424         return;

  1411     }

  1425     }

  1412 

  1426 

  1414         request->errno = EIO;

  1428         request->errno = EIO;

  1415         fsm->state = ec_fsm_coe_error;

  1429         fsm->state = ec_fsm_coe_error;

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

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

  1417         ec_datagram_print_wc_error(datagram);

  1431         ec_datagram_print_wc_error(datagram);

  1418         return;

  1432         return;

  1419     }

  1433     }

  1420 

  1434 

  1422     if (IS_ERR(data)) {

  1436     if (IS_ERR(data)) {

  1423         request->errno = PTR_ERR(data);

  1437         request->errno = PTR_ERR(data);

  1424         fsm->state = ec_fsm_coe_error;

  1438         fsm->state = ec_fsm_coe_error;

  1425         return;

  1439         return;

  1426     }

  1440     }

  1433         return;

  1447         return;

  1434     }

  1448     }

  1435 

  1449 

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

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

  1437         // check for CoE response again

  1451         // check for CoE response again

  1439         fsm->retries = EC_FSM_RETRIES;

  1453         fsm->retries = EC_FSM_RETRIES;

  1440         fsm->state = ec_fsm_coe_down_check;

  1454         fsm->state = ec_fsm_coe_down_check;

  1441         return;

  1455         return;

  1442     }

  1456     }

  1443 

  1457 

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

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

  1486                     " Retrying...\n");

  1500                     " Retrying...\n");

  1487             ec_print_data(data, rec_size);

  1501             ec_print_data(data, rec_size);

  1488         }

  1502         }

  1489         // check for CoE response again

  1503         // check for CoE response again

  1491         fsm->retries = EC_FSM_RETRIES;

  1505         fsm->retries = EC_FSM_RETRIES;

  1492         fsm->state = ec_fsm_coe_down_check;

  1506         fsm->state = ec_fsm_coe_down_check;

  1493         return;

  1507         return;

  1494     }

  1508     }

  1495 

  1509 

  1507    CoE state: DOWN SEG CHECK.

  1521    CoE state: DOWN SEG CHECK.

  1508 */

  1522 */

  1509 

  1523 

  1510 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 */)

  1511 {

  1525 {

  1513     ec_slave_t *slave = fsm->slave;

  1528     ec_slave_t *slave = fsm->slave;

  1514 

  1529 

  1519         fsm->request->errno = EIO;

  1534         fsm->request->errno = EIO;

  1520         fsm->state = ec_fsm_coe_error;

  1535         fsm->state = ec_fsm_coe_error;

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

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

  1522         ec_datagram_print_state(datagram);

  1537         ec_datagram_print_state(datagram);

  1523         return;

  1538         return;

  1524     }

  1539     }

  1525 

  1540 

  1527         fsm->request->errno = EIO;

  1542         fsm->request->errno = EIO;

  1528         fsm->state = ec_fsm_coe_error;

  1543         fsm->state = ec_fsm_coe_error;

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

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

  1530                 " datagram failed: ");

  1545                 " datagram failed: ");

  1531         ec_datagram_print_wc_error(datagram);

  1546         ec_datagram_print_wc_error(datagram);

  1532         return;

  1547         return;

  1533     }

  1548     }

  1534 

  1549 

  1536         unsigned long diff_ms =

  1551         unsigned long diff_ms =

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

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

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

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

  1539             fsm->request->errno = EIO;

  1554             fsm->request->errno = EIO;

  1540             fsm->state = ec_fsm_coe_error;

  1555             fsm->state = ec_fsm_coe_error;

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

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

  1542                     " segment response.\n");

  1557                     " segment response.\n");

  1543             return;

  1558             return;

  1544         }

  1559         }

  1545 

  1560 

  1547         fsm->retries = EC_FSM_RETRIES;

  1562         fsm->retries = EC_FSM_RETRIES;

  1548         return;

  1563         return;

  1549     }

  1564     }

  1550 

  1565 

  1551     // Fetch response

  1566     // Fetch response

  1553     fsm->retries = EC_FSM_RETRIES;

  1568     fsm->retries = EC_FSM_RETRIES;

  1554     fsm->state = ec_fsm_coe_down_seg_response;

  1569     fsm->state = ec_fsm_coe_down_seg_response;

  1555 }

  1570 }

  1556 

  1571 

  1557 /*****************************************************************************/

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

  1563 

  1578 

  1564 void ec_fsm_coe_down_seg_response(

  1579 void ec_fsm_coe_down_seg_response(

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

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

  1566         )

  1581         )

  1567 {

  1582 {

  1569     ec_slave_t *slave = fsm->slave;

  1585     ec_slave_t *slave = fsm->slave;

  1570     uint8_t *data, mbox_prot;

  1586     uint8_t *data, mbox_prot;

  1571     size_t rec_size;

  1587     size_t rec_size;

  1572     ec_sdo_request_t *request = fsm->request;

  1588     ec_sdo_request_t *request = fsm->request;

  1573 

  1589 

  1575         return; // FIXME: request again?

  1591         return; // FIXME: request again?

  1576 

  1592 

  1578         request->errno = EIO;

  1594         request->errno = EIO;

  1579         fsm->state = ec_fsm_coe_error;

  1595         fsm->state = ec_fsm_coe_error;

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

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

  1581                 " datagram: ");

  1597                 " datagram: ");

  1582         ec_datagram_print_state(datagram);

  1598         ec_datagram_print_state(datagram);

  1583         return;

  1599         return;

  1584     }

  1600     }

  1585 

  1601 

  1587         request->errno = EIO;

  1603         request->errno = EIO;

  1588         fsm->state = ec_fsm_coe_error;

  1604         fsm->state = ec_fsm_coe_error;

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

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

  1590         ec_datagram_print_wc_error(datagram);

  1606         ec_datagram_print_wc_error(datagram);

  1591         return;

  1607         return;

  1592     }

  1608     }

  1593 

  1609 

  1595     if (IS_ERR(data)) {

  1611     if (IS_ERR(data)) {

  1596         request->errno = PTR_ERR(data);

  1612         request->errno = PTR_ERR(data);

  1597         fsm->state = ec_fsm_coe_error;

  1613         fsm->state = ec_fsm_coe_error;

  1598         return;

  1614         return;

  1599     }

  1615     }

  1606         return;

  1622         return;

  1607     }

  1623     }

  1608 

  1624 

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

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

  1610         // check for CoE response again

  1626         // check for CoE response again

  1612         fsm->retries = EC_FSM_RETRIES;

  1628         fsm->retries = EC_FSM_RETRIES;

  1613         fsm->state = ec_fsm_coe_down_check;

  1629         fsm->state = ec_fsm_coe_down_check;

  1614         return;

  1630         return;

  1615     }

  1631     }

  1616 

  1632 

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

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

  1657                     " Retrying...\n");

  1673                     " Retrying...\n");

  1658             ec_print_data(data, rec_size);

  1674             ec_print_data(data, rec_size);

  1659         }

  1675         }

  1660         // check for CoE response again

  1676         // check for CoE response again

  1662         fsm->retries = EC_FSM_RETRIES;

  1678         fsm->retries = EC_FSM_RETRIES;

  1663         fsm->state = ec_fsm_coe_down_seg_check;

  1679         fsm->state = ec_fsm_coe_down_seg_check;

  1664         return;

  1680         return;

  1665     }

  1681     }

  1666 

  1682 

  1687    CoE state: UP START.

  1703    CoE state: UP START.

  1688 */

  1704 */

  1689 

  1705 

  1690 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 */)

  1691 {

  1707 {

  1693     ec_slave_t *slave = fsm->slave;

  1709     ec_slave_t *slave = fsm->slave;

  1694     ec_master_t *master = slave->master;

  1710     ec_master_t *master = slave->master;

  1695     ec_sdo_request_t *request = fsm->request;

  1711     ec_sdo_request_t *request = fsm->request;

  1696     uint8_t *data;

  1712     uint8_t *data;

  1697 

  1713 

  1703         request->errno = EPROTONOSUPPORT;

  1719         request->errno = EPROTONOSUPPORT;

  1704         fsm->state = ec_fsm_coe_error;

  1720         fsm->state = ec_fsm_coe_error;

  1705         return;

  1721         return;

  1706     }

  1722     }

  1707 

  1723 

  1709     if (IS_ERR(data)) {

  1725     if (IS_ERR(data)) {

  1710         request->errno = PTR_ERR(data);

  1726         request->errno = PTR_ERR(data);

  1711         fsm->state = ec_fsm_coe_error;

  1727         fsm->state = ec_fsm_coe_error;

  1712         return;

  1728         return;

  1713     }

  1729     }

  1735    \todo Timeout behavior

  1751    \todo Timeout behavior

  1736 */

  1752 */

  1737 

  1753 

  1738 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 */)

  1739 {

  1755 {

  1741     ec_slave_t *slave = fsm->slave;

  1758     ec_slave_t *slave = fsm->slave;

  1742     unsigned long diff_ms;

  1759     unsigned long diff_ms;

  1743 

  1760 

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

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

  1746 

  1763 

  1748         fsm->request->errno = EIO;

  1765         fsm->request->errno = EIO;

  1749         fsm->state = ec_fsm_coe_error;

  1766         fsm->state = ec_fsm_coe_error;

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

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

  1751         ec_datagram_print_state(datagram);

  1768         ec_datagram_print_state(datagram);

  1752         return;

  1769         return;

  1753     }

  1770     }

  1754 

  1771 

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

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

  1756 

  1773 

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

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

  1760 #if DEBUG_RETRIES

  1777 #if DEBUG_RETRIES

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

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

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

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

  1763                         diff_ms);

  1780                         diff_ms);

  1782     }

  1799     }

  1783 #endif

  1800 #endif

  1784 

  1801 

  1785     fsm->jiffies_start = datagram->jiffies_sent;

  1802     fsm->jiffies_start = datagram->jiffies_sent;

  1786 

  1803 

  1788     fsm->retries = EC_FSM_RETRIES;

  1805     fsm->retries = EC_FSM_RETRIES;

  1789     fsm->state = ec_fsm_coe_up_check;

  1806     fsm->state = ec_fsm_coe_up_check;

  1790 }

  1807 }

  1791 

  1808 

  1792 /*****************************************************************************/

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

  1795    CoE state: UP CHECK.

  1812    CoE state: UP CHECK.

  1796 */

  1813 */

  1797 

  1814 

  1798 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 */)

  1799 {

  1816 {

  1801     ec_slave_t *slave = fsm->slave;

  1819     ec_slave_t *slave = fsm->slave;

  1802 

  1820 

  1807         fsm->request->errno = EIO;

  1825         fsm->request->errno = EIO;

  1808         fsm->state = ec_fsm_coe_error;

  1826         fsm->state = ec_fsm_coe_error;

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

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

  1810         ec_datagram_print_state(datagram);

  1828         ec_datagram_print_state(datagram);

  1811         return;

  1829         return;

  1812     }

  1830     }

  1813 

  1831 

  1815         fsm->request->errno = EIO;

  1833         fsm->request->errno = EIO;

  1816         fsm->state = ec_fsm_coe_error;

  1834         fsm->state = ec_fsm_coe_error;

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

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

  1818                 " datagram failed: ");

  1836                 " datagram failed: ");

  1819         ec_datagram_print_wc_error(datagram);

  1837         ec_datagram_print_wc_error(datagram);

  1820         return;

  1838         return;

  1821     }

  1839     }

  1822 

  1840 

  1824         unsigned long diff_ms =

  1842         unsigned long diff_ms =

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

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

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

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

  1827             fsm->request->errno = EIO;

  1845             fsm->request->errno = EIO;

  1828             fsm->state = ec_fsm_coe_error;

  1846             fsm->state = ec_fsm_coe_error;

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

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

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

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

  1832             return;

  1850             return;

  1833         }

  1851         }

  1834 

  1852 

  1836         fsm->retries = EC_FSM_RETRIES;

  1854         fsm->retries = EC_FSM_RETRIES;

  1837         return;

  1855         return;

  1838     }

  1856     }

  1839 

  1857 

  1840     // Fetch response

  1858     // Fetch response

  1842     fsm->retries = EC_FSM_RETRIES;

  1860     fsm->retries = EC_FSM_RETRIES;

  1843     fsm->state = ec_fsm_coe_up_response;

  1861     fsm->state = ec_fsm_coe_up_response;

  1844 }

  1862 }

  1845 

  1863 

  1846 /*****************************************************************************/

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

  1850 void ec_fsm_coe_up_prepare_segment_request(

  1868 void ec_fsm_coe_up_prepare_segment_request(

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

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

  1852         )

  1870         )

  1853 {

  1871 {

  1854     uint8_t *data =

  1872     uint8_t *data =

  1856     if (IS_ERR(data)) {

  1874     if (IS_ERR(data)) {

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

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

  1858         fsm->state = ec_fsm_coe_error;

  1876         fsm->state = ec_fsm_coe_error;

  1859         return;

  1877         return;

  1860     }

  1878     }

  1877    \todo Timeout behavior

  1895    \todo Timeout behavior

  1878 */

  1896 */

  1879 

  1897 

  1880 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 */)

  1881 {

  1899 {

  1883     ec_slave_t *slave = fsm->slave;

  1902     ec_slave_t *slave = fsm->slave;

  1884     ec_master_t *master = slave->master;

  1903     ec_master_t *master = slave->master;

  1885     uint16_t rec_index;

  1904     uint16_t rec_index;

  1886     uint8_t *data, mbox_prot, rec_subindex;

  1905     uint8_t *data, mbox_prot, rec_subindex;

  1887     size_t rec_size, data_size;

  1906     size_t rec_size, data_size;

  1888     ec_sdo_request_t *request = fsm->request;

  1907     ec_sdo_request_t *request = fsm->request;

  1889     unsigned int expedited, size_specified;

  1908     unsigned int expedited, size_specified;

  1890     int ret;

  1909     int ret;

  1891 

  1910 

  1893         return; // FIXME: request again?

  1912         return; // FIXME: request again?

  1894 

  1913 

  1896         request->errno = EIO;

  1915         request->errno = EIO;

  1897         fsm->state = ec_fsm_coe_error;

  1916         fsm->state = ec_fsm_coe_error;

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

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

  1899                 " datagram: ");

  1918                 " datagram: ");

  1900         ec_datagram_print_state(datagram);

  1919         ec_datagram_print_state(datagram);

  1901         return;

  1920         return;

  1902     }

  1921     }

  1903 

  1922 

  1905         request->errno = EIO;

  1924         request->errno = EIO;

  1906         fsm->state = ec_fsm_coe_error;

  1925         fsm->state = ec_fsm_coe_error;

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

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

  1908         ec_datagram_print_wc_error(datagram);

  1927         ec_datagram_print_wc_error(datagram);

  1909         return;

  1928         return;

  1910     }

  1929     }

  1911 

  1930 

  1913     if (IS_ERR(data)) {

  1932     if (IS_ERR(data)) {

  1914         request->errno = PTR_ERR(data);

  1933         request->errno = PTR_ERR(data);

  1915         fsm->state = ec_fsm_coe_error;

  1934         fsm->state = ec_fsm_coe_error;

  1916         return;

  1935         return;

  1917     }

  1936     }

  1929         return;

  1948         return;

  1930     }

  1949     }

  1931 

  1950 

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

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

  1933         // check for CoE response again

  1952         // check for CoE response again

  1935         fsm->retries = EC_FSM_RETRIES;

  1954         fsm->retries = EC_FSM_RETRIES;

  1936         fsm->state = ec_fsm_coe_up_check;

  1955         fsm->state = ec_fsm_coe_up_check;

  1937         return;

  1956         return;

  1938     }

  1957     }

  1939 

  1958 

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

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

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

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

  1982         ec_print_data(data, rec_size);

  2001         ec_print_data(data, rec_size);

  1983 

  2002 

  1984         // check for CoE response again

  2003         // check for CoE response again

  1986         fsm->retries = EC_FSM_RETRIES;

  2005         fsm->retries = EC_FSM_RETRIES;

  1987         fsm->state = ec_fsm_coe_up_check;

  2006         fsm->state = ec_fsm_coe_up_check;

  1988         return;

  2007         return;

  1989     }

  2008     }

  1990 

  2009 

  2076    \todo Timeout behavior

  2095    \todo Timeout behavior

  2077 */

  2096 */

  2078 

  2097 

  2079 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 */)

  2080 {

  2099 {

  2082     ec_slave_t *slave = fsm->slave;

  2102     ec_slave_t *slave = fsm->slave;

  2083 

  2103 

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

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

  2086 

  2106 

  2088         fsm->request->errno = EIO;

  2108         fsm->request->errno = EIO;

  2089         fsm->state = ec_fsm_coe_error;

  2109         fsm->state = ec_fsm_coe_error;

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

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

  2091                 " request datagram: ");

  2111                 " request datagram: ");

  2092         ec_datagram_print_state(datagram);

  2112         ec_datagram_print_state(datagram);

  2093         return;

  2113         return;

  2094     }

  2114     }

  2095 

  2115 

  2097         fsm->request->errno = EIO;

  2117         fsm->request->errno = EIO;

  2098         fsm->state = ec_fsm_coe_error;

  2118         fsm->state = ec_fsm_coe_error;

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

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

  2100                 " request failed: ");

  2120                 " request failed: ");

  2101         ec_datagram_print_wc_error(datagram);

  2121         ec_datagram_print_wc_error(datagram);

  2102         return;

  2122         return;

  2103     }

  2123     }

  2104 

  2124 

  2105     fsm->jiffies_start = datagram->jiffies_sent;

  2125     fsm->jiffies_start = datagram->jiffies_sent;

  2106 

  2126 

  2108     fsm->retries = EC_FSM_RETRIES;

  2128     fsm->retries = EC_FSM_RETRIES;

  2109     fsm->state = ec_fsm_coe_up_seg_check;

  2129     fsm->state = ec_fsm_coe_up_seg_check;

  2110 }

  2130 }

  2111 

  2131 

  2112 /*****************************************************************************/

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

  2115    CoE state: UP CHECK.

  2135    CoE state: UP CHECK.

  2116 */

  2136 */

  2117 

  2137 

  2118 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 */)

  2119 {

  2139 {

  2121     ec_slave_t *slave = fsm->slave;

  2142     ec_slave_t *slave = fsm->slave;

  2122 

  2143 

  2127         fsm->request->errno = EIO;

  2148         fsm->request->errno = EIO;

  2128         fsm->state = ec_fsm_coe_error;

  2149         fsm->state = ec_fsm_coe_error;

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

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

  2130                 " datagram: ");

  2151                 " datagram: ");

  2131         ec_datagram_print_state(datagram);

  2152         ec_datagram_print_state(datagram);

  2132         return;

  2153         return;

  2133     }

  2154     }

  2134 

  2155 

  2136         fsm->request->errno = EIO;

  2157         fsm->request->errno = EIO;

  2137         fsm->state = ec_fsm_coe_error;

  2158         fsm->state = ec_fsm_coe_error;

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

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

  2139                 " failed: ");

  2160                 " failed: ");

  2140         ec_datagram_print_wc_error(datagram);

  2161         ec_datagram_print_wc_error(datagram);

  2141         return;

  2162         return;

  2142     }

  2163     }

  2143 

  2164 

  2145         unsigned long diff_ms =

  2166         unsigned long diff_ms =

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

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

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

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

  2148             fsm->request->errno = EIO;

  2169             fsm->request->errno = EIO;

  2149             fsm->state = ec_fsm_coe_error;

  2170             fsm->state = ec_fsm_coe_error;

  2150             EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload"

  2171             EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload"