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_WARN("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             EC_WARN("SDO dictionary of slave %i contains index 0x0000.\n",

   399                     slave->ring_position);

   400             continue;

   401         }

   402 

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

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

   405             fsm->state = ec_fsm_coe_error;

   406             return;

   407         }

   408 

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

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

   411             fsm->state = ec_fsm_coe_error;

   412             return;

   413         }

   414 

   415         if (kobject_add(&sdo->kobj)) {

   416             EC_ERR("Failed to add kobject.\n");

   417             kobject_put(&sdo->kobj); // free

   418             fsm->state = ec_fsm_coe_error;

   419             return;

   420         }

   421 

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

   423     }

   424 

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

   426         fsm->cycles_start = datagram->cycles_sent;

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

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

   429         fsm->state = ec_fsm_coe_dict_check;

   430         return;

   431     }

   432 

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

   434         // no SDOs in dictionary. finished.

   435         fsm->state = ec_fsm_coe_end; // success

   436         return;

   437     }

   438 

   439     // fetch SDO descriptions

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

   441 

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

   443         fsm->state = ec_fsm_coe_error;

   444         return;

   445     }

   446 

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

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

   449     EC_WRITE_U8 (data + 3, 0x00);

   450     EC_WRITE_U16(data + 4, 0x0000);

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

   452 

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

   454     fsm->state = ec_fsm_coe_dict_desc_request;

   455 }

   456 

   457 /*****************************************************************************/

   458 

   459 /**

   460    CoE state: DICT DESC REQUEST.

   461 */

   462 

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

   464 {

   465     ec_datagram_t *datagram = fsm->datagram;

   466     ec_slave_t *slave = fsm->slave;

   467 

   468     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   470         fsm->state = ec_fsm_coe_error;

   471         EC_ERR("Reception of CoE SDO description"

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

   473         return;

   474     }

   475 

   476     fsm->cycles_start = datagram->cycles_sent;

   477 

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

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

   480     fsm->state = ec_fsm_coe_dict_desc_check;

   481 }

   482 

   483 /*****************************************************************************/

   484 

   485 /**

   486    CoE state: DICT DESC CHECK.

   487 */

   488 

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

   490 {

   491     ec_datagram_t *datagram = fsm->datagram;

   492     ec_slave_t *slave = fsm->slave;

   493 

   494     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   496         fsm->state = ec_fsm_coe_error;

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

   498                slave->ring_position);

   499         return;

   500     }

   501 

   502     if (!ec_slave_mbox_check(datagram)) {

   503         if (datagram->cycles_received

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

   505             fsm->state = ec_fsm_coe_error;

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

   507                    slave->ring_position);

   508             return;

   509         }

   510 

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

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

   513         return;

   514     }

   515 

   516     // Fetch response

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

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

   519     fsm->state = ec_fsm_coe_dict_desc_response;

   520 }

   521 

   522 /*****************************************************************************/

   523 

   524 /**

   525    CoE state: DICT DESC RESPONSE.

   526 */

   527 

   528 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm

   529                                    /**< finite state machine */)

   530 {

   531     ec_datagram_t *datagram = fsm->datagram;

   532     ec_slave_t *slave = fsm->slave;

   533     ec_sdo_t *sdo = fsm->sdo;

   534     uint8_t *data, mbox_prot;

   535     size_t rec_size, name_size;

   536 

   537     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   539         fsm->state = ec_fsm_coe_error;

   540         EC_ERR("Reception of CoE SDO description"

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

   542         return;

   543     }

   544 

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

   546 				     &mbox_prot, &rec_size))) {

   547         fsm->state = ec_fsm_coe_error;

   548         return;

   549     }

   550 

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

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

   553         fsm->state = ec_fsm_coe_error;

   554 	return;

   555     }

   556 

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

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

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

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

   561                sdo->index);

   562         ec_canopen_abort_msg(EC_READ_U32(data + 6));

   563         fsm->state = ec_fsm_coe_error;

   564 	return;

   565     }

   566 

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

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

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

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

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

   572                sdo->index);

   573         ec_print_data(data, rec_size);

   574         fsm->state = ec_fsm_coe_error;

   575 	return;

   576     }

   577 

   578     if (rec_size < 12) {

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

   580         ec_print_data(data, rec_size);

   581         fsm->state = ec_fsm_coe_error;

   582 	return;

   583     }

   584 

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

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

   587 

   588     name_size = rec_size - 12;

   589     if (name_size) {

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

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

   592             fsm->state = ec_fsm_coe_error;

   593             return;

   594         }

   595 

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

   597         sdo->name[name_size] = 0;

   598     }

   599 

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

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

   602         fsm->state = ec_fsm_coe_error;

   603 	return;

   604     }

   605 

   606     // start fetching entries

   607 

   608     fsm->subindex = 0;

   609 

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

   611         fsm->state = ec_fsm_coe_error;

   612         return;

   613     }

   614 

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

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

   617     EC_WRITE_U8 (data + 3, 0x00);

   618     EC_WRITE_U16(data + 4, 0x0000);

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

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

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

   622 

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

   624     fsm->state = ec_fsm_coe_dict_entry_request;

   625 }

   626 

   627 /*****************************************************************************/

   628 

   629 /**

   630    CoE state: DICT ENTRY REQUEST.

   631 */

   632 

   633 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm

   634                                    /**< finite state machine */)

   635 {

   636     ec_datagram_t *datagram = fsm->datagram;

   637     ec_slave_t *slave = fsm->slave;

   638 

   639     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   641         fsm->state = ec_fsm_coe_error;

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

   643                slave->ring_position);

   644         return;

   645     }

   646 

   647     fsm->cycles_start = datagram->cycles_sent;

   648 

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

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

   651     fsm->state = ec_fsm_coe_dict_entry_check;

   652 }

   653 

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

   655 

   656 /**

   657    CoE state: DICT ENTRY CHECK.

   658 */

   659 

   660 void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *fsm

   661                                  /**< finite state machine */)

   662 {

   663     ec_datagram_t *datagram = fsm->datagram;

   664     ec_slave_t *slave = fsm->slave;

   665 

   666     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   668         fsm->state = ec_fsm_coe_error;

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

   670                slave->ring_position);

   671         return;

   672     }

   673 

   674     if (!ec_slave_mbox_check(datagram)) {

   675         if (datagram->cycles_received

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

   677             fsm->state = ec_fsm_coe_error;

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

   679                    slave->ring_position);

   680             return;

   681         }

   682 

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

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

   685         return;

   686     }

   687 

   688     // Fetch response

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

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

   691     fsm->state = ec_fsm_coe_dict_entry_response;

   692 }

   693 

   694 /*****************************************************************************/

   695 

   696 /**

   697    CoE state: DICT ENTRY RESPONSE.

   698 */

   699 

   700 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm

   701                                     /**< finite state machine */)

   702 {

   703     ec_datagram_t *datagram = fsm->datagram;

   704     ec_slave_t *slave = fsm->slave;

   705     ec_sdo_t *sdo = fsm->sdo;

   706     uint8_t *data, mbox_prot;

   707     size_t rec_size, data_size;

   708     ec_sdo_entry_t *entry;

   709 

   710     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   712         fsm->state = ec_fsm_coe_error;

   713         EC_ERR("Reception of CoE SDO description"

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

   715         return;

   716     }

   717 

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

   719 				     &mbox_prot, &rec_size))) {

   720         fsm->state = ec_fsm_coe_error;

   721         return;

   722     }

   723 

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

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

   726         fsm->state = ec_fsm_coe_error;

   727 	return;

   728     }

   729 

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

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

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

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

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

   735         ec_canopen_abort_msg(EC_READ_U32(data + 6));

   736         fsm->state = ec_fsm_coe_error;

   737 	return;

   738     }

   739 

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

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

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

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

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

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

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

   747         ec_print_data(data, rec_size);

   748         fsm->state = ec_fsm_coe_error;

   749 	return;

   750     }

   751 

   752     if (rec_size < 16) {

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

   754         ec_print_data(data, rec_size);

   755         fsm->state = ec_fsm_coe_error;

   756 	return;

   757     }

   758 

   759     data_size = rec_size - 16;

   760 

   761     if (!(entry = (ec_sdo_entry_t *)

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

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

   764         fsm->state = ec_fsm_coe_error;

   765 	return;

   766     }

   767 

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

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

   770         fsm->state = ec_fsm_coe_error;

   771 	return;

   772     }

   773 

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

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

   776 

   777     if (data_size) {

   778         if (!(entry->description = kmalloc(data_size + 1, GFP_ATOMIC))) {

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

   780             fsm->state = ec_fsm_coe_error;

   781             return;

   782         }

   783         memcpy(entry->description, data + 16, data_size);

   784         entry->description[data_size] = 0;

   785     }

   786 

   787     if (kobject_add(&entry->kobj)) {

   788         EC_ERR("Failed to add kobject.\n");

   789         kobject_put(&entry->kobj); // free

   790         fsm->state = ec_fsm_coe_error;

   791         return;

   792     }

   793 

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

   795 

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

   797         fsm->subindex++;

   798 

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

   800             fsm->state = ec_fsm_coe_error;

   801             return;

   802         }

   803 

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

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

   806         EC_WRITE_U8 (data + 3, 0x00);

   807         EC_WRITE_U16(data + 4, 0x0000);

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

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

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

   811 

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

   813         fsm->state = ec_fsm_coe_dict_entry_request;

   814         return;

   815     }

   816 

   817     // another SDO description to fetch?

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

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

   820 

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

   822             fsm->state = ec_fsm_coe_error;

   823             return;

   824         }

   825 

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

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

   828         EC_WRITE_U8 (data + 3, 0x00);

   829         EC_WRITE_U16(data + 4, 0x0000);

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

   831 

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

   833         fsm->state = ec_fsm_coe_dict_desc_request;

   834         return;

   835     }

   836 

   837     fsm->state = ec_fsm_coe_end;

   838 }

   839 

   840 /******************************************************************************

   841  *  CoE state machine

   842  *****************************************************************************/

   843 

   844 /**

   845    CoE state: DOWN START.

   846 */

   847 

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

   849 {

   850     ec_datagram_t *datagram = fsm->datagram;

   851     ec_slave_t *slave = fsm->slave;

   852     ec_sdo_data_t *sdodata = fsm->sdodata;

   853     uint8_t *data;

   854 

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

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

   857 

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

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

   860         fsm->state = ec_fsm_coe_error;

   861         return;

   862     }

   863 

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

   865                                             sdodata->size + 10))) {

   866         fsm->state = ec_fsm_coe_error;

   867         return;

   868     }

   869 

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

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

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

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

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

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

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

   877 

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

   879     fsm->state = ec_fsm_coe_down_request;

   880 }

   881 

   882 /*****************************************************************************/

   883 

   884 /**

   885    CoE state: DOWN REQUEST.

   886 */

   887 

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

   889 {

   890     ec_datagram_t *datagram = fsm->datagram;

   891     ec_slave_t *slave = fsm->slave;

   892 

   893     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   895         fsm->state = ec_fsm_coe_error;

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

   897         return;

   898     }

   899 

   900     fsm->cycles_start = datagram->cycles_sent;

   901 

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

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

   904     fsm->state = ec_fsm_coe_down_check;

   905 }

   906 

   907 /*****************************************************************************/

   908 

   909 /**

   910    CoE state: DOWN CHECK.

   911 */

   912 

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

   914 {

   915     ec_datagram_t *datagram = fsm->datagram;

   916     ec_slave_t *slave = fsm->slave;

   917 

   918     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   920         fsm->state = ec_fsm_coe_error;

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

   922         return;

   923     }

   924 

   925     if (!ec_slave_mbox_check(datagram)) {

   926         if (datagram->cycles_received

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

   928             fsm->state = ec_fsm_coe_error;

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

   930                    slave->ring_position);

   931             return;

   932         }

   933 

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

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

   936         return;

   937     }

   938 

   939     // Fetch response

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

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

   942     fsm->state = ec_fsm_coe_down_response;

   943 }

   944 

   945 /*****************************************************************************/

   946 

   947 /**

   948    CoE state: DOWN RESPONSE.

   949 */

   950 

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

   952 {

   953     ec_datagram_t *datagram = fsm->datagram;

   954     ec_slave_t *slave = fsm->slave;

   955     uint8_t *data, mbox_prot;

   956     size_t rec_size;

   957     ec_sdo_data_t *sdodata = fsm->sdodata;

   958 

   959     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   961         fsm->state = ec_fsm_coe_error;

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

   963         return;

   964     }

   965 

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

   967 				     &mbox_prot, &rec_size))) {

   968         fsm->state = ec_fsm_coe_error;

   969         return;

   970     }

   971 

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

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

   974         fsm->state = ec_fsm_coe_error;

   975 	return;

   976     }

   977 

   978     if (rec_size < 6) {

   979         fsm->state = ec_fsm_coe_error;

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

   981         ec_print_data(data, rec_size);

   982         return;

   983     }

   984 

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

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

   987         fsm->state = ec_fsm_coe_error;

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

   989                sdodata->index, sdodata->subindex, sdodata->size,

   990                slave->ring_position);

   991         if (rec_size < 10) {

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

   993             ec_print_data(data, rec_size);

   994         }

   995         else

   996             ec_canopen_abort_msg(EC_READ_U32(data + 6));

   997         return;

   998     }

   999 

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

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

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

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

  1004         fsm->state = ec_fsm_coe_error;

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

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

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

  1008                slave->ring_position);

  1009         ec_print_data(data, rec_size);

  1010         return;

  1011     }

  1012 

  1013     fsm->state = ec_fsm_coe_end; // success

  1014 }

  1015 

  1016 /*****************************************************************************/

  1017 

  1018 /**

  1019    CoE state: UP START.

  1020 */

  1021 

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

  1023 {

  1024     ec_datagram_t *datagram = fsm->datagram;

  1025     ec_slave_t *slave = fsm->slave;

  1026     ec_master_t *master = slave->master;

  1027     ec_sdo_request_t *request = fsm->request;

  1028     ec_sdo_t *sdo = request->sdo;

  1029     ec_sdo_entry_t *entry = request->entry;

  1030     uint8_t *data;

  1031 

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

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

  1034 

  1035     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 6))) {

  1036         fsm->state = ec_fsm_coe_error;

  1037         return;

  1038     }

  1039 

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

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

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

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

  1044 

  1045     if (master->debug_level) {

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

  1047         ec_print_data(data, 6);

  1048     }

  1049 

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

  1051     fsm->state = ec_fsm_coe_up_request;

  1052 }

  1053 

  1054 /*****************************************************************************/

  1055 

  1056 /**

  1057    CoE state: UP REQUEST.

  1058 */

  1059 

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

  1061 {

  1062     ec_datagram_t *datagram = fsm->datagram;

  1063     ec_slave_t *slave = fsm->slave;

  1064 

  1065     if (datagram->state != EC_DATAGRAM_RECEIVED

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

  1067         fsm->state = ec_fsm_coe_error;

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

  1069         return;

  1070     }

  1071 

  1072     fsm->cycles_start = datagram->cycles_sent;

  1073 

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

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

  1076     fsm->state = ec_fsm_coe_up_check;

  1077 }

  1078 

  1079 /*****************************************************************************/

  1080 

  1081 /**

  1082    CoE state: UP CHECK.

  1083 */

  1084 

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

  1086 {

  1087     ec_datagram_t *datagram = fsm->datagram;

  1088     ec_slave_t *slave = fsm->slave;

  1089 

  1090     if (datagram->state != EC_DATAGRAM_RECEIVED

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

  1092         fsm->state = ec_fsm_coe_error;

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

  1094         return;

  1095     }

  1096 

  1097     if (!ec_slave_mbox_check(datagram)) {

  1098         if (datagram->cycles_received

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

  1100             fsm->state = ec_fsm_coe_error;

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

  1102                    slave->ring_position);

  1103             return;

  1104         }

  1105 

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

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

  1108         return;

  1109     }

  1110 

  1111     // Fetch response

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

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

  1114     fsm->state = ec_fsm_coe_up_response;

  1115 }

  1116 

  1117 /*****************************************************************************/

  1118 

  1119 /**

  1120    CoE state: UP RESPONSE.

  1121 */

  1122 

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

  1124 {

  1125     ec_datagram_t *datagram = fsm->datagram;

  1126     ec_slave_t *slave = fsm->slave;

  1127     ec_master_t *master = slave->master;

  1128     uint8_t *data, mbox_prot;

  1129     size_t rec_size, data_size;

  1130     ec_sdo_request_t *request = fsm->request;

  1131     ec_sdo_t *sdo = request->sdo;

  1132     ec_sdo_entry_t *entry = request->entry;

  1133     uint32_t complete_size;

  1134     unsigned int expedited, size_specified;

  1135 

  1136     if (datagram->state != EC_DATAGRAM_RECEIVED