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 slave state machines.

    37 */

    38 

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

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "slave_config.h"

    45 #include "fsm_slave.h"

    46 

    47 /*****************************************************************************/

    48 

    49 void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *);

    50 void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *);

    51 void ec_fsm_slave_scan_state_state(ec_fsm_slave_t *);

    52 void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *);

    53 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *);

    54 void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *);

    55 void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *);

    56 

    57 void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *);

    58 void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *);

    59 void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *);

    60 void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *);

    61 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *);

    62 void ec_fsm_slave_conf_state_sdo_conf(ec_fsm_slave_t *);

    63 void ec_fsm_slave_conf_state_mapping(ec_fsm_slave_t *);

    64 void ec_fsm_slave_conf_state_pdo_conf(ec_fsm_slave_t *);

    65 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *);

    66 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *);

    67 void ec_fsm_slave_conf_state_safeop(ec_fsm_slave_t *);

    68 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *);

    69 

    70 void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *);

    71 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *);

    72 void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *);

    73 void ec_fsm_slave_conf_enter_mapping(ec_fsm_slave_t *);

    74 void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *);

    75 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *);

    76 void ec_fsm_slave_conf_enter_safeop(ec_fsm_slave_t *);

    77 

    78 void ec_fsm_slave_state_end(ec_fsm_slave_t *);

    79 void ec_fsm_slave_state_error(ec_fsm_slave_t *);

    80 

    81 /*****************************************************************************/

    82 

    83 /** Constructor.

    84  */

    85 void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */

    86         ec_datagram_t *datagram /**< datagram structure to use */

    87         )

    88 {

    89     fsm->datagram = datagram;

    90 

    91     // init sub state machines

    92     ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram);

    93     ec_fsm_change_init(&fsm->fsm_change, fsm->datagram);

    94     ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);

    95     ec_fsm_pdo_mapping_init(&fsm->fsm_pdo_map, &fsm->fsm_coe);

    96     ec_fsm_pdo_config_init(&fsm->fsm_pdo_conf, &fsm->fsm_coe);

    97 }

    98 

    99 /*****************************************************************************/

   100 

   101 /** Destructor.

   102  */

   103 void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */)

   104 {

   105     // clear sub state machines

   106     ec_fsm_sii_clear(&fsm->fsm_sii);

   107     ec_fsm_change_clear(&fsm->fsm_change);

   108     ec_fsm_coe_clear(&fsm->fsm_coe);

   109     ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_map);

   110     ec_fsm_pdo_config_clear(&fsm->fsm_pdo_conf);

   111 }

   112 

   113 /*****************************************************************************/

   114 

   115 /**

   116  * Start slave scan state machine.

   117  */

   118 

   119 void ec_fsm_slave_start_scan(ec_fsm_slave_t *fsm, /**< slave state machine */

   120         ec_slave_t *slave /**< slave to configure */

   121         )

   122 {

   123     fsm->slave = slave;

   124     fsm->state = ec_fsm_slave_scan_state_start;

   125 }

   126 

   127 /*****************************************************************************/

   128 

   129 /**

   130  * Start slave configuration state machine.

   131  */

   132 

   133 void ec_fsm_slave_start_conf(ec_fsm_slave_t *fsm, /**< slave state machine */

   134         ec_slave_t *slave /**< slave to configure */

   135         )

   136 {

   137     fsm->slave = slave;

   138     fsm->state = ec_fsm_slave_conf_state_start;

   139 }

   140 

   141 /*****************************************************************************/

   142 

   143 /**

   144    \return false, if state machine has terminated

   145 */

   146 

   147 int ec_fsm_slave_running(const ec_fsm_slave_t *fsm /**< slave state machine */)

   148 {

   149     return fsm->state != ec_fsm_slave_state_end

   150         && fsm->state != ec_fsm_slave_state_error;

   151 }

   152 

   153 /*****************************************************************************/

   154 

   155 /**

   156    Executes the current state of the state machine.

   157    If the state machine's datagram is not sent or received yet, the execution

   158    of the state machine is delayed to the next cycle.

   159    \return false, if state machine has terminated

   160 */

   161 

   162 int ec_fsm_slave_exec(ec_fsm_slave_t *fsm /**< slave state machine */)

   163 {

   164     if (fsm->datagram->state == EC_DATAGRAM_SENT

   165         || fsm->datagram->state == EC_DATAGRAM_QUEUED) {

   166         // datagram was not sent or received yet.

   167         return ec_fsm_slave_running(fsm);

   168     }

   169 

   170     fsm->state(fsm);

   171     return ec_fsm_slave_running(fsm);

   172 }

   173 

   174 /*****************************************************************************/

   175 

   176 /**

   177    \return true, if the state machine terminated gracefully

   178 */

   179 

   180 int ec_fsm_slave_success(const ec_fsm_slave_t *fsm /**< slave state machine */)

   181 {

   182     return fsm->state == ec_fsm_slave_state_end;

   183 }

   184 

   185 /******************************************************************************

   186  *  slave scan state machine

   187  *****************************************************************************/

   188 

   189 /**

   190    Slave scan state: START.

   191    First state of the slave state machine. Writes the station address to the

   192    slave, according to its ring position.

   193 */

   194 

   195 void ec_fsm_slave_scan_state_start(ec_fsm_slave_t *fsm /**< slave state machine */)

   196 {

   197     // write station address

   198     ec_datagram_apwr(fsm->datagram, fsm->slave->ring_position, 0x0010, 2);

   199     EC_WRITE_U16(fsm->datagram->data, fsm->slave->station_address);

   200     fsm->retries = EC_FSM_RETRIES;

   201     fsm->state = ec_fsm_slave_scan_state_address;

   202 }

   203 

   204 /*****************************************************************************/

   205 

   206 /**

   207    Slave scan state: ADDRESS.

   208 */

   209 

   210 void ec_fsm_slave_scan_state_address(ec_fsm_slave_t *fsm /**< slave state machine */)

   211 {

   212     ec_datagram_t *datagram = fsm->datagram;

   213 

   214     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   215         return;

   216 

   217     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   218         fsm->state = ec_fsm_slave_state_error;

   219         EC_ERR("Failed to receive station address datagram for slave %i"

   220                 " (datagram state %i)\n",

   221                 fsm->slave->ring_position, datagram->state);

   222         return;

   223     }

   224 

   225     if (datagram->working_counter != 1) {

   226         fsm->slave->error_flag = 1;

   227         fsm->state = ec_fsm_slave_state_error;

   228         EC_ERR("Failed to write station address on slave %i: ",

   229                fsm->slave->ring_position);

   230         ec_datagram_print_wc_error(datagram);

   231         return;

   232     }

   233 

   234     // Read AL state

   235     ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0130, 2);

   236     fsm->retries = EC_FSM_RETRIES;

   237     fsm->state = ec_fsm_slave_scan_state_state;

   238 }

   239 

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

   241 

   242 /**

   243    Slave scan state: STATE.

   244 */

   245 

   246 void ec_fsm_slave_scan_state_state(

   247         ec_fsm_slave_t *fsm /**< slave state machine */

   248         )

   249 {

   250     ec_datagram_t *datagram = fsm->datagram;

   251     ec_slave_t *slave = fsm->slave;

   252 

   253     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   254         return;

   255 

   256     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   257         fsm->state = ec_fsm_slave_state_error;

   258         EC_ERR("Failed to receive AL state datagram from slave %i"

   259                 " (datagram state %i).\n",

   260                fsm->slave->ring_position, datagram->state);

   261         return;

   262     }

   263 

   264     if (datagram->working_counter != 1) {

   265         fsm->slave->error_flag = 1;

   266         fsm->state = ec_fsm_slave_state_error;

   267         EC_ERR("Failed to read AL state of slave %i: ",

   268                fsm->slave->ring_position);

   269         ec_datagram_print_wc_error(datagram);

   270         return;

   271     }

   272 

   273     slave->current_state = EC_READ_U8(datagram->data);

   274     if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   275         char state_str[EC_STATE_STRING_SIZE];

   276         ec_state_string(slave->current_state, state_str);

   277         EC_WARN("Slave %i has state error bit set (%s)!\n",

   278                 slave->ring_position, state_str);

   279     }

   280 

   281     // read base data

   282     ec_datagram_fprd(datagram, fsm->slave->station_address, 0x0000, 6);

   283     fsm->retries = EC_FSM_RETRIES;

   284     fsm->state = ec_fsm_slave_scan_state_base;

   285 }

   286 

   287 /*****************************************************************************/

   288 

   289 /**

   290    Slave scan state: BASE.

   291 */

   292 

   293 void ec_fsm_slave_scan_state_base(ec_fsm_slave_t *fsm /**< slave state machine */)

   294 {

   295     ec_datagram_t *datagram = fsm->datagram;

   296     ec_slave_t *slave = fsm->slave;

   297 

   298     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   299         return;

   300 

   301     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   302         fsm->state = ec_fsm_slave_state_error;

   303         EC_ERR("Failed to receive base data datagram for slave %i"

   304                 " (datagram state %i).\n",

   305                slave->ring_position, datagram->state);

   306         return;

   307     }

   308 

   309     if (datagram->working_counter != 1) {

   310         fsm->slave->error_flag = 1;

   311         fsm->state = ec_fsm_slave_state_error;

   312         EC_ERR("Failed to read base data from slave %i: ",

   313                slave->ring_position);

   314         ec_datagram_print_wc_error(datagram);

   315         return;

   316     }

   317 

   318     slave->base_type       = EC_READ_U8 (datagram->data);

   319     slave->base_revision   = EC_READ_U8 (datagram->data + 1);

   320     slave->base_build      = EC_READ_U16(datagram->data + 2);

   321     slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4);

   322 

   323     if (slave->base_fmmu_count > EC_MAX_FMMUS) {

   324         EC_WARN("Slave %u has more FMMUs (%u) than the master can"

   325                 " handle (%u).\n", slave->ring_position,

   326                 slave->base_fmmu_count, EC_MAX_FMMUS);

   327         slave->base_fmmu_count = EC_MAX_FMMUS;

   328     }

   329 

   330     // read data link status

   331     ec_datagram_fprd(datagram, slave->station_address, 0x0110, 2);

   332     fsm->retries = EC_FSM_RETRIES;

   333     fsm->state = ec_fsm_slave_scan_state_datalink;

   334 }

   335 

   336 /*****************************************************************************/

   337 

   338 /**

   339    Slave scan state: DATALINK.

   340 */

   341 

   342 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_t *fsm /**< slave state machine */)

   343 {

   344     ec_datagram_t *datagram = fsm->datagram;

   345     ec_slave_t *slave = fsm->slave;

   346     uint16_t dl_status;

   347     unsigned int i;

   348 

   349     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   350         return;

   351 

   352     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   353         fsm->state = ec_fsm_slave_state_error;

   354         EC_ERR("Failed to receive DL status datagram from slave %i"

   355                 " (datagram state %i).\n",

   356                slave->ring_position, datagram->state);

   357         return;

   358     }

   359 

   360     if (datagram->working_counter != 1) {

   361         fsm->slave->error_flag = 1;

   362         fsm->state = ec_fsm_slave_state_error;

   363         EC_ERR("Failed to read DL status from slave %i: ",

   364                slave->ring_position);

   365         ec_datagram_print_wc_error(datagram);

   366         return;

   367     }

   368 

   369     dl_status = EC_READ_U16(datagram->data);

   370     for (i = 0; i < 4; i++) {

   371         slave->dl_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;

   372         slave->dl_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;

   373         slave->dl_signal[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;

   374     }

   375 

   376     // Start fetching EEPROM size

   377 

   378     fsm->sii_offset = EC_FIRST_EEPROM_CATEGORY_OFFSET; // first category header

   379     ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   380             EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   381     fsm->state = ec_fsm_slave_scan_state_eeprom_size;

   382     fsm->state(fsm); // execute state immediately

   383 }

   384 

   385 /*****************************************************************************/

   386 

   387 /**

   388    Slave scan state: EEPROM SIZE.

   389 */

   390 

   391 void ec_fsm_slave_scan_state_eeprom_size(ec_fsm_slave_t *fsm /**< slave state machine */)

   392 {

   393     ec_slave_t *slave = fsm->slave;

   394     uint16_t cat_type, cat_size;

   395 

   396     if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   397 

   398     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   399         fsm->slave->error_flag = 1;

   400         fsm->state = ec_fsm_slave_state_error;

   401         EC_ERR("Failed to read EEPROM size of slave %i.\n",

   402                slave->ring_position);

   403         return;

   404     }

   405 

   406     cat_type = EC_READ_U16(fsm->fsm_sii.value);

   407     cat_size = EC_READ_U16(fsm->fsm_sii.value + 2);

   408 

   409     if (cat_type != 0xFFFF) { // not the last category

   410         off_t next_offset = 2UL + fsm->sii_offset + cat_size;

   411         if (next_offset >= EC_MAX_EEPROM_SIZE) {

   412             EC_WARN("EEPROM size of slave %i exceeds"

   413                     " %u words (0xffff limiter missing?).\n",

   414                     slave->ring_position, EC_MAX_EEPROM_SIZE);

   415             // cut off category data...

   416             slave->eeprom_size = EC_FIRST_EEPROM_CATEGORY_OFFSET * 2;

   417             goto alloc_eeprom;

   418         }

   419         fsm->sii_offset = next_offset;

   420         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   421                         EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   422         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   423         return;

   424     }

   425 

   426     slave->eeprom_size = (fsm->sii_offset + 1) * 2;

   427 

   428 alloc_eeprom:

   429     if (slave->eeprom_data) {

   430         EC_WARN("Freeing old EEPROM data on slave %i...\n",

   431                 slave->ring_position);

   432         kfree(slave->eeprom_data);

   433     }

   434 

   435     if (!(slave->eeprom_data =

   436                 (uint8_t *) kmalloc(slave->eeprom_size, GFP_ATOMIC))) {

   437         EC_ERR("Failed to allocate %u bytes of EEPROM data for slave %u.\n",

   438                slave->eeprom_size, slave->ring_position);

   439         slave->eeprom_size = 0;

   440         slave->error_flag = 1;

   441         fsm->state = ec_fsm_slave_state_error;

   442         return;

   443     }

   444 

   445     // Start fetching EEPROM contents

   446 

   447     fsm->state = ec_fsm_slave_scan_state_eeprom_data;

   448     fsm->sii_offset = 0x0000;

   449     ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   450             EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   451     ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   452 }

   453 

   454 /*****************************************************************************/

   455 

   456 /**

   457    Slave scan state: EEPROM DATA.

   458 */

   459 

   460 void ec_fsm_slave_scan_state_eeprom_data(ec_fsm_slave_t *fsm /**< slave state machine */)

   461 {

   462     ec_slave_t *slave = fsm->slave;

   463     uint16_t *cat_word, cat_type, cat_size, eeprom_word_size = slave->eeprom_size / 2;

   464 

   465     if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;

   466 

   467     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {

   468         fsm->slave->error_flag = 1;

   469         fsm->state = ec_fsm_slave_state_error;

   470         EC_ERR("Failed to fetch EEPROM contents of slave %i.\n",

   471                slave->ring_position);

   472         return;

   473     }

   474 

   475     // 2 words fetched

   476 

   477     if (fsm->sii_offset + 2 <= eeprom_word_size) { // 2 words fit

   478         memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   479                fsm->fsm_sii.value, 4);

   480     }

   481     else { // copy the last word

   482         memcpy(slave->eeprom_data + fsm->sii_offset * 2,

   483                fsm->fsm_sii.value, 2);

   484     }

   485 

   486     if (fsm->sii_offset + 2 < eeprom_word_size) {

   487         // fetch the next 2 words

   488         fsm->sii_offset += 2;

   489         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,

   490                         EC_FSM_SII_USE_CONFIGURED_ADDRESS);

   491         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately

   492         return;

   493     }

   494 

   495     // Evaluate EEPROM contents

   496 

   497     slave->sii_alias =

   498         EC_READ_U16(slave->eeprom_data + 2 * 0x0004);

   499     slave->sii_vendor_id =

   500         EC_READ_U32(slave->eeprom_data + 2 * 0x0008);

   501     slave->sii_product_code =

   502         EC_READ_U32(slave->eeprom_data + 2 * 0x000A);

   503     slave->sii_revision_number =

   504         EC_READ_U32(slave->eeprom_data + 2 * 0x000C);

   505     slave->sii_serial_number =

   506         EC_READ_U32(slave->eeprom_data + 2 * 0x000E);

   507     slave->sii_rx_mailbox_offset =

   508         EC_READ_U16(slave->eeprom_data + 2 * 0x0018);

   509     slave->sii_rx_mailbox_size =

   510         EC_READ_U16(slave->eeprom_data + 2 * 0x0019);

   511     slave->sii_tx_mailbox_offset =

   512         EC_READ_U16(slave->eeprom_data + 2 * 0x001A);

   513     slave->sii_tx_mailbox_size =

   514         EC_READ_U16(slave->eeprom_data + 2 * 0x001B);

   515     slave->sii_mailbox_protocols =

   516         EC_READ_U16(slave->eeprom_data + 2 * 0x001C);

   517 

   518     if (eeprom_word_size == EC_FIRST_EEPROM_CATEGORY_OFFSET) {

   519         // eeprom does not contain category data

   520         fsm->state = ec_fsm_slave_state_end;

   521         return;

   522     }

   523 

   524     if (eeprom_word_size < EC_FIRST_EEPROM_CATEGORY_OFFSET + 1) {

   525         EC_ERR("Unexpected end of EEPROM data in slave %u:"

   526                 " First category header missing.\n",

   527                 slave->ring_position);

   528         goto end;

   529     }

   530 

   531     // evaluate category data

   532     cat_word =

   533         (uint16_t *) slave->eeprom_data + EC_FIRST_EEPROM_CATEGORY_OFFSET;

   534     while (EC_READ_U16(cat_word) != 0xFFFF) {

   535 

   536         // type and size words must fit

   537         if (cat_word + 2 - (uint16_t *) slave->eeprom_data

   538                 > eeprom_word_size) {

   539             EC_ERR("Unexpected end of EEPROM data in slave %u:"

   540                     " Category header incomplete.\n",

   541                     slave->ring_position);

   542             goto end;

   543         }

   544 

   545         cat_type = EC_READ_U16(cat_word) & 0x7FFF;

   546         cat_size = EC_READ_U16(cat_word + 1);

   547         cat_word += 2;

   548 

   549         if (cat_word + cat_size - (uint16_t *) slave->eeprom_data

   550                 > eeprom_word_size) {

   551             EC_WARN("Unexpected end of EEPROM data in slave %u:"

   552                     " Category data incomplete.\n",

   553                     slave->ring_position);

   554             goto end;

   555         }

   556 

   557         switch (cat_type) {

   558             case 0x000A:

   559                 if (ec_slave_fetch_sii_strings(slave, (uint8_t *) cat_word,

   560                             cat_size * 2))

   561                     goto end;

   562                 break;

   563             case 0x001E:

   564                 if (ec_slave_fetch_sii_general(slave, (uint8_t *) cat_word,

   565                             cat_size * 2))

   566                     goto end;

   567                 break;

   568             case 0x0028:

   569                 break;

   570             case 0x0029:

   571                 if (ec_slave_fetch_sii_syncs(slave, (uint8_t *) cat_word,

   572                             cat_size * 2))

   573                     goto end;

   574                 break;

   575             case 0x0032:

   576                 if (ec_slave_fetch_sii_pdos( slave, (uint8_t *) cat_word,

   577                             cat_size * 2, EC_DIR_INPUT)) // TxPdo

   578                     goto end;

   579                 break;

   580             case 0x0033:

   581                 if (ec_slave_fetch_sii_pdos( slave, (uint8_t *) cat_word,

   582                             cat_size * 2, EC_DIR_OUTPUT)) // RxPdo

   583                     goto end;

   584                 break;

   585             default:

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

   587                     EC_WARN("Unknown category type 0x%04X in slave %i.\n",

   588                             cat_type, slave->ring_position);

   589         }

   590 

   591         cat_word += cat_size;

   592         if (cat_word - (uint16_t *) slave->eeprom_data >= eeprom_word_size) {

   593             EC_WARN("Unexpected end of EEPROM data in slave %u:"

   594                     " Next category header missing.\n",

   595                     slave->ring_position);

   596             goto end;

   597         }

   598     }

   599 

   600     fsm->state = ec_fsm_slave_state_end;

   601     return;

   602 

   603 end:

   604     EC_ERR("Failed to analyze category data.\n");

   605     fsm->slave->error_flag = 1;

   606     fsm->state = ec_fsm_slave_state_error;

   607 }

   608 

   609 /******************************************************************************

   610  *  slave configuration state machine

   611  *****************************************************************************/

   612 

   613 /**

   614    Slave configuration state: START.

   615 */

   616 

   617 void ec_fsm_slave_conf_state_start(ec_fsm_slave_t *fsm /**< slave state machine */)

   618 {

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

   620         EC_DBG("Configuring slave %i...\n", fsm->slave->ring_position);

   621     }

   622 

   623     ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT);

   624     ec_fsm_change_exec(&fsm->fsm_change);

   625     fsm->state = ec_fsm_slave_conf_state_init;

   626 }

   627 

   628 /*****************************************************************************/

   629 

   630 /**

   631    Slave configuration state: INIT.

   632 */

   633 

   634 void ec_fsm_slave_conf_state_init(ec_fsm_slave_t *fsm /**< slave state machine */)

   635 {

   636     ec_master_t *master = fsm->slave->master;

   637     ec_slave_t *slave = fsm->slave;

   638     ec_datagram_t *datagram = fsm->datagram;

   639 

   640     if (ec_fsm_change_exec(&fsm->fsm_change)) return;

   641 

   642     if (!ec_fsm_change_success(&fsm->fsm_change)) {

   643         if (!fsm->fsm_change.spontaneous_change)

   644             slave->error_flag = 1;

   645         fsm->state = ec_fsm_slave_state_error;

   646         return;

   647     }

   648 

   649     slave->self_configured = 1;

   650 

   651     if (master->debug_level) {

   652         EC_DBG("Slave %i is now in INIT.\n", slave->ring_position);

   653     }

   654 

   655     // check and reset CRC fault counters

   656     //ec_slave_check_crc(slave);

   657     // TODO: Implement state machine for CRC checking.

   658 

   659     if (!slave->base_fmmu_count) { // skip FMMU configuration

   660         ec_fsm_slave_conf_enter_mbox_sync(fsm);

   661         return;

   662     }

   663 

   664     if (master->debug_level)

   665         EC_DBG("Clearing FMMU configurations of slave %i...\n",

   666                slave->ring_position);

   667 

   668     // clear FMMU configurations

   669     ec_datagram_fpwr(datagram, slave->station_address,

   670             0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   671     memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

   672     fsm->retries = EC_FSM_RETRIES;

   673     fsm->state = ec_fsm_slave_conf_state_clear_fmmus;

   674 }

   675 

   676 /*****************************************************************************/

   677 

   678 /**

   679    Slave configuration state: CLEAR FMMU.

   680 */

   681 

   682 void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *fsm

   683                                         /**< slave state machine */)

   684 {

   685     ec_datagram_t *datagram = fsm->datagram;

   686 

   687     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   688         return;

   689 

   690     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   691         fsm->state = ec_fsm_slave_state_error;

   692         EC_ERR("Failed receive FMMU clearing datagram for slave %i.\n",

   693                fsm->slave->ring_position);

   694         return;

   695     }

   696 

   697     if (datagram->working_counter != 1) {

   698         fsm->slave->error_flag = 1;

   699         fsm->state = ec_fsm_slave_state_error;

   700         EC_ERR("Failed to clear FMMUs of slave %i: ",

   701                fsm->slave->ring_position);

   702         ec_datagram_print_wc_error(datagram);

   703         return;

   704     }

   705 

   706     ec_fsm_slave_conf_enter_mbox_sync(fsm);

   707 }

   708 

   709 /*****************************************************************************/

   710 

   711 /**

   712  * Check for mailbox sync managers to be configured.

   713  */

   714 

   715 void ec_fsm_slave_conf_enter_mbox_sync(

   716         ec_fsm_slave_t *fsm /**< slave state machine */

   717         )

   718 {

   719     ec_master_t *master = fsm->slave->master;

   720     ec_slave_t *slave = fsm->slave;

   721     ec_datagram_t *datagram = fsm->datagram;

   722     unsigned int i;

   723 

   724     // slave is now in INIT

   725     if (slave->current_state == slave->requested_state) {

   726         fsm->state = ec_fsm_slave_state_end; // successful

   727         if (master->debug_level) {

   728             EC_DBG("Finished configuration of slave %i.\n",

   729                    slave->ring_position);

   730         }

   731         return;

   732     }

   733 

   734     if (!slave->sii_mailbox_protocols) {

   735         // no mailbox protocols supported

   736         if (master->debug_level)

   737             EC_DBG("Slave %u does not support mailbox communication.\n",

   738                     slave->ring_position);

   739         ec_fsm_slave_conf_enter_preop(fsm);

   740         return;

   741     }

   742 

   743     if (master->debug_level) {

   744         EC_DBG("Configuring mailbox sync managers of slave %i.\n",

   745                slave->ring_position);

   746     }

   747 

   748     if (slave->sii_sync_count >= 2) { // mailbox configuration provided

   749         ec_datagram_fpwr(datagram, slave->station_address, 0x0800,

   750                 EC_SYNC_PAGE_SIZE * slave->sii_sync_count);

   751         memset(datagram->data, 0x00,

   752                 EC_SYNC_PAGE_SIZE * slave->sii_sync_count);

   753 

   754         for (i = 0; i < 2; i++) {

   755             ec_sync_config(&slave->sii_syncs[i], slave->sii_syncs[i].length,

   756                     datagram->data + EC_SYNC_PAGE_SIZE * i);

   757         }

   758     } else { // no mailbox sync manager configurations provided

   759         ec_sync_t sync;

   760 

   761         if (master->debug_level)

   762             EC_DBG("Slave %u does not provide"

   763                     " mailbox sync manager configurations.\n",

   764                     slave->ring_position);

   765 

   766         ec_datagram_fpwr(datagram, slave->station_address, 0x0800,

   767                 EC_SYNC_PAGE_SIZE * 2);

   768         memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * 2);

   769 

   770         ec_sync_init(&sync, slave, 0);

   771         sync.physical_start_address = slave->sii_rx_mailbox_offset;

   772         sync.control_register = 0x26;

   773         sync.enable = 1;

   774         ec_sync_config(&sync, slave->sii_rx_mailbox_size,

   775                 datagram->data + EC_SYNC_PAGE_SIZE * sync.index);

   776 

   777         ec_sync_init(&sync, slave, 1);

   778         sync.physical_start_address = slave->sii_tx_mailbox_offset;

   779         sync.control_register = 0x22;

   780         sync.enable = 1;

   781         ec_sync_config(&sync, slave->sii_tx_mailbox_size,

   782                 datagram->data + EC_SYNC_PAGE_SIZE * sync.index);

   783     }

   784 

   785     fsm->retries = EC_FSM_RETRIES;

   786     fsm->state = ec_fsm_slave_conf_state_mbox_sync;

   787 }

   788 

   789 /*****************************************************************************/

   790 

   791 /**

   792    Slave configuration state: SYNC.

   793 */

   794 

   795 void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *fsm /**< slave state machine */)

   796 {

   797     ec_datagram_t *datagram = fsm->datagram;

   798     ec_slave_t *slave = fsm->slave;

   799 

   800     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   801         return;

   802 

   803     if (datagram->state != EC_DATAGRAM_RECEIVED) {

   804         fsm->state = ec_fsm_slave_state_error;

   805         EC_ERR("Failed to receive sync manager configuration datagram for"

   806                " slave %i (datagram state %i).\n",

   807                slave->ring_position, datagram->state);

   808         return;

   809     }

   810 

   811     if (datagram->working_counter != 1) {

   812         slave->error_flag = 1;

   813         fsm->state = ec_fsm_slave_state_error;

   814         EC_ERR("Failed to set sync managers of slave %i: ",

   815                slave->ring_position);

   816         ec_datagram_print_wc_error(datagram);

   817         return;

   818     }

   819 

   820     ec_fsm_slave_conf_enter_preop(fsm);

   821 }

   822 

   823 /*****************************************************************************/

   824 

   825 /**

   826  * Request PREOP state.

   827  */

   828 

   829 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *fsm /**< slave state machine */)

   830 {

   831     fsm->state = ec_fsm_slave_conf_state_preop;

   832     ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP);

   833     ec_fsm_change_exec(&fsm->fsm_change); // execute immediately

   834 }

   835 

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

   837 

   838 /**

   839    Slave configuration state: PREOP.

   840 */

   841 

   842 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *fsm /**< slave state machine */)

   843 {

   844     ec_slave_t *slave = fsm->slave;

   845     ec_master_t *master = fsm->slave->master;

   846 

   847     if (ec_fsm_change_exec(&fsm->fsm_change)) return;

   848 

   849     if (!ec_fsm_change_success(&fsm->fsm_change)) {

   850         if (!fsm->fsm_change.spontaneous_change)

   851             slave->error_flag = 1;

   852         fsm->state = ec_fsm_slave_state_error;

   853         return;

   854     }

   855 

   856     // slave is now in PREOP

   857     slave->jiffies_preop = fsm->datagram->jiffies_received;

   858 

   859     if (master->debug_level) {

   860         EC_DBG("Slave %i is now in PREOP.\n", slave->ring_position);

   861     }

   862 

   863     if (slave->current_state == slave->requested_state) {

   864         fsm->state = ec_fsm_slave_state_end; // successful

   865         if (master->debug_level) {

   866             EC_DBG("Finished configuration of slave %i.\n",

   867                    slave->ring_position);

   868         }

   869         return;

   870     }

   871 

   872     ec_fsm_slave_conf_enter_sdo_conf(fsm);

   873 }

   874 

   875 /*****************************************************************************/

   876 

   877 /**

   878  * Check for Sdo configurations to be applied.

   879  */

   880 

   881 void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *fsm /**< slave state machine */)

   882 {

   883     ec_slave_t *slave = fsm->slave;

   884 

   885     if (!slave->config) {

   886         EC_DBG("Slave %u is not configured.\n", slave->ring_position);

   887         ec_fsm_slave_conf_enter_safeop(fsm);

   888         return;

   889     }

   890 

   891     // No CoE configuration to be applied?

   892     if (list_empty(&slave->config->sdo_configs)) { // skip Sdo configuration

   893         ec_fsm_slave_conf_enter_mapping(fsm);

   894         return;

   895     }

   896 

   897     // start Sdo configuration

   898     fsm->state = ec_fsm_slave_conf_state_sdo_conf;

   899     fsm->sdodata =

   900         list_entry(fsm->slave->config->sdo_configs.next, ec_sdo_data_t, list);

   901     ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);

   902     ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   903 }

   904 

   905 /*****************************************************************************/

   906 

   907 /**

   908    Slave configuration state: SDO_CONF.

   909 */

   910 

   911 void ec_fsm_slave_conf_state_sdo_conf(

   912         ec_fsm_slave_t *fsm /**< slave state machine */

   913         )

   914 {

   915     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;

   916 

   917     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {

   918         EC_ERR("Sdo configuration failed for slave %u.\n",

   919                 fsm->slave->ring_position);

   920         fsm->slave->error_flag = 1;

   921         fsm->state = ec_fsm_slave_state_error;

   922         return;

   923     }

   924 

   925     // Another Sdo to configure?

   926     if (fsm->sdodata->list.next != &fsm->slave->config->sdo_configs) {

   927         fsm->sdodata =

   928             list_entry(fsm->sdodata->list.next, ec_sdo_data_t, list);

   929         ec_fsm_coe_download(&fsm->fsm_coe, fsm->slave, fsm->sdodata);

   930         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately

   931         return;

   932     }

   933 

   934     // All Sdos are now configured.

   935     ec_fsm_slave_conf_enter_mapping(fsm);

   936 }

   937 

   938 /*****************************************************************************/

   939 

   940 /**

   941  * Check for Pdo mappings to be applied.

   942  */

   943 

   944 void ec_fsm_slave_conf_enter_mapping(

   945         ec_fsm_slave_t *fsm /**< slave state machine */

   946         )

   947 {

   948     // start configuring Pdo mapping

   949     fsm->state = ec_fsm_slave_conf_state_mapping;

   950     ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_map, fsm->slave);

   951     ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map); // execute immediately

   952 }

   953 

   954 /*****************************************************************************/

   955 

   956 /**

   957    Slave configuration state: MAPPING.

   958 */

   959 

   960 void ec_fsm_slave_conf_state_mapping(

   961         ec_fsm_slave_t *fsm /**< slave state machine */

   962         )

   963 {

   964     if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_map)) return;

   965 

   966     if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_map)) {

   967         EC_ERR("Pdo mapping configuration failed for slave %u.\n",

   968                 fsm->slave->ring_position);

   969         fsm->slave->error_flag = 1;

   970         fsm->state = ec_fsm_slave_state_error;

   971         return;

   972     }

   973 

   974     // Start Pdo configuration

   975     fsm->state = ec_fsm_slave_conf_state_pdo_conf;

   976     ec_fsm_pdo_config_start(&fsm->fsm_pdo_conf, fsm->slave);

   977     ec_fsm_pdo_config_exec(&fsm->fsm_pdo_conf); // execute immediately

   978 }

   979 

   980 /*****************************************************************************/

   981 

   982 /**

   983    Slave configuration state: PDO_CONF.

   984 */

   985 

   986 void ec_fsm_slave_conf_state_pdo_conf(

   987         ec_fsm_slave_t *fsm /**< slave state machine */

   988         )

   989 {

   990     if (ec_fsm_pdo_config_exec(&fsm->fsm_pdo_conf)) return;

   991 

   992     if (!ec_fsm_pdo_config_success(&fsm->fsm_pdo_conf)) {

   993         EC_ERR("Pdo configuration failed for slave %u.\n",

   994                 fsm->slave->ring_position);

   995         fsm->slave->error_flag = 1;

   996         fsm->state = ec_fsm_slave_state_error;

   997         return;

   998     }

   999 

  1000     ec_fsm_slave_conf_enter_pdo_sync(fsm);

  1001 }

  1002 

  1003 /*****************************************************************************/

  1004 

  1005 /**

  1006  * Check for Pdo sync managers to be configured.

  1007  */

  1008 

  1009 void ec_fsm_slave_conf_enter_pdo_sync(

  1010         ec_fsm_slave_t *fsm /**< slave state machine */

  1011         )

  1012 {

  1013     ec_slave_t *slave = fsm->slave;

  1014     ec_datagram_t *datagram = fsm->datagram;

  1015     unsigned int i, offset, num_syncs;

  1016     const ec_sync_t *sync;

  1017     ec_direction_t dir;

  1018     uint16_t size;

  1019 

  1020     if (!slave->sii_sync_count) {

  1021         ec_fsm_slave_conf_enter_fmmu(fsm);

  1022         return;

  1023     }

  1024 

  1025     if (slave->sii_mailbox_protocols) {

  1026         offset = 2; // slave has mailboxes

  1027     } else {

  1028         offset = 0;

  1029     }

  1030     num_syncs = slave->sii_sync_count - offset;

  1031 

  1032     // configure sync managers for process data

  1033     ec_datagram_fpwr(datagram, slave->station_address,

  1034             0x0800 + EC_SYNC_PAGE_SIZE * offset,

  1035             EC_SYNC_PAGE_SIZE * num_syncs);

  1036     memset(datagram->data, 0x00, EC_SYNC_PAGE_SIZE * num_syncs);

  1037 

  1038     for (i = 0; i < num_syncs; i++) {

  1039         sync = &slave->sii_syncs[i + offset];

  1040         dir = ec_sync_direction(sync);

  1041         size = ec_pdo_mapping_total_size(&slave->config->mapping[dir]);

  1042         ec_sync_config(sync, size, datagram->data + EC_SYNC_PAGE_SIZE * i);

  1043     }

  1044 

  1045     fsm->retries = EC_FSM_RETRIES;

  1046     fsm->state = ec_fsm_slave_conf_state_pdo_sync;

  1047 }

  1048 

  1049 /*****************************************************************************/

  1050 

  1051 /**

  1052  * Configure Pdo sync managers.

  1053  */

  1054 

  1055 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *fsm /**< slave state machine */)

  1056 {

  1057     ec_datagram_t *datagram = fsm->datagram;

  1058     ec_slave_t *slave = fsm->slave;

  1059 

  1060     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

  1061         return;

  1062 

  1063     if (datagram->state != EC_DATAGRAM_RECEIVED) {

  1064         fsm->state = ec_fsm_slave_state_error;

  1065         EC_ERR("Failed to receive process data sync manager configuration"

  1066                " datagram for slave %i (datagram state %i).\n",

  1067                slave->ring_position, datagram->state);

  1068         return;

  1069     }

  1070 

  1071     if (datagram->working_counter != 1) {

  1072         slave->error_flag = 1;

  1073         fsm->state = ec_fsm_slave_state_error;

  1074         EC_ERR("Failed to set process data sync managers of slave %i: ",

  1075                 slave->ring_position);

  1076         ec_datagram_print_wc_error(datagram);

  1077         return;

  1078     }

  1079 

  1080     ec_fsm_slave_conf_enter_fmmu(fsm);

  1081 }

  1082 

  1083 /*****************************************************************************/

  1084 

  1085 /**

  1086  * Check for FMMUs to be configured.

  1087  */

  1088 

  1089 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *fsm /**< slave state machine */)

  1090 {

  1091     ec_slave_t *slave = fsm->slave;

  1092     ec_datagram_t *datagram = fsm->datagram;

  1093     unsigned int i;

  1094     const ec_fmmu_config_t *fmmu;

  1095     const ec_sync_t *sync;

  1096 

  1097     if (slave->base_fmmu_count < slave->config->used_fmmus) {

  1098         slave->error_flag = 1;

  1099         fsm->state = ec_fsm_slave_state_error;

  1100         EC_ERR("Slave %u has less FMMUs (%u) than requested (%u).\n",

  1101                 slave->ring_position, slave->base_fmmu_count,

  1102                 slave->config->used_fmmus);

  1103         return;

  1104     }

  1105 

  1106     if (!slave->base_fmmu_count) { // skip FMMU configuration

  1107         ec_fsm_slave_conf_enter_safeop(fsm);

  1108         return;

  1109     }

  1110 

  1111     // configure FMMUs

  1112     ec_datagram_fpwr(datagram, slave->station_address,

  1113                      0x0600, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

  1114     memset(datagram->data, 0x00, EC_FMMU_PAGE_SIZE * slave->base_fmmu_count);

  1115     for (i = 0; i < slave->config->used_fmmus; i++) {

  1116         fmmu = &slave->config->fmmu_configs[i];

  1117         if (!(sync = ec_slave_get_pdo_sync(slave, fmmu->dir))) {

  1118             slave->error_flag = 1;

  1119             fsm->state = ec_fsm_slave_state_error;

  1120             EC_ERR("Failed to determine Pdo sync manager for FMMU on slave"

  1121                     " %u!\n", slave->ring_position);

  1122             return;

  1123         }

  1124         ec_fmmu_config_page(fmmu, sync,

  1125                 datagram->data + EC_FMMU_PAGE_SIZE * i);

  1126     }

  1127 

  1128     fsm->retries = EC_FSM_RETRIES;

  1129     fsm->state = ec_fsm_slave_conf_state_fmmu;

  1130 }

  1131 

  1132 /*****************************************************************************/

  1133 

  1134 /**

  1135    Slave configuration state: FMMU.

  1136 */