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     ec_sdo_request_init(&fsm->request);

    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     ec_sdo_request_clear(&fsm->request);

    80 }

    81 

    82 /*****************************************************************************/

    83 

    84 /** Start Pdo configuration state machine.

    85  */

    86 void ec_fsm_pdo_config_start(

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

    88         ec_slave_t *slave /**< slave to configure */

    89         )

    90 {

    91     fsm->slave = slave;

    92     fsm->state = ec_fsm_pdo_config_state_start;

    93 }

    94 

    95 /*****************************************************************************/

    96 

    97 /** Get running state.

    98  *

    99  * \return false, if state machine has terminated

   100  */

   101 int ec_fsm_pdo_config_running(

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

   103         )

   104 {

   105     return fsm->state != ec_fsm_pdo_config_state_end

   106         && fsm->state != ec_fsm_pdo_config_state_error;

   107 }

   108 

   109 /*****************************************************************************/

   110 

   111 /** Executes the current state.

   112  *

   113  * \return false, if state machine has terminated

   114  */

   115 int ec_fsm_pdo_config_exec(

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

   117         )

   118 {

   119     fsm->state(fsm);

   120     return ec_fsm_pdo_config_running(fsm);

   121 }

   122 

   123 /*****************************************************************************/

   124 

   125 /** Get execution result.

   126  *

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

   128  */

   129 int ec_fsm_pdo_config_success(

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

   131         )

   132 {

   133     return fsm->state == ec_fsm_pdo_config_state_end;

   134 }

   135 

   136 /******************************************************************************

   137  * State functions.

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

   139 

   140 /** Start Pdo configuration.

   141  */

   142 void ec_fsm_pdo_config_state_start(

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

   144         )

   145 {

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

   147         fsm->state = ec_fsm_pdo_config_state_end;

   148         return;

   149     }

   150 

   151     fsm->pdo = NULL;

   152     ec_fsm_pdo_config_next_pdo(fsm);

   153 }

   154 

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

   156 

   157 /** Process configuration of next Pdo.

   158  */

   159 void ec_fsm_pdo_config_next_pdo(

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

   161         )

   162 {

   163     ec_direction_t dir;

   164     const ec_pdo_mapping_t *map;

   165     const ec_pdo_t *pdo, *mapped_pdo;

   166     

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

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

   169 

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

   171             if (fsm->pdo) { // there was a Pdo configured in the last run

   172                 if (pdo == fsm->pdo) // this is the last Pdo

   173                     fsm->pdo = NULL; // take the next one

   174             } else {

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

   176                     if (ec_pdo_equal_entries(pdo, mapped_pdo))

   177                         continue; // Pdo configured correctly

   178 

   179                 fsm->pdo = pdo;

   180                 break;

   181             }

   182         }

   183     }

   184 

   185     if (!fsm->pdo) {

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

   187             EC_DBG("Pdo configuration finished for slave %u.\n",

   188                     fsm->slave->ring_position);

   189         fsm->state = ec_fsm_pdo_config_state_end;

   190         return;

   191     }

   192 

   193     // Pdo configuration has to be changed. Does the slave support this?

   194     if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)

   195             || (fsm->slave->sii.has_general

   196                 && !fsm->slave->sii.coe_details.enable_pdo_configuration)) {

   197         EC_ERR("Slave %u does not support changing the Pdo configuration!\n",

   198                 fsm->slave->ring_position);

   199         fsm->state = ec_fsm_pdo_config_state_error;

   200         return;

   201     }

   202 

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

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

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

   206     }

   207 

   208     if (ec_sdo_request_alloc(&fsm->request, 4)) {

   209         fsm->state = ec_fsm_pdo_config_state_error;

   210         return;

   211     }

   212 

   213     // set mapped Pdo count to zero

   214     EC_WRITE_U8(fsm->request.data, 0);

   215     fsm->request.data_size = 1;

   216     ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);

   217     ecrt_sdo_request_write(&fsm->request);

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

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

   220                 fsm->pdo->index);

   221 

   222     fsm->state = ec_fsm_pdo_config_state_zero_count;

   223     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   225 }

   226 

   227 /*****************************************************************************/

   228 

   229 /** Process next Pdo entry.

   230  */

   231 ec_pdo_entry_t *ec_fsm_pdo_config_next_entry(

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

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

   234         )

   235 {

   236     list = list->next; 

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

   238         return NULL; // no next entry

   239     return list_entry(list, ec_pdo_entry_t, list);

   240 }

   241 

   242 /*****************************************************************************/

   243 

   244 /** Starts to add a Pdo entry.

   245  */

   246 void ec_fsm_pdo_config_add_entry(

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

   248         )

   249 {

   250     uint32_t value;

   251 

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

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

   254     EC_WRITE_U32(fsm->request.data, value);

   255     fsm->request.data_size = 4;

   256     ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count);

   257     ecrt_sdo_request_write(&fsm->request);

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

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

   260                 value, fsm->entry_count);

   261     

   262     fsm->state = ec_fsm_pdo_config_state_add_entry;

   263     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   265 }

   266 

   267 /*****************************************************************************/

   268 

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

   270  */

   271 void ec_fsm_pdo_config_state_zero_count(

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

   273         )

   274 {

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

   276 

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

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

   279                 fsm->slave->ring_position);

   280         fsm->state = ec_fsm_pdo_config_state_error;

   281         return;

   282     }

   283 

   284     // find first entry

   285     if (!(fsm->entry =

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

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

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

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

   290         ec_fsm_pdo_config_next_pdo(fsm);

   291         return;

   292     }

   293 

   294     // add first entry

   295     fsm->entry_count = 1;

   296     ec_fsm_pdo_config_add_entry(fsm);

   297 }

   298 

   299 /*****************************************************************************/

   300 

   301 /** Add a Pdo entry.

   302  */

   303 void ec_fsm_pdo_config_state_add_entry(

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

   305         )

   306 {

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

   308 

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

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

   311                 fsm->entry->index, fsm->entry->subindex,

   312                 fsm->slave->ring_position);

   313         fsm->state = ec_fsm_pdo_config_state_error;

   314         return;

   315     }

   316 

   317     // find next entry

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

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

   320         EC_WRITE_U8(fsm->request.data, fsm->entry_count);

   321         fsm->request.data_size = 1;

   322         ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0);

   323         ecrt_sdo_request_write(&fsm->request);

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

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

   326                     fsm->entry_count);

   327         

   328         fsm->state = ec_fsm_pdo_config_state_entry_count;

   329         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);

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

   331         return;

   332     }

   333 

   334     // add next entry

   335     fsm->entry_count++;

   336     ec_fsm_pdo_config_add_entry(fsm);

   337 }

   338 

   339 /*****************************************************************************/

   340 

   341 /** Set the number of entries.

   342  */

   343 void ec_fsm_pdo_config_state_entry_count(

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

   345         )

   346 {

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

   348 

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

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

   351                 fsm->slave->ring_position);

   352         fsm->state = ec_fsm_pdo_config_state_error;

   353         return;

   354     }

   355 

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

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

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

   359 

   360     ec_fsm_pdo_config_next_pdo(fsm);

   361 }

   362 

   363 /******************************************************************************

   364  * Common state functions

   365  *****************************************************************************/

   366 

   367 /** State: ERROR.

   368  */

   369 void ec_fsm_pdo_config_state_error(

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

   371         )

   372 {

   373 }

   374 

   375 /*****************************************************************************/

   376 

   377 /** State: END.

   378  */

   379 void ec_fsm_pdo_config_state_end(

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

   381         )

   382 {

   383 }

   384 

   385 /*****************************************************************************/