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_entry(ec_fsm_pdo_mapping_t *);

    52 void ec_fsm_pdo_mapping_state_entry_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_pdo(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, /**< Pdo mapping 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_mapping_clear(

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

    77         )

    78 {

    79     ec_sdo_request_clear(&fsm->request);

    80 }

    81 

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

    83 

    84 /** Start Pdo mapping state machine.

    85  */

    86 void ec_fsm_pdo_mapping_start(

    87         ec_fsm_pdo_mapping_t *fsm, /**< Pdo mapping state machine. */

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

    89         )

    90 {

    91     fsm->slave = slave;

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

   102         const ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   103         )

   104 {

   105     return fsm->state != ec_fsm_pdo_mapping_state_end

   106         && fsm->state != ec_fsm_pdo_mapping_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_mapping_exec(

   116         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   117         )

   118 {

   119     fsm->state(fsm);

   120     return ec_fsm_pdo_mapping_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_mapping_success(

   130         const ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   131         )

   132 {

   133     return fsm->state == ec_fsm_pdo_mapping_state_end;

   134 }

   135 

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

   137  * State functions.

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

   139 

   140 /** Start Pdo mapping.

   141  */

   142 void ec_fsm_pdo_mapping_state_start(

   143         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   144         )

   145 {

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

   147         fsm->state = ec_fsm_pdo_mapping_state_end;

   148         return;

   149     }

   150 

   151     fsm->pdo = NULL;

   152     fsm->num_configured_pdos = 0;

   153     ec_fsm_pdo_mapping_next_pdo(fsm);

   154 }

   155 

   156 /*****************************************************************************/

   157 

   158 /** Process mapping of next Pdo.

   159  */

   160 void ec_fsm_pdo_mapping_next_pdo(

   161         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   162         )

   163 {

   164     uint8_t sync_index;

   165     const ec_pdo_list_t *pdos;

   166     const ec_pdo_t *pdo, *assigned_pdo;

   167     

   168     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {

   169         pdos = &fsm->slave->config->sync_configs[sync_index].pdos;

   170 

   171         list_for_each_entry(pdo, &pdos->list, list) {

   172             if (fsm->pdo) { // there was a Pdo mapping changed in the last run

   173                 if (pdo == fsm->pdo) // this is the previously configured Pdo

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

   175             } else {

   176                 if ((assigned_pdo = ec_slave_find_pdo(fsm->slave, pdo->index)))

   177                     if (ec_pdo_equal_entries(pdo, assigned_pdo))

   178                         continue; // Pdo entries mapped correctly

   179 

   180                 fsm->pdo = pdo;

   181                 fsm->num_configured_pdos++;

   182                 break;

   183             }

   184         }

   185     }

   186 

   187     if (!fsm->pdo) {

   188         if (fsm->slave->master->debug_level && !fsm->num_configured_pdos)

   189             EC_DBG("Pdo mappings are already configured correctly.\n");

   190         fsm->state = ec_fsm_pdo_mapping_state_end;

   191         return;

   192     }

   193 

   194     // Pdo mapping has to be changed. Does the slave support this?

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

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

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

   198         EC_WARN("Slave %u does not support Pdo mapping configuration!\n",

   199                 fsm->slave->ring_position);

   200         fsm->state = ec_fsm_pdo_mapping_state_error;

   201         return;

   202     }

   203 

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

   205         EC_DBG("Changing mapping of Pdo 0x%04X.\n", fsm->pdo->index);

   206     }

   207 

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

   209         fsm->state = ec_fsm_pdo_mapping_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.\n");

   220 

   221     fsm->state = ec_fsm_pdo_mapping_state_zero_count;

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

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

   224 }

   225 

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

   227 

   228 /** Process next Pdo entry.

   229  */

   230 ec_pdo_entry_t *ec_fsm_pdo_mapping_next_entry(

   231         const ec_fsm_pdo_mapping_t *fsm, /**< Pdo mapping state machine. */

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

   233         )

   234 {

   235     list = list->next; 

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

   237         return NULL; // no next entry

   238     return list_entry(list, ec_pdo_entry_t, list);

   239 }

   240 

   241 /*****************************************************************************/

   242 

   243 /** Starts to add a Pdo entry.

   244  */

   245 void ec_fsm_pdo_mapping_add_entry(

   246         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   247         )

   248 {

   249     uint32_t value;

   250 

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

   252         EC_DBG("Mapping Pdo entry 0x%04X:%02X (%u bit) at position %u.\n",

   253                 fsm->entry->index, fsm->entry->subindex,

   254                 fsm->entry->bit_length, fsm->entry_count);

   255 

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

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

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

   259     fsm->request.data_size = 4;

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

   261     ecrt_sdo_request_write(&fsm->request);

   262     

   263     fsm->state = ec_fsm_pdo_mapping_state_add_entry;

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

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

   266 }

   267 

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

   269 

   270 /** Set the number of mapped entries to zero.

   271  */

   272 void ec_fsm_pdo_mapping_state_zero_count(

   273         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   274         )

   275 {

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

   277 

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

   279         EC_WARN("Failed to clear mapping of Pdo 0x%04X.\n", fsm->pdo->index);

   280         fsm->state = ec_fsm_pdo_mapping_state_error;

   281         return;

   282     }

   283 

   284     // find first entry

   285     if (!(fsm->entry =

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

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

   288             EC_DBG("No entries to map.\n");

   289         ec_fsm_pdo_mapping_next_pdo(fsm);

   290         return;

   291     }

   292 

   293     // add first entry

   294     fsm->entry_count = 1;

   295     ec_fsm_pdo_mapping_add_entry(fsm);

   296 }

   297 

   298 /*****************************************************************************/

   299 

   300 /** Add a Pdo entry.

   301  */

   302 void ec_fsm_pdo_mapping_state_add_entry(

   303         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   304         )

   305 {

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

   307 

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

   309         EC_WARN("Failed to map Pdo entry 0x%04X:%02X at 0x%04X:%02X.\n",

   310                 fsm->entry->index, fsm->entry->subindex, fsm->pdo->index,

   311                 fsm->entry_count);

   312         fsm->state = ec_fsm_pdo_mapping_state_error;

   313         return;

   314     }

   315 

   316     // find next entry

   317     if (!(fsm->entry = ec_fsm_pdo_mapping_next_entry(fsm, &fsm->entry->list))) {

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

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

   320         fsm->request.data_size = 1;

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

   322         ecrt_sdo_request_write(&fsm->request);

   323 

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

   325             EC_DBG("Setting number of Pdo entries to %u.\n", fsm->entry_count);

   326         

   327         fsm->state = ec_fsm_pdo_mapping_state_entry_count;

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

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

   330         return;

   331     }

   332 

   333     // add next entry

   334     fsm->entry_count++;

   335     ec_fsm_pdo_mapping_add_entry(fsm);

   336 }

   337 

   338 /*****************************************************************************/

   339 

   340 /** Set the number of entries.

   341  */

   342 void ec_fsm_pdo_mapping_state_entry_count(

   343         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   344         )

   345 {

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

   347 

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

   349         EC_WARN("Failed to set number of entries for Pdo 0x%04X.\n",

   350                 fsm->pdo->index);

   351         fsm->state = ec_fsm_pdo_mapping_state_error;

   352         return;

   353     }

   354 

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

   356         EC_DBG("Pdo mapping configuration successful.\n");

   357 

   358     ec_fsm_pdo_mapping_next_pdo(fsm);

   359 }

   360 

   361 /******************************************************************************

   362  * Common state functions

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

   364 

   365 /** State: ERROR.

   366  */

   367 void ec_fsm_pdo_mapping_state_error(

   368         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   369         )

   370 {

   371 }

   372 

   373 /*****************************************************************************/

   374 

   375 /** State: END.

   376  */

   377 void ec_fsm_pdo_mapping_state_end(

   378         ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */

   379         )

   380 {

   381 }

   382 

   383 /*****************************************************************************/