|
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 #include <stdlib.h> |
|
35 #include <sys/ioctl.h> |
|
36 #include <stdio.h> |
|
37 #include <errno.h> |
|
38 #include <string.h> |
|
39 |
|
40 #include "slave_config.h" |
|
41 #include "domain.h" |
|
42 #include "master.h" |
|
43 #include "master/ioctl.h" |
|
44 |
|
45 /*****************************************************************************/ |
|
46 |
|
47 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
|
48 ec_direction_t dir) |
|
49 { |
|
50 ec_ioctl_config_t data; |
|
51 unsigned int i; |
|
52 |
|
53 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
|
54 return -1; |
|
55 |
|
56 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
|
57 data.config_index = sc->index; |
|
58 data.syncs[sync_index].dir = dir; |
|
59 |
|
60 if (ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data) == -1) { |
|
61 fprintf(stderr, "Failed to config sync manager: %s\n", |
|
62 strerror(errno)); |
|
63 return -1; |
|
64 } |
|
65 |
|
66 return 0; |
|
67 } |
|
68 |
|
69 /*****************************************************************************/ |
|
70 |
|
71 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
|
72 uint8_t sync_index, uint16_t pdo_index) |
|
73 { |
|
74 ec_ioctl_config_pdo_t data; |
|
75 |
|
76 data.config_index = sc->index; |
|
77 data.sync_index = sync_index; |
|
78 data.index = pdo_index; |
|
79 |
|
80 if (ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data) == -1) { |
|
81 fprintf(stderr, "Failed to add Pdo: %s\n", |
|
82 strerror(errno)); |
|
83 return -1; |
|
84 } |
|
85 |
|
86 return 0; |
|
87 } |
|
88 |
|
89 /*****************************************************************************/ |
|
90 |
|
91 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
|
92 uint8_t sync_index) |
|
93 { |
|
94 ec_ioctl_config_pdo_t data; |
|
95 |
|
96 data.config_index = sc->index; |
|
97 data.sync_index = sync_index; |
|
98 |
|
99 if (ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data) == -1) { |
|
100 fprintf(stderr, "Failed to clear Pdos: %s\n", |
|
101 strerror(errno)); |
|
102 } |
|
103 } |
|
104 |
|
105 /*****************************************************************************/ |
|
106 |
|
107 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
|
108 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
|
109 uint8_t entry_bit_length) |
|
110 { |
|
111 ec_ioctl_add_pdo_entry_t data; |
|
112 |
|
113 data.config_index = sc->index; |
|
114 data.pdo_index = pdo_index; |
|
115 data.entry_index = entry_index; |
|
116 data.entry_subindex = entry_subindex; |
|
117 data.entry_bit_length = entry_bit_length; |
|
118 |
|
119 if (ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data) == -1) { |
|
120 fprintf(stderr, "Failed to add Pdo entry: %s\n", |
|
121 strerror(errno)); |
|
122 return -1; |
|
123 } |
|
124 |
|
125 return 0; |
|
126 } |
|
127 |
|
128 /*****************************************************************************/ |
|
129 |
|
130 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
|
131 uint16_t pdo_index) |
|
132 { |
|
133 ec_ioctl_config_pdo_t data; |
|
134 |
|
135 data.config_index = sc->index; |
|
136 data.index = pdo_index; |
|
137 |
|
138 if (ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data) == -1) { |
|
139 fprintf(stderr, "Failed to clear Pdo entries: %s\n", |
|
140 strerror(errno)); |
|
141 } |
|
142 } |
|
143 |
|
144 /*****************************************************************************/ |
|
145 |
|
146 int ecrt_slave_config_pdos(ec_slave_config_t *sc, |
|
147 unsigned int n_syncs, const ec_sync_info_t syncs[]) |
|
148 { |
|
149 unsigned int i, j, k; |
|
150 const ec_sync_info_t *sync_info; |
|
151 const ec_pdo_info_t *pdo_info; |
|
152 const ec_pdo_entry_info_t *entry_info; |
|
153 |
|
154 if (!syncs) |
|
155 return 0; |
|
156 |
|
157 for (i = 0; i < n_syncs; i++) { |
|
158 sync_info = &syncs[i]; |
|
159 |
|
160 if (sync_info->index == (uint8_t) EC_END) |
|
161 break; |
|
162 |
|
163 if (sync_info->index >= EC_MAX_SYNC_MANAGERS) { |
|
164 fprintf(stderr, "Invalid sync manager index %u!\n", |
|
165 sync_info->index); |
|
166 return -1; |
|
167 } |
|
168 |
|
169 if (ecrt_slave_config_sync_manager( |
|
170 sc, sync_info->index, sync_info->dir)) |
|
171 return -1; |
|
172 |
|
173 if (sync_info->n_pdos && sync_info->pdos) { |
|
174 ecrt_slave_config_pdo_assign_clear(sc, sync_info->index); |
|
175 |
|
176 for (j = 0; j < sync_info->n_pdos; j++) { |
|
177 pdo_info = &sync_info->pdos[j]; |
|
178 |
|
179 if (ecrt_slave_config_pdo_assign_add( |
|
180 sc, sync_info->index, pdo_info->index)) |
|
181 return -1; |
|
182 |
|
183 if (pdo_info->n_entries && pdo_info->entries) { |
|
184 ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index); |
|
185 |
|
186 for (k = 0; k < pdo_info->n_entries; k++) { |
|
187 entry_info = &pdo_info->entries[k]; |
|
188 |
|
189 if (ecrt_slave_config_pdo_mapping_add(sc, |
|
190 pdo_info->index, entry_info->index, |
|
191 entry_info->subindex, |
|
192 entry_info->bit_length)) |
|
193 return -1; |
|
194 } |
|
195 } |
|
196 } |
|
197 } |
|
198 } |
|
199 |
|
200 return 0; |
|
201 } |
|
202 |
|
203 /*****************************************************************************/ |
|
204 |
|
205 int ecrt_slave_config_reg_pdo_entry( |
|
206 ec_slave_config_t *sc, |
|
207 uint16_t index, |
|
208 uint8_t subindex, |
|
209 ec_domain_t *domain, |
|
210 unsigned int *bit_position |
|
211 ) |
|
212 { |
|
213 ec_ioctl_reg_pdo_entry_t data; |
|
214 int ret; |
|
215 |
|
216 data.config_index = sc->index; |
|
217 data.entry_index = index; |
|
218 data.entry_subindex = subindex; |
|
219 data.domain_index = domain->index; |
|
220 |
|
221 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data); |
|
222 if (ret == -1) { |
|
223 fprintf(stderr, "Failed to register Pdo entry: %s\n", |
|
224 strerror(errno)); |
|
225 return -2; |
|
226 } |
|
227 |
|
228 if (bit_position) { |
|
229 *bit_position = data.bit_position; |
|
230 } else { |
|
231 if (data.bit_position) { |
|
232 fprintf(stderr, "Pdo entry 0x%04X:%02X does not byte-align " |
|
233 "in config %u:%u.\n", index, subindex, |
|
234 sc->alias, sc->position); |
|
235 return -3; |
|
236 } |
|
237 } |
|
238 |
|
239 return ret; |
|
240 } |
|
241 |
|
242 /*****************************************************************************/ |
|
243 |
|
244 int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, |
|
245 uint8_t subindex, const uint8_t *sdo_data, size_t size) |
|
246 { |
|
247 ec_ioctl_sc_sdo_t data; |
|
248 |
|
249 data.config_index = sc->index; |
|
250 data.index = index; |
|
251 data.subindex = subindex; |
|
252 data.data = sdo_data; |
|
253 data.size = size; |
|
254 |
|
255 if (ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data) == -1) { |
|
256 fprintf(stderr, "Failed to configure Sdo.\n"); |
|
257 return -1; |
|
258 } |
|
259 |
|
260 return 0; |
|
261 } |
|
262 |
|
263 /*****************************************************************************/ |
|
264 |
|
265 int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index, |
|
266 uint8_t subindex, uint8_t value) |
|
267 { |
|
268 uint8_t data[1]; |
|
269 |
|
270 EC_WRITE_U8(data, value); |
|
271 return ecrt_slave_config_sdo(sc, index, subindex, data, 1); |
|
272 } |
|
273 |
|
274 /*****************************************************************************/ |
|
275 |
|
276 int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index, |
|
277 uint8_t subindex, uint16_t value) |
|
278 { |
|
279 uint8_t data[2]; |
|
280 |
|
281 EC_WRITE_U16(data, value); |
|
282 return ecrt_slave_config_sdo(sc, index, subindex, data, 2); |
|
283 } |
|
284 |
|
285 /*****************************************************************************/ |
|
286 |
|
287 int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index, |
|
288 uint8_t subindex, uint32_t value) |
|
289 { |
|
290 uint8_t data[4]; |
|
291 |
|
292 EC_WRITE_U32(data, value); |
|
293 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
|
294 } |
|
295 |
|
296 /*****************************************************************************/ |
|
297 |
|
298 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, |
|
299 uint16_t index, uint8_t subindex, size_t size) |
|
300 { |
|
301 return 0; |
|
302 } |
|
303 |
|
304 /*****************************************************************************/ |
|
305 |
|
306 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc, |
|
307 size_t size) |
|
308 { |
|
309 return 0; |
|
310 } |
|
311 |
|
312 /*****************************************************************************/ |
|
313 |
|
314 void ecrt_slave_config_state(const ec_slave_config_t *sc, |
|
315 ec_slave_config_state_t *state) |
|
316 { |
|
317 } |
|
318 |
|
319 /*****************************************************************************/ |