|
1 /****************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * Copyright (C) 2006-2012 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 and/or |
|
10 * modify it under the terms of the GNU General Public License version 2, as |
|
11 * published by the Free Software Foundation. |
|
12 * |
|
13 * The IgH EtherCAT Master is distributed in the hope that it will be useful, |
|
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General |
|
16 * Public License for more details. |
|
17 * |
|
18 * You should have received a copy of the GNU General Public License along |
|
19 * with the IgH EtherCAT Master; if not, write to the Free Software |
|
20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|
21 * |
|
22 * --- |
|
23 * |
|
24 * The license mentioned above concerns the source code only. Using the |
|
25 * EtherCAT technology and brand is only permitted in compliance with the |
|
26 * industrial property and similar rights of Beckhoff Automation GmbH. |
|
27 * |
|
28 *****************************************************************************/ |
|
29 |
|
30 /** |
|
31 \file |
|
32 EtherCAT datagram pair methods. |
|
33 */ |
|
34 |
|
35 /*****************************************************************************/ |
|
36 |
|
37 #include <linux/slab.h> |
|
38 |
|
39 #include "master.h" |
|
40 #include "datagram_pair.h" |
|
41 |
|
42 /*****************************************************************************/ |
|
43 |
|
44 /** Datagram pair constructor. |
|
45 * |
|
46 * \return Zero on success, otherwise a negative error code. |
|
47 */ |
|
48 int ec_datagram_pair_init( |
|
49 ec_datagram_pair_t *pair, /**< Datagram pair. */ |
|
50 ec_domain_t *domain, /**< Parent domain. */ |
|
51 uint32_t logical_offset, /**< Logical offset. */ |
|
52 uint8_t *data, /**< Data pointer. */ |
|
53 size_t data_size, /**< Data size. */ |
|
54 const unsigned int used[] /**< input/output use count. */ |
|
55 ) |
|
56 { |
|
57 ec_device_index_t dev_idx; |
|
58 int ret; |
|
59 |
|
60 INIT_LIST_HEAD(&pair->list); |
|
61 pair->domain = domain; |
|
62 |
|
63 for (dev_idx = EC_DEVICE_MAIN; |
|
64 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
65 ec_datagram_init(&pair->datagrams[dev_idx]); |
|
66 snprintf(pair->datagrams[dev_idx].name, |
|
67 EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index, |
|
68 logical_offset, ec_device_names[dev_idx != 0]); |
|
69 pair->datagrams[dev_idx].device_index = dev_idx; |
|
70 } |
|
71 |
|
72 pair->expected_working_counter = 0U; |
|
73 |
|
74 for (dev_idx = EC_DEVICE_BACKUP; |
|
75 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
76 /* backup datagrams have their own memory */ |
|
77 ret = ec_datagram_prealloc(&pair->datagrams[dev_idx], data_size); |
|
78 if (ret) { |
|
79 goto out_datagrams; |
|
80 } |
|
81 } |
|
82 |
|
83 #if EC_MAX_NUM_DEVICES > 1 |
|
84 if (!(pair->send_buffer = kmalloc(data_size, GFP_KERNEL))) { |
|
85 EC_MASTER_ERR(domain->master, |
|
86 "Failed to allocate domain send buffer!\n"); |
|
87 ret = -ENOMEM; |
|
88 goto out_datagrams; |
|
89 } |
|
90 #endif |
|
91 |
|
92 /* The ec_datagram_lxx() calls below can not fail, because either the |
|
93 * datagram has external memory or it is preallocated. */ |
|
94 |
|
95 if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs |
|
96 ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN], |
|
97 logical_offset, data_size, data); |
|
98 |
|
99 for (dev_idx = EC_DEVICE_BACKUP; |
|
100 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
101 ec_datagram_lrw(&pair->datagrams[dev_idx], |
|
102 logical_offset, data_size); |
|
103 } |
|
104 |
|
105 // If LRW is used, output FMMUs increment the working counter by 2, |
|
106 // while input FMMUs increment it by 1. |
|
107 pair->expected_working_counter = |
|
108 used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT]; |
|
109 } else if (used[EC_DIR_OUTPUT]) { // outputs only |
|
110 ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN], |
|
111 logical_offset, data_size, data); |
|
112 for (dev_idx = EC_DEVICE_BACKUP; |
|
113 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
114 ec_datagram_lwr(&pair->datagrams[dev_idx], |
|
115 logical_offset, data_size); |
|
116 } |
|
117 |
|
118 pair->expected_working_counter = used[EC_DIR_OUTPUT]; |
|
119 } else { // inputs only (or nothing) |
|
120 ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN], |
|
121 logical_offset, data_size, data); |
|
122 for (dev_idx = EC_DEVICE_BACKUP; |
|
123 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
124 ec_datagram_lrd(&pair->datagrams[dev_idx], logical_offset, |
|
125 data_size); |
|
126 } |
|
127 |
|
128 pair->expected_working_counter = used[EC_DIR_INPUT]; |
|
129 } |
|
130 |
|
131 for (dev_idx = EC_DEVICE_MAIN; |
|
132 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
133 ec_datagram_zero(&pair->datagrams[dev_idx]); |
|
134 } |
|
135 |
|
136 return 0; |
|
137 |
|
138 out_datagrams: |
|
139 for (dev_idx = EC_DEVICE_MAIN; |
|
140 dev_idx < ec_master_num_devices(domain->master); dev_idx++) { |
|
141 ec_datagram_clear(&pair->datagrams[dev_idx]); |
|
142 } |
|
143 |
|
144 return ret; |
|
145 } |
|
146 |
|
147 /*****************************************************************************/ |
|
148 |
|
149 /** Datagram pair destructor. |
|
150 */ |
|
151 void ec_datagram_pair_clear( |
|
152 ec_datagram_pair_t *pair /**< Datagram pair. */ |
|
153 ) |
|
154 { |
|
155 unsigned int dev_idx; |
|
156 |
|
157 for (dev_idx = EC_DEVICE_MAIN; |
|
158 dev_idx < ec_master_num_devices(pair->domain->master); |
|
159 dev_idx++) { |
|
160 ec_datagram_clear(&pair->datagrams[dev_idx]); |
|
161 } |
|
162 |
|
163 #if EC_MAX_NUM_DEVICES > 1 |
|
164 if (pair->send_buffer) { |
|
165 kfree(pair->send_buffer); |
|
166 } |
|
167 #endif |
|
168 } |
|
169 |
|
170 /*****************************************************************************/ |
|
171 |
|
172 /** Process received data. |
|
173 * |
|
174 * \return Working counter sum over all devices. |
|
175 */ |
|
176 uint16_t ec_datagram_pair_process( |
|
177 ec_datagram_pair_t *pair, /**< Datagram pair. */ |
|
178 uint16_t wc_sum[] /**< Working counter sums. */ |
|
179 ) |
|
180 { |
|
181 unsigned int dev_idx; |
|
182 uint16_t pair_wc = 0; |
|
183 |
|
184 for (dev_idx = 0; dev_idx < ec_master_num_devices(pair->domain->master); |
|
185 dev_idx++) { |
|
186 ec_datagram_t *datagram = &pair->datagrams[dev_idx]; |
|
187 |
|
188 #ifdef EC_RT_SYSLOG |
|
189 ec_datagram_output_stats(datagram); |
|
190 #endif |
|
191 |
|
192 if (datagram->state == EC_DATAGRAM_RECEIVED) { |
|
193 pair_wc += datagram->working_counter; |
|
194 wc_sum[dev_idx] += datagram->working_counter; |
|
195 } |
|
196 } |
|
197 |
|
198 return pair_wc; |
|
199 } |
|
200 |
|
201 /*****************************************************************************/ |