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_sync(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->sync_index = 0xff; // next is zero

   155     fsm->num_configured_syncs = 0;

   156     ec_fsm_pdo_assign_next_sync(fsm);

   157 }

   158 

   159 /*****************************************************************************/

   160 

   161 /** Process Pdo assignment of next sync manager.

   162  */

   163 void ec_fsm_pdo_assign_next_sync(

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

   165         )

   166 {

   167     fsm->sync_index++;

   168 

   169     for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {

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

   171         

   172         if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {

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

   174                 EC_WARN("Slave %u does not provide a configuration "

   175                         "for SM%u!\n", fsm->slave->ring_position,

   176                         fsm->sync_index);

   177                 fsm->state = ec_fsm_pdo_assign_state_error;

   178                 return;

   179             }

   180             continue;

   181         }

   182 

   183         // check if assignment has to be altered

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

   185             continue;

   186 

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

   188             EC_DBG("Pdo assignment of SM%u differs:\n", fsm->sync_index);

   189             EC_DBG("Currently assigned Pdos: ");

   190             ec_pdo_list_print(&fsm->sync->pdos);

   191             printk("\n");

   192             EC_DBG("Pdos to assign: ");

   193             ec_pdo_list_print(fsm->pdos);

   194             printk("\n");

   195         }

   196 

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

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

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

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

   201             EC_WARN("Slave %u does not support assigning Pdos!\n",

   202                     fsm->slave->ring_position);

   203             fsm->state = ec_fsm_pdo_assign_state_error;

   204             return;

   205         }

   206 

   207         fsm->num_configured_syncs++;

   208 

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

   210             fsm->state = ec_fsm_pdo_assign_state_error;

   211             return;

   212         }

   213 

   214         // set mapped Pdo count to zero

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

   216         fsm->request.data_size = 1;

   217         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);

   218         ecrt_sdo_request_write(&fsm->request);

   219 

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

   221             EC_DBG("Setting Pdo count to zero.\n");

   222 

   223         fsm->state = ec_fsm_pdo_assign_state_zero_count;

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

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

   226         return;

   227     }

   228 

   229     if (fsm->slave->master->debug_level && !fsm->num_configured_syncs)

   230         EC_DBG("Pdo assignments are already configured correctly.\n");

   231     fsm->state = ec_fsm_pdo_assign_state_end;

   232 }

   233 

   234 /*****************************************************************************/

   235 

   236 /** Assign next Pdo.

   237  */

   238 ec_pdo_t *ec_fsm_pdo_assign_next_pdo(

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

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

   241         )

   242 {

   243     list = list->next; 

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

   245         return NULL; // no next Pdo

   246     return list_entry(list, ec_pdo_t, list);

   247 }

   248 

   249 /*****************************************************************************/

   250 

   251 /** Assign a Pdo.

   252  */

   253 void ec_fsm_pdo_assign_add_pdo(

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

   255         )

   256 {

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

   258     fsm->request.data_size = 2;

   259     ec_sdo_request_address(&fsm->request,

   260             0x1C10 + fsm->sync_index, fsm->pdo_count);

   261     ecrt_sdo_request_write(&fsm->request);

   262 

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

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

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

   266     

   267     fsm->state = ec_fsm_pdo_assign_state_add_pdo;

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

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

   270 }

   271 

   272 /*****************************************************************************/

   273 

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

   275  */

   276 void ec_fsm_pdo_assign_state_zero_count(

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

   278         )

   279 {

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

   281 

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

   283         EC_WARN("Failed to clear Pdo assignment of SM%u.\n", fsm->sync_index);

   284         fsm->state = ec_fsm_pdo_assign_state_error;

   285         return;

   286     }

   287 

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

   289     

   290     // find first Pdo

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

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

   293             EC_DBG("No Pdos to assign.\n");

   294         ec_fsm_pdo_assign_next_sync(fsm);

   295         return;

   296     }

   297 

   298     // assign first Pdo

   299     fsm->pdo_count = 1;

   300     ec_fsm_pdo_assign_add_pdo(fsm);

   301 }

   302 

   303 /*****************************************************************************/

   304 

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

   306  */

   307 void ec_fsm_pdo_assign_state_add_pdo(

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

   309         )

   310 {

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

   312 

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

   314         EC_WARN("Failed to assign Pdo 0x%04X to position %u in SM%u.\n",

   315                 fsm->pdo->index, fsm->pdo_count, fsm->sync_index);

   316         fsm->state = ec_fsm_pdo_assign_state_error;

   317         return;

   318     }

   319 

   320     // find next Pdo

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

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

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

   324         fsm->request.data_size = 1;

   325         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);

   326         ecrt_sdo_request_write(&fsm->request);

   327 

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

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

   330                     fsm->pdo_count);

   331         

   332         fsm->state = ec_fsm_pdo_assign_state_pdo_count;

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

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

   335         return;

   336     }

   337 

   338     // add next Pdo to assignment

   339     fsm->pdo_count++;

   340     ec_fsm_pdo_assign_add_pdo(fsm);

   341 }

   342 

   343 /*****************************************************************************/

   344 

   345 /** Set the number of assigned Pdos.

   346  */

   347 void ec_fsm_pdo_assign_state_pdo_count(

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

   349         )

   350 {

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

   352 

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

   354         EC_WARN("Failed to set number of assigned Pdos for SM%u.\n",

   355                 fsm->sync_index);

   356         fsm->state = ec_fsm_pdo_assign_state_error;

   357         return;

   358     }

   359 

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

   361         EC_DBG("Pdo assignment successful for SM%u.\n", fsm->sync_index);

   362 

   363     // assignment for this sync manager finished

   364     ec_fsm_pdo_assign_next_sync(fsm);

   365 }

   366 

   367 /******************************************************************************

   368  * Common state functions

   369  *****************************************************************************/

   370 

   371 /** State: ERROR.

   372  */

   373 void ec_fsm_pdo_assign_state_error(

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

   375         )

   376 {

   377 }

   378 

   379 /*****************************************************************************/

   380 

   381 /** State: END.

   382  */

   383 void ec_fsm_pdo_assign_state_end(

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

   385         )

   386 {

   387 }

   388 

   389 /*****************************************************************************/