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 /** \file

    35  * EtherCAT Pdo configuration state machine.

    36  */

    37 

    38 /*****************************************************************************/

    39 

    40 #include "globals.h"

    41 #include "master.h"

    42 #include "mailbox.h"

    43 #include "slave_config.h"

    44 

    45 #include "fsm_pdo_config.h"

    46 

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

    48 

    49 void ec_fsm_pdo_config_state_start(ec_fsm_pdo_config_t *);

    50 void ec_fsm_pdo_config_state_zero_count(ec_fsm_pdo_config_t *);

    51 void ec_fsm_pdo_config_state_add_entry(ec_fsm_pdo_config_t *);

    52 void ec_fsm_pdo_config_state_entry_count(ec_fsm_pdo_config_t *);

    53 void ec_fsm_pdo_config_state_end(ec_fsm_pdo_config_t *);

    54 void ec_fsm_pdo_config_state_error(ec_fsm_pdo_config_t *);

    55 

    56 void ec_fsm_pdo_config_next_pdo(ec_fsm_pdo_config_t *);

    57 

    58 /*****************************************************************************/

    59 

    60 /** Constructor.

    61  */

    62 void ec_fsm_pdo_config_init(

    63         ec_fsm_pdo_config_t *fsm, /**< pdo_config state machine */

    64         ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */

    65         )

    66 {

    67     fsm->fsm_coe = fsm_coe;

    68     fsm->sdodata.data = (uint8_t *) &fsm->sdo_value;

    69 }

    70 

    71 /*****************************************************************************/

    72 

    73 /** Destructor.

    74  */

    75 void ec_fsm_pdo_config_clear(

    76         ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */

    77         )

    78 {

    79 }

    80 

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

    82 

    83 /** Start Pdo configuration state machine.

    84  */

    85 void ec_fsm_pdo_config_start(

    86         ec_fsm_pdo_config_t *fsm, /**< Pdo configuration state machine */

    87         ec_slave_t *slave /**< slave to configure */

    88         )

    89 {

    90     fsm->slave = slave;

    91     fsm->state = ec_fsm_pdo_config_state_start;

    92 }

    93 

    94 /*****************************************************************************/

    95 

    96 /** Get running state.

    97  *

    98  * \return false, if state machine has terminated

    99  */

   100 int ec_fsm_pdo_config_running(

   101         const ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   102         )

   103 {

   104     return fsm->state != ec_fsm_pdo_config_state_end

   105         && fsm->state != ec_fsm_pdo_config_state_error;

   106 }

   107 

   108 /*****************************************************************************/

   109 

   110 /** Executes the current state.

   111  *

   112  * \return false, if state machine has terminated

   113  */

   114 int ec_fsm_pdo_config_exec(

   115         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   116         )

   117 {

   118     fsm->state(fsm);

   119     return ec_fsm_pdo_config_running(fsm);

   120 }

   121 

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

   123 

   124 /** Get execution result.

   125  *

   126  * \return true, if the state machine terminated gracefully

   127  */

   128 int ec_fsm_pdo_config_success(

   129         const ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   130         )

   131 {

   132     return fsm->state == ec_fsm_pdo_config_state_end;

   133 }

   134 

   135 /******************************************************************************

   136  * State functions.

   137  *****************************************************************************/

   138 

   139 /** Start Pdo configuration.

   140  */

   141 void ec_fsm_pdo_config_state_start(

   142         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   143         )

   144 {

   145     if (!fsm->slave->config) {

   146         fsm->state = ec_fsm_pdo_config_state_end;

   147         return;

   148     }

   149 

   150     fsm->pdo = NULL;

   151     ec_fsm_pdo_config_next_pdo(fsm);

   152 }

   153 

   154 /*****************************************************************************/

   155 

   156 /** Process configuration of next Pdo.

   157  */

   158 void ec_fsm_pdo_config_next_pdo(

   159         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   160         )

   161 {

   162     ec_direction_t dir;

   163     const ec_pdo_mapping_t *map;

   164     const ec_pdo_t *pdo, *mapped_pdo;

   165     

   166     for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) {

   167         map = &fsm->slave->config->mapping[dir];

   168 

   169         list_for_each_entry(pdo, &map->pdos, list) {

   170             if (fsm->pdo) {

   171                 if (pdo == fsm->pdo)

   172                     fsm->pdo = NULL;

   173             } else {

   174                 if ((mapped_pdo = ec_slave_find_pdo(fsm->slave, pdo->index)))

   175                     if (ec_pdo_equal_entries(pdo, mapped_pdo))

   176                         continue; // Pdo configured correctly

   177 

   178                 fsm->pdo = pdo;

   179                 goto configure;

   180             }

   181         }

   182     }

   183 

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

   185         EC_DBG("No more Pdos to configure for slave %u.\n",

   186                 fsm->slave->ring_position);

   187     }

   188     fsm->state = ec_fsm_pdo_config_state_end;

   189     return;

   190 

   191 configure:

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

   193         EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n",

   194                 fsm->pdo->index, fsm->slave->ring_position);

   195     }

   196 

   197     // set mapped PDO count to zero

   198     fsm->sdodata.index = fsm->pdo->index;

   199     fsm->sdodata.subindex = 0; // number of configured entries

   200     EC_WRITE_U8(&fsm->sdo_value, 0);

   201     fsm->sdodata.size = 1;

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

   203         EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n",

   204                 fsm->pdo->index);

   205 

   206     fsm->state = ec_fsm_pdo_config_state_zero_count;

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

   208     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   209 }

   210 

   211 /*****************************************************************************/

   212 

   213 /** Process next Pdo entry.

   214  */

   215 ec_pdo_entry_t *ec_fsm_pdo_config_next_entry(

   216         const ec_fsm_pdo_config_t *fsm, /**< Pdo configuration state machine */

   217         const struct list_head *list /**< current entry list item */

   218         )

   219 {

   220     list = list->next; 

   221     if (list == &fsm->pdo->entries)

   222         return NULL; // no next entry

   223     return list_entry(list, ec_pdo_entry_t, list);

   224 }

   225 

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

   227 

   228 /** Starts to add a Pdo entry.

   229  */

   230 void ec_fsm_pdo_config_add_entry(

   231         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine. */

   232         )

   233 {

   234     uint32_t value;

   235 

   236     fsm->sdodata.subindex = fsm->entry_count;

   237     value = fsm->entry->index << 16

   238         | fsm->entry->subindex << 8 | fsm->entry->bit_length;

   239     EC_WRITE_U32(&fsm->sdo_value, value);

   240     fsm->sdodata.size = 4;

   241 

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

   243         EC_DBG("Configuring Pdo entry %08X at position %u.\n",

   244                 value, fsm->entry_count);

   245     

   246     fsm->state = ec_fsm_pdo_config_state_add_entry;

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

   248     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   249 }

   250 

   251 /*****************************************************************************/

   252 

   253 /** Set the number of configured entries to zero.

   254  */

   255 void ec_fsm_pdo_config_state_zero_count(

   256         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   257         )

   258 {

   259     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   260 

   261     if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   262         EC_ERR("Failed to clear Pdo configuration for slave %u.\n",

   263                 fsm->slave->ring_position);

   264         fsm->state = ec_fsm_pdo_config_state_error;

   265         return;

   266     }

   267 

   268     // find first entry

   269     if (!(fsm->entry =

   270                 ec_fsm_pdo_config_next_entry(fsm, &fsm->pdo->entries))) {

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

   272             EC_DBG("No entries to configure for Pdo 0x%04X of slave %u.\n",

   273                     fsm->pdo->index, fsm->slave->ring_position);

   274         ec_fsm_pdo_config_next_pdo(fsm);

   275         return;

   276     }

   277 

   278     // add first entry

   279     fsm->entry_count = 1;

   280     ec_fsm_pdo_config_add_entry(fsm);

   281 }

   282 

   283 /*****************************************************************************/

   284 

   285 /** Add a Pdo entry.

   286  */

   287 void ec_fsm_pdo_config_state_add_entry(

   288         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   289         )

   290 {

   291     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   292 

   293     if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   294         EC_ERR("Failed to add entry 0x%04X:%u for slave %u.\n",

   295                 fsm->entry->index, fsm->entry->subindex,

   296                 fsm->slave->ring_position);

   297         fsm->state = ec_fsm_pdo_config_state_error;

   298         return;

   299     }

   300 

   301     // find next entry

   302     if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) {

   303         // No more entries to add. Write entry count.

   304         fsm->sdodata.subindex = 0;

   305         EC_WRITE_U8(&fsm->sdo_value, fsm->entry_count);

   306         fsm->sdodata.size = 1;

   307 

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

   309             EC_DBG("Setting number of Pdo entries to %u.\n",

   310                     fsm->entry_count);

   311         

   312         fsm->state = ec_fsm_pdo_config_state_entry_count;

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

   314         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately

   315         return;

   316     }

   317 

   318     // add next entry

   319     fsm->entry_count++;

   320     ec_fsm_pdo_config_add_entry(fsm);

   321 }

   322 

   323 /*****************************************************************************/

   324 

   325 /** Set the number of entries.

   326  */

   327 void ec_fsm_pdo_config_state_entry_count(

   328         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   329         )

   330 {

   331     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;

   332 

   333     if (!ec_fsm_coe_success(fsm->fsm_coe)) {

   334         EC_ERR("Failed to set number of entries for slave %u.\n",

   335                 fsm->slave->ring_position);

   336         fsm->state = ec_fsm_pdo_config_state_error;

   337         return;

   338     }

   339 

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

   341         EC_DBG("Successfully configured Pdo 0x%04X on slave %u.\n",

   342                 fsm->pdo->index, fsm->slave->ring_position);

   343 

   344     ec_fsm_pdo_config_next_pdo(fsm);

   345 }

   346 

   347 /******************************************************************************

   348  * Common state functions

   349  *****************************************************************************/

   350 

   351 /** State: ERROR.

   352  */

   353 void ec_fsm_pdo_config_state_error(

   354         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   355         )

   356 {

   357 }

   358 

   359 /*****************************************************************************/

   360 

   361 /** State: END.

   362  */

   363 void ec_fsm_pdo_config_state_end(

   364         ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine */

   365         )

   366 {

   367 }

   368 

   369 /*****************************************************************************/