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 assignment 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_assign.h"

    46 

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

    48 

    49 void ec_fsm_pdo_assign_state_start(ec_fsm_pdo_assign_t *);

    50 void ec_fsm_pdo_assign_state_zero_count(ec_fsm_pdo_assign_t *);

    51 void ec_fsm_pdo_assign_state_add_pdo(ec_fsm_pdo_assign_t *);

    52 void ec_fsm_pdo_assign_state_pdo_count(ec_fsm_pdo_assign_t *);

    53 void ec_fsm_pdo_assign_state_end(ec_fsm_pdo_assign_t *);

    54 void ec_fsm_pdo_assign_state_error(ec_fsm_pdo_assign_t *);

    55 

    56 void ec_fsm_pdo_assign_next_dir(ec_fsm_pdo_assign_t *);

    57 

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

    59 

    60 /** Constructor.

    61  */

    62 void ec_fsm_pdo_assign_init(

    63         ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment 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_assign_clear(

    76         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

    77         )

    78 {

    79     ec_sdo_request_clear(&fsm->request);

    80 }

    81 

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

    83 

    84 /** Start Pdo assignment state machine.

    85  */

    86 void ec_fsm_pdo_assign_start(

    87         ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */

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

    89         )

    90 {

    91     fsm->slave = slave;

    92     fsm->state = ec_fsm_pdo_assign_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_assign_running(

   102         const ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   103         )

   104 {

   105     return fsm->state != ec_fsm_pdo_assign_state_end

   106         && fsm->state != ec_fsm_pdo_assign_state_error;

   107 }

   108 

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

   110 

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

   112  *

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

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

   115  *

   116  * \return false, if state machine has terminated

   117  */

   118 int ec_fsm_pdo_assign_exec(

   119         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   120         )

   121 {

   122     fsm->state(fsm);

   123     return ec_fsm_pdo_assign_running(fsm);

   124 }

   125 

   126 /*****************************************************************************/

   127 

   128 /** Get execution result.

   129  *

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

   131  */

   132 int ec_fsm_pdo_assign_success(

   133         const ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   134         )

   135 {

   136     return fsm->state == ec_fsm_pdo_assign_state_end;

   137 }

   138 

   139 /******************************************************************************

   140  * State functions.

   141  *****************************************************************************/

   142 

   143 /** Start Pdo assignment.

   144  */

   145 void ec_fsm_pdo_assign_state_start(

   146         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   147         )

   148 {

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

   150         fsm->state = ec_fsm_pdo_assign_state_end;

   151         return;

   152     }

   153 

   154     fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT

   155     ec_fsm_pdo_assign_next_dir(fsm);

   156 }

   157 

   158 /*****************************************************************************/

   159 

   160 /** Process Pdo assignment of next direction.

   161  */

   162 void ec_fsm_pdo_assign_next_dir(

   163         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   164         )

   165 {

   166     fsm->dir++;

   167 

   168     for (; fsm->dir <= EC_DIR_INPUT; fsm->dir++) {

   169         fsm->pdos = &fsm->slave->config->pdos[fsm->dir];

   170         

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

   172             if (!list_empty(&fsm->pdos->list)) {

   173                 EC_ERR("No sync manager for direction %u!\n", fsm->dir);

   174                 fsm->state = ec_fsm_pdo_assign_state_end;

   175                 return;

   176             }

   177             continue;

   178         }

   179 

   180         // check if assignment has to be altered

   181         if (ec_pdo_list_equal(&fsm->sync->pdos, fsm->pdos))

   182             continue;

   183 

   184         // Pdo assignment has to be changed. Does the slave support this?

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

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

   187                     && !fsm->slave->sii.coe_details.enable_pdo_assign)) {

   188             EC_ERR("Slave %u does not support assigning Pdos!\n",

   189                     fsm->slave->ring_position);

   190             fsm->state = ec_fsm_pdo_assign_state_error;

   191             return;

   192         }

   193 

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

   195             EC_DBG("Changing Pdo assignment for SM%u of slave %u.\n",

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

   197         }

   198 

   199         if (ec_sdo_request_alloc(&fsm->request, 2)) {

   200             fsm->state = ec_fsm_pdo_assign_state_error;

   201             return;

   202         }

   203 

   204         // set mapped Pdo count to zero

   205         EC_WRITE_U8(fsm->request.data, 0); // zero Pdos mapped

   206         fsm->request.data_size = 1;

   207         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0);

   208         ecrt_sdo_request_write(&fsm->request);

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

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

   211 

   212         fsm->state = ec_fsm_pdo_assign_state_zero_count;

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

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

   215         return;

   216     }

   217 

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

   219         EC_DBG("Pdo assignment finished for slave %u.\n",

   220                 fsm->slave->ring_position);

   221     fsm->state = ec_fsm_pdo_assign_state_end;

   222 }

   223 

   224 /*****************************************************************************/

   225 

   226 /** Assign next Pdo.

   227  */

   228 ec_pdo_t *ec_fsm_pdo_assign_next_pdo(

   229         const ec_fsm_pdo_assign_t *fsm, /**< Pdo assignment state machine. */

   230         const struct list_head *list /**< current Pdo list item */

   231         )

   232 {

   233     list = list->next; 

   234     if (list == &fsm->pdos->list)

   235         return NULL; // no next Pdo

   236     return list_entry(list, ec_pdo_t, list);

   237 }

   238 

   239 /*****************************************************************************/

   240 

   241 /** Assign a Pdo.

   242  */

   243 void ec_fsm_pdo_assign_add_pdo(

   244         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   245         )

   246 {

   247     EC_WRITE_U16(fsm->request.data, fsm->pdo->index);

   248     fsm->request.data_size = 2;

   249     ec_sdo_request_address(&fsm->request,

   250             0x1C10 + fsm->sync->index, fsm->pdo_count);

   251     ecrt_sdo_request_write(&fsm->request);

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

   253         EC_DBG("Assigning Pdo 0x%04X at position %u.\n",

   254                 fsm->pdo->index, fsm->pdo_count);

   255     

   256     fsm->state = ec_fsm_pdo_assign_state_add_pdo;

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

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

   259 }

   260 

   261 /*****************************************************************************/

   262 

   263 /** Set the number of assigned Pdos to zero.

   264  */

   265 void ec_fsm_pdo_assign_state_zero_count(

   266         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   267         )

   268 {

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

   270 

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

   272         EC_ERR("Failed to clear Pdo assignment of slave %u.\n",

   273                 fsm->slave->ring_position);

   274         fsm->state = ec_fsm_pdo_assign_state_error;

   275         return;

   276     }

   277 

   278     // assign all Pdos belonging to the current sync manager

   279     

   280     // find first Pdo

   281     if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdos->list))) {

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

   283             EC_DBG("No Pdos to assign for SM%u of slave %u.\n",

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

   285         ec_fsm_pdo_assign_next_dir(fsm);

   286         return;

   287     }

   288 

   289     // assign first Pdo

   290     fsm->pdo_count = 1;

   291     ec_fsm_pdo_assign_add_pdo(fsm);

   292 }

   293 

   294 /*****************************************************************************/

   295 

   296 /** Add a Pdo to the sync managers Pdo assignment.

   297  */

   298 void ec_fsm_pdo_assign_state_add_pdo(

   299         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   300         )

   301 {

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

   303 

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

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

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

   307         fsm->state = ec_fsm_pdo_assign_state_error;

   308         return;

   309     }

   310 

   311     // find next Pdo

   312     if (!(fsm->pdo = ec_fsm_pdo_assign_next_pdo(fsm, &fsm->pdo->list))) {

   313         // no more Pdos to map. write Pdo count

   314         EC_WRITE_U8(fsm->request.data, fsm->pdo_count);

   315         fsm->request.data_size = 1;

   316         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0);

   317         ecrt_sdo_request_write(&fsm->request);

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

   319             EC_DBG("Setting number of assigned Pdos to %u.\n",

   320                     fsm->pdo_count);

   321         

   322         fsm->state = ec_fsm_pdo_assign_state_pdo_count;

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

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

   325         return;

   326     }

   327 

   328     // add next Pdo to assignment

   329     fsm->pdo_count++;

   330     ec_fsm_pdo_assign_add_pdo(fsm);

   331 }

   332 

   333 /*****************************************************************************/

   334 

   335 /** Set the number of assigned Pdos.

   336  */

   337 void ec_fsm_pdo_assign_state_pdo_count(

   338         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   339         )

   340 {

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

   342 

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

   344         EC_ERR("Failed to set number of assigned Pdos for slave %u.\n",

   345                 fsm->slave->ring_position);

   346         fsm->state = ec_fsm_pdo_assign_state_error;

   347         return;

   348     }

   349 

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

   351         EC_DBG("Successfully set Pdo assignment for SM%u of slave %u.\n",

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

   353 

   354     // assignment for this direction finished

   355     ec_fsm_pdo_assign_next_dir(fsm);

   356 }

   357 

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

   359  * Common state functions

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

   361 

   362 /** State: ERROR.

   363  */

   364 void ec_fsm_pdo_assign_state_error(

   365         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   366         )

   367 {

   368 }

   369 

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

   371 

   372 /** State: END.

   373  */

   374 void ec_fsm_pdo_assign_state_end(

   375         ec_fsm_pdo_assign_t *fsm /**< Pdo assignment state machine. */

   376         )

   377 {

   378 }

   379 

   380 /*****************************************************************************/