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

    37 */

    38 

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

    40 

    41 #include "globals.h"

    42 #include "fsm.h"

    43 #include "master.h"

    44 #include "mailbox.h"

    45 

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

    47 

    48 void ec_fsm_master_start(ec_fsm_t *);

    49 void ec_fsm_master_broadcast(ec_fsm_t *);

    50 void ec_fsm_master_read_states(ec_fsm_t *);

    51 void ec_fsm_master_acknowledge(ec_fsm_t *);

    52 void ec_fsm_master_validate_vendor(ec_fsm_t *);

    53 void ec_fsm_master_validate_product(ec_fsm_t *);

    54 void ec_fsm_master_rewrite_addresses(ec_fsm_t *);

    55 void ec_fsm_master_configure_slave(ec_fsm_t *);

    56 void ec_fsm_master_scan_slaves(ec_fsm_t *);

    57 void ec_fsm_master_write_eeprom(ec_fsm_t *);

    58 void ec_fsm_master_sdodict(ec_fsm_t *);

    59 void ec_fsm_master_sdo_request(ec_fsm_t *);

    60 void ec_fsm_master_end(ec_fsm_t *);

    61 void ec_fsm_master_error(ec_fsm_t *);

    62 

    63 void ec_fsm_slavescan_start(ec_fsm_t *);

    64 void ec_fsm_slavescan_address(ec_fsm_t *);

    65 void ec_fsm_slavescan_state(ec_fsm_t *);

    66 void ec_fsm_slavescan_base(ec_fsm_t *);

    67 void ec_fsm_slavescan_datalink(ec_fsm_t *);

    68 void ec_fsm_slavescan_eeprom_size(ec_fsm_t *);

    69 void ec_fsm_slavescan_eeprom_data(ec_fsm_t *);

    70 

    71 void ec_fsm_slaveconf_state_start(ec_fsm_t *);

    72 void ec_fsm_slaveconf_state_init(ec_fsm_t *);

    73 void ec_fsm_slaveconf_state_clear_fmmus(ec_fsm_t *);

    74 void ec_fsm_slaveconf_state_sync(ec_fsm_t *);

    75 void ec_fsm_slaveconf_state_preop(ec_fsm_t *);

    76 void ec_fsm_slaveconf_state_sync2(ec_fsm_t *);

    77 void ec_fsm_slaveconf_state_fmmu(ec_fsm_t *);

    78 void ec_fsm_slaveconf_state_sdoconf(ec_fsm_t *);

    79 void ec_fsm_slaveconf_state_saveop(ec_fsm_t *);

    80 void ec_fsm_slaveconf_state_op(ec_fsm_t *);

    81 

    82 void ec_fsm_slaveconf_enter_sync(ec_fsm_t *);

    83 void ec_fsm_slaveconf_enter_preop(ec_fsm_t *);

    84 void ec_fsm_slaveconf_enter_sync2(ec_fsm_t *);

    85 void ec_fsm_slaveconf_enter_fmmu(ec_fsm_t *);

    86 void ec_fsm_slaveconf_enter_sdoconf(ec_fsm_t *);

    87 void ec_fsm_slaveconf_enter_saveop(ec_fsm_t *);

    88 

    89 void ec_fsm_slave_state_end(ec_fsm_t *);

    90 void ec_fsm_slave_state_error(ec_fsm_t *);

    91 

    92 /*****************************************************************************/

    93 

    94 /**

    95    Constructor.

    96 */

    97 

    98 int ec_fsm_init(ec_fsm_t *fsm, /**< finite state machine */

    99                 ec_master_t *master /**< EtherCAT master */

   100                 )

   101 {

   102     fsm->master = master;

   103     fsm->master_state = ec_fsm_master_start;

   104     fsm->master_slaves_responding = 0;

   105     fsm->master_slave_states = EC_SLAVE_STATE_UNKNOWN;

   106     fsm->master_validation = 0;

   107 

   108     ec_datagram_init(&fsm->datagram);

   109     if (ec_datagram_prealloc(&fsm->datagram, EC_MAX_DATA_SIZE)) {

   110         EC_ERR("Failed to allocate FSM datagram.\n");

   111         return -1;

   112     }

   113 

   114     // init sub-state-machines

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

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

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

   118 

   119     return 0;

   120 }

   121 

   122 /*****************************************************************************/

   123 

   124 /**

   125    Destructor.

   126 */

   127 

   128 void ec_fsm_clear(ec_fsm_t *fsm /**< finite state machine */)

   129 {

   130     // clear sub-state machines

   131     ec_fsm_sii_clear(&fsm->fsm_sii);

   132     ec_fsm_change_clear(&fsm->fsm_change);

   133     ec_fsm_coe_clear(&fsm->fsm_coe);

   134 

   135     ec_datagram_clear(&fsm->datagram);

   136 }

   137 

   138 /*****************************************************************************/

   139 

   140 /**

   141    Executes the current state of the state machine.

   142    \return false, if state machine has terminated

   143 */

   144 

   145 int ec_fsm_exec(ec_fsm_t *fsm /**< finite state machine */)

   146 {

   147     fsm->master_state(fsm);

   148 

   149     return ec_fsm_running(fsm);

   150 }

   151 

   152 /*****************************************************************************/

   153 

   154 /**

   155    \return false, if state machine has terminated

   156 */

   157 

   158 int ec_fsm_running(ec_fsm_t *fsm /**< finite state machine */)

   159 {

   160     return fsm->master_state != ec_fsm_master_end

   161         && fsm->master_state != ec_fsm_master_error;

   162 }

   163 

   164 /*****************************************************************************/

   165 

   166 /**

   167    \return true, if the master state machine terminated gracefully

   168 */

   169 

   170 int ec_fsm_success(ec_fsm_t *fsm /**< finite state machine */)

   171 {

   172     return fsm->master_state == ec_fsm_master_end;

   173 }

   174 

   175 /******************************************************************************

   176  *  operation/idle state machine

   177  *****************************************************************************/

   178 

   179 /**

   180    Master state: START.

   181    Starts with getting slave count and slave states.

   182 */

   183 

   184 void ec_fsm_master_start(ec_fsm_t *fsm)

   185 {

   186     ec_datagram_brd(&fsm->datagram, 0x0130, 2);

   187     ec_master_queue_datagram(fsm->master, &fsm->datagram);

   188     fsm->master_state = ec_fsm_master_broadcast;

   189 }

   190 

   191 /*****************************************************************************/

   192 

   193 /**

   194    Master state: BROADCAST.

   195    Processes the broadcast read slave count and slaves states.

   196 */

   197 

   198 void ec_fsm_master_broadcast(ec_fsm_t *fsm /**< finite state machine */)

   199 {

   200     ec_datagram_t *datagram = &fsm->datagram;

   201     unsigned int topology_change, states_change, i;

   202     ec_slave_t *slave;

   203     ec_master_t *master = fsm->master;

   204 

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

   206         if (!master->device->link_state) {

   207             fsm->master_slaves_responding = 0;

   208             list_for_each_entry(slave, &master->slaves, list) {

   209                 slave->online = 0;

   210             }

   211         }

   212         else {

   213             EC_ERR("Failed to receive broadcast datagram.\n");

   214         }

   215         fsm->master_state = ec_fsm_master_error;

   216         return;

   217     }

   218 

   219     topology_change = (datagram->working_counter !=

   220                        fsm->master_slaves_responding);

   221     states_change = (EC_READ_U8(datagram->data) != fsm->master_slave_states);

   222 

   223     fsm->master_slave_states = EC_READ_U8(datagram->data);

   224     fsm->master_slaves_responding = datagram->working_counter;

   225 

   226     if (topology_change) {

   227         EC_INFO("%i slave%s responding.\n",

   228                 fsm->master_slaves_responding,

   229                 fsm->master_slaves_responding == 1 ? "" : "s");

   230 

   231         if (master->mode == EC_MASTER_MODE_OPERATION) {

   232             if (fsm->master_slaves_responding == master->slave_count) {

   233                 fsm->master_validation = 1; // start validation later

   234             }

   235             else {

   236                 EC_WARN("Invalid slave count. Bus in tainted state.\n");

   237             }

   238         }

   239     }

   240 

   241     if (states_change) {

   242         char states[EC_STATE_STRING_SIZE];

   243         ec_state_string(fsm->master_slave_states, states);

   244         EC_INFO("Slave states: %s.\n", states);

   245     }

   246 

   247     // topology change in idle mode: clear all slaves and scan the bus

   248     if (topology_change && master->mode == EC_MASTER_MODE_IDLE) {

   249 

   250         ec_master_eoe_stop(master);

   251         ec_master_destroy_slaves(master);

   252 

   253         master->slave_count = datagram->working_counter;

   254 

   255         if (!master->slave_count) {

   256             // no slaves present -> finish state machine.

   257             fsm->master_state = ec_fsm_master_end;

   258             return;

   259         }

   260 

   261         // init slaves

   262         for (i = 0; i < master->slave_count; i++) {

   263             if (!(slave = (ec_slave_t *) kmalloc(sizeof(ec_slave_t),

   264                                                  GFP_ATOMIC))) {

   265                 EC_ERR("Failed to allocate slave %i!\n", i);

   266                 ec_master_destroy_slaves(master);

   267                 fsm->master_state = ec_fsm_master_error;

   268                 return;

   269             }

   270 

   271             if (ec_slave_init(slave, master, i, i + 1)) {

   272                 // freeing of "slave" already done

   273                 ec_master_destroy_slaves(master);

   274                 fsm->master_state = ec_fsm_master_error;

   275                 return;

   276             }

   277 

   278             list_add_tail(&slave->list, &master->slaves);

   279         }

   280 

   281         EC_INFO("Scanning bus.\n");

   282 

   283         // begin scanning of slaves

   284         fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);

   285         fsm->slave_state = ec_fsm_slavescan_start;

   286         fsm->master_state = ec_fsm_master_scan_slaves;

   287         fsm->master_state(fsm); // execute immediately

   288         return;

   289     }

   290 

   291     // fetch state from each slave

   292     fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);

   293     ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address, 0x0130, 2);

   294     ec_master_queue_datagram(master, &fsm->datagram);

   295     fsm->master_state = ec_fsm_master_read_states;

   296 }

   297 

   298 /*****************************************************************************/

   299 

   300 /**

   301    Master action: PROC_STATES.

   302    Processes the slave states.

   303 */

   304 

   305 void ec_fsm_master_action_process_states(ec_fsm_t *fsm

   306                                          /**< finite state machine */

   307                                          )

   308 {

   309     ec_master_t *master = fsm->master;

   310     ec_slave_t *slave;

   311     char old_state[EC_STATE_STRING_SIZE], new_state[EC_STATE_STRING_SIZE];

   312 

   313     // check if any slaves are not in the state, they're supposed to be

   314     list_for_each_entry(slave, &master->slaves, list) {

   315         if (slave->error_flag

   316             || !slave->online

   317             || slave->requested_state == EC_SLAVE_STATE_UNKNOWN

   318             || (slave->current_state == slave->requested_state

   319                 && slave->configured)) continue;

   320 

   321         if (master->debug_level) {

   322             ec_state_string(slave->current_state, old_state);

   323             if (slave->current_state != slave->requested_state) {

   324                 ec_state_string(slave->requested_state, new_state);

   325                 EC_DBG("Changing state of slave %i (%s -> %s).\n",

   326                        slave->ring_position, old_state, new_state);

   327             }

   328             else if (!slave->configured) {

   329                 EC_DBG("Reconfiguring slave %i (%s).\n",

   330                        slave->ring_position, old_state);

   331             }

   332         }

   333 

   334         fsm->master_state = ec_fsm_master_configure_slave;

   335         fsm->slave = slave;

   336         fsm->slave_state = ec_fsm_slaveconf_state_start;

   337         fsm->slave_state(fsm); // execute immediately

   338         return;

   339     }

   340 

   341     // Check, if EoE processing has to be started

   342     ec_master_eoe_start(master);

   343 

   344     if (master->mode == EC_MASTER_MODE_IDLE) {

   345 

   346         // Check for a pending SDO request

   347         if (master->sdo_seq_master != master->sdo_seq_user) {

   348             if (master->debug_level)

   349                 EC_DBG("Processing SDO request...\n");

   350             slave = master->sdo_request->sdo->slave;

   351             if (slave->current_state == EC_SLAVE_STATE_INIT

   352                 || !slave->online) {

   353                 EC_ERR("Failed to process SDO request, slave %i not ready.\n",

   354                        slave->ring_position);

   355                 master->sdo_request->return_code = -1;

   356                 master->sdo_seq_master++;

   357             }

   358             else {

   359                 // start uploading SDO

   360                 fsm->slave = slave;

   361                 fsm->master_state = ec_fsm_master_sdo_request;

   362                 fsm->sdo_request = master->sdo_request;

   363                 ec_fsm_coe_upload(&fsm->fsm_coe, slave, fsm->sdo_request);

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

   365                 return;

   366             }

   367         }

   368 

   369         // check, if slaves have an SDO dictionary to read out.

   370         list_for_each_entry(slave, &master->slaves, list) {

   371             if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)

   372                 || slave->sdo_dictionary_fetched

   373                 || slave->current_state == EC_SLAVE_STATE_INIT

   374                 || jiffies - slave->jiffies_preop < EC_WAIT_SDO_DICT * HZ

   375                 || !slave->online

   376                 || slave->error_flag) continue;

   377 

   378             if (master->debug_level) {

   379                 EC_DBG("Fetching SDO dictionary from slave %i.\n",

   380                        slave->ring_position);

   381             }

   382 

   383             slave->sdo_dictionary_fetched = 1;

   384 

   385             // start fetching SDO dictionary

   386             fsm->slave = slave;

   387             fsm->master_state = ec_fsm_master_sdodict;

   388             ec_fsm_coe_dictionary(&fsm->fsm_coe, slave);

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

   390             return;

   391         }

   392 

   393         // check for pending EEPROM write operations.

   394         list_for_each_entry(slave, &master->slaves, list) {

   395             if (!slave->new_eeprom_data) continue;

   396 

   397             if (!slave->online || slave->error_flag) {

   398                 kfree(slave->new_eeprom_data);

   399                 slave->new_eeprom_data = NULL;

   400                 EC_ERR("Discarding EEPROM data, slave %i not ready.\n",

   401                        slave->ring_position);

   402                 continue;

   403             }

   404 

   405             // found pending EEPROM write operation. execute it!

   406             EC_INFO("Writing EEPROM of slave %i...\n", slave->ring_position);

   407             fsm->slave = slave;

   408             fsm->sii_offset = 0x0000;

   409             ec_fsm_sii_write(&fsm->fsm_sii, slave, fsm->sii_offset,

   410                              slave->new_eeprom_data, EC_FSM_SII_NODE);

   411             fsm->master_state = ec_fsm_master_write_eeprom;

   412             fsm->master_state(fsm); // execute immediately

   413             return;

   414         }

   415     }

   416 

   417     fsm->master_state = ec_fsm_master_end;

   418 }

   419 

   420 /*****************************************************************************/

   421 

   422 /**

   423    Master action: Get state of next slave.

   424 */

   425 

   426 void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm

   427                                            /**< finite state machine */)

   428 {

   429     ec_master_t *master = fsm->master;

   430     ec_slave_t *slave = fsm->slave;

   431 

   432     // is there another slave to query?

   433     if (slave->list.next != &master->slaves) {

   434         // process next slave

   435         fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   436         ec_datagram_nprd(&fsm->datagram, fsm->slave->station_address,

   437                          0x0130, 2);

   438         ec_master_queue_datagram(master, &fsm->datagram);

   439         fsm->master_state = ec_fsm_master_read_states;

   440         return;

   441     }

   442 

   443     // all slave states read

   444 

   445     // check, if a bus validation has to be done

   446     if (fsm->master_validation) {

   447         fsm->master_validation = 0;

   448         list_for_each_entry(slave, &master->slaves, list) {

   449             if (slave->online) continue;

   450 

   451             // At least one slave is offline. validate!

   452             EC_INFO("Validating bus.\n");

   453             fsm->slave = list_entry(master->slaves.next, ec_slave_t, list);

   454             fsm->master_state = ec_fsm_master_validate_vendor;

   455             ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x0008, EC_FSM_SII_POSITION);

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

   457             return;

   458         }

   459     }

   460 

   461     ec_fsm_master_action_process_states(fsm);

   462 }

   463 

   464 /*****************************************************************************/

   465 

   466 /**

   467    Master state: READ STATES.

   468    Fetches the AL- and online state of a slave.

   469 */

   470 

   471 void ec_fsm_master_read_states(ec_fsm_t *fsm /**< finite state machine */)

   472 {

   473     ec_slave_t *slave = fsm->slave;

   474     ec_datagram_t *datagram = &fsm->datagram;

   475     uint8_t new_state;

   476 

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

   478         EC_ERR("Failed to receive AL state datagram for slave %i!\n",

   479                slave->ring_position);

   480         fsm->master_state = ec_fsm_master_error;

   481         return;

   482     }

   483 

   484     // did the slave not respond to its station address?

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

   486         if (slave->online) {

   487             slave->online = 0;

   488             if (slave->master->debug_level)

   489                 EC_DBG("Slave %i: offline.\n", slave->ring_position);

   490         }

   491         ec_fsm_master_action_next_slave_state(fsm);

   492         return;

   493     }

   494 

   495     // slave responded

   496     new_state = EC_READ_U8(datagram->data);

   497     if (!slave->online) { // slave was offline before

   498         slave->online = 1;

   499         slave->error_flag = 0; // clear error flag

   500         slave->current_state = new_state;

   501         if (slave->master->debug_level) {

   502             char cur_state[EC_STATE_STRING_SIZE];

   503             ec_state_string(slave->current_state, cur_state);

   504             EC_DBG("Slave %i: online (%s).\n",

   505                    slave->ring_position, cur_state);

   506         }

   507     }

   508     else if (new_state != slave->current_state) {

   509         if (slave->master->debug_level) {

   510             char old_state[EC_STATE_STRING_SIZE],

   511                 cur_state[EC_STATE_STRING_SIZE];

   512             ec_state_string(slave->current_state, old_state);

   513             ec_state_string(new_state, cur_state);

   514             EC_DBG("Slave %i: %s -> %s.\n",

   515                    slave->ring_position, old_state, cur_state);

   516         }

   517         slave->current_state = new_state;

   518     }

   519 

   520     // check, if new slave state has to be acknowledged

   521     if (slave->current_state & EC_SLAVE_STATE_ACK_ERR && !slave->error_flag) {

   522         ec_fsm_change_ack(&fsm->fsm_change, slave);

   523         ec_fsm_change_exec(&fsm->fsm_change);

   524         fsm->master_state = ec_fsm_master_acknowledge;

   525         return;

   526     }

   527 

   528     ec_fsm_master_action_next_slave_state(fsm);

   529 }

   530 

   531 /*****************************************************************************/

   532 

   533 /**

   534    Master state: ACKNOWLEDGE

   535 */

   536 

   537 void ec_fsm_master_acknowledge(ec_fsm_t *fsm /**< finite state machine */)

   538 {

   539     ec_slave_t *slave = fsm->slave;

   540 

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

   542 

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

   544         fsm->slave->error_flag = 1;

   545         EC_ERR("Failed to acknowledge state change on slave %i.\n",

   546                slave->ring_position);

   547         fsm->master_state = ec_fsm_master_error;

   548         return;

   549     }

   550 

   551     ec_fsm_master_action_next_slave_state(fsm);

   552 }

   553 

   554 /*****************************************************************************/

   555 

   556 /**

   557    Master state: VALIDATE_VENDOR.

   558    Validates the vendor ID of a slave.

   559 */

   560 

   561 void ec_fsm_master_validate_vendor(ec_fsm_t *fsm /**< finite state machine */)

   562 {

   563     ec_slave_t *slave = fsm->slave;

   564 

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

   566 

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

   568         fsm->slave->error_flag = 1;

   569         EC_ERR("Failed to validate vendor ID of slave %i.\n",

   570                slave->ring_position);

   571         fsm->master_state = ec_fsm_master_error;

   572         return;

   573     }

   574 

   575     if (EC_READ_U32(fsm->fsm_sii.value) != slave->sii_vendor_id) {

   576         EC_ERR("Slave %i has an invalid vendor ID!\n", slave->ring_position);

   577         fsm->master_state = ec_fsm_master_error;

   578         return;

   579     }

   580 

   581     // vendor ID is ok. check product code.

   582     fsm->master_state = ec_fsm_master_validate_product;

   583     ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x000A, EC_FSM_SII_POSITION);

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

   585 }

   586 

   587 /*****************************************************************************/

   588 

   589 /**

   590    Master action: ADDRESS.

   591    Looks for slave, that have lost their configuration and writes

   592    their station address, so that they can be reconfigured later.

   593 */

   594 

   595 void ec_fsm_master_action_addresses(ec_fsm_t *fsm /**< finite state machine */)

   596 {

   597     ec_datagram_t *datagram = &fsm->datagram;

   598 

   599     while (fsm->slave->online) {

   600         if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?

   601             fsm->master_state = ec_fsm_master_start;

   602             fsm->master_state(fsm); // execute immediately

   603             return;

   604         }

   605         // check next slave

   606         fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   607     }

   608 

   609     if (fsm->master->debug_level)

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

   611 

   612     // write station address

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

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

   615     ec_master_queue_datagram(fsm->master, datagram);

   616     fsm->master_state = ec_fsm_master_rewrite_addresses;

   617 }

   618 

   619 /*****************************************************************************/

   620 

   621 /**

   622    Master state: VALIDATE_PRODUCT.

   623    Validates the product ID of a slave.

   624 */

   625 

   626 void ec_fsm_master_validate_product(ec_fsm_t *fsm /**< finite state machine */)

   627 {

   628     ec_slave_t *slave = fsm->slave;

   629 

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

   631 

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

   633         fsm->slave->error_flag = 1;

   634         EC_ERR("Failed to validate product code of slave %i.\n",

   635                slave->ring_position);

   636         fsm->master_state = ec_fsm_master_error;

   637         return;

   638     }

   639 

   640     if (EC_READ_U32(fsm->fsm_sii.value) != slave->sii_product_code) {

   641         EC_ERR("Slave %i: invalid product code!\n", slave->ring_position);

   642         EC_ERR("expected 0x%08X, got 0x%08X.\n", slave->sii_product_code,

   643                EC_READ_U32(fsm->fsm_sii.value));

   644         fsm->master_state = ec_fsm_master_error;

   645         return;

   646     }

   647 

   648     // have all states been validated?

   649     if (slave->list.next == &fsm->master->slaves) {

   650         fsm->slave = list_entry(fsm->master->slaves.next, ec_slave_t, list);

   651         // start writing addresses to offline slaves

   652         ec_fsm_master_action_addresses(fsm);

   653         return;

   654     }

   655 

   656     // validate next slave

   657     fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   658     fsm->master_state = ec_fsm_master_validate_vendor;

   659     ec_fsm_sii_read(&fsm->fsm_sii, slave, 0x0008, EC_FSM_SII_POSITION);

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

   661 }

   662 

   663 /*****************************************************************************/

   664 

   665 /**

   666    Master state: REWRITE ADDRESS.

   667    Checks, if the new station address has been written to the slave.

   668 */

   669 

   670 void ec_fsm_master_rewrite_addresses(ec_fsm_t *fsm

   671                                      /**< finite state machine */

   672                                      )

   673 {

   674     ec_slave_t *slave = fsm->slave;

   675     ec_datagram_t *datagram = &fsm->datagram;

   676 

   677     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   679         EC_ERR("Failed to write station address on slave %i.\n",

   680                slave->ring_position);

   681     }

   682 

   683     if (fsm->slave->list.next == &fsm->master->slaves) { // last slave?

   684         fsm->master_state = ec_fsm_master_start;

   685         fsm->master_state(fsm); // execute immediately

   686         return;

   687     }

   688 

   689     // check next slave

   690     fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   691     // Write new station address to slave

   692     ec_fsm_master_action_addresses(fsm);

   693 }

   694 

   695 /*****************************************************************************/

   696 

   697 /**

   698    Master state: SCAN SLAVES.

   699    Executes the sub-statemachine for the scanning of a slave.

   700 */

   701 

   702 void ec_fsm_master_scan_slaves(ec_fsm_t *fsm /**< finite state machine */)

   703 {

   704     ec_master_t *master = fsm->master;

   705     ec_slave_t *slave = fsm->slave;

   706 

   707     fsm->slave_state(fsm); // execute slave state machine

   708 

   709     if (fsm->slave_state != ec_fsm_slave_state_end

   710         && fsm->slave_state != ec_fsm_slave_state_error) return;

   711 

   712     // another slave to fetch?

   713     if (slave->list.next != &master->slaves) {

   714         fsm->slave = list_entry(fsm->slave->list.next, ec_slave_t, list);

   715         fsm->slave_state = ec_fsm_slavescan_start;

   716         fsm->slave_state(fsm); // execute immediately

   717         return;

   718     }

   719 

   720     EC_INFO("Bus scanning completed.\n");

   721 

   722     ec_master_calc_addressing(master);

   723 

   724     // set initial states of all slaves to PREOP to make mailbox

   725     // communication possible

   726     list_for_each_entry(slave, &master->slaves, list) {

   727         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);

   728     }

   729 

   730     fsm->master_state = ec_fsm_master_end;

   731 }

   732 

   733 /*****************************************************************************/

   734 

   735 /**

   736    Master state: CONFIGURE SLAVES.

   737    Starts configuring a slave.

   738 */

   739 

   740 void ec_fsm_master_configure_slave(ec_fsm_t *fsm

   741                                    /**< finite state machine */

   742                                    )

   743 {

   744     fsm->slave_state(fsm); // execute slave's state machine

   745 

   746     if (fsm->slave_state != ec_fsm_slave_state_end

   747         && fsm->slave_state != ec_fsm_slave_state_error) return;

   748 

   749     ec_fsm_master_action_process_states(fsm);

   750 }

   751 

   752 /*****************************************************************************/

   753 

   754 /**

   755    Master state: WRITE EEPROM.

   756 */

   757 

   758 void ec_fsm_master_write_eeprom(ec_fsm_t *fsm /**< finite state machine */)

   759 {

   760     ec_slave_t *slave = fsm->slave;

   761 

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

   763 

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

   765         fsm->slave->error_flag = 1;

   766         EC_ERR("Failed to write EEPROM contents to slave %i.\n",

   767                slave->ring_position);

   768         kfree(slave->new_eeprom_data);

   769         slave->new_eeprom_data = NULL;

   770         fsm->master_state = ec_fsm_master_error;

   771         return;

   772     }

   773 

   774     fsm->sii_offset++;

   775     if (fsm->sii_offset < slave->new_eeprom_size) {

   776         ec_fsm_sii_write(&fsm->fsm_sii, slave, fsm->sii_offset,

   777                          slave->new_eeprom_data + fsm->sii_offset,

   778                          EC_FSM_SII_NODE);

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

   780         return;

   781     }

   782 

   783     // finished writing EEPROM

   784     EC_INFO("Finished writing EEPROM of slave %i.\n", slave->ring_position);

   785     kfree(slave->new_eeprom_data);

   786     slave->new_eeprom_data = NULL;

   787 

   788     // TODO: Evaluate new EEPROM contents!

   789 

   790     // restart master state machine.

   791     fsm->master_state = ec_fsm_master_start;

   792     fsm->master_state(fsm); // execute immediately

   793 }

   794 

   795 /*****************************************************************************/

   796 

   797 /**

   798    Master state: SDODICT.

   799 */

   800 

   801 void ec_fsm_master_sdodict(ec_fsm_t *fsm /**< finite state machine */)

   802 {

   803     ec_slave_t *slave = fsm->slave;

   804     ec_master_t *master = fsm->master;

   805 

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

   807 

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

   809         fsm->master_state = ec_fsm_master_error;

   810         return;

   811     }

   812 

   813     // SDO dictionary fetching finished

   814 

   815     if (master->debug_level) {

   816         unsigned int sdo_count, entry_count;

   817         ec_slave_sdo_dict_info(slave, &sdo_count, &entry_count);

   818         EC_DBG("Fetched %i SDOs and %i entries from slave %i.\n",

   819                sdo_count, entry_count, slave->ring_position);

   820     }

   821 

   822     // restart master state machine.

   823     fsm->master_state = ec_fsm_master_start;

   824     fsm->master_state(fsm); // execute immediately

   825 }

   826 

   827 /*****************************************************************************/

   828 

   829 /**

   830    Master state: SDO REQUEST.

   831 */

   832 

   833 void ec_fsm_master_sdo_request(ec_fsm_t *fsm /**< finite state machine */)

   834 {

   835     ec_master_t *master = fsm->master;

   836     ec_sdo_request_t *request = fsm->sdo_request;

   837 

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

   839 

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

   841         request->return_code = -1;

   842         master->sdo_seq_master++;

   843         fsm->master_state = ec_fsm_master_error;

   844         return;

   845     }

   846 

   847     // SDO dictionary fetching finished

   848 

   849     request->return_code = 1;

   850     master->sdo_seq_master++;

   851 

   852     // restart master state machine.

   853     fsm->master_state = ec_fsm_master_start;

   854     fsm->master_state(fsm); // execute immediately

   855 }

   856 

   857 /*****************************************************************************/

   858 

   859 /**

   860    State: ERROR.

   861 */

   862 

   863 void ec_fsm_master_error(ec_fsm_t *fsm /**< finite state machine */)

   864 {

   865     fsm->master_state = ec_fsm_master_start;

   866 }

   867 

   868 /*****************************************************************************/

   869 

   870 /**

   871    State: END.

   872 */

   873 

   874 void ec_fsm_master_end(ec_fsm_t *fsm /**< finite state machine */)

   875 {

   876     fsm->master_state = ec_fsm_master_start;

   877 }

   878 

   879 /******************************************************************************

   880  *  slave scan state machine

   881  *****************************************************************************/

   882 

   883 /**

   884    Slave scan state: START.

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

   886    slave, according to its ring position.

   887 */

   888 

   889 void ec_fsm_slavescan_start(ec_fsm_t *fsm /**< finite state machine */)

   890 {

   891     ec_datagram_t *datagram = &fsm->datagram;

   892 

   893     // write station address

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

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

   896     ec_master_queue_datagram(fsm->master, datagram);

   897     fsm->slave_state = ec_fsm_slavescan_address;

   898 }

   899 

   900 /*****************************************************************************/

   901 

   902 /**

   903    Slave scan state: ADDRESS.

   904 */

   905 

   906 void ec_fsm_slavescan_address(ec_fsm_t *fsm /**< finite state machine */)

   907 {

   908     ec_datagram_t *datagram = &fsm->datagram;

   909 

   910     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   912         fsm->slave->error_flag = 1;

   913         fsm->slave_state = ec_fsm_slave_state_error;

   914         EC_ERR("Failed to write station address of slave %i.\n",

   915                fsm->slave->ring_position);

   916         return;

   917     }

   918 

   919     // Read AL state

   920     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0130, 2);

   921     ec_master_queue_datagram(fsm->master, datagram);

   922     fsm->slave_state = ec_fsm_slavescan_state;

   923 }

   924 

   925 /*****************************************************************************/

   926 

   927 /**

   928    Slave scan state: STATE.

   929 */

   930 

   931 void ec_fsm_slavescan_state(ec_fsm_t *fsm /**< finite state machine */)

   932 {

   933     ec_datagram_t *datagram = &fsm->datagram;

   934     ec_slave_t *slave = fsm->slave;

   935 

   936     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   938         fsm->slave->error_flag = 1;

   939         fsm->slave_state = ec_fsm_slave_state_error;

   940         EC_ERR("Failed to read AL state of slave %i.\n",

   941                fsm->slave->ring_position);

   942         return;

   943     }

   944 

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

   946     if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

   947         char state_str[EC_STATE_STRING_SIZE];

   948         ec_state_string(slave->current_state, state_str);

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

   950                 slave->ring_position, state_str);

   951     }

   952 

   953     // read base data

   954     ec_datagram_nprd(datagram, fsm->slave->station_address, 0x0000, 6);

   955     ec_master_queue_datagram(fsm->master, datagram);

   956     fsm->slave_state = ec_fsm_slavescan_base;

   957 }

   958 

   959 /*****************************************************************************/

   960 

   961 /**

   962    Slave scan state: BASE.

   963 */

   964 

   965 void ec_fsm_slavescan_base(ec_fsm_t *fsm /**< finite state machine */)

   966 {

   967     ec_datagram_t *datagram = &fsm->datagram;

   968     ec_slave_t *slave = fsm->slave;

   969 

   970     if (datagram->state != EC_DATAGRAM_RECEIVED

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

   972         fsm->slave->error_flag = 1;

   973         fsm->slave_state = ec_fsm_slave_state_error;

   974         EC_ERR("Failed to read base data of slave %i.\n",

   975                slave->ring_position);

   976         return;

   977     }

   978 

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

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

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

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

   983     slave->base_sync_count = EC_READ_U8 (datagram->data + 5);

   984 

   985     if (slave->base_fmmu_count > EC_MAX_FMMUS)

   986         slave->base_fmmu_count = EC_MAX_FMMUS;

   987 

   988     // read data link status

   989     ec_datagram_nprd(datagram, slave->station_address, 0x0110, 2);

   990     ec_master_queue_datagram(slave->master, datagram);

   991     fsm->slave_state = ec_fsm_slavescan_datalink;

   992 }

   993 

   994 /*****************************************************************************/

   995 

   996 /**

   997    Slave scan state: DATALINK.

   998 */

   999 

  1000 void ec_fsm_slavescan_datalink(ec_fsm_t *fsm /**< finite state machine */)

  1001 {

  1002     ec_datagram_t *datagram = &fsm->datagram;

  1003     ec_slave_t *slave = fsm->slave;

  1004     uint16_t dl_status;

  1005     unsigned int i;

  1006 

  1007     if (datagram->state != EC_DATAGRAM_RECEIVED

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

  1009         fsm->slave->error_flag = 1;

  1010         fsm->slave_state = ec_fsm_slave_state_error;

  1011         EC_ERR("Failed to read DL status of slave %i.\n",

  1012                slave->ring_position);

  1013         return;

  1014     }

  1015 

  1016     dl_status = EC_READ_U16(datagram->data);

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

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

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

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

  1021     }

  1022 

  1023     // Start fetching EEPROM size

  1024 

  1025     fsm->sii_offset = 0x0040; // first category header

  1026     ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

  1027     fsm->slave_state = ec_fsm_slavescan_eeprom_size;

  1028     fsm->slave_state(fsm); // execute state immediately

  1029 }

  1030 

  1031 /*****************************************************************************/

  1032 

  1033 /**

  1034    Slave scan state: EEPROM SIZE.

  1035 */

  1036 

  1037 void ec_fsm_slavescan_eeprom_size(ec_fsm_t *fsm /**< finite state machine */)

  1038 {

  1039     ec_slave_t *slave = fsm->slave;

  1040     uint16_t cat_type, cat_size;

  1041 

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

  1043 

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

  1045         fsm->slave->error_flag = 1;

  1046         fsm->slave_state = ec_fsm_slave_state_error;

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

  1048                slave->ring_position);

  1049         return;

  1050     }

  1051 

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

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

  1054 

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

  1056         fsm->sii_offset += cat_size + 2;

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

  1058                         EC_FSM_SII_NODE);

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

  1060         return;

  1061     }

  1062 

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

  1064 

  1065     if (slave->eeprom_data) {

  1066         EC_INFO("Freeing old EEPROM data on slave %i...\n",

  1067                 slave->ring_position);

  1068         kfree(slave->eeprom_data);

  1069     }

  1070 

  1071     if (!(slave->eeprom_data =

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

  1073         fsm->slave->error_flag = 1;

  1074         fsm->slave_state = ec_fsm_slave_state_error;

  1075         EC_ERR("Failed to allocate EEPROM data on slave %i.\n",

  1076                slave->ring_position);

  1077         return;

  1078     }

  1079 

  1080     // Start fetching EEPROM contents

  1081 

  1082     fsm->slave_state = ec_fsm_slavescan_eeprom_data;

  1083     fsm->sii_offset = 0x0000;

  1084     ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_NODE);

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

  1086 }

  1087 

  1088 /*****************************************************************************/

  1089 

  1090 /**

  1091    Slave scan state: EEPROM DATA.

  1092 */

  1093 

  1094 void ec_fsm_slavescan_eeprom_data(ec_fsm_t *fsm /**< finite state machine */)

  1095 {

  1096     ec_slave_t *slave = fsm->slave;

  1097     uint16_t *cat_word, cat_type, cat_size;

  1098 

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

  1100 

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

  1102         fsm->slave->error_flag = 1;

  1103         fsm->slave_state = ec_fsm_slave_state_error;

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

  1105                slave->ring_position);

  1106         return;

  1107     }

  1108 

  1109     // 2 words fetched

  1110 

  1111     if (fsm->sii_offset + 2 <= slave->eeprom_size / 2) { // 2 words fit

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

  1113                fsm->fsm_sii.value, 4);

  1114     }

  1115     else { // copy the last word

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

  1117                fsm->fsm_sii.value, 2);

  1118     }

  1119 

  1120     if (fsm->sii_offset + 2 < slave->eeprom_size / 2) {

  1121         // fetch the next 2 words

  1122         fsm->sii_offset += 2;

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

  1124                         EC_FSM_SII_NODE);

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

  1126         return;

  1127     }

  1128 

  1129     // Evaluate EEPROM contents

  1130 

  1131     slave->sii_alias =

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

  1133     slave->sii_vendor_id =

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

  1135     slave->sii_product_code =