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 PDO mapping state machine.

    37 */

    38 

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

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "fsm_mapping.h"

    45 

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

    47 

    48 void ec_fsm_mapping_state_start(ec_fsm_mapping_t *);

    49 void ec_fsm_mapping_state_zero_count(ec_fsm_mapping_t *);

    50 void ec_fsm_mapping_state_add_pdo(ec_fsm_mapping_t *);

    51 void ec_fsm_mapping_state_pdo_count(ec_fsm_mapping_t *);

    52 void ec_fsm_mapping_state_end(ec_fsm_mapping_t *);

    53 void ec_fsm_mapping_state_error(ec_fsm_mapping_t *);

    54 

    55 void ec_fsm_mapping_next_sync(ec_fsm_mapping_t *);

    56 

    57 /*****************************************************************************/

    58 

    59 /**

    60  * Constructor.

    61  */

    62 

    63 void ec_fsm_mapping_init(ec_fsm_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 /**

    74  * Destructor.

    75  */

    76 

    77 void ec_fsm_mapping_clear(ec_fsm_mapping_t *fsm /**< mapping state machine */)

    78 {

    79 }

    80 

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

    82 

    83 /**

    84  * Start PDO mapping configuration state machine.

    85  */

    86 

    87 void ec_fsm_mapping_start(

    88         ec_fsm_mapping_t *fsm, /**< mapping state machine */

    89         ec_slave_t *slave /**< slave to configure */

    90         )

    91 {

    92     fsm->slave = slave;

    93     fsm->state = ec_fsm_mapping_state_start;

    94 }

    95 

    96 /*****************************************************************************/

    97 

    98 /**

    99  * \return false, if state machine has terminated

   100  */

   101 

   102 int ec_fsm_mapping_running(

   103         const ec_fsm_mapping_t *fsm /**< mapping state machine */

   104         )

   105 {

   106     return fsm->state != ec_fsm_mapping_state_end

   107         && fsm->state != ec_fsm_mapping_state_error;

   108 }

   109 

   110 /*****************************************************************************/

   111 

   112 /**

   113  * Executes the current state of the state machine.

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

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

   116  * \return false, if state machine has terminated

   117  */

   118 

   119 int ec_fsm_mapping_exec(

   120         ec_fsm_mapping_t *fsm /**< mapping state machine */

   121         )

   122 {

   123     fsm->state(fsm);

   124     return ec_fsm_mapping_running(fsm);

   125 }

   126 

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

   128 

   129 /**

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

   131  */

   132 

   133 int ec_fsm_mapping_success(

   134         const ec_fsm_mapping_t *fsm /**< mapping state machine */

   135         )

   136 {

   137     return fsm->state == ec_fsm_mapping_state_end;

   138 }

   139 

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

   141  * PDO mapping state machine

   142  *****************************************************************************/

   143 

   144 /**

   145  */

   146 

   147 void ec_fsm_mapping_state_start(

   148         ec_fsm_mapping_t *fsm /**< mapping state machine */

   149         )

   150 {

   151     fsm->dir = EC_DIR_OUTPUT;

   152     ec_fsm_mapping_next_sync(fsm);

   153 }

   154 

   155 /*****************************************************************************/

   156 

   157 /**

   158  */

   159 

   160 void ec_fsm_mapping_next_sync(

   161         ec_fsm_mapping_t *fsm /**< mapping state machine */

   162         )

   163 {

   164     while (1) {

   165         if (fsm->dir > EC_DIR_INPUT) {

   166             // no more directions to configure mappings for

   167             fsm->state = ec_fsm_mapping_state_end;

   168             return;

   169         }

   170 

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

   172             // no sync manager found for this direction

   173             fsm->dir++;

   174             continue;

   175         }

   176 

   177         fsm->dir++;

   178         if (fsm->sync->alt_mapping)

   179             break;

   180     }

   181 

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

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

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

   185     }

   186 

   187     // set mapped PDO count to zero

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

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

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

   191     fsm->sdodata.size = 1;

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

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

   194 

   195     fsm->state = ec_fsm_mapping_state_zero_count;

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

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

   198 }

   199 

   200 /*****************************************************************************/

   201 

   202 /**

   203  */

   204 

   205 ec_pdo_t *ec_fsm_mapping_next_pdo(

   206         ec_fsm_mapping_t *fsm, /**< mapping state machine */

   207         struct list_head *list /**< current PDO list item */

   208         )

   209 {

   210     ec_pdo_t *pdo;

   211 

   212     do {

   213         list = list->next; 

   214         if (list == &fsm->sync->pdos)

   215             return NULL; // no next PDO

   216         pdo = list_entry(list, ec_pdo_t, list);

   217     }

   218     while (pdo->sync_index != fsm->sync->index);

   219     

   220     return pdo;

   221 }

   222 

   223 /*****************************************************************************/

   224 

   225 /**

   226  */

   227 

   228 void ec_fsm_mapping_state_zero_count(

   229         ec_fsm_mapping_t *fsm /**< mapping state machine */

   230         )

   231 {

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

   233 

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

   235         EC_ERR("Failed to clear PDO mapping for slave %u.\n",

   236                 fsm->slave->ring_position);

   237         fsm->state = ec_fsm_mapping_state_error;

   238         return;

   239     }

   240 

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

   242     

   243     // find first PDO

   244     if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   245                     fsm, &fsm->sync->pdos))) {

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

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

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

   249         ec_fsm_mapping_next_sync(fsm);

   250         return;

   251     }

   252 

   253     // add first PDO to mapping

   254     fsm->pdo_count = 1;

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

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

   257     fsm->sdodata.size = 2;

   258 

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

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

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

   262     

   263     fsm->state = ec_fsm_mapping_state_add_pdo;

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

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

   266 }

   267 

   268 /*****************************************************************************/

   269 

   270 /**

   271  */

   272 

   273 void ec_fsm_mapping_state_add_pdo(

   274         ec_fsm_mapping_t *fsm /**< mapping state machine */

   275         )

   276 {

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

   278 

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

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

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

   282         fsm->state = ec_fsm_mapping_state_error;

   283         return;

   284     }

   285 

   286     // find next PDO

   287     if (!(fsm->pdo = ec_fsm_mapping_next_pdo(

   288                     fsm, &fsm->pdo->list))) {

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

   290         fsm->sdodata.subindex = 0;

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

   292         fsm->sdodata.size = 1;

   293 

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

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

   296                     fsm->pdo_count);

   297         

   298         fsm->state = ec_fsm_mapping_state_pdo_count;

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

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

   301         return;

   302     }

   303 

   304     // add next PDO to mapping

   305     fsm->pdo_count++;

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

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

   308     fsm->sdodata.size = 2;

   309 

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

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

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

   313 

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

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

   316 }

   317 

   318 /*****************************************************************************/

   319 

   320 /**

   321  */

   322 

   323 void ec_fsm_mapping_state_pdo_count(

   324         ec_fsm_mapping_t *fsm /**< mapping state machine */

   325         )

   326 {

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

   328 

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

   330         EC_ERR("Failed to set number of mapped PDOs for slave %u.\n",

   331                 fsm->slave->ring_position);

   332         fsm->state = ec_fsm_mapping_state_error;

   333         return;

   334     }

   335 

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

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

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

   339 

   340     // mapping configuration for this sync manager complete.

   341     ec_fsm_mapping_next_sync(fsm);

   342 }

   343 

   344 /******************************************************************************

   345  * Common state functions

   346  *****************************************************************************/

   347 

   348 /**

   349    State: ERROR.

   350 */

   351 

   352 void ec_fsm_mapping_state_error(

   353         ec_fsm_mapping_t *fsm /**< mapping state machine */

   354         )

   355 {

   356 }

   357 

   358 /*****************************************************************************/

   359 

   360 /**

   361    State: END.

   362 */

   363 

   364 void ec_fsm_mapping_state_end(

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

   366         )

   367 {

   368 }

   369 

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