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    If the state machine's datagram is not sent or received yet, the execution

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

   144    \return false, if state machine has terminated

   145 */

   146 

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

   148 {

   149     if (fsm->datagram.state == EC_DATAGRAM_SENT

   150         || fsm->datagram.state == EC_DATAGRAM_QUEUED) {

   151         // datagram was not sent or received yet.

   152         return ec_fsm_running(fsm);

   153     }

   154 

   155     fsm->master_state(fsm);

   156     return ec_fsm_running(fsm);

   157 }

   158 

   159 /*****************************************************************************/

   160 

   161 /**

   162    \return false, if state machine has terminated

   163 */

   164 

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

   166 {

   167     return fsm->master_state != ec_fsm_master_end

   168         && fsm->master_state != ec_fsm_master_error;

   169 }

   170 

   171 /*****************************************************************************/

   172 

   173 /**

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

   175 */

   176 

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

   178 {

   179     return fsm->master_state == ec_fsm_master_end;

   180 }

   181 

   182 /******************************************************************************

   183  *  operation/idle state machine

   184  *****************************************************************************/

   185 

   186 /**

   187    Master state: START.

   188    Starts with getting slave count and slave states.

   189 */

   190 

   191 void ec_fsm_master_start(ec_fsm_t *fsm)

   192 {

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

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

   195     fsm->master_state = ec_fsm_master_broadcast;

   196 }

   197 

   198 /*****************************************************************************/

   199 

   200 /**

   201    Master state: BROADCAST.

   202    Processes the broadcast read slave count and slaves states.

   203 */

   204 

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

   206 {

   207     ec_datagram_t *datagram = &fsm->datagram;

   208     unsigned int topology_change, states_change, i;

   209     ec_slave_t *slave;

   210     ec_master_t *master = fsm->master;

   211 

   212     if (datagram->state == EC_DATAGRAM_TIMED_OUT) {

   213         // always retry

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

   215         return;

   216     }

   217 

   218     if (datagram->state != EC_DATAGRAM_RECEIVED) { // EC_DATAGRAM_ERROR

   219         // link is down

   220         fsm->master_slaves_responding = 0;

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

   222             slave->online = 0;

   223         }

   224         fsm->master_state = ec_fsm_master_error;

   225         return;

   226     }

   227 

   228     topology_change = (datagram->working_counter !=

   229                        fsm->master_slaves_responding);

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

   231 

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

   233     fsm->master_slaves_responding = datagram->working_counter;

   234 

   235     if (topology_change) {

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

   237                 fsm->master_slaves_responding,

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

   239 

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

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

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

   243             }

   244             else {

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

   246             }

   247         }

   248     }

   249 

   250     if (states_change) {

   251         char states[EC_STATE_STRING_SIZE];

   252         ec_state_string(fsm->master_slave_states, states);

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

   254     }

   255 

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

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

   258 

   259         ec_master_eoe_stop(master);

   260         ec_master_destroy_slaves(master);

   261 

   262         master->slave_count = datagram->working_counter;

   263 

   264         if (!master->slave_count) {

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

   266             fsm->master_state = ec_fsm_master_end;

   267             return;

   268         }

   269 

   270         // init slaves

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

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

   273                                                  GFP_ATOMIC))) {

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

   275                 ec_master_destroy_slaves(master);

   276                 fsm->master_state = ec_fsm_master_error;

   277                 return;

   278             }

   279 

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

   281                 // freeing of "slave" already done

   282                 ec_master_destroy_slaves(master);

   283                 fsm->master_state = ec_fsm_master_error;

   284                 return;

   285             }

   286 

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

   288         }

   289 

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

   291 

   292         // begin scanning of slaves

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

   294         fsm->slave_state = ec_fsm_slavescan_start;

   295         fsm->master_state = ec_fsm_master_scan_slaves;

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

   297         return;

   298     }

   299 

   300     // fetch state from each slave

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

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

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

   304     fsm->retries = EC_FSM_RETRIES;

   305     fsm->master_state = ec_fsm_master_read_states;

   306 }

   307 

   308 /*****************************************************************************/

   309 

   310 /**

   311    Master action: PROC_STATES.

   312    Processes the slave states.

   313 */

   314 

   315 void ec_fsm_master_action_process_states(ec_fsm_t *fsm

   316                                          /**< finite state machine */

   317                                          )

   318 {

   319     ec_master_t *master = fsm->master;

   320     ec_slave_t *slave;

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

   322 

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

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

   325         if (slave->error_flag

   326             || !slave->online

   327             || slave->requested_state == EC_SLAVE_STATE_UNKNOWN

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

   329                 && slave->self_configured)) continue;

   330 

   331         if (master->debug_level) {

   332             ec_state_string(slave->current_state, old_state);

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

   334                 ec_state_string(slave->requested_state, new_state);

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

   336                        slave->ring_position, old_state, new_state);

   337             }

   338             else if (!slave->self_configured) {

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

   340                        slave->ring_position, old_state);

   341             }

   342         }

   343 

   344         fsm->master_state = ec_fsm_master_configure_slave;

   345         fsm->slave = slave;

   346         fsm->slave_state = ec_fsm_slaveconf_state_start;

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

   348         return;

   349     }

   350 

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

   352     ec_master_eoe_start(master);

   353 

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

   355 

   356         // Check for a pending SDO request

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

   358             if (master->debug_level)

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

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

   361             if (slave->current_state == EC_SLAVE_STATE_INIT

   362                 || !slave->online) {

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

   364                        slave->ring_position);

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

   366                 master->sdo_seq_master++;

   367             }

   368             else {

   369                 // start uploading SDO

   370                 fsm->slave = slave;

   371                 fsm->master_state = ec_fsm_master_sdo_request;

   372                 fsm->sdo_request = master->sdo_request;

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

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

   375                 return;

   376             }

   377         }

   378 

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

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

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

   382                 || slave->sdo_dictionary_fetched

   383                 || slave->current_state == EC_SLAVE_STATE_INIT

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

   385                 || !slave->online

   386                 || slave->error_flag) continue;

   387 

   388             if (master->debug_level) {

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

   390                        slave->ring_position);

   391             }

   392 

   393             slave->sdo_dictionary_fetched = 1;

   394 

   395             // start fetching SDO dictionary

   396             fsm->slave = slave;

   397             fsm->master_state = ec_fsm_master_sdodict;

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

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

   400             return;

   401         }

   402 

   403         // check for pending EEPROM write operations.

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

   405             if (!slave->new_eeprom_data) continue;

   406 

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

   408                 kfree(slave->new_eeprom_data);

   409                 slave->new_eeprom_data = NULL;

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

   411                        slave->ring_position);

   412                 continue;

   413             }

   414 

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

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

   417             fsm->slave = slave;

   418             fsm->sii_offset = 0x0000;

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

   420                              slave->new_eeprom_data, EC_FSM_SII_NODE);

   421             fsm->master_state = ec_fsm_master_write_eeprom;

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

   423             return;

   424         }

   425     }

   426 

   427     fsm->master_state = ec_fsm_master_end;

   428 }

   429 

   430 /*****************************************************************************/

   431 

   432 /**

   433    Master action: Get state of next slave.

   434 */

   435 

   436 void ec_fsm_master_action_next_slave_state(ec_fsm_t *fsm

   437                                            /**< finite state machine */)

   438 {

   439     ec_master_t *master = fsm->master;

   440     ec_slave_t *slave = fsm->slave;

   441 

   442     // is there another slave to query?

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

   444         // process next slave

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

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

   447                          0x0130, 2);

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

   449         fsm->retries = EC_FSM_RETRIES;

   450         fsm->master_state = ec_fsm_master_read_states;

   451         return;

   452     }

   453 

   454     // all slave states read

   455 

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

   457     if (fsm->master_validation) {

   458         fsm->master_validation = 0;

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

   460             if (slave->online) continue;

   461 

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

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

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

   465             fsm->master_state = ec_fsm_master_validate_vendor;

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

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

   468             return;

   469         }

   470     }

   471 

   472     ec_fsm_master_action_process_states(fsm);

   473 }

   474 

   475 /*****************************************************************************/

   476 

   477 /**

   478    Master state: READ STATES.

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

   480 */

   481 

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

   483 {

   484     ec_slave_t *slave = fsm->slave;

   485     ec_datagram_t *datagram = &fsm->datagram;

   486     uint8_t new_state;

   487 

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

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

   490         return;

   491     }

   492 

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

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

   495                slave->ring_position);

   496         fsm->master_state = ec_fsm_master_error;

   497         return;

   498     }

   499 

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

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

   502         if (slave->online) {

   503             slave->online = 0;

   504             if (slave->master->debug_level)

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

   506         }

   507         ec_fsm_master_action_next_slave_state(fsm);

   508         return;

   509     }

   510 

   511     // slave responded

   512     new_state = EC_READ_U8(datagram->data);

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

   514         slave->online = 1;

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

   516         slave->current_state = new_state;

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

   518             char cur_state[EC_STATE_STRING_SIZE];

   519             ec_state_string(slave->current_state, cur_state);

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

   521                    slave->ring_position, cur_state);

   522         }

   523     }

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

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

   526             char old_state[EC_STATE_STRING_SIZE],

   527                 cur_state[EC_STATE_STRING_SIZE];

   528             ec_state_string(slave->current_state, old_state);

   529             ec_state_string(new_state, cur_state);

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

   531                    slave->ring_position, old_state, cur_state);

   532         }

   533         slave->current_state = new_state;

   534     }

   535 

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

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

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

   539         ec_fsm_change_exec(&fsm->fsm_change);

   540         fsm->master_state = ec_fsm_master_acknowledge;

   541         return;

   542     }

   543 

   544     ec_fsm_master_action_next_slave_state(fsm);

   545 }

   546 

   547 /*****************************************************************************/

   548 

   549 /**

   550    Master state: ACKNOWLEDGE

   551 */

   552 

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

   554 {

   555     ec_slave_t *slave = fsm->slave;

   556 

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

   558 

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

   560         fsm->slave->error_flag = 1;

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

   562                slave->ring_position);

   563         fsm->master_state = ec_fsm_master_error;

   564         return;

   565     }

   566 

   567     ec_fsm_master_action_next_slave_state(fsm);

   568 }

   569 

   570 /*****************************************************************************/

   571 

   572 /**

   573    Master state: VALIDATE_VENDOR.

   574    Validates the vendor ID of a slave.

   575 */

   576 

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

   578 {

   579     ec_slave_t *slave = fsm->slave;

   580 

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

   582 

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

   584         fsm->slave->error_flag = 1;

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

   586                slave->ring_position);

   587         fsm->master_state = ec_fsm_master_error;

   588         return;

   589     }

   590 

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

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

   593         fsm->master_state = ec_fsm_master_error;

   594         return;

   595     }

   596 

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

   598     fsm->master_state = ec_fsm_master_validate_product;

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

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

   601 }

   602 

   603 /*****************************************************************************/

   604 

   605 /**

   606    Master action: ADDRESS.

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

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

   609 */

   610 

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

   612 {

   613     ec_datagram_t *datagram = &fsm->datagram;

   614 

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

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

   617             fsm->master_state = ec_fsm_master_start;

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

   619             return;

   620         }

   621         // check next slave

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

   623     }

   624 

   625     if (fsm->master->debug_level)

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

   627 

   628     // write station address

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

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

   631     ec_master_queue_datagram(fsm->master, datagram);

   632     fsm->retries = EC_FSM_RETRIES;

   633     fsm->master_state = ec_fsm_master_rewrite_addresses;

   634 }

   635 

   636 /*****************************************************************************/

   637 

   638 /**

   639    Master state: VALIDATE_PRODUCT.

   640    Validates the product ID of a slave.

   641 */

   642 

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

   644 {

   645     ec_slave_t *slave = fsm->slave;

   646 

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

   648 

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

   650         fsm->slave->error_flag = 1;

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

   652                slave->ring_position);

   653         fsm->master_state = ec_fsm_master_error;

   654         return;

   655     }

   656 

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

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

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

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

   661         fsm->master_state = ec_fsm_master_error;

   662         return;

   663     }

   664 

   665     // have all states been validated?

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

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

   668         // start writing addresses to offline slaves

   669         ec_fsm_master_action_addresses(fsm);

   670         return;

   671     }

   672 

   673     // validate next slave

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

   675     fsm->master_state = ec_fsm_master_validate_vendor;

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

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

   678 }

   679 

   680 /*****************************************************************************/

   681 

   682 /**

   683    Master state: REWRITE ADDRESS.

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

   685 */

   686 

   687 void ec_fsm_master_rewrite_addresses(ec_fsm_t *fsm

   688                                      /**< finite state machine */

   689                                      )

   690 {

   691     ec_slave_t *slave = fsm->slave;

   692     ec_datagram_t *datagram = &fsm->datagram;

   693 

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

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

   696         return;

   697     }

   698 

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

   700         EC_ERR("Failed to receive address datagram for slave %i.\n",

   701                slave->ring_position);

   702         fsm->master_state = ec_fsm_master_error;

   703         return;

   704     }

   705 

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

   707         EC_ERR("Failed to write station address - slave %i did not respond.\n",

   708                slave->ring_position);

   709         fsm->master_state = ec_fsm_master_error;

   710         return;

   711     }

   712 

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

   714         fsm->master_state = ec_fsm_master_start;

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

   716         return;

   717     }

   718 

   719     // check next slave

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

   721     // Write new station address to slave

   722     ec_fsm_master_action_addresses(fsm);

   723 }

   724 

   725 /*****************************************************************************/

   726 

   727 /**

   728    Master state: SCAN SLAVES.

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

   730 */

   731 

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

   733 {

   734     ec_master_t *master = fsm->master;

   735     ec_slave_t *slave = fsm->slave;

   736 

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

   738 

   739     if (fsm->slave_state != ec_fsm_slave_state_end

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

   741 

   742     // another slave to fetch?

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

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

   745         fsm->slave_state = ec_fsm_slavescan_start;

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

   747         return;

   748     }

   749 

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

   751 

   752     ec_master_calc_addressing(master);

   753 

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

   755     // communication possible

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

   757         ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);

   758     }

   759 

   760     fsm->master_state = ec_fsm_master_end;

   761 }

   762 

   763 /*****************************************************************************/

   764 

   765 /**

   766    Master state: CONFIGURE SLAVES.

   767    Starts configuring a slave.

   768 */

   769 

   770 void ec_fsm_master_configure_slave(ec_fsm_t *fsm

   771                                    /**< finite state machine */

   772                                    )

   773 {

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

   775 

   776     if (fsm->slave_state != ec_fsm_slave_state_end

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

   778 

   779     ec_fsm_master_action_process_states(fsm);

   780 }

   781 

   782 /*****************************************************************************/

   783 

   784 /**

   785    Master state: WRITE EEPROM.

   786 */

   787 

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

   789 {

   790     ec_slave_t *slave = fsm->slave;

   791 

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

   793 

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

   795         fsm->slave->error_flag = 1;

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

   797                slave->ring_position);

   798         kfree(slave->new_eeprom_data);

   799         slave->new_eeprom_data = NULL;

   800         fsm->master_state = ec_fsm_master_error;

   801         return;

   802     }

   803 

   804     fsm->sii_offset++;

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

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

   807                          slave->new_eeprom_data + fsm->sii_offset,

   808                          EC_FSM_SII_NODE);

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

   810         return;

   811     }

   812 

   813     // finished writing EEPROM

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

   815     kfree(slave->new_eeprom_data);

   816     slave->new_eeprom_data = NULL;

   817 

   818     // TODO: Evaluate new EEPROM contents!

   819 

   820     // restart master state machine.

   821     fsm->master_state = ec_fsm_master_start;

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

   823 }

   824 

   825 /*****************************************************************************/

   826 

   827 /**

   828    Master state: SDODICT.

   829 */

   830 

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

   832 {

   833     ec_slave_t *slave = fsm->slave;

   834     ec_master_t *master = fsm->master;

   835 

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

   837 

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

   839         fsm->master_state = ec_fsm_master_error;

   840         return;

   841     }

   842 

   843     // SDO dictionary fetching finished

   844 

   845     if (master->debug_level) {

   846         unsigned int sdo_count, entry_count;

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

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

   849                sdo_count, entry_count, slave->ring_position);

   850     }

   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    Master state: SDO REQUEST.

   861 */

   862 

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

   864 {

   865     ec_master_t *master = fsm->master;

   866     ec_sdo_request_t *request = fsm->sdo_request;

   867 

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

   869 

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

   871         request->return_code = -1;

   872         master->sdo_seq_master++;

   873         fsm->master_state = ec_fsm_master_error;

   874         return;

   875     }

   876 

   877     // SDO dictionary fetching finished

   878 

   879     request->return_code = 1;

   880     master->sdo_seq_master++;

   881 

   882     // restart master state machine.

   883     fsm->master_state = ec_fsm_master_start;

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

   885 }

   886 

   887 /*****************************************************************************/

   888 

   889 /**

   890    State: ERROR.

   891 */

   892 

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

   894 {

   895     fsm->master_state = ec_fsm_master_start;

   896 }

   897 

   898 /*****************************************************************************/

   899 

   900 /**

   901    State: END.

   902 */

   903 

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

   905 {

   906     fsm->master_state = ec_fsm_master_start;

   907 }

   908 

   909 /******************************************************************************

   910  *  slave scan state machine

   911  *****************************************************************************/

   912 

   913 /**

   914    Slave scan state: START.

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

   916    slave, according to its ring position.

   917 */

   918 

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

   920 {

   921     ec_datagram_t *datagram = &fsm->datagram;

   922 

   923     // write station address

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

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

   926     ec_master_queue_datagram(fsm->master, datagram);

   927     fsm->retries = EC_FSM_RETRIES;

   928     fsm->slave_state = ec_fsm_slavescan_address;

   929 }

   930 

   931 /*****************************************************************************/

   932 

   933 /**

   934    Slave scan state: ADDRESS.

   935 */

   936 

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

   938 {

   939     ec_datagram_t *datagram = &fsm->datagram;

   940 

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

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

   943         return;

   944     }

   945 

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

   947         fsm->slave_state = ec_fsm_slave_state_error;

   948         EC_ERR("Failed to receive station address datagram for slave %i.\n",

   949                fsm->slave->ring_position);

   950         return;

   951     }

   952 

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

   954         fsm->slave->error_flag = 1;

   955         fsm->slave_state = ec_fsm_slave_state_error;

   956         EC_ERR("Failed to write station address - slave %i did not respond.\n",

   957                fsm->slave->ring_position);

   958         return;

   959     }

   960 

   961     // Read AL state

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

   963     ec_master_queue_datagram(fsm->master, datagram);

   964     fsm->retries = EC_FSM_RETRIES;

   965     fsm->slave_state = ec_fsm_slavescan_state;

   966 }

   967 

   968 /*****************************************************************************/

   969 

   970 /**

   971    Slave scan state: STATE.

   972 */

   973 

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

   975 {

   976     ec_datagram_t *datagram = &fsm->datagram;

   977     ec_slave_t *slave = fsm->slave;

   978 

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

   980         ec_master_queue_datagram(fsm->master, datagram);

   981         return;

   982     }

   983 

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

   985         fsm->slave_state = ec_fsm_slave_state_error;

   986         EC_ERR("Failed to receive AL state datagram from slave %i.\n",

   987                fsm->slave->ring_position);

   988         return;

   989     }

   990 

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

   992         fsm->slave->error_flag = 1;

   993         fsm->slave_state = ec_fsm_slave_state_error;

   994         EC_ERR("Failed to read AL state - slave %i did not respond.\n",

   995                fsm->slave->ring_position);

   996         return;

   997     }

   998 

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

  1000     if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {

  1001         char state_str[EC_STATE_STRING_SIZE];

  1002         ec_state_string(slave->current_state, state_str);

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

  1004                 slave->ring_position, state_str);

  1005     }

  1006 

  1007     // read base data

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

  1009     ec_master_queue_datagram(fsm->master, datagram);

  1010     fsm->retries = EC_FSM_RETRIES;

  1011     fsm->slave_state = ec_fsm_slavescan_base;

  1012 }

  1013 

  1014 /*****************************************************************************/

  1015 

  1016 /**

  1017    Slave scan state: BASE.

  1018 */

  1019 

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

  1021 {

  1022     ec_datagram_t *datagram = &fsm->datagram;

  1023     ec_slave_t *slave = fsm->slave;

  1024 

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

  1026         ec_master_queue_datagram(fsm->master, datagram);

  1027         return;

  1028     }

  1029 

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

  1031         fsm->slave_state = ec_fsm_slave_state_error;

  1032         EC_ERR("Failed to receive base data datagram for slave %i.\n",

  1033                slave->ring_position);

  1034         return;

  1035     }

  1036 

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

  1038         fsm->slave->error_flag = 1;

  1039         fsm->slave_state = ec_fsm_slave_state_error;

  1040         EC_ERR("Failed to read base data - slave %i did not respond.\n",

  1041                slave->ring_position);

  1042         return;

  1043     }

  1044 

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

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

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

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

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

  1050 

  1051     if (slave->base_fmmu_count > EC_MAX_FMMUS)

  1052         slave->base_fmmu_count = EC_MAX_FMMUS;

  1053 

  1054     // read data link status

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

  1056     ec_master_queue_datagram(slave->master, datagram);

  1057     fsm->retries = EC_FSM_RETRIES;

  1058     fsm->slave_state = ec_fsm_slavescan_datalink;

  1059 }

  1060 

  1061 /*****************************************************************************/

  1062 

  1063 /**

  1064    Slave scan state: DATALINK.

  1065 */

  1066 

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

  1068 {

  1069     ec_datagram_t *datagram = &fsm->datagram;

  1070     ec_slave_t *slave = fsm->slave;

  1071     uint16_t dl_status;

  1072     unsigned int i;

  1073 

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

  1075         ec_master_queue_datagram(fsm->master, datagram);

  1076         return;

  1077     }

  1078 

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

  1080         fsm->slave_state = ec_fsm_slave_state_error;

  1081         EC_ERR("Failed to receive DL status datagram from slave %i.\n",

  1082                slave->ring_position);

  1083         return;

  1084     }

  1085 

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

  1087         fsm->slave->error_flag = 1;

  1088         fsm->slave_state = ec_fsm_slave_state_error;

  1089         EC_ERR("Failed to read DL status - slave %i did not respond.\n",

  1090                slave->ring_position);

  1091         return;

  1092     }

  1093 

  1094     dl_status = EC_READ_U16(datagram->data);

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

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

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

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

  1099     }

  1100 

  1101     // Start fetching EEPROM size

  1102 

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

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

  1105     fsm->slave_state = ec_fsm_slavescan_eeprom_size;

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

  1107 }

  1108 

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

  1110 

  1111 /**

  1112    Slave scan state: EEPROM SIZE.

  1113 */

  1114 

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

  1116 {

  1117     ec_slave_t *slave = fsm->slave;

  1118     uint16_t cat_type, cat_size;

  1119 

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

  1121 

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

  1123         fsm->slave->error_flag = 1;

  1124         fsm->slave_state = ec_fsm_slave_state_error;

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

  1126                slave->ring_position);

  1127         return;

  1128     }

  1129 

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

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

  1132 

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

  1134         fsm->sii_offset += cat_size + 2;

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

  1136                         EC_FSM_SII_NODE);