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 list methods.

    37 */

    38 

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

    40 

    41 #include <linux/module.h>

    42 

    43 #include "globals.h"

    44 #include "pdo.h"

    45 #include "slave_config.h"

    46 #include "master.h"

    47 

    48 #include "pdo_list.h"

    49 

    50 /*****************************************************************************/

    51 

    52 /** Pdo list constructor.

    53  */

    54 void ec_pdo_list_init(

    55         ec_pdo_list_t *pl /**< Pdo list. */

    56         )

    57 {

    58     INIT_LIST_HEAD(&pl->list);

    59 }

    60 

    61 /*****************************************************************************/

    62 

    63 /** Pdo list destructor.

    64  */

    65 void ec_pdo_list_clear(ec_pdo_list_t *pl /**< Pdo list. */)

    66 {

    67     ec_pdo_list_clear_pdos(pl);

    68 }

    69 

    70 /*****************************************************************************/

    71 

    72 /** Clears the list of mapped Pdos.

    73  */

    74 void ec_pdo_list_clear_pdos(ec_pdo_list_t *pl /**< Pdo list. */)

    75 {

    76     ec_pdo_t *pdo, *next;

    77 

    78     list_for_each_entry_safe(pdo, next, &pl->list, list) {

    79         list_del_init(&pdo->list);

    80         ec_pdo_clear(pdo);

    81         kfree(pdo);

    82     }

    83 }

    84 

    85 /*****************************************************************************/

    86 

    87 /** Calculates the total size of the mapped Pdo entries.

    88  *

    89  * \retval Data size in byte.

    90  */

    91 uint16_t ec_pdo_list_total_size(

    92         const ec_pdo_list_t *pl /**< Pdo list. */

    93         )

    94 {

    95     unsigned int bit_size;

    96     const ec_pdo_t *pdo;

    97     const ec_pdo_entry_t *pdo_entry;

    98     uint16_t byte_size;

    99 

   100     bit_size = 0;

   101     list_for_each_entry(pdo, &pl->list, list) {

   102         list_for_each_entry(pdo_entry, &pdo->entries, list) {

   103             bit_size += pdo_entry->bit_length;

   104         }

   105     }

   106 

   107     if (bit_size % 8) // round up to full bytes

   108         byte_size = bit_size / 8 + 1;

   109     else

   110         byte_size = bit_size / 8;

   111 

   112     return byte_size;

   113 }

   114 

   115 /*****************************************************************************/

   116 

   117 /** Add a new Pdo to the list.

   118  *

   119  * \retval >0 Pointer to new Pdo.

   120  * \retval NULL No memory.

   121  */

   122 ec_pdo_t *ec_pdo_list_add_pdo(

   123         ec_pdo_list_t *pl, /**< Pdo list. */

   124         ec_direction_t dir, /**< Direction. */

   125         uint16_t index /**< Pdo index. */

   126         )

   127 {

   128     ec_pdo_t *pdo;

   129 

   130     if (!(pdo = (ec_pdo_t *) kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   131         EC_ERR("Failed to allocate memory for Pdo.\n");

   132         return NULL;

   133     }

   134 

   135     ec_pdo_init(pdo);

   136     pdo->dir = dir;

   137     pdo->index = index;

   138     list_add_tail(&pdo->list, &pl->list);

   139     return pdo;

   140 }

   141 

   142 /*****************************************************************************/

   143 

   144 /** Add the copy of an existing Pdo to the list.

   145  *

   146  * \return 0 on success, else < 0

   147  */

   148 int ec_pdo_list_add_pdo_copy(

   149         ec_pdo_list_t *pl, /**< Pdo list. */

   150         const ec_pdo_t *pdo /**< Pdo to add. */

   151         )

   152 {

   153     ec_pdo_t *mapped_pdo;

   154 

   155     // Pdo already mapped?

   156     list_for_each_entry(mapped_pdo, &pl->list, list) {

   157         if (mapped_pdo->index != pdo->index) continue;

   158         EC_ERR("Pdo 0x%04X is already mapped!\n", pdo->index);

   159         return -1;

   160     }

   161     

   162     if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

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

   164         return -1;

   165     }

   166 

   167     if (ec_pdo_init_copy(mapped_pdo, pdo)) {

   168         kfree(mapped_pdo);

   169         return -1;

   170     }

   171 

   172     list_add_tail(&mapped_pdo->list, &pl->list);

   173     return 0;

   174 }

   175 

   176 /*****************************************************************************/

   177 

   178 /** Makes a deep copy of another Pdo list.

   179  *

   180  * \return 0 on success, else < 0

   181  */

   182 int ec_pdo_list_copy(

   183         ec_pdo_list_t *pl, /**< Pdo list. */

   184         const ec_pdo_list_t *other /**< Pdo list to copy from. */

   185         )

   186 {

   187     ec_pdo_t *other_pdo;

   188 

   189     ec_pdo_list_clear_pdos(pl);

   190 

   191     // Pdo already mapped?

   192     list_for_each_entry(other_pdo, &other->list, list) {

   193         if (ec_pdo_list_add_pdo_copy(pl, other_pdo))

   194             return -1;

   195     }

   196     

   197     return 0;

   198 }

   199 

   200 /*****************************************************************************/

   201 

   202 /** Compares two Pdo lists.

   203  *

   204  * Only the list is compared, not the Pdo entries (i. e. the Pdo

   205  * configuration).

   206  *

   207  * \retval 1 The given Pdo lists are equal.

   208  * \retval 0 The given Pdo lists differ.

   209  */

   210 int ec_pdo_list_equal(

   211         const ec_pdo_list_t *pl1, /**< First list. */

   212         const ec_pdo_list_t *pl2 /**< Second list. */

   213         )

   214 {

   215     const struct list_head *h1, *h2, *l1, *l2;

   216     const ec_pdo_t *p1, *p2;

   217 

   218     h1 = l1 = &pl1->list;

   219     h2 = l2 = &pl2->list;

   220 

   221     while (1) {

   222         l1 = l1->next;

   223         l2 = l2->next;

   224 

   225         if ((l1 == h1) ^ (l2 == h2)) // unequal lengths

   226             return 0;

   227         if (l1 == h1) // both finished

   228             break;

   229 

   230         p1 = list_entry(l1, ec_pdo_t, list);

   231         p2 = list_entry(l2, ec_pdo_t, list);

   232 

   233         if (p1->index != p2->index)

   234             return 0;

   235     }

   236 

   237     return 1;

   238 }

   239 

   240 /*****************************************************************************/

   241 

   242 /** Finds a Pdo with the given index.

   243  */

   244 ec_pdo_t *ec_pdo_list_find_pdo(

   245         const ec_pdo_list_t *pl, /**< Pdo list. */

   246         uint16_t index /**< Pdo index. */

   247         )

   248 {

   249     ec_pdo_t *pdo;

   250 

   251     list_for_each_entry(pdo, &pl->list, list) {

   252         if (pdo->index != index)

   253             continue;

   254         return pdo;

   255     }

   256 

   257     return NULL;

   258 }

   259 

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

   261 

   262 /** Finds a Pdo with the given index and returns a const pointer.

   263  */

   264 const ec_pdo_t *ec_pdo_list_find_pdo_const(

   265         const ec_pdo_list_t *pl, /**< Pdo list. */

   266         uint16_t index /**< Pdo index. */

   267         )

   268 {

   269     const ec_pdo_t *pdo;

   270 

   271     list_for_each_entry(pdo, &pl->list, list) {

   272         if (pdo->index != index)

   273             continue;

   274         return pdo;

   275     }

   276 

   277     return NULL;

   278 }

   279 

   280 /*****************************************************************************/