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

    46 

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

    48 

    49 void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *);

    50 void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *);

    51 void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *);

    52 void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *);

    53 

    54 void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *);

    55 void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *);

    56 

    57 void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *);

    58 void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *);

    59 void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *);

    60 void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *);

    61 void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *);

    62 void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *);

    63 void ec_fsm_pdo_conf_state_entries(ec_fsm_pdo_t *);

    64 

    65 void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *);

    66 void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *);

    67 void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *);

    68 void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *);

    69 void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *);

    70 void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *);

    71 

    72 void ec_fsm_pdo_state_end(ec_fsm_pdo_t *);

    73 void ec_fsm_pdo_state_error(ec_fsm_pdo_t *);

    74 

    75 /*****************************************************************************/

    76 

    77 /** Constructor.

    78  */

    79 void ec_fsm_pdo_init(

    80         ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

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

    82         )

    83 {

    84     fsm->fsm_coe = fsm_coe;

    85     ec_fsm_pdo_entry_init(&fsm->fsm_pdo_entry, fsm_coe);

    86     ec_pdo_list_init(&fsm->pdos);

    87     ec_sdo_request_init(&fsm->request);

    88     ec_pdo_init(&fsm->slave_pdo);

    89 }

    90 

    91 /*****************************************************************************/

    92 

    93 /** Destructor.

    94  */

    95 void ec_fsm_pdo_clear(

    96         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

    97         )

    98 {

    99     ec_fsm_pdo_entry_clear(&fsm->fsm_pdo_entry);

   100     ec_pdo_list_clear(&fsm->pdos);

   101     ec_sdo_request_clear(&fsm->request);

   102     ec_pdo_clear(&fsm->slave_pdo);

   103 }

   104 

   105 /*****************************************************************************/

   106 

   107 /** Start reading the Pdo configuration.

   108  */

   109 void ec_fsm_pdo_start_reading(

   110         ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

   111         ec_slave_t *slave /**< slave to configure */

   112         )

   113 {

   114     fsm->slave = slave;

   115     fsm->state = ec_fsm_pdo_read_state_start;

   116 }

   117 

   118 /*****************************************************************************/

   119 

   120 /** Start writing the Pdo configuration.

   121  */

   122 void ec_fsm_pdo_start_configuration(

   123         ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

   124         ec_slave_t *slave /**< slave to configure */

   125         )

   126 {

   127     fsm->slave = slave;

   128     fsm->state = ec_fsm_pdo_conf_state_start;

   129 }

   130 

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

   132 

   133 /** Get running state.

   134  *

   135  * \return false, if state machine has terminated

   136  */

   137 int ec_fsm_pdo_running(

   138         const ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   139         )

   140 {

   141     return fsm->state != ec_fsm_pdo_state_end

   142         && fsm->state != ec_fsm_pdo_state_error;

   143 }

   144 

   145 /*****************************************************************************/

   146 

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

   148  *

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

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

   151  *

   152  * \return false, if state machine has terminated

   153  */

   154 int ec_fsm_pdo_exec(

   155         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   156         )

   157 {

   158     fsm->state(fsm);

   159     return ec_fsm_pdo_running(fsm);

   160 }

   161 

   162 /*****************************************************************************/

   163 

   164 /** Get execution result.

   165  *

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

   167  */

   168 int ec_fsm_pdo_success(

   169         const ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   170         )

   171 {

   172     return fsm->state == ec_fsm_pdo_state_end;

   173 }

   174 

   175 /******************************************************************************

   176  * Reading state funtions.

   177  *****************************************************************************/

   178 

   179 /** Start reading Pdo assignment.

   180  */

   181 void ec_fsm_pdo_read_state_start(

   182         ec_fsm_pdo_t *fsm /**< finite state machine */

   183         )

   184 {

   185     // read Pdo assignment for first sync manager not reserved for mailbox

   186     fsm->sync_index = 1; // next is 2

   187     ec_fsm_pdo_read_action_next_sync(fsm);

   188 }

   189 

   190 /*****************************************************************************/

   191 

   192 /** Read Pdo assignment of next sync manager.

   193  */

   194 void ec_fsm_pdo_read_action_next_sync(

   195         ec_fsm_pdo_t *fsm /**< Finite state machine */

   196         )

   197 {

   198     ec_slave_t *slave = fsm->slave;

   199 

   200     fsm->sync_index++;

   201 

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

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

   204             continue;

   205 

   206         if (slave->master->debug_level)

   207             EC_DBG("Reading Pdo assignment of SM%u.\n", fsm->sync_index);

   208 

   209         ec_pdo_list_clear_pdos(&fsm->pdos);

   210 

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

   212         ecrt_sdo_request_read(&fsm->request);

   213         fsm->state = ec_fsm_pdo_read_state_pdo_count;

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

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

   216         return;

   217     }

   218 

   219     if (slave->master->debug_level)

   220         EC_DBG("Reading of Pdo configuration finished.\n");

   221 

   222     fsm->state = ec_fsm_pdo_state_end;

   223 }

   224 

   225 /*****************************************************************************/

   226 

   227 /** Count assigned Pdos.

   228  */

   229 void ec_fsm_pdo_read_state_pdo_count(

   230         ec_fsm_pdo_t *fsm /**< finite state machine */

   231         )

   232 {

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

   234 

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

   236         EC_ERR("Failed to read number of assigned Pdos for SM%u.\n",

   237                 fsm->sync_index);

   238         fsm->state = ec_fsm_pdo_state_error;

   239         return;

   240     }

   241 

   242     if (fsm->request.data_size != sizeof(uint8_t)) {

   243         EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   244                 "from slave %u.\n", fsm->request.data_size,

   245                 fsm->request.index, fsm->request.subindex,

   246                 fsm->slave->ring_position);

   247         fsm->state = ec_fsm_pdo_state_error;

   248         return;

   249     }

   250     fsm->pdo_count = EC_READ_U8(fsm->request.data);

   251 

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

   253         EC_DBG("%u Pdos assigned.\n", fsm->pdo_count);

   254 

   255     // read first Pdo

   256     fsm->pdo_pos = 1;

   257     ec_fsm_pdo_read_action_next_pdo(fsm);

   258 }

   259 

   260 /*****************************************************************************/

   261 

   262 /** Read next Pdo.

   263  */

   264 void ec_fsm_pdo_read_action_next_pdo(

   265         ec_fsm_pdo_t *fsm /**< finite state machine */

   266         )

   267 {

   268     if (fsm->pdo_pos <= fsm->pdo_count) {

   269         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index,

   270                 fsm->pdo_pos);

   271         ecrt_sdo_request_read(&fsm->request);

   272         fsm->state = ec_fsm_pdo_read_state_pdo;

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

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

   275         return;

   276     }

   277 

   278     // finished reading Pdo configuration

   279     

   280     if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) {

   281         fsm->state = ec_fsm_pdo_state_error;

   282         return;

   283     }

   284 

   285     fsm->sync->assign_source = EC_ASSIGN_COE;

   286     ec_pdo_list_clear_pdos(&fsm->pdos);

   287 

   288     // next sync manager

   289     ec_fsm_pdo_read_action_next_sync(fsm);

   290 }

   291 

   292 /*****************************************************************************/

   293 

   294 /** Fetch Pdo information.

   295  */

   296 void ec_fsm_pdo_read_state_pdo(

   297         ec_fsm_pdo_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 index of assigned Pdo %u from SM%u.\n",

   304                 fsm->pdo_pos, fsm->sync_index);

   305         fsm->state = ec_fsm_pdo_state_error;

   306         return;

   307     }

   308 

   309     if (fsm->request.data_size != sizeof(uint16_t)) {

   310         EC_ERR("Invalid data size %u returned when uploading Sdo 0x%04X:%02X "

   311                 "from slave %u.\n", fsm->request.data_size,

   312                 fsm->request.index, fsm->request.subindex,

   313                 fsm->slave->ring_position);

   314         fsm->state = ec_fsm_pdo_state_error;

   315         return;

   316     }

   317 

   318     if (!(fsm->pdo = (ec_pdo_t *)

   319                 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   320         EC_ERR("Failed to allocate Pdo.\n");

   321         fsm->state = ec_fsm_pdo_state_error;

   322         return;

   323     }

   324 

   325     ec_pdo_init(fsm->pdo);

   326     fsm->pdo->index = EC_READ_U16(fsm->request.data);

   327     fsm->pdo->sync_index = fsm->sync_index;

   328 

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

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

   331 

   332     list_add_tail(&fsm->pdo->list, &fsm->pdos.list);

   333 

   334     fsm->state = ec_fsm_pdo_read_state_pdo_entries;

   335     ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);

   336     fsm->state(fsm); // execute immediately

   337 }

   338 

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

   340 

   341 /** Fetch Pdo information.

   342  */

   343 void ec_fsm_pdo_read_state_pdo_entries(

   344         ec_fsm_pdo_t *fsm /**< finite state machine */

   345         )

   346 {

   347     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   348         return;

   349 

   350     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {

   351         EC_ERR("Failed to read mapped Pdo entries for Pdo 0x%04X.\n",

   352                 fsm->pdo->index);

   353         fsm->state = ec_fsm_pdo_state_error;

   354         return;

   355     }

   356 

   357     // next Pdo

   358     fsm->pdo_pos++;

   359     ec_fsm_pdo_read_action_next_pdo(fsm);

   360 }

   361 

   362 /******************************************************************************

   363  * Writing state functions.

   364  *****************************************************************************/

   365 

   366 /** Start Pdo configuration.

   367  */

   368 void ec_fsm_pdo_conf_state_start(

   369         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   370         )

   371 {

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

   373         fsm->state = ec_fsm_pdo_state_end;

   374         return;

   375     }

   376 

   377     fsm->sync_index = 0xff; // next is zero

   378     ec_fsm_pdo_conf_action_next_sync(fsm);

   379 }

   380 

   381 /*****************************************************************************/

   382 

   383 /** Assign next Pdo.

   384  */

   385 ec_pdo_t *ec_fsm_pdo_conf_action_next_pdo(

   386         const ec_fsm_pdo_t *fsm, /**< Pdo configuration state machine. */

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

   388         )

   389 {

   390     list = list->next; 

   391     if (list == &fsm->pdos.list)

   392         return NULL; // no next Pdo

   393     return list_entry(list, ec_pdo_t, list);

   394 }

   395 

   396 /*****************************************************************************/

   397 

   398 /** Get the next sync manager for a pdo configuration.

   399  */

   400 void ec_fsm_pdo_conf_action_next_sync(

   401         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   402         )

   403 {

   404     fsm->sync_index++;

   405 

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

   407         if (ec_pdo_list_copy(&fsm->pdos,

   408                     &fsm->slave->config->sync_configs[fsm->sync_index].pdos)) {

   409             fsm->state = ec_fsm_pdo_state_error;

   410             return;

   411         }

   412         

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

   414             if (!list_empty(&fsm->pdos.list))

   415                 EC_WARN("Pdos configured for SM%u, but slave %u does not "

   416                         "provide a sync manager configuration!\n",

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

   418             continue;

   419         }

   420 

   421         // get first configured Pdo

   422         if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {

   423             // no pdos configured

   424             ec_fsm_pdo_conf_action_check_assignment(fsm);

   425             return;

   426         }

   427 

   428         ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   429         return;

   430     }

   431 

   432     fsm->state = ec_fsm_pdo_state_end;

   433 }

   434 

   435 /*****************************************************************************/

   436 

   437 /**

   438  */

   439 void ec_fsm_pdo_conf_action_pdo_mapping(

   440         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   441         )

   442 {

   443     const ec_pdo_t *assigned_pdo;

   444 

   445     fsm->slave_pdo.index = fsm->pdo->index;

   446 

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

   448         ec_pdo_copy_entries(&fsm->slave_pdo, assigned_pdo);

   449     } else { // configured Pdo is not assigned and thus unknown

   450         ec_pdo_clear_entries(&fsm->slave_pdo);

   451     }

   452 

   453     if (list_empty(&fsm->slave_pdo.entries)) {

   454 

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

   456             EC_DBG("Reading mapping of Pdo 0x%04X.\n",

   457                     fsm->pdo->index);

   458             

   459         // pdo mapping is unknown; start loading it

   460         ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,

   461                 &fsm->slave_pdo);

   462         fsm->state = ec_fsm_pdo_conf_state_read_mapping;

   463         fsm->state(fsm); // execute immediately

   464         return;

   465     }

   466 

   467     // pdo mapping is known, check if it most be re-configured

   468     ec_fsm_pdo_conf_action_check_mapping(fsm);

   469 }

   470 

   471 /*****************************************************************************/

   472 

   473 /**

   474  */

   475 void ec_fsm_pdo_conf_state_read_mapping(

   476         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   477         )

   478 {

   479     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   480         return;

   481 

   482     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))

   483         EC_WARN("Failed to read mapped Pdo entries for Pdo 0x%04X.\n",

   484                 fsm->pdo->index);

   485 

   486     // check if the mapping must be re-configured

   487     ec_fsm_pdo_conf_action_check_mapping(fsm);

   488 }

   489 

   490 /*****************************************************************************/

   491 

   492 /**

   493  */

   494 void ec_fsm_pdo_conf_action_check_mapping(

   495         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   496         )

   497 {

   498     if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {

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

   500             EC_DBG("Mapping of Pdo 0x%04X is already configured correctly.\n",

   501                     fsm->pdo->index);

   502         ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);

   503         return;

   504     }

   505 

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

   507         // TODO display diff

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

   509     }

   510 

   511     ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,

   512             fsm->pdo);

   513     fsm->state = ec_fsm_pdo_conf_state_mapping;

   514     fsm->state(fsm); // execure immediately

   515 }

   516 

   517 /*****************************************************************************/

   518 

   519 /**

   520  */

   521 void ec_fsm_pdo_conf_state_mapping(

   522         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   523         )

   524 {

   525     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))

   526         return;

   527 

   528     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))

   529         EC_WARN("Failed to configure mapping of Pdo 0x%04X.\n",

   530                 fsm->pdo->index);

   531 

   532     ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);

   533 }

   534 

   535 /*****************************************************************************/

   536 

   537 /**

   538  */

   539 void ec_fsm_pdo_conf_action_next_pdo_mapping(

   540         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   541         )

   542 {

   543     // get next configured Pdo

   544     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {

   545         // no more configured pdos

   546         ec_fsm_pdo_conf_action_check_assignment(fsm);

   547         return;

   548     }

   549 

   550     ec_fsm_pdo_conf_action_pdo_mapping(fsm);

   551 }

   552 

   553 /*****************************************************************************/

   554 

   555 /**

   556  */

   557 void ec_fsm_pdo_conf_action_check_assignment(

   558         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   559         )

   560 {

   561     // check if assignment has to be re-configured

   562     if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {

   563 

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

   565             EC_DBG("Pdo assignment for SM%u is already configured "

   566                     "correctly.\n", fsm->sync_index);

   567 

   568         ec_fsm_pdo_conf_action_next_sync(fsm);

   569         return;

   570     }

   571 

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

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

   574         EC_DBG("Currently assigned Pdos: ");

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

   576         printk("\n");

   577         EC_DBG("Pdos to assign: ");

   578         ec_pdo_list_print(&fsm->pdos);

   579         printk("\n");

   580     }

   581 

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

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

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

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

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

   587                 fsm->slave->ring_position);

   588         ec_fsm_pdo_conf_action_next_sync(fsm);

   589         return;

   590     }

   591 

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

   593         fsm->state = ec_fsm_pdo_state_error;

   594         return;

   595     }

   596 

   597     // set mapped Pdo count to zero

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

   599     fsm->request.data_size = 1;

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

   601     ecrt_sdo_request_write(&fsm->request);

   602 

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

   604         EC_DBG("Setting number of assigned Pdos to zero.\n");

   605 

   606     fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;

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

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

   609 }

   610 

   611 /*****************************************************************************/

   612 

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

   614  */

   615 void ec_fsm_pdo_conf_state_zero_pdo_count(

   616         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   617         )

   618 {

   619     if (ec_fsm_coe_exec(fsm->fsm_coe))

   620         return;

   621 

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

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

   624         fsm->state = ec_fsm_pdo_state_error;

   625         return;

   626     }

   627 

   628     // the sync manager's assigned Pdos have been cleared

   629     ec_pdo_list_clear_pdos(&fsm->sync->pdos);

   630 

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

   632     

   633     // find first Pdo

   634     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {

   635 

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

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

   638 

   639         // check for mapping to be altered

   640         ec_fsm_pdo_conf_action_next_sync(fsm);

   641         return;

   642     }

   643 

   644     // assign first Pdo

   645     fsm->pdo_pos = 1;

   646 	ec_fsm_pdo_conf_action_assign_pdo(fsm);

   647 }

   648 

   649 /*****************************************************************************/

   650 

   651 /** Assign a Pdo.

   652  */

   653 void ec_fsm_pdo_conf_action_assign_pdo(

   654         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   655         )

   656 {

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

   658     fsm->request.data_size = 2;

   659     ec_sdo_request_address(&fsm->request,

   660             0x1C10 + fsm->sync_index, fsm->pdo_pos);

   661     ecrt_sdo_request_write(&fsm->request);

   662 

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

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

   665                 fsm->pdo->index, fsm->pdo_pos);

   666     

   667     fsm->state = ec_fsm_pdo_conf_state_assign_pdo;

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

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

   670 }

   671 

   672 /*****************************************************************************/

   673 

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

   675  */

   676 void ec_fsm_pdo_conf_state_assign_pdo(

   677         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   678         )

   679 {

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

   681 

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

   683         EC_WARN("Failed to assign Pdo 0x%04X at position %u of SM%u.\n",

   684                 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index);

   685         fsm->state = ec_fsm_pdo_state_error;

   686         return;

   687     }

   688 

   689     // find next Pdo

   690     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) {

   691 

   692         // no more Pdos to assign, set Pdo count

   693         EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);

   694         fsm->request.data_size = 1;

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

   696         ecrt_sdo_request_write(&fsm->request);

   697 

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

   699             EC_DBG("Setting number of assigned Pdos to %u.\n", fsm->pdo_pos);

   700         

   701         fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;

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

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

   704         return;

   705     }

   706 

   707     // add next Pdo to assignment

   708     fsm->pdo_pos++;

   709     ec_fsm_pdo_conf_action_assign_pdo(fsm);

   710 }

   711     

   712 /*****************************************************************************/

   713 

   714 /** Set the number of assigned Pdos.

   715  */

   716 void ec_fsm_pdo_conf_state_set_pdo_count(

   717         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   718         )

   719 {

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

   721 

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

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

   724                 fsm->sync_index);

   725         fsm->state = ec_fsm_pdo_state_error;

   726         return;

   727     }

   728 

   729     // Pdos have been configured

   730     ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);

   731 

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

   733         EC_DBG("Successfully configured Pdo assignment of SM%u.\n",

   734                 fsm->sync_index);

   735 

   736     // check if Pdo mapping has to be altered

   737     ec_fsm_pdo_conf_action_next_sync(fsm);

   738 }

   739 

   740 /******************************************************************************

   741  * Common state functions

   742  *****************************************************************************/

   743 

   744 /** State: ERROR.

   745  */

   746 void ec_fsm_pdo_state_error(

   747         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   748         )

   749 {

   750 }

   751 

   752 /*****************************************************************************/

   753 

   754 /** State: END.

   755  */

   756 void ec_fsm_pdo_state_end(

   757         ec_fsm_pdo_t *fsm /**< Pdo configuration state machine. */

   758         )

   759 {

   760 }

   761 

   762 /*****************************************************************************/