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 /**

    35    \file

    36    EtherCAT Pdo information state machines.

    37 */

    38 

    39 /*****************************************************************************/

    40 

    41 #include "globals.h"

    42 #include "master.h"

    43 #include "mailbox.h"

    44 #include "sdo_request.h"

    45 #include "fsm_coe_map.h"

    46 

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

    48 

    49 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *);

    50 void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *);

    51 void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *);

    52 void ec_fsm_coe_map_state_pdo_entry_count(ec_fsm_coe_map_t *);

    53 void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);

    54 

    55 void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);

    56 void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);

    57 

    58 void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *);

    59 void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);

    60 void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);

    61 

    62 /*****************************************************************************/

    63 

    64 /**

    65    Constructor.

    66 */

    67 

    68 void ec_fsm_coe_map_init(

    69         ec_fsm_coe_map_t *fsm, /**< finite state machine */

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

    71         )

    72 {

    73     fsm->fsm_coe = fsm_coe;

    74     fsm->state = NULL;

    75     ec_sdo_request_init(&fsm->request);

    76     ec_pdo_list_init(&fsm->pdos);

    77 }

    78 

    79 /*****************************************************************************/

    80 

    81 /**

    82    Destructor.

    83 */

    84 

    85 void ec_fsm_coe_map_clear(ec_fsm_coe_map_t *fsm /**< finite state machine */)

    86 {

    87     ec_sdo_request_clear(&fsm->request);

    88     ec_pdo_list_clear(&fsm->pdos);

    89 }

    90 

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

    92 

    93 /**

    94    Starts to upload an Sdo from a slave.

    95 */

    96 

    97 void ec_fsm_coe_map_start(

    98         ec_fsm_coe_map_t *fsm, /**< finite state machine */

    99         ec_slave_t *slave /**< EtherCAT slave */

   100         )

   101 {

   102     fsm->slave = slave;

   103     fsm->state = ec_fsm_coe_map_state_start;

   104 }

   105 

   106 /*****************************************************************************/

   107 

   108 /**

   109    Executes the current state of the state machine.

   110    \return false, if state machine has terminated

   111 */

   112 

   113 int ec_fsm_coe_map_exec(ec_fsm_coe_map_t *fsm /**< finite state machine */)

   114 {

   115     fsm->state(fsm);

   116 

   117     return fsm->state != ec_fsm_coe_map_state_end

   118         && fsm->state != ec_fsm_coe_map_state_error;

   119 }

   120 

   121 /*****************************************************************************/

   122 

   123 /**

   124    Returns, if the state machine terminated with success.

   125    \return non-zero if successful.

   126 */

   127 

   128 int ec_fsm_coe_map_success(ec_fsm_coe_map_t *fsm /**< Finite state machine */)

   129 {

   130     return fsm->state == ec_fsm_coe_map_state_end;

   131 }

   132 

   133 /******************************************************************************

   134  *  state functions

   135  *****************************************************************************/

   136 

   137 /** Start reading Pdo assignment.

   138  */

   139 void ec_fsm_coe_map_state_start(

   140         ec_fsm_coe_map_t *fsm /**< finite state machine */

   141         )

   142 {

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

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

   145     ec_fsm_coe_map_action_next_sync(fsm);

   146 }

   147 

   148 /*****************************************************************************/

   149 

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

   151  */

   152 void ec_fsm_coe_map_action_next_sync(

   153         ec_fsm_coe_map_t *fsm /**< Finite state machine */

   154         )

   155 {

   156     ec_slave_t *slave = fsm->slave;

   157 

   158     fsm->sync_index++;

   159 

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

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

   162             continue;

   163 

   164         fsm->sync_sdo_index = 0x1C10 + fsm->sync_index;

   165 

   166         if (slave->master->debug_level)

   167             EC_DBG("Reading Pdo assignment of SM%u of slave %u.\n",

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

   169 

   170         ec_pdo_list_clear_pdos(&fsm->pdos);

   171 

   172         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0);

   173         ecrt_sdo_request_read(&fsm->request);

   174         fsm->state = ec_fsm_coe_map_state_pdo_count;

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

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

   177         return;

   178     }

   179 

   180     if (slave->master->debug_level)

   181         EC_DBG("Reading of Pdo assignment finished for slave %u.\n",

   182                 slave->ring_position);

   183 

   184     fsm->state = ec_fsm_coe_map_state_end;

   185 }

   186 

   187 /*****************************************************************************/

   188 

   189 /**

   190  * Count assigned Pdos.

   191  */

   192 

   193 void ec_fsm_coe_map_state_pdo_count(

   194         ec_fsm_coe_map_t *fsm /**< finite state machine */

   195         )

   196 {

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

   198 

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

   200         EC_ERR("Failed to read number of assigned Pdos from slave %u.\n",

   201                 fsm->slave->ring_position);

   202         fsm->state = ec_fsm_coe_map_state_error;

   203         return;

   204     }

   205 

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

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

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

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

   210                 fsm->slave->ring_position);

   211         fsm->state = ec_fsm_coe_map_state_error;

   212         return;

   213     }

   214     fsm->sync_subindices = EC_READ_U8(fsm->request.data);

   215 

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

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

   218 

   219     // read first Pdo

   220     fsm->sync_subindex = 1;

   221     ec_fsm_coe_map_action_next_pdo(fsm);

   222 }

   223 

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

   225 

   226 /**

   227  * Read next Pdo.

   228  */

   229 

   230 void ec_fsm_coe_map_action_next_pdo(

   231         ec_fsm_coe_map_t *fsm /**< finite state machine */

   232         )

   233 {

   234     if (fsm->sync_subindex <= fsm->sync_subindices) {

   235         ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index,

   236                 fsm->sync_subindex);

   237         ecrt_sdo_request_read(&fsm->request);

   238         fsm->state = ec_fsm_coe_map_state_pdo;

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

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

   241         return;

   242     }

   243 

   244     // finished reading Pdo assignment/mapping

   245     

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

   247         fsm->state = ec_fsm_coe_map_state_error;

   248         return;

   249     }

   250 

   251     fsm->sync->assign_source = EC_ASSIGN_COE;

   252     ec_pdo_list_clear_pdos(&fsm->pdos);

   253 

   254     // next sync manager

   255     ec_fsm_coe_map_action_next_sync(fsm);

   256 }

   257 

   258 /*****************************************************************************/

   259 

   260 /**

   261  * Fetch Pdo information.

   262  */

   263 

   264 void ec_fsm_coe_map_state_pdo(

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

   266         )

   267 {

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

   269 

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

   271         EC_ERR("Failed to read assigned Pdo index from slave %u.\n",

   272                 fsm->slave->ring_position);

   273         fsm->state = ec_fsm_coe_map_state_error;

   274         return;

   275     }

   276 

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

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

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

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

   281                 fsm->slave->ring_position);

   282         fsm->state = ec_fsm_coe_map_state_error;

   283         return;

   284     }

   285 

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

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

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

   289         fsm->state = ec_fsm_coe_map_state_error;

   290         return;

   291     }

   292 

   293     ec_pdo_init(fsm->pdo);

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

   295     fsm->pdo->sync_index = fsm->sync_index;

   296 

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

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

   299 

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

   301 

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

   303     ecrt_sdo_request_read(&fsm->request);

   304     fsm->state = ec_fsm_coe_map_state_pdo_entry_count;

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

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

   307 }

   308 

   309 /*****************************************************************************/

   310 

   311 /**

   312  * Read number of mapped Pdo entries.

   313  */

   314 

   315 void ec_fsm_coe_map_state_pdo_entry_count(

   316         ec_fsm_coe_map_t *fsm /**< finite state machine */

   317         )

   318 {

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

   320 

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

   322         EC_ERR("Failed to read number of mapped Pdo entries from slave %u.\n",

   323                 fsm->slave->ring_position);

   324         fsm->state = ec_fsm_coe_map_state_error;

   325         return;

   326     }

   327 

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

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

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

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

   332                 fsm->slave->ring_position);

   333         fsm->state = ec_fsm_coe_map_state_error;

   334         return;

   335     }

   336     fsm->pdo_subindices = EC_READ_U8(fsm->request.data);

   337 

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

   339         EC_DBG("%u Pdo entries mapped.\n", fsm->pdo_subindices);

   340 

   341     // read first Pdo entry

   342     fsm->pdo_subindex = 1;

   343     ec_fsm_coe_map_action_next_pdo_entry(fsm);

   344 }

   345 

   346 /*****************************************************************************/

   347 

   348 /**

   349  * Read next Pdo entry.

   350  */

   351 

   352 void ec_fsm_coe_map_action_next_pdo_entry(

   353         ec_fsm_coe_map_t *fsm /**< finite state machine */

   354         )

   355 {

   356     if (fsm->pdo_subindex <= fsm->pdo_subindices) {

   357         ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex);

   358         ecrt_sdo_request_read(&fsm->request);

   359         fsm->state = ec_fsm_coe_map_state_pdo_entry;

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

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

   362         return;

   363     }

   364 

   365     // next Pdo

   366     fsm->sync_subindex++;

   367     ec_fsm_coe_map_action_next_pdo(fsm);

   368 }

   369 

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

   371 

   372 /**

   373  * Read Pdo entry information.

   374  */

   375 

   376 void ec_fsm_coe_map_state_pdo_entry(

   377         ec_fsm_coe_map_t *fsm /**< finite state machine */

   378         )

   379 {

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

   381 

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

   383         EC_ERR("Failed to read mapped Pdo entry from slave %u.\n",

   384                 fsm->slave->ring_position);

   385         fsm->state = ec_fsm_coe_map_state_error;

   386         return;

   387     }

   388 

   389     if (fsm->request.data_size != sizeof(uint32_t)) {

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

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

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

   393                 fsm->slave->ring_position);

   394         fsm->state = ec_fsm_coe_map_state_error;

   395     } else {

   396         uint32_t pdo_entry_info;

   397         ec_pdo_entry_t *pdo_entry;

   398 

   399         pdo_entry_info = EC_READ_U32(fsm->request.data);

   400 

   401         if (!(pdo_entry = (ec_pdo_entry_t *)

   402                     kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {

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

   404             fsm->state = ec_fsm_coe_map_state_error;

   405             return;

   406         }

   407 

   408         ec_pdo_entry_init(pdo_entry);

   409         pdo_entry->index = pdo_entry_info >> 16;

   410         pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;

   411         pdo_entry->bit_length = pdo_entry_info & 0xFF;

   412 

   413         if (!pdo_entry->index && !pdo_entry->subindex) {

   414             if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {

   415                 ec_pdo_entry_clear(pdo_entry);

   416                 kfree(pdo_entry);

   417                 fsm->state = ec_fsm_coe_map_state_error;

   418                 return;

   419             }

   420         }

   421 

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

   423             EC_DBG("Pdo entry 0x%04X:%02X, %u bit, \"%s\".\n",

   424                     pdo_entry->index, pdo_entry->subindex,

   425                     pdo_entry->bit_length,

   426                     pdo_entry->name ? pdo_entry->name : "???");

   427         }

   428 

   429         // next Pdo entry

   430         list_add_tail(&pdo_entry->list, &fsm->pdo->entries);

   431         fsm->pdo_subindex++;

   432         ec_fsm_coe_map_action_next_pdo_entry(fsm);

   433     }

   434 }

   435 

   436 /*****************************************************************************/

   437 

   438 /**

   439    State: ERROR.

   440 */

   441 

   442 void ec_fsm_coe_map_state_error(

   443         ec_fsm_coe_map_t *fsm /**< finite state machine */

   444         )

   445 {

   446 }

   447 

   448 /*****************************************************************************/

   449 

   450 /**

   451    State: END.

   452 */

   453 

   454 void ec_fsm_coe_map_state_end(

   455         ec_fsm_coe_map_t *fsm /**< finite state machine */

   456         )

   457 {

   458 }

   459 

   460 /*****************************************************************************/