1 /******************************************************************************

     2  *

     3  *  $Id$

     4  *

     5  *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH

     6  *

     7  *  This file is part of the IgH EtherCAT Master.

     8  *

     9  *  The IgH EtherCAT Master is free software; you can redistribute it

    10  *  and/or modify it under the terms of the GNU General Public License

    11  *  as published by the Free Software Foundation; either version 2 of the

    12  *  License, or (at your option) any later version.

    13  *

    14  *  The IgH EtherCAT Master is distributed in the hope that it will be

    15  *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17  *  GNU General Public License for more details.

    18  *

    19  *  You should have received a copy of the GNU General Public License

    20  *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22  *

    23  *  The right to use EtherCAT Technology is granted and comes free of

    24  *  charge under condition of compatibility of product made by

    25  *  Licensee. People intending to distribute/sell products based on the

    26  *  code, have to sign an agreement to guarantee that products using

    27  *  software based on IgH EtherCAT master stay compatible with the actual

    28  *  EtherCAT specification (which are released themselves as an open

    29  *  standard) as the (only) precondition to have the right to use EtherCAT

    30  *  Technology, IP and trade marks.

    31  *

    32  *****************************************************************************/

    33 

    34 /**

    35    \file

    36    EtherCAT CoE state machines.

    37 */

    38 

    39 /*****************************************************************************/

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "fsm_coe.h"

    45 

    46 /*****************************************************************************/

    47 

    48 void ec_fsm_coe_dict_start(ec_fsm_coe_t *);

    49 void ec_fsm_coe_dict_request(ec_fsm_coe_t *);

    50 void ec_fsm_coe_dict_check(ec_fsm_coe_t *);

    51 void ec_fsm_coe_dict_response(ec_fsm_coe_t *);

    52 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *);

    53 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *);

    54 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *);

    55 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *);

    56 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *);

    57 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *);

    58 

    59 void ec_fsm_coe_down_start(ec_fsm_coe_t *);

    60 void ec_fsm_coe_down_request(ec_fsm_coe_t *);

    61 void ec_fsm_coe_down_check(ec_fsm_coe_t *);

    62 void ec_fsm_coe_down_response(ec_fsm_coe_t *);

    63 

    64 void ec_fsm_coe_up_start(ec_fsm_coe_t *);

    65 void ec_fsm_coe_up_request(ec_fsm_coe_t *);

    66 void ec_fsm_coe_up_check(ec_fsm_coe_t *);

    67 void ec_fsm_coe_up_response(ec_fsm_coe_t *);

    68 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *);

    69 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *);

    70 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *);

    71 

    72 void ec_fsm_coe_end(ec_fsm_coe_t *);

    73 void ec_fsm_coe_error(ec_fsm_coe_t *);

    74 

    75 /*****************************************************************************/

    76 

    77 /**

    78    SDO abort messages.

    79    The "abort SDO transfer request" supplies an abort code,

    80    which can be translated to clear text. This table does

    81    the mapping of the codes and messages.

    82 */

    83 

    84 const ec_code_msg_t sdo_abort_messages[] = {

    85     {0x05030000, "Toggle bit not changed"},

    86     {0x05040000, "SDO protocol timeout"},

    87     {0x05040001, "Client/Server command specifier not valid or unknown"},

    88     {0x05040005, "Out of memory"},

    89     {0x06010000, "Unsupported access to an object"},

    90     {0x06010001, "Attempt to read a write-only object"},

    91     {0x06010002, "Attempt to write a read-only object"},

    92     {0x06020000, "This object does not exist in the object directory"},

    93     {0x06040041, "The object cannot be mapped into the PDO"},

    94     {0x06040042, "The number and length of the objects to be mapped would"

    95      " exceed the PDO length"},

    96     {0x06040043, "General parameter incompatibility reason"},

    97     {0x06040047, "Gerneral internal incompatibility in device"},

    98     {0x06060000, "Access failure due to a hardware error"},

    99     {0x06070010, "Data type does not match, length of service parameter does"

   100      " not match"},

   101     {0x06070012, "Data type does not match, length of service parameter too"

   102      " high"},

   103     {0x06070013, "Data type does not match, length of service parameter too"

   104      " low"},

   105     {0x06090011, "Subindex does not exist"},

   106     {0x06090030, "Value range of parameter exceeded"},

   107     {0x06090031, "Value of parameter written too high"},

   108     {0x06090032, "Value of parameter written too low"},

   109     {0x06090036, "Maximum value is less than minimum value"},

   110     {0x08000000, "General error"},

   111     {0x08000020, "Data cannot be transferred or stored to the application"},

   112     {0x08000021, "Data cannot be transferred or stored to the application"

   113      " because of local control"},

   114     {0x08000022, "Data cannot be transferred or stored to the application"

   115      " because of the present device state"},

   116     {0x08000023, "Object dictionary dynamic generation fails or no object"

   117      " dictionary is present"},

   118     {}

   119 };

   120 

   121 /*****************************************************************************/

   122 

   123 /**

   124    Outputs an SDO abort message.

   125 */

   126 

   127 void ec_canopen_abort_msg(uint32_t abort_code)

   128 {

   129     const ec_code_msg_t *abort_msg;

   130 

   131     for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) {

   132         if (abort_msg->code == abort_code) {

   133             EC_ERR("SDO abort message 0x%08X: \"%s\".\n",

   134                    abort_msg->code, abort_msg->message);

   135             return;

   136         }

   137     }

   138 

   139     EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code);

   140 }

   141 

   142 /*****************************************************************************/

   143 

   144 /**

   145    Constructor.

   146 */

   147 

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

   149                      ec_datagram_t *datagram /**< datagram */

   150                      )

   151 {

   152     fsm->state = NULL;

   153     fsm->datagram = datagram;

   154 }

   155 

   156 /*****************************************************************************/

   157 

   158 /**

   159    Destructor.

   160 */

   161 

   162 void ec_fsm_coe_clear(ec_fsm_coe_t *fsm /**< finite state machine */)

   163 {

   164 }

   165 

   166 /*****************************************************************************/

   167 

   168 /**

   169    Starts reading a slaves' SDO dictionary.

   170 */

   171 

   172 void ec_fsm_coe_dictionary(ec_fsm_coe_t *fsm, /**< finite state machine */

   173                            ec_slave_t *slave /**< EtherCAT slave */

   174                            )

   175 {

   176     fsm->slave = slave;

   177     fsm->state = ec_fsm_coe_dict_start;

   178 }

   179 

   180 /*****************************************************************************/

   181 

   182 /**

   183    Starts to download an SDO to a slave.

   184 */

   185 

   186 void ec_fsm_coe_download(ec_fsm_coe_t *fsm, /**< finite state machine */

   187                          ec_slave_t *slave, /**< EtherCAT slave */

   188                          ec_sdo_data_t *sdodata /**< SDO data object */

   189                          )

   190 {

   191     fsm->slave = slave;

   192     fsm->sdodata = sdodata;

   193     fsm->state = ec_fsm_coe_down_start;

   194 }

   195 

   196 /*****************************************************************************/

   197 

   198 /**

   199    Starts to upload an SDO from a slave.

   200 */

   201 

   202 void ec_fsm_coe_upload(ec_fsm_coe_t *fsm, /**< finite state machine */

   203                        ec_slave_t *slave, /**< EtherCAT slave */

   204                        ec_sdo_request_t *request /**< SDO request */

   205                        )

   206 {

   207     fsm->slave = slave;

   208     fsm->request = request;

   209     fsm->state = ec_fsm_coe_up_start;

   210 }

   211 

   212 /*****************************************************************************/

   213 

   214 /**

   215    Executes the current state of the state machine.

   216    \return false, if state machine has terminated

   217 */

   218 

   219 int ec_fsm_coe_exec(ec_fsm_coe_t *fsm /**< finite state machine */)

   220 {

   221     fsm->state(fsm);

   222 

   223     return fsm->state != ec_fsm_coe_end && fsm->state != ec_fsm_coe_error;

   224 }

   225 

   226 /*****************************************************************************/

   227 

   228 /**

   229    Returns, if the state machine terminated with success.

   230    \return non-zero if successful.

   231 */

   232 

   233 int ec_fsm_coe_success(ec_fsm_coe_t *fsm /**< Finite state machine */)

   234 {

   235     return fsm->state == ec_fsm_coe_end;

   236 }

   237 

   238 /******************************************************************************

   239  *  CoE dictionary state machine

   240  *****************************************************************************/

   241 

   242 /**

   243    CoE state: DICT START.

   244 */

   245 

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

   247 {

   248     ec_datagram_t *datagram = fsm->datagram;

   249     ec_slave_t *slave = fsm->slave;

   250     uint8_t *data;

   251 

   252     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {

   253         fsm->state = ec_fsm_coe_error;

   254         return;

   255     }

   256 

   257     EC_WRITE_U16(data, 0x8 << 12); // SDO information

   258     EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request

   259     EC_WRITE_U8 (data + 3, 0x00);

   260     EC_WRITE_U16(data + 4, 0x0000);

   261     EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs!

   262 

   263     ec_master_queue_datagram(fsm->slave->master, datagram);

   264     fsm->state = ec_fsm_coe_dict_request;

   265 }

   266 

   267 /*****************************************************************************/

   268 

   269 /**

   270    CoE state: DICT REQUEST.

   271 */

   272 

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

   274 {

   275     ec_datagram_t *datagram = fsm->datagram;

   276     ec_slave_t *slave = fsm->slave;

   277 

   278     if (datagram->state != EC_DATAGRAM_RECEIVED

   279         || datagram->working_counter != 1) {

   280         fsm->state = ec_fsm_coe_error;

   281         EC_ERR("Reception of CoE dictionary request failed on slave %i.\n",

   282                slave->ring_position);

   283         return;

   284     }

   285 

   286     fsm->cycles_start = datagram->cycles_sent;

   287 

   288     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   289     ec_master_queue_datagram(fsm->slave->master, datagram);

   290     fsm->state = ec_fsm_coe_dict_check;

   291 }

   292 

   293 /*****************************************************************************/

   294 

   295 /**

   296    CoE state: DICT CHECK.

   297 */

   298 

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

   300 {

   301     ec_datagram_t *datagram = fsm->datagram;

   302     ec_slave_t *slave = fsm->slave;

   303 

   304     if (datagram->state != EC_DATAGRAM_RECEIVED

   305         || datagram->working_counter != 1) {

   306         fsm->state = ec_fsm_coe_error;

   307         EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",

   308                slave->ring_position);

   309         return;

   310     }

   311 

   312     if (!ec_slave_mbox_check(datagram)) {

   313         if (datagram->cycles_received

   314             - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {

   315             fsm->state = ec_fsm_coe_error;

   316             EC_ERR("Timeout while checking SDO dictionary on slave %i.\n",

   317                    slave->ring_position);

   318             return;

   319         }

   320 

   321         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   322         ec_master_queue_datagram(fsm->slave->master, datagram);

   323         return;

   324     }

   325 

   326     // Fetch response

   327     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   328     ec_master_queue_datagram(fsm->slave->master, datagram);

   329     fsm->state = ec_fsm_coe_dict_response;

   330 }

   331 

   332 /*****************************************************************************/

   333 

   334 /**

   335    CoE state: DICT RESPONSE.

   336 */

   337 

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

   339 {

   340     ec_datagram_t *datagram = fsm->datagram;

   341     ec_slave_t *slave = fsm->slave;

   342     uint8_t *data, mbox_prot;

   343     size_t rec_size;

   344     unsigned int sdo_count, i;

   345     uint16_t sdo_index;

   346     ec_sdo_t *sdo;

   347 

   348     if (datagram->state != EC_DATAGRAM_RECEIVED

   349         || datagram->working_counter != 1) {

   350         fsm->state = ec_fsm_coe_error;

   351         EC_ERR("Reception of CoE dictionary response failed on slave %i.\n",

   352                slave->ring_position);

   353         return;

   354     }

   355 

   356     if (!(data = ec_slave_mbox_fetch(slave, datagram,

   357 				     &mbox_prot, &rec_size))) {

   358         fsm->state = ec_fsm_coe_error;

   359         return;

   360     }

   361 

   362     if (mbox_prot != 0x03) { // CoE

   363         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   364         fsm->state = ec_fsm_coe_error;

   365 	return;

   366     }

   367 

   368     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information

   369         (EC_READ_U8(data + 2) & 0x7F) == 0x07) { // error response

   370         EC_ERR("SDO information error response at slave %i!\n",

   371                slave->ring_position);

   372         ec_canopen_abort_msg(EC_READ_U32(data + 6));

   373         fsm->state = ec_fsm_coe_error;

   374 	return;

   375     }

   376 

   377     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information

   378         (EC_READ_U8 (data + 2) & 0x7F) != 0x02) { // Get OD List response

   379         EC_ERR("Invalid SDO list response at slave %i!\n",

   380                slave->ring_position);

   381         ec_print_data(data, rec_size);

   382         fsm->state = ec_fsm_coe_error;

   383 	return;

   384     }

   385 

   386     if (rec_size < 8) {

   387         EC_ERR("Invalid data size!\n");

   388         ec_print_data(data, rec_size);

   389         fsm->state = ec_fsm_coe_error;

   390 	return;

   391     }

   392 

   393     sdo_count = (rec_size - 8) / 2;

   394 

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

   396         sdo_index = EC_READ_U16(data + 8 + i * 2);

   397         if (!sdo_index) {

   398             if (slave->master->debug_level)

   399                 EC_WARN("SDO dictionary of slave %i contains index 0x0000.\n",

   400                         slave->ring_position);

   401             continue;

   402         }

   403 

   404         if (!(sdo = (ec_sdo_t *) kmalloc(sizeof(ec_sdo_t), GFP_ATOMIC))) {

   405             EC_ERR("Failed to allocate memory for SDO!\n");

   406             fsm->state = ec_fsm_coe_error;

   407             return;

   408         }

   409 

   410         if (ec_sdo_init(sdo, sdo_index, slave)) {

   411             EC_ERR("Failed to init SDO!\n");

   412             fsm->state = ec_fsm_coe_error;

   413             return;

   414         }

   415 

   416         list_add_tail(&sdo->list, &slave->sdo_dictionary);

   417     }

   418 

   419     if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again.

   420         fsm->cycles_start = datagram->cycles_sent;

   421         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   422         ec_master_queue_datagram(fsm->slave->master, datagram);

   423         fsm->state = ec_fsm_coe_dict_check;

   424         return;

   425     }

   426 

   427     if (list_empty(&slave->sdo_dictionary)) {

   428         // no SDOs in dictionary. finished.

   429         fsm->state = ec_fsm_coe_end; // success

   430         return;

   431     }

   432 

   433     // fetch SDO descriptions

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

   435 

   436     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {

   437         fsm->state = ec_fsm_coe_error;

   438         return;

   439     }

   440 

   441     EC_WRITE_U16(data, 0x8 << 12); // SDO information

   442     EC_WRITE_U8 (data + 2, 0x03); // Get object description request

   443     EC_WRITE_U8 (data + 3, 0x00);

   444     EC_WRITE_U16(data + 4, 0x0000);

   445     EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index

   446 

   447     ec_master_queue_datagram(fsm->slave->master, datagram);

   448     fsm->state = ec_fsm_coe_dict_desc_request;

   449 }

   450 

   451 /*****************************************************************************/

   452 

   453 /**

   454    CoE state: DICT DESC REQUEST.

   455 */

   456 

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

   458 {

   459     ec_datagram_t *datagram = fsm->datagram;

   460     ec_slave_t *slave = fsm->slave;

   461 

   462     if (datagram->state != EC_DATAGRAM_RECEIVED

   463         || datagram->working_counter != 1) {

   464         fsm->state = ec_fsm_coe_error;

   465         EC_ERR("Reception of CoE SDO description"

   466                " request failed on slave %i.\n", slave->ring_position);

   467         return;

   468     }

   469 

   470     fsm->cycles_start = datagram->cycles_sent;

   471 

   472     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   473     ec_master_queue_datagram(fsm->slave->master, datagram);

   474     fsm->state = ec_fsm_coe_dict_desc_check;

   475 }

   476 

   477 /*****************************************************************************/

   478 

   479 /**

   480    CoE state: DICT DESC CHECK.

   481 */

   482 

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

   484 {

   485     ec_datagram_t *datagram = fsm->datagram;

   486     ec_slave_t *slave = fsm->slave;

   487 

   488     if (datagram->state != EC_DATAGRAM_RECEIVED

   489         || datagram->working_counter != 1) {

   490         fsm->state = ec_fsm_coe_error;

   491         EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",

   492                slave->ring_position);

   493         return;

   494     }

   495 

   496     if (!ec_slave_mbox_check(datagram)) {

   497         if (datagram->cycles_received

   498             - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {

   499             fsm->state = ec_fsm_coe_error;

   500             EC_ERR("Timeout while checking SDO description on slave %i.\n",

   501                    slave->ring_position);

   502             return;

   503         }

   504 

   505         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   506         ec_master_queue_datagram(fsm->slave->master, datagram);

   507         return;

   508     }

   509 

   510     // Fetch response

   511     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   512     ec_master_queue_datagram(fsm->slave->master, datagram);

   513     fsm->state = ec_fsm_coe_dict_desc_response;

   514 }

   515 

   516 /*****************************************************************************/

   517 

   518 /**

   519    CoE state: DICT DESC RESPONSE.

   520 */

   521 

   522 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm

   523                                    /**< finite state machine */)

   524 {

   525     ec_datagram_t *datagram = fsm->datagram;

   526     ec_slave_t *slave = fsm->slave;

   527     ec_sdo_t *sdo = fsm->sdo;

   528     uint8_t *data, mbox_prot;

   529     size_t rec_size, name_size;

   530 

   531     if (datagram->state != EC_DATAGRAM_RECEIVED

   532         || datagram->working_counter != 1) {

   533         fsm->state = ec_fsm_coe_error;

   534         EC_ERR("Reception of CoE SDO description"

   535                "response failed on slave %i.\n", slave->ring_position);

   536         return;

   537     }

   538 

   539     if (!(data = ec_slave_mbox_fetch(slave, datagram,

   540 				     &mbox_prot, &rec_size))) {

   541         fsm->state = ec_fsm_coe_error;

   542         return;

   543     }

   544 

   545     if (mbox_prot != 0x03) { // CoE

   546         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   547         fsm->state = ec_fsm_coe_error;

   548 	return;

   549     }

   550 

   551     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information

   552         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response

   553         EC_ERR("SDO information error response at slave %i while"

   554                " fetching SDO 0x%04X!\n", slave->ring_position,

   555                sdo->index);

   556         ec_canopen_abort_msg(EC_READ_U32(data + 6));

   557         fsm->state = ec_fsm_coe_error;

   558 	return;

   559     }

   560 

   561     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information

   562         (EC_READ_U8 (data + 2) & 0x7F) != 0x04 || // Object desc. response

   563         EC_READ_U16(data + 6) != sdo->index) { // SDO index

   564         EC_ERR("Invalid object description response at slave %i while"

   565                " fetching SDO 0x%04X!\n", slave->ring_position,

   566                sdo->index);

   567         ec_print_data(data, rec_size);

   568         fsm->state = ec_fsm_coe_error;

   569 	return;

   570     }

   571 

   572     if (rec_size < 12) {

   573         EC_ERR("Invalid data size!\n");

   574         ec_print_data(data, rec_size);

   575         fsm->state = ec_fsm_coe_error;

   576 	return;

   577     }

   578 

   579     sdo->subindices = EC_READ_U8(data + 10);

   580     sdo->object_code = EC_READ_U8(data + 11);

   581 

   582     name_size = rec_size - 12;

   583     if (name_size) {

   584         if (!(sdo->name = kmalloc(name_size + 1, GFP_ATOMIC))) {

   585             EC_ERR("Failed to allocate SDO name!\n");

   586             fsm->state = ec_fsm_coe_error;

   587             return;

   588         }

   589 

   590         memcpy(sdo->name, data + 12, name_size);

   591         sdo->name[name_size] = 0;

   592     }

   593 

   594     if (EC_READ_U8(data + 2) & 0x80) {

   595         EC_ERR("Fragment follows (not implemented)!\n");

   596         fsm->state = ec_fsm_coe_error;

   597 	return;

   598     }

   599 

   600     // start fetching entries

   601 

   602     fsm->subindex = 0;

   603 

   604     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {

   605         fsm->state = ec_fsm_coe_error;

   606         return;

   607     }

   608 

   609     EC_WRITE_U16(data, 0x8 << 12); // SDO information

   610     EC_WRITE_U8 (data + 2, 0x05); // Get entry description request

   611     EC_WRITE_U8 (data + 3, 0x00);

   612     EC_WRITE_U16(data + 4, 0x0000);

   613     EC_WRITE_U16(data + 6, sdo->index); // SDO index

   614     EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex

   615     EC_WRITE_U8 (data + 9, 0x00); // value info (no values)

   616 

   617     ec_master_queue_datagram(fsm->slave->master, datagram);

   618     fsm->state = ec_fsm_coe_dict_entry_request;

   619 }

   620 

   621 /*****************************************************************************/

   622 

   623 /**

   624    CoE state: DICT ENTRY REQUEST.

   625 */

   626 

   627 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm

   628                                    /**< finite state machine */)

   629 {

   630     ec_datagram_t *datagram = fsm->datagram;

   631     ec_slave_t *slave = fsm->slave;

   632 

   633     if (datagram->state != EC_DATAGRAM_RECEIVED

   634         || datagram->working_counter != 1) {

   635         fsm->state = ec_fsm_coe_error;

   636         EC_ERR("Reception of CoE SDO entry request failed on slave %i.\n",

   637                slave->ring_position);

   638         return;

   639     }

   640 

   641     fsm->cycles_start = datagram->cycles_sent;

   642 

   643     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   644     ec_master_queue_datagram(fsm->slave->master, datagram);

   645     fsm->state = ec_fsm_coe_dict_entry_check;

   646 }

   647 

   648 /*****************************************************************************/

   649 

   650 /**

   651    CoE state: DICT ENTRY CHECK.

   652 */

   653 

   654 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm

   655                                  /**< finite state machine */)

   656 {

   657     ec_datagram_t *datagram = fsm->datagram;

   658     ec_slave_t *slave = fsm->slave;

   659 

   660     if (datagram->state != EC_DATAGRAM_RECEIVED

   661         || datagram->working_counter != 1) {

   662         fsm->state = ec_fsm_coe_error;

   663         EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n",

   664                slave->ring_position);

   665         return;

   666     }

   667 

   668     if (!ec_slave_mbox_check(datagram)) {

   669         if (datagram->cycles_received

   670             - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {

   671             fsm->state = ec_fsm_coe_error;

   672             EC_ERR("Timeout while checking SDO entry on slave %i.\n",

   673                    slave->ring_position);

   674             return;

   675         }

   676 

   677         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   678         ec_master_queue_datagram(fsm->slave->master, datagram);

   679         return;

   680     }

   681 

   682     // Fetch response

   683     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   684     ec_master_queue_datagram(fsm->slave->master, datagram);

   685     fsm->state = ec_fsm_coe_dict_entry_response;

   686 }

   687 

   688 /*****************************************************************************/

   689 

   690 /**

   691    CoE state: DICT ENTRY RESPONSE.

   692 */

   693 

   694 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm

   695                                     /**< finite state machine */)

   696 {

   697     ec_datagram_t *datagram = fsm->datagram;

   698     ec_slave_t *slave = fsm->slave;

   699     ec_sdo_t *sdo = fsm->sdo;

   700     uint8_t *data, mbox_prot;

   701     size_t rec_size, data_size;

   702     ec_sdo_entry_t *entry;

   703 

   704     if (datagram->state != EC_DATAGRAM_RECEIVED

   705         || datagram->working_counter != 1) {

   706         fsm->state = ec_fsm_coe_error;

   707         EC_ERR("Reception of CoE SDO description"

   708                " response failed on slave %i.\n", slave->ring_position);

   709         return;

   710     }

   711 

   712     if (!(data = ec_slave_mbox_fetch(slave, datagram,

   713 				     &mbox_prot, &rec_size))) {

   714         fsm->state = ec_fsm_coe_error;

   715         return;

   716     }

   717 

   718     if (mbox_prot != 0x03) { // CoE

   719         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   720         fsm->state = ec_fsm_coe_error;

   721 	return;

   722     }

   723 

   724     if (EC_READ_U16(data) >> 12 == 0x8 && // SDO information

   725         (EC_READ_U8 (data + 2) & 0x7F) == 0x07) { // error response

   726         EC_ERR("SDO information error response at slave %i while"

   727                " fetching SDO entry 0x%04X:%i!\n", slave->ring_position,

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

   729         ec_canopen_abort_msg(EC_READ_U32(data + 6));

   730         fsm->state = ec_fsm_coe_error;

   731 	return;

   732     }

   733 

   734     if (EC_READ_U16(data) >> 12 != 0x8 || // SDO information

   735         (EC_READ_U8(data + 2) & 0x7F) != 0x06 || // Entry desc. response

   736         EC_READ_U16(data + 6) != sdo->index || // SDO index

   737         EC_READ_U8(data + 8) != fsm->subindex) { // SDO subindex

   738         EC_ERR("Invalid entry description response at slave %i while"

   739                " fetching SDO entry 0x%04X:%i!\n", slave->ring_position,

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

   741         ec_print_data(data, rec_size);

   742         fsm->state = ec_fsm_coe_error;

   743 	return;

   744     }

   745 

   746     if (rec_size < 16) {

   747         EC_ERR("Invalid data size!\n");

   748         ec_print_data(data, rec_size);

   749         fsm->state = ec_fsm_coe_error;

   750 	return;

   751     }

   752 

   753     data_size = rec_size - 16;

   754 

   755     if (!(entry = (ec_sdo_entry_t *)

   756           kmalloc(sizeof(ec_sdo_entry_t), GFP_ATOMIC))) {

   757         EC_ERR("Failed to allocate entry!\n");

   758         fsm->state = ec_fsm_coe_error;

   759 	return;

   760     }

   761 

   762     if (ec_sdo_entry_init(entry, fsm->subindex, sdo)) {

   763         EC_ERR("Failed to init entry!\n");

   764         fsm->state = ec_fsm_coe_error;

   765 	return;

   766     }

   767 

   768     entry->data_type = EC_READ_U16(data + 10);

   769     entry->bit_length = EC_READ_U16(data + 12);

   770 

   771     if (data_size) {

   772         uint8_t *desc;

   773         if (!(desc = kmalloc(data_size + 1, GFP_ATOMIC))) {

   774             EC_ERR("Failed to allocate SDO entry name!\n");

   775             fsm->state = ec_fsm_coe_error;

   776             return;

   777         }

   778         memcpy(desc, data + 16, data_size);

   779         desc[data_size] = 0;

   780         entry->description = desc;

   781     }

   782 

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

   784 

   785     if (fsm->subindex < sdo->subindices) {

   786         fsm->subindex++;

   787 

   788         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {

   789             fsm->state = ec_fsm_coe_error;

   790             return;

   791         }

   792 

   793         EC_WRITE_U16(data, 0x8 << 12); // SDO information

   794         EC_WRITE_U8 (data + 2, 0x05); // Get entry description request

   795         EC_WRITE_U8 (data + 3, 0x00);

   796         EC_WRITE_U16(data + 4, 0x0000);

   797         EC_WRITE_U16(data + 6, sdo->index); // SDO index

   798         EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex

   799         EC_WRITE_U8 (data + 9, 0x00); // value info (no values)

   800 

   801         ec_master_queue_datagram(fsm->slave->master, datagram);

   802         fsm->state = ec_fsm_coe_dict_entry_request;

   803         return;

   804     }

   805 

   806     // another SDO description to fetch?

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

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

   809 

   810         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 8))) {

   811             fsm->state = ec_fsm_coe_error;

   812             return;

   813         }

   814 

   815         EC_WRITE_U16(data, 0x8 << 12); // SDO information

   816         EC_WRITE_U8 (data + 2, 0x03); // Get object description request

   817         EC_WRITE_U8 (data + 3, 0x00);

   818         EC_WRITE_U16(data + 4, 0x0000);

   819         EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index

   820 

   821         ec_master_queue_datagram(fsm->slave->master, datagram);

   822         fsm->state = ec_fsm_coe_dict_desc_request;

   823         return;

   824     }

   825 

   826     fsm->state = ec_fsm_coe_end;

   827 }

   828 

   829 /******************************************************************************

   830  *  CoE state machine

   831  *****************************************************************************/

   832 

   833 /**

   834    CoE state: DOWN START.

   835 */

   836 

   837 void ec_fsm_coe_down_start(ec_fsm_coe_t *fsm /**< finite state machine */)

   838 {

   839     ec_datagram_t *datagram = fsm->datagram;

   840     ec_slave_t *slave = fsm->slave;

   841     ec_sdo_data_t *sdodata = fsm->sdodata;

   842     uint8_t *data;

   843 

   844     if (fsm->slave->master->debug_level)

   845         EC_DBG("Downloading SDO 0x%04X:%i to slave %i.\n",

   846                sdodata->index, sdodata->subindex, slave->ring_position);

   847 

   848     if (slave->sii_rx_mailbox_size < 6 + 10 + sdodata->size) {

   849         EC_ERR("SDO fragmenting not supported yet!\n");

   850         fsm->state = ec_fsm_coe_error;

   851         return;

   852     }

   853 

   854     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03,

   855                                             sdodata->size + 10))) {

   856         fsm->state = ec_fsm_coe_error;

   857         return;

   858     }

   859 

   860     EC_WRITE_U16(data, 0x2 << 12); // SDO request

   861     EC_WRITE_U8 (data + 2, (0x1 // size specified

   862                             | 0x1 << 5)); // Download request

   863     EC_WRITE_U16(data + 3, sdodata->index);

   864     EC_WRITE_U8 (data + 5, sdodata->subindex);

   865     EC_WRITE_U32(data + 6, sdodata->size);

   866     memcpy(data + 10, sdodata->data, sdodata->size);

   867 

   868     ec_master_queue_datagram(fsm->slave->master, datagram);

   869     fsm->state = ec_fsm_coe_down_request;

   870 }

   871 

   872 /*****************************************************************************/

   873 

   874 /**

   875    CoE state: DOWN REQUEST.

   876 */

   877 

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

   879 {

   880     ec_datagram_t *datagram = fsm->datagram;

   881     ec_slave_t *slave = fsm->slave;

   882 

   883     if (datagram->state != EC_DATAGRAM_RECEIVED

   884         || datagram->working_counter != 1) {

   885         fsm->state = ec_fsm_coe_error;

   886         EC_ERR("Reception of CoE download request failed.\n");

   887         return;

   888     }

   889 

   890     fsm->cycles_start = datagram->cycles_sent;

   891 

   892     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   893     ec_master_queue_datagram(fsm->slave->master, datagram);

   894     fsm->state = ec_fsm_coe_down_check;

   895 }

   896 

   897 /*****************************************************************************/

   898 

   899 /**

   900    CoE state: DOWN CHECK.

   901 */

   902 

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

   904 {

   905     ec_datagram_t *datagram = fsm->datagram;

   906     ec_slave_t *slave = fsm->slave;

   907 

   908     if (datagram->state != EC_DATAGRAM_RECEIVED

   909         || datagram->working_counter != 1) {

   910         fsm->state = ec_fsm_coe_error;

   911         EC_ERR("Reception of CoE mailbox check datagram failed.\n");

   912         return;

   913     }

   914 

   915     if (!ec_slave_mbox_check(datagram)) {

   916         if (datagram->cycles_received

   917             - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {

   918             fsm->state = ec_fsm_coe_error;

   919             EC_ERR("Timeout while checking SDO configuration on slave %i.\n",

   920                    slave->ring_position);

   921             return;

   922         }

   923 

   924         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

   925         ec_master_queue_datagram(fsm->slave->master, datagram);

   926         return;

   927     }

   928 

   929     // Fetch response

   930     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

   931     ec_master_queue_datagram(fsm->slave->master, datagram);

   932     fsm->state = ec_fsm_coe_down_response;

   933 }

   934 

   935 /*****************************************************************************/

   936 

   937 /**

   938    CoE state: DOWN RESPONSE.

   939 */

   940 

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

   942 {

   943     ec_datagram_t *datagram = fsm->datagram;

   944     ec_slave_t *slave = fsm->slave;

   945     uint8_t *data, mbox_prot;

   946     size_t rec_size;

   947     ec_sdo_data_t *sdodata = fsm->sdodata;

   948 

   949     if (datagram->state != EC_DATAGRAM_RECEIVED

   950         || datagram->working_counter != 1) {

   951         fsm->state = ec_fsm_coe_error;

   952         EC_ERR("Reception of CoE download response failed.\n");

   953         return;

   954     }

   955 

   956     if (!(data = ec_slave_mbox_fetch(slave, datagram,

   957 				     &mbox_prot, &rec_size))) {

   958         fsm->state = ec_fsm_coe_error;

   959         return;

   960     }

   961 

   962     if (mbox_prot != 0x03) { // CoE

   963         EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

   964         fsm->state = ec_fsm_coe_error;

   965 	return;

   966     }

   967 

   968     if (rec_size < 6) {

   969         fsm->state = ec_fsm_coe_error;

   970         EC_ERR("Received data is too small (%i bytes):\n", rec_size);

   971         ec_print_data(data, rec_size);

   972         return;

   973     }

   974 

   975     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

   976         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

   977         fsm->state = ec_fsm_coe_error;

   978         EC_ERR("SDO download 0x%04X:%X (%i bytes) aborted on slave %i.\n",

   979                sdodata->index, sdodata->subindex, sdodata->size,

   980                slave->ring_position);

   981         if (rec_size < 10) {

   982             EC_ERR("Incomplete Abort command:\n");

   983             ec_print_data(data, rec_size);

   984         }

   985         else

   986             ec_canopen_abort_msg(EC_READ_U32(data + 6));

   987         return;

   988     }

   989 

   990     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

   991         EC_READ_U8 (data + 2) >> 5 != 0x3 || // Download response

   992         EC_READ_U16(data + 3) != sdodata->index || // index

   993         EC_READ_U8 (data + 5) != sdodata->subindex) { // subindex

   994         fsm->state = ec_fsm_coe_error;

   995         EC_ERR("SDO download 0x%04X:%X (%i bytes) failed:\n",

   996                sdodata->index, sdodata->subindex, sdodata->size);

   997         EC_ERR("Invalid SDO download response at slave %i!\n",

   998                slave->ring_position);

   999         ec_print_data(data, rec_size);

  1000         return;

  1001     }

  1002 

  1003     fsm->state = ec_fsm_coe_end; // success

  1004 }

  1005 

  1006 /*****************************************************************************/

  1007 

  1008 /**

  1009    CoE state: UP START.

  1010 */

  1011 

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

  1013 {

  1014     ec_datagram_t *datagram = fsm->datagram;

  1015     ec_slave_t *slave = fsm->slave;

  1016     ec_master_t *master = slave->master;

  1017     ec_sdo_request_t *request = fsm->request;

  1018     ec_sdo_t *sdo = request->sdo;

  1019     ec_sdo_entry_t *entry = request->entry;

  1020     uint8_t *data;

  1021 

  1022     if (master->debug_level)

  1023         EC_DBG("Uploading SDO 0x%04X:%i from slave %i.\n",

  1024                sdo->index, entry->subindex, slave->ring_position);

  1025 

  1026     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 10))) {

  1027         fsm->state = ec_fsm_coe_error;

  1028         return;

  1029     }

  1030 

  1031     EC_WRITE_U16(data, 0x2 << 12); // SDO request

  1032     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request

  1033     EC_WRITE_U16(data + 3, sdo->index);

  1034     EC_WRITE_U8 (data + 5, entry->subindex);

  1035     memset(data + 6, 0x00, 4);

  1036 

  1037     if (master->debug_level) {

  1038         EC_DBG("Upload request:\n");

  1039         ec_print_data(data, 10);

  1040     }

  1041 

  1042     ec_master_queue_datagram(fsm->slave->master, datagram);

  1043     fsm->state = ec_fsm_coe_up_request;

  1044 }

  1045 

  1046 /*****************************************************************************/

  1047 

  1048 /**

  1049    CoE state: UP REQUEST.

  1050 */

  1051 

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

  1053 {

  1054     ec_datagram_t *datagram = fsm->datagram;

  1055     ec_slave_t *slave = fsm->slave;

  1056 

  1057     if (datagram->state != EC_DATAGRAM_RECEIVED

  1058         || datagram->working_counter != 1) {

  1059         fsm->state = ec_fsm_coe_error;

  1060         EC_ERR("Reception of CoE upload request failed.\n");

  1061         return;

  1062     }

  1063 

  1064     fsm->cycles_start = datagram->cycles_sent;

  1065 

  1066     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  1067     ec_master_queue_datagram(fsm->slave->master, datagram);

  1068     fsm->state = ec_fsm_coe_up_check;

  1069 }

  1070 

  1071 /*****************************************************************************/

  1072 

  1073 /**

  1074    CoE state: UP CHECK.

  1075 */

  1076 

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

  1078 {

  1079     ec_datagram_t *datagram = fsm->datagram;

  1080     ec_slave_t *slave = fsm->slave;

  1081 

  1082     if (datagram->state != EC_DATAGRAM_RECEIVED

  1083         || datagram->working_counter != 1) {

  1084         fsm->state = ec_fsm_coe_error;

  1085         EC_ERR("Reception of CoE mailbox check datagram failed.\n");

  1086         return;

  1087     }

  1088 

  1089     if (!ec_slave_mbox_check(datagram)) {

  1090         if (datagram->cycles_received

  1091             - fsm->cycles_start >= (cycles_t) 100 * cpu_khz) {

  1092             fsm->state = ec_fsm_coe_error;

  1093             EC_ERR("Timeout while checking SDO upload on slave %i.\n",

  1094                    slave->ring_position);

  1095             return;

  1096         }

  1097 

  1098         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  1099         ec_master_queue_datagram(fsm->slave->master, datagram);

  1100         return;

  1101     }

  1102 

  1103     // Fetch response

  1104     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

  1105     ec_master_queue_datagram(fsm->slave->master, datagram);

  1106     fsm->state = ec_fsm_coe_up_response;

  1107 }

  1108 

  1109 /*****************************************************************************/

  1110 

  1111 /**

  1112    CoE state: UP RESPONSE.

  1113 */

  1114 

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

  1116 {

  1117     ec_datagram_t *datagram = fsm->datagram;

  1118     ec_slave_t *slave = fsm->slave;

  1119     ec_master_t *master = slave->master;

  1120     uint8_t *data, mbox_prot;

  1121     size_t rec_size, data_size;

  1122     ec_sdo_request_t *request = fsm->request;

  1123     ec_sdo_t *sdo = request->sdo;

  1124     ec_sdo_entry_t *entry = request->entry;

  1125     uint32_t complete_size;

  1126     unsigned int expedited, size_specified;

  1127 

  1128     if (datagram->state != EC_DATAGRAM_RECEIVED

  1129         || datagram->working_counter != 1) {

  1130         fsm->state = ec_fsm_coe_error;

  1131         EC_ERR("Reception of CoE upload response failed.\n");

  1132         return;

  1133     }

  1134 

  1135     if (!(data = ec_slave_mbox_fetch(slave, datagram,