1 /******************************************************************************

     2  *

     3  *  $Id: fsm_coe.c 920 2007-09-12 10:07:55Z fp $

     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 CoE mapping state machines.

    37 */

    38 

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

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "canopen.h"

    45 #include "fsm_coe_map.h"

    46 

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

    48 

    49 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);

    50 void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);

    51 void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);

    52 void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *);

    53 void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);

    54 

    55 void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);

    56 void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);

    57 

    58 void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *);

    59 void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);

    60 void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);

    61 

    62 /*****************************************************************************/

    63 

    64 /**

    65    Constructor.

    66 */

    67 

    68 void ec_fsm_coe_map_init(

    69         ec_fsm_coe_map_t *fsm, /**< finite state machine */

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

    71         )

    72 {

    73     fsm->state = NULL;

    74     fsm->fsm_coe = fsm_coe;

    75 }

    76 

    77 /*****************************************************************************/

    78 

    79 /**

    80    Destructor.

    81 */

    82 

    83 void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)

    84 {

    85 }

    86 

    87 /*****************************************************************************/

    88 

    89 /**

    90    Starts to upload an SDO from a slave.

    91 */

    92 

    93 void ec_fsm_coe_map_start(

    94         ec_fsm_coe_map_t *fsm, /**< finite state machine */

    95         ec_slave_t *slave /**< EtherCAT slave */

    96         )

    97 {

    98     fsm->slave = slave;

    99     fsm->state = ec_fsm_coe_map_state_start;

   100 }

   101 

   102 /*****************************************************************************/

   103 

   104 /**

   105    Executes the current state of the state machine.

   106    \return false, if state machine has terminated

   107 */

   108 

   109 int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   110 {

   111     fsm->state(fsm);

   112 

   113     return fsm->state != ec_fsm_coe_map_state_end

   114         && fsm->state != ec_fsm_coe_map_state_error;

   115 }

   116 

   117 /*****************************************************************************/

   118 

   119 /**

   120    Returns, if the state machine terminated with success.

   121    \return non-zero if successful.

   122 */

   123 

   124 int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */)

   125 {

   126     return fsm->state == ec_fsm_coe_map_state_end;

   127 }

   128 

   129 /******************************************************************************

   130  *  CoE dictionary state machine

   131  *****************************************************************************/

   132 

   133 /**

   134  */

   135 

   136 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   137 {

   138     // read mapping of first sync manager

   139     fsm->sync_index = 0;

   140     ec_fsm_coe_map_action_next_sync(fsm);

   141 }

   142 

   143 /*****************************************************************************/

   144 

   145 /**

   146  */

   147 

   148 void ec_fsm_coe_map_action_next_sync(

   149         ec_fsm_coe_map_t *fsm /**< finite state machine */

   150         )

   151 {

   152     ec_slave_t *slave = fsm->slave;

   153     ec_sdo_entry_t *entry;

   154 

   155     for (; fsm->sync_index < 4; fsm->sync_index++) {

   156         if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index)))

   157             continue;

   158 

   159         if (slave->master->debug_level)

   160             EC_DBG("Reading PDO mapping of sync manager %u of slave %u.\n",

   161                     fsm->sync_index, slave->ring_position);

   162 

   163         if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) {

   164             EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n",

   165                     fsm->sync_sdo->index,

   166                     fsm->slave->ring_position);

   167             fsm->state = ec_fsm_coe_map_state_error;

   168             return;

   169         }

   170 

   171         ec_sdo_request_init_read(&fsm->request, entry);

   172         fsm->state = ec_fsm_coe_map_state_pdo_count;

   173         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   175         return;

   176     }

   177 

   178     fsm->state = ec_fsm_coe_map_state_end;

   179 }

   180 

   181 /*****************************************************************************/

   182 

   183 /**

   184  */

   185 

   186 void ec_fsm_coe_map_state_pdo_count(

   187         ec_fsm_coe_map_t *fsm /**< finite state machine */

   188         )

   189 {

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

   191 

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

   193         EC_ERR("Failed to read number of mapped PDOs from slave %u.\n",

   194                 fsm->slave->ring_position);

   195         fsm->state = ec_fsm_coe_map_state_error;

   196         return;

   197     }

   198 

   199     fsm->sync_subindices = EC_READ_U8(fsm->request.data);

   200 

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

   202         EC_DBG("  %u PDOs mapped.\n", fsm->sync_subindices);

   203 

   204     // read first PDO

   205     fsm->sync_subindex = 1;

   206     ec_fsm_coe_map_action_next_pdo(fsm);

   207 }

   208 

   209 /*****************************************************************************/

   210 

   211 /**

   212  */

   213 

   214 void ec_fsm_coe_map_action_next_pdo(

   215         ec_fsm_coe_map_t *fsm /**< finite state machine */

   216         )

   217 {

   218     ec_sdo_entry_t *entry;

   219 

   220     if (fsm->sync_subindex <= fsm->sync_subindices) {

   221         if (!(entry = ec_sdo_get_entry(fsm->sync_sdo,

   222                         fsm->sync_subindex))) {

   223             EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n",

   224                     fsm->sync_sdo->index,

   225                     fsm->sync_subindex,

   226                     fsm->slave->ring_position);

   227             fsm->state = ec_fsm_coe_map_state_error;

   228             return;

   229         }

   230 

   231         ec_sdo_request_init_read(&fsm->request, entry);

   232         fsm->state = ec_fsm_coe_map_state_pdo;

   233         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   235         return;

   236     }

   237 

   238     // next sync manager

   239     fsm->sync_index++;

   240     ec_fsm_coe_map_action_next_sync(fsm);

   241 }

   242 

   243 /*****************************************************************************/

   244 

   245 /**

   246  */

   247 

   248 void ec_fsm_coe_map_state_pdo(

   249         ec_fsm_coe_map_t *fsm /**< finite state machine */

   250         )

   251 {

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

   253 

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

   255         EC_ERR("Failed to read mapped PDO index from slave %u.\n",

   256                 fsm->slave->ring_position);

   257         fsm->state = ec_fsm_coe_map_state_error;

   258         return;

   259     }

   260 

   261     {

   262         uint16_t pdo_index = EC_READ_U16(fsm->request.data);

   263         ec_sdo_entry_t *entry;

   264 

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

   266             EC_DBG("  PDO 0x%04X.\n", pdo_index);

   267 

   268         if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, pdo_index))) {

   269             EC_ERR("Slave %u has no SDO 0x%04X.\n",

   270                     fsm->slave->ring_position, pdo_index);

   271             fsm->state = ec_fsm_coe_map_state_error;

   272             return;

   273         }

   274 

   275         if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) {

   276             EC_ERR("SDO 0x%04X has no subindex 0 on slave %u.\n",

   277                     fsm->pdo_sdo->index,

   278                     fsm->slave->ring_position);

   279             fsm->state = ec_fsm_coe_map_state_error;

   280             return;

   281         }

   282 

   283         ec_sdo_request_init_read(&fsm->request, entry);

   284         fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

   285         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   287         return;

   288     }

   289 }

   290 

   291 /*****************************************************************************/

   292 

   293 /**

   294  */

   295 

   296 void ec_fsm_coe_map_state_pdo_entry_count(

   297         ec_fsm_coe_map_t *fsm /**< finite state machine */

   298         )

   299 {

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

   301 

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

   303         EC_ERR("Failed to read number of mapped PDO entries from slave %u.\n",

   304                 fsm->slave->ring_position);

   305         fsm->state = ec_fsm_coe_map_state_error;

   306         return;

   307     }

   308 

   309     fsm->pdo_subindices = EC_READ_U8(fsm->request.data);

   310 

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

   312         EC_DBG("    %u PDO entries mapped.\n", fsm->pdo_subindices);

   313 

   314     // read first PDO entry

   315     fsm->pdo_subindex = 1;

   316     ec_fsm_coe_map_action_next_pdo_entry(fsm);

   317 }

   318 

   319 /*****************************************************************************/

   320 

   321 /**

   322  */

   323 

   324 void ec_fsm_coe_map_action_next_pdo_entry(

   325         ec_fsm_coe_map_t *fsm /**< finite state machine */

   326         )

   327 {

   328     ec_sdo_entry_t *entry;

   329 

   330     if (fsm->pdo_subindex <= fsm->pdo_subindices) {

   331         if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo,

   332                         fsm->pdo_subindex))) {

   333             EC_ERR("SDO 0x%04X has no subindex %u on slave %u.\n",

   334                     fsm->pdo_sdo->index, fsm->pdo_subindex,

   335                     fsm->slave->ring_position);

   336             fsm->state = ec_fsm_coe_map_state_error;

   337             return;

   338         }

   339 

   340         ec_sdo_request_init_read(&fsm->request, entry);

   341         fsm->state = ec_fsm_coe_map_state_pdo_entry;

   342         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   344         return;

   345     }

   346 

   347     // next PDO

   348     fsm->sync_subindex++;

   349     ec_fsm_coe_map_action_next_pdo(fsm);

   350 }

   351 

   352 /*****************************************************************************/

   353 

   354 /**

   355  */

   356 

   357 void ec_fsm_coe_map_state_pdo_entry(

   358         ec_fsm_coe_map_t *fsm /**< finite state machine */

   359         )

   360 {

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

   362 

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

   364         EC_ERR("Failed to read index of mapped PDO entry from slave %u.\n",

   365                 fsm->slave->ring_position);

   366         fsm->state = ec_fsm_coe_map_state_error;

   367         return;

   368     }

   369 

   370     {

   371         uint32_t pdo_entry_info;

   372         uint16_t pdo_entry_index;

   373         ec_sdo_t *sdo;

   374 

   375         pdo_entry_info = EC_READ_U32(fsm->request.data);

   376         pdo_entry_index = pdo_entry_info >> 16;

   377 

   378         if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry_index))) {

   379             EC_ERR("Slave %u has no SDO 0x%04X.\n",

   380                     fsm->slave->ring_position, pdo_entry_index);

   381             fsm->state = ec_fsm_coe_map_state_error;

   382             return;

   383         }

   384 

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

   386             size_t bitsize = pdo_entry_info & 0xFFFF;

   387             EC_DBG("    PDO entry 0x%04X \"%s\" (%u bit).\n",

   388                     pdo_entry_index, sdo->name, bitsize);

   389         }

   390 

   391         // next PDO entry

   392         fsm->pdo_subindex++;

   393         ec_fsm_coe_map_action_next_pdo_entry(fsm);

   394     }

   395 }

   396 

   397 /*****************************************************************************/

   398 

   399 /**

   400    State: ERROR.

   401 */

   402 

   403 void ec_fsm_coe_map_state_error(

   404         ec_fsm_coe_map_t *fsm /**< finite state machine */

   405         )

   406 {

   407 }

   408 

   409 /*****************************************************************************/

   410 

   411 /**

   412    State: END.

   413 */

   414 

   415 void ec_fsm_coe_map_state_end(

   416         ec_fsm_coe_map_t *fsm /**< finite state machine */

   417         )

   418 {

   419 }

   420 

   421 /*****************************************************************************/