Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master is free software; you can redistribute it
* and/or modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* The IgH EtherCAT Master is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* The right to use EtherCAT Technology is granted and comes free of
* charge under condition of compatibility of product made by
* Licensee. People intending to distribute/sell products based on the
* code, have to sign an agreement to guarantee that products using
* software based on IgH EtherCAT master stay compatible with the actual
* EtherCAT specification (which are released themselves as an open
* standard) as the (only) precondition to have the right to use EtherCAT
* Technology, IP and trade marks.
*
*****************************************************************************/
/**
\file
EtherCAT Pdo mapping methods.
*/
/*****************************************************************************/
#include <linux/module.h>
#include "globals.h"
#include "pdo.h"
#include "slave_config.h"
#include "master.h"
#include "pdo_mapping.h"
/*****************************************************************************/
/** Pdo mapping constructor.
*/
void ec_pdo_mapping_init(
ec_pdo_mapping_t *pm /**< Pdo mapping. */
)
{
INIT_LIST_HEAD(&pm->pdos);
pm->default_mapping = 1;
}
/*****************************************************************************/
/** Pdo mapping destructor.
*/
void ec_pdo_mapping_clear(ec_pdo_mapping_t *pm /**< Pdo mapping. */)
{
ec_pdo_mapping_clear_pdos(pm);
}
/*****************************************************************************/
/** Clears the list of mapped Pdos.
*/
void ec_pdo_mapping_clear_pdos(ec_pdo_mapping_t *pm /**< Pdo mapping. */)
{
ec_pdo_t *pdo, *next;
list_for_each_entry_safe(pdo, next, &pm->pdos, list) {
list_del_init(&pdo->list);
ec_pdo_clear(pdo);
kfree(pdo);
}
}
/*****************************************************************************/
/** Calculates the total size of the mapped Pdo entries.
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
*
* \retval Data size in byte.
*/
uint16_t ec_pdo_mapping_total_size(
const ec_pdo_mapping_t *pm /**< Pdo mapping. */
)
{
unsigned int bit_size;
const ec_pdo_t *pdo;
const ec_pdo_entry_t *pdo_entry;
uint16_t byte_size;
bit_size = 0;
list_for_each_entry(pdo, &pm->pdos, list) {
list_for_each_entry(pdo_entry, &pdo->entries, list) {
bit_size += pdo_entry->bit_length;
}
}
if (bit_size % 8) // round up to full bytes
byte_size = bit_size / 8 + 1;
else
byte_size = bit_size / 8;
return byte_size;
}
/*****************************************************************************/
/** Add a new Pdo to the mapping.
*
* \retval >0 Pointer to new Pdo.
* \retval NULL No memory.
*/
ec_pdo_t *ec_pdo_mapping_add_pdo(
ec_pdo_mapping_t *pm, /**< Pdo mapping. */
ec_direction_t dir, /**< Direction. */
uint16_t index /**< Pdo index. */
)
{
ec_pdo_t *pdo;
if (!(pdo = (ec_pdo_t *) kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate memory for Pdo.\n");
return NULL;
}
ec_pdo_init(pdo);
pdo->dir = dir;
pdo->index = index;
list_add_tail(&pdo->list, &pm->pdos);
return pdo;
}
/*****************************************************************************/
/** Add the copy of an existing Pdo to the mapping.
*
* \return 0 on success, else < 0
*/
int ec_pdo_mapping_add_pdo_copy(
ec_pdo_mapping_t *pm, /**< Pdo mapping. */
)
{
ec_pdo_t *mapped_pdo;
list_for_each_entry(mapped_pdo, &pm->pdos, list) {
if (mapped_pdo->index != pdo->index) continue;
EC_ERR("Pdo 0x%04X is already mapped!\n", pdo->index);
return -1;
}
if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
EC_ERR("Failed to allocate Pdo memory.\n");
return -1;
}
if (ec_pdo_init_copy(mapped_pdo, pdo)) {
kfree(mapped_pdo);
return -1;
}
list_add_tail(&mapped_pdo->list, &pm->pdos);
return 0;
}
/*****************************************************************************/
/** Makes a deep copy of another Pdo mapping.
*
* \return 0 on success, else < 0
*/
int ec_pdo_mapping_copy(
ec_pdo_mapping_t *pm, /**< Pdo mapping. */
const ec_pdo_mapping_t *other /**< Pdo mapping to copy from. */
)
{
ec_pdo_t *other_pdo;
ec_pdo_mapping_clear_pdos(pm);
list_for_each_entry(other_pdo, &other->pdos, list) {
if (ec_pdo_mapping_add_pdo_copy(pm, other_pdo))
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
return -1;
}
return 0;
}
/*****************************************************************************/
/** Compares two Pdo mappings.
*
* Only the mapping is compared, not the Pdo entries (i. e. the Pdo
* configuration).
*
* \retval 1 The given Pdo mappings are equal.
* \retval 0 The given Pdo mappings differ.
*/
int ec_pdo_mapping_equal(
const ec_pdo_mapping_t *pm1, /**< First mapping. */
const ec_pdo_mapping_t *pm2 /**< Second mapping. */
)
{
const struct list_head *h1, *h2, *l1, *l2;
const ec_pdo_t *p1, *p2;
h1 = l1 = &pm1->pdos;
h2 = l2 = &pm2->pdos;
while (1) {
l1 = l1->next;
l2 = l2->next;
if ((l1 == h1) ^ (l2 == h2)) // unequal lengths
return 0;
break;
p1 = list_entry(l1, ec_pdo_t, list);
p2 = list_entry(l2, ec_pdo_t, list);
if (p1->index != p2->index)
return 0;
}
return 1;
}
/*****************************************************************************/
/** Finds a Pdo with the given index.
*/
ec_pdo_t *ec_pdo_mapping_find_pdo(
const ec_pdo_mapping_t *pm, /**< Pdo mapping. */
uint16_t index /**< Pdo index. */
)
{
ec_pdo_t *pdo;
list_for_each_entry(pdo, &pm->pdos, list) {
if (pdo->index != index)
continue;
return pdo;
}
return NULL;
}
/*****************************************************************************/
/** Finds a Pdo with the given index and returns a const pointer.
const ec_pdo_t *ec_pdo_mapping_find_pdo_const(
const ec_pdo_mapping_t *pm, /**< Pdo mapping. */
uint16_t index /**< Pdo index. */
)
{
const ec_pdo_t *pdo;