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 mapping 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_mapping.h"

    46 

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

    48 

    49 void ec_fsm_pdo_mapping_state_start(ec_fsm_pdo_mapping_t *);

    50 void ec_fsm_pdo_mapping_state_zero_count(ec_fsm_pdo_mapping_t *);

    51 void ec_fsm_pdo_mapping_state_add_pdo(ec_fsm_pdo_mapping_t *);

    52 void ec_fsm_pdo_mapping_state_pdo_count(ec_fsm_pdo_mapping_t *);

    53 void ec_fsm_pdo_mapping_state_end(ec_fsm_pdo_mapping_t *);

    54 void ec_fsm_pdo_mapping_state_error(ec_fsm_pdo_mapping_t *);

    55 

    56 void ec_fsm_pdo_mapping_next_sync(ec_fsm_pdo_mapping_t *);

    57 

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

    59 

    60 /** Constructor.

    61  */

    62 void ec_fsm_pdo_mapping_init(

    63         ec_fsm_pdo_mapping_t *fsm, /**< mapping 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_mapping_clear(

    76         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

    77         )

    78 {

    79 }

    80 

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

    82 

    83 /** Start PDO mapping configuration state machine.

    84  */

    85 void ec_fsm_pdo_mapping_start(

    86         ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */

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

    88         )

    89 {

    90     fsm->slave = slave;

    91     fsm->state = ec_fsm_pdo_mapping_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_mapping_running(

   101         const ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   102         )

   103 {

   104     return fsm->state != ec_fsm_pdo_mapping_state_end

   105         && fsm->state != ec_fsm_pdo_mapping_state_error;

   106 }

   107 

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

   109 

   110 /** Executes the current state of the state machine.

   111  *

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

   113  * of the state machine is delayed to the next cycle.

   114  *

   115  * \return false, if state machine has terminated

   116  */

   117 int ec_fsm_pdo_mapping_exec(

   118         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   119         )

   120 {

   121     fsm->state(fsm);

   122     return ec_fsm_pdo_mapping_running(fsm);

   123 }

   124 

   125 /*****************************************************************************/

   126 

   127 /** Get execution result.

   128  *

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

   130  */

   131 int ec_fsm_pdo_mapping_success(

   132         const ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   133         )

   134 {

   135     return fsm->state == ec_fsm_pdo_mapping_state_end;

   136 }

   137 

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

   139  * State functions.

   140  *****************************************************************************/

   141 

   142 /** Start mapping configuration.

   143  */

   144 void ec_fsm_pdo_mapping_state_start(

   145         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   146         )

   147 {

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

   149         fsm->state = ec_fsm_pdo_mapping_state_end;

   150         return;

   151     }

   152 

   153     fsm->sync = NULL;

   154     ec_fsm_pdo_mapping_next_sync(fsm);

   155 }

   156 

   157 /*****************************************************************************/

   158 

   159 /** Process mapping of next sync manager.

   160  */

   161 void ec_fsm_pdo_mapping_next_sync(

   162         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   163         )

   164 {

   165     ec_direction_t dir;

   166     const ec_sync_t *sync;

   167     const ec_pdo_mapping_t *map;

   168 

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

   170         if (!(sync = ec_slave_get_pdo_sync(fsm->slave, dir))) {

   171             EC_WARN("No sync manager for direction %u!\n", dir);

   172             continue;

   173         }

   174 

   175         if (fsm->sync) { // there is a last SM

   176             if (sync == fsm->sync) // this is the last SM

   177                 fsm->sync = NULL; // take the next one

   178         } else {

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

   180             if (ec_pdo_mapping_equal(&sync->mapping, map))

   181                 continue;

   182 

   183             fsm->sync = sync;

   184             fsm->mapping = map;

   185             break;

   186         }

   187     }

   188 

   189     if (!sync) {

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

   191             EC_DBG("Pdo mapping finished for slave %u.\n",

   192                     fsm->slave->ring_position);

   193         fsm->state = ec_fsm_pdo_mapping_state_end;

   194         return;

   195     }

   196 

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

   198         EC_DBG("Configuring PDO mapping for SM%u of slave %u.\n",

   199                 fsm->sync->index, fsm->slave->ring_position);

   200     }

   201 

   202     // set mapped PDO count to zero

   203     fsm->sdodata.index = 0x1C10 + fsm->sync->index;

   204     fsm->sdodata.subindex = 0; // mapped PDO count

   205     EC_WRITE_U8(&fsm->sdo_value, 0); // zero PDOs mapped

   206     fsm->sdodata.size = 1;

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

   208         EC_DBG("Setting PDO count to zero for SM%u.\n", fsm->sync->index);

   209 

   210     fsm->state = ec_fsm_pdo_mapping_state_zero_count;

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

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

   213 }

   214 

   215 /*****************************************************************************/

   216 

   217 /** Process mapping of next PDO.

   218  */

   219 ec_pdo_t *ec_fsm_pdo_mapping_next_pdo(

   220         const ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */

   221         const struct list_head *list /**< current PDO list item */

   222         )

   223 {

   224     list = list->next; 

   225     if (list == &fsm->mapping->pdos)

   226         return NULL; // no next PDO

   227     return list_entry(list, ec_pdo_t, list);

   228 }

   229 

   230 /*****************************************************************************/

   231 

   232 /** Add a Pdo to the mapping.

   233  */

   234 void ec_fsm_pdo_mapping_add_pdo(

   235         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   236         )

   237 {

   238     fsm->sdodata.subindex = fsm->pdo_count;

   239     EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index);

   240     fsm->sdodata.size = 2;

   241 

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

   243         EC_DBG("Mapping PDO 0x%04X at position %u.\n",

   244                 fsm->pdo->index, fsm->sdodata.subindex);

   245     

   246     fsm->state = ec_fsm_pdo_mapping_state_add_pdo;

   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 mapped PDOs to zero.

   254  */

   255 void ec_fsm_pdo_mapping_state_zero_count(

   256         ec_fsm_pdo_mapping_t *fsm /**< mapping 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 mapping for slave %u.\n",

   263                 fsm->slave->ring_position);

   264         fsm->state = ec_fsm_pdo_mapping_state_error;

   265         return;

   266     }

   267 

   268     // map all PDOs belonging to the current sync manager

   269     

   270     // find first PDO

   271     if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->mapping->pdos))) {

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

   273             EC_DBG("No PDOs to map for SM%u of slave %u.\n",

   274                     fsm->sync->index, fsm->slave->ring_position);

   275         ec_fsm_pdo_mapping_next_sync(fsm);

   276         return;

   277     }

   278 

   279     // add first PDO to mapping

   280     fsm->pdo_count = 1;

   281     ec_fsm_pdo_mapping_add_pdo(fsm);

   282 }

   283 

   284 /*****************************************************************************/

   285 

   286 /** Add a PDO to the sync managers mapping.

   287  */

   288 void ec_fsm_pdo_mapping_state_add_pdo(

   289         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   290         )

   291 {

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

   293 

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

   295         EC_ERR("Failed to map PDO 0x%04X for SM%u of slave %u.\n",

   296                 fsm->pdo->index, fsm->sync->index, fsm->slave->ring_position);

   297         fsm->state = ec_fsm_pdo_mapping_state_error;

   298         return;

   299     }

   300 

   301     // find next PDO

   302     if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) {

   303         // no more PDOs to map. write PDO count

   304         fsm->sdodata.subindex = 0;

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

   306         fsm->sdodata.size = 1;

   307 

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

   309             EC_DBG("Setting number of mapped PDOs to %u.\n",

   310                     fsm->pdo_count);

   311         

   312         fsm->state = ec_fsm_pdo_mapping_state_pdo_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 PDO to mapping

   319     fsm->pdo_count++;

   320     ec_fsm_pdo_mapping_add_pdo(fsm);

   321 }

   322 

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

   324 

   325 /** Set the number of mapped PDOs.

   326  */

   327 void ec_fsm_pdo_mapping_state_pdo_count(

   328         ec_fsm_pdo_mapping_t *fsm /**< mapping 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 mapped PDOs for slave %u.\n",

   335                 fsm->slave->ring_position);

   336         fsm->state = ec_fsm_pdo_mapping_state_error;

   337         return;

   338     }

   339 

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

   341         EC_DBG("Successfully set PDO mapping for SM%u of slave %u.\n",

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

   343 

   344     // mapping configuration for this sync manager complete.

   345     ec_fsm_pdo_mapping_next_sync(fsm);

   346 }

   347 

   348 /******************************************************************************

   349  * Common state functions

   350  *****************************************************************************/

   351 

   352 /** State: ERROR.

   353  */

   354 void ec_fsm_pdo_mapping_state_error(

   355         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   356         )

   357 {

   358 }

   359 

   360 /*****************************************************************************/

   361 

   362 /** State: END.

   363  */

   364 void ec_fsm_pdo_mapping_state_end(

   365         ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */

   366         )

   367 {

   368 }

   369 

   370 /*****************************************************************************/