17 * GNU Lesser General Public License for more details. |
17 * GNU Lesser General Public License for more details. |
18 * |
18 * |
19 * You should have received a copy of the GNU Lesser General Public License |
19 * You should have received a copy of the GNU Lesser General Public License |
20 * along with the IgH EtherCAT master userspace library. If not, see |
20 * along with the IgH EtherCAT master userspace library. If not, see |
21 * <http://www.gnu.org/licenses/>. |
21 * <http://www.gnu.org/licenses/>. |
22 * |
22 * |
23 * --- |
23 * --- |
24 * |
24 * |
25 * The license mentioned above concerns the source code only. Using the |
25 * The license mentioned above concerns the source code only. Using the |
26 * EtherCAT technology and brand is only permitted in compliance with the |
26 * EtherCAT technology and brand is only permitted in compliance with the |
27 * industrial property and similar rights of Beckhoff Automation GmbH. |
27 * industrial property and similar rights of Beckhoff Automation GmbH. |
28 * |
28 * |
29 *****************************************************************************/ |
29 *****************************************************************************/ |
30 |
30 |
31 #include <stdlib.h> |
31 #include <stdlib.h> |
32 #include <sys/ioctl.h> |
|
33 #include <stdio.h> |
32 #include <stdio.h> |
34 #include <errno.h> |
|
35 #include <string.h> |
33 #include <string.h> |
36 |
34 #include <errno.h> /* ENOENT */ |
|
35 |
|
36 #include "ioctl.h" |
37 #include "slave_config.h" |
37 #include "slave_config.h" |
38 #include "domain.h" |
38 #include "domain.h" |
39 #include "sdo_request.h" |
39 #include "sdo_request.h" |
|
40 #include "reg_request.h" |
40 #include "voe_handler.h" |
41 #include "voe_handler.h" |
41 #include "master.h" |
42 #include "master.h" |
42 #include "master/ioctl.h" |
|
43 |
43 |
44 /*****************************************************************************/ |
44 /*****************************************************************************/ |
45 |
45 |
46 void ec_slave_config_clear(ec_slave_config_t *sc) |
46 void ec_slave_config_clear(ec_slave_config_t *sc) |
47 { |
47 { |
48 ec_sdo_request_t *r, *next_r; |
48 ec_sdo_request_t *r, *next_r; |
|
49 ec_reg_request_t *e, *next_e; |
49 ec_voe_handler_t *v, *next_v; |
50 ec_voe_handler_t *v, *next_v; |
50 |
51 |
51 r = sc->first_sdo_request; |
52 r = sc->first_sdo_request; |
52 while (r) { |
53 while (r) { |
53 next_r = r->next; |
54 next_r = r->next; |
54 ec_sdo_request_clear(r); |
55 ec_sdo_request_clear(r); |
55 r = next_r; |
56 r = next_r; |
56 } |
57 } |
57 |
58 |
|
59 e = sc->first_reg_request; |
|
60 while (e) { |
|
61 next_e = e->next; |
|
62 ec_reg_request_clear(e); |
|
63 e = next_e; |
|
64 } |
58 |
65 |
59 v = sc->first_voe_handler; |
66 v = sc->first_voe_handler; |
60 while (v) { |
67 while (v) { |
61 next_v = v->next; |
68 next_v = v->next; |
62 ec_voe_handler_clear(v); |
69 ec_voe_handler_clear(v); |
68 |
75 |
69 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
76 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
70 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
77 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
71 { |
78 { |
72 ec_ioctl_config_t data; |
79 ec_ioctl_config_t data; |
73 unsigned int i; |
80 int ret; |
74 |
81 |
75 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
82 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
76 return -ENOENT; |
83 return -ENOENT; |
77 |
84 |
78 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
85 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
79 data.config_index = sc->index; |
86 data.config_index = sc->index; |
80 data.syncs[sync_index].dir = dir; |
87 data.syncs[sync_index].dir = dir; |
81 data.syncs[sync_index].watchdog_mode = watchdog_mode; |
88 data.syncs[sync_index].watchdog_mode = watchdog_mode; |
82 data.syncs[sync_index].config_this = 1; |
89 data.syncs[sync_index].config_this = 1; |
83 |
90 |
84 if (ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data) == -1) { |
91 ret = ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data); |
|
92 if (EC_IOCTL_IS_ERROR(ret)) { |
85 fprintf(stderr, "Failed to config sync manager: %s\n", |
93 fprintf(stderr, "Failed to config sync manager: %s\n", |
86 strerror(errno)); |
94 strerror(EC_IOCTL_ERRNO(ret))); |
87 return -1; // FIXME |
95 return -EC_IOCTL_ERRNO(ret); |
88 } |
96 } |
89 |
97 |
90 return 0; |
98 return 0; |
91 } |
99 } |
92 |
100 |
93 /*****************************************************************************/ |
101 /*****************************************************************************/ |
94 |
102 |
95 void ecrt_slave_config_watchdog(ec_slave_config_t *sc, |
103 void ecrt_slave_config_watchdog(ec_slave_config_t *sc, |
96 uint16_t divider, uint16_t intervals) |
104 uint16_t divider, uint16_t intervals) |
97 { |
105 { |
98 ec_ioctl_config_t data; |
106 ec_ioctl_config_t data; |
|
107 int ret; |
99 |
108 |
100 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
109 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
101 data.config_index = sc->index; |
110 data.config_index = sc->index; |
102 data.watchdog_divider = divider; |
111 data.watchdog_divider = divider; |
103 data.watchdog_intervals = intervals; |
112 data.watchdog_intervals = intervals; |
104 |
113 |
105 if (ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data) == -1) { |
114 ret = ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data); |
|
115 if (EC_IOCTL_IS_ERROR(ret)) { |
106 fprintf(stderr, "Failed to config watchdog: %s\n", |
116 fprintf(stderr, "Failed to config watchdog: %s\n", |
107 strerror(errno)); |
117 strerror(EC_IOCTL_ERRNO(ret))); |
108 } |
|
109 } |
|
110 |
|
111 /*****************************************************************************/ |
|
112 |
|
113 void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc, |
|
114 uint8_t allow_overlapping_pdos) |
|
115 { |
|
116 ec_ioctl_config_t data; |
|
117 |
|
118 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
|
119 data.config_index = sc->index; |
|
120 data.allow_overlapping_pdos = allow_overlapping_pdos; |
|
121 |
|
122 if (ioctl(sc->master->fd, EC_IOCTL_SC_OVERLAPPING_IO, &data) == -1) { |
|
123 fprintf(stderr, "Failed to config overlapping PDOs: %s\n", |
|
124 strerror(errno)); |
|
125 } |
118 } |
126 } |
119 } |
127 |
120 |
128 /*****************************************************************************/ |
121 /*****************************************************************************/ |
129 |
122 |
130 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
123 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
131 uint8_t sync_index, uint16_t pdo_index) |
124 uint8_t sync_index, uint16_t pdo_index) |
132 { |
125 { |
133 ec_ioctl_config_pdo_t data; |
126 ec_ioctl_config_pdo_t data; |
|
127 int ret; |
134 |
128 |
135 data.config_index = sc->index; |
129 data.config_index = sc->index; |
136 data.sync_index = sync_index; |
130 data.sync_index = sync_index; |
137 data.index = pdo_index; |
131 data.index = pdo_index; |
138 |
132 |
139 if (ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data) == -1) { |
133 ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data); |
|
134 if (EC_IOCTL_IS_ERROR(ret)) { |
140 fprintf(stderr, "Failed to add PDO: %s\n", |
135 fprintf(stderr, "Failed to add PDO: %s\n", |
141 strerror(errno)); |
136 strerror(EC_IOCTL_ERRNO(ret))); |
142 return -1; // FIXME |
137 return -EC_IOCTL_ERRNO(ret); |
143 } |
138 } |
144 |
139 |
145 return 0; |
140 return 0; |
146 } |
141 } |
147 |
142 |
148 /*****************************************************************************/ |
143 /*****************************************************************************/ |
149 |
144 |
150 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
145 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
151 uint8_t sync_index) |
146 uint8_t sync_index) |
152 { |
147 { |
153 ec_ioctl_config_pdo_t data; |
148 ec_ioctl_config_pdo_t data; |
|
149 int ret; |
154 |
150 |
155 data.config_index = sc->index; |
151 data.config_index = sc->index; |
156 data.sync_index = sync_index; |
152 data.sync_index = sync_index; |
157 |
153 |
158 if (ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data) == -1) { |
154 ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data); |
|
155 if (EC_IOCTL_IS_ERROR(ret)) { |
159 fprintf(stderr, "Failed to clear PDOs: %s\n", |
156 fprintf(stderr, "Failed to clear PDOs: %s\n", |
160 strerror(errno)); |
157 strerror(EC_IOCTL_ERRNO(ret))); |
161 } |
158 } |
162 } |
159 } |
163 |
160 |
164 /*****************************************************************************/ |
161 /*****************************************************************************/ |
165 |
162 |
166 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
163 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
167 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
164 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
168 uint8_t entry_bit_length) |
165 uint8_t entry_bit_length) |
169 { |
166 { |
170 ec_ioctl_add_pdo_entry_t data; |
167 ec_ioctl_add_pdo_entry_t data; |
|
168 int ret; |
171 |
169 |
172 data.config_index = sc->index; |
170 data.config_index = sc->index; |
173 data.pdo_index = pdo_index; |
171 data.pdo_index = pdo_index; |
174 data.entry_index = entry_index; |
172 data.entry_index = entry_index; |
175 data.entry_subindex = entry_subindex; |
173 data.entry_subindex = entry_subindex; |
176 data.entry_bit_length = entry_bit_length; |
174 data.entry_bit_length = entry_bit_length; |
177 |
175 |
178 if (ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data) == -1) { |
176 ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data); |
|
177 if (EC_IOCTL_IS_ERROR(ret)) { |
179 fprintf(stderr, "Failed to add PDO entry: %s\n", |
178 fprintf(stderr, "Failed to add PDO entry: %s\n", |
180 strerror(errno)); |
179 strerror(EC_IOCTL_ERRNO(ret))); |
181 return -1; // FIXME |
180 return -EC_IOCTL_ERRNO(ret); |
182 } |
181 } |
183 |
182 |
184 return 0; |
183 return 0; |
185 } |
184 } |
186 |
185 |
187 /*****************************************************************************/ |
186 /*****************************************************************************/ |
188 |
187 |
189 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
188 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
190 uint16_t pdo_index) |
189 uint16_t pdo_index) |
191 { |
190 { |
192 ec_ioctl_config_pdo_t data; |
191 ec_ioctl_config_pdo_t data; |
|
192 int ret; |
193 |
193 |
194 data.config_index = sc->index; |
194 data.config_index = sc->index; |
195 data.index = pdo_index; |
195 data.index = pdo_index; |
196 |
196 |
197 if (ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data) == -1) { |
197 ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data); |
|
198 if (EC_IOCTL_IS_ERROR(ret)) { |
198 fprintf(stderr, "Failed to clear PDO entries: %s\n", |
199 fprintf(stderr, "Failed to clear PDO entries: %s\n", |
199 strerror(errno)); |
200 strerror(EC_IOCTL_ERRNO(ret))); |
200 } |
201 } |
201 } |
202 } |
202 |
203 |
203 /*****************************************************************************/ |
204 /*****************************************************************************/ |
204 |
205 |
280 data.entry_index = index; |
281 data.entry_index = index; |
281 data.entry_subindex = subindex; |
282 data.entry_subindex = subindex; |
282 data.domain_index = domain->index; |
283 data.domain_index = domain->index; |
283 |
284 |
284 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data); |
285 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data); |
285 if (ret == -1) { |
286 if (EC_IOCTL_IS_ERROR(ret)) { |
286 fprintf(stderr, "Failed to register PDO entry: %s\n", |
287 fprintf(stderr, "Failed to register PDO entry: %s\n", |
287 strerror(errno)); |
288 strerror(EC_IOCTL_ERRNO(ret))); |
288 return -2; // FIXME |
289 return -EC_IOCTL_ERRNO(ret); |
289 } |
290 } |
290 |
291 |
291 if (bit_position) { |
292 if (bit_position) { |
292 *bit_position = data.bit_position; |
293 *bit_position = data.bit_position; |
293 } else { |
294 } else { |
294 if (data.bit_position) { |
295 if (data.bit_position) { |
295 fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align " |
296 fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align " |
296 "in config %u:%u.\n", index, subindex, |
297 "in config %u:%u.\n", index, subindex, |
297 sc->alias, sc->position); |
298 sc->alias, sc->position); |
298 return -3; // FIXME |
299 return -EFAULT; |
299 } |
300 } |
300 } |
301 } |
301 |
302 |
302 return ret; |
303 return ret; |
303 } |
304 } |
304 |
305 |
305 /*****************************************************************************/ |
306 /*****************************************************************************/ |
306 |
307 |
|
308 int ecrt_slave_config_reg_pdo_entry_pos( |
|
309 ec_slave_config_t *sc, |
|
310 uint8_t sync_index, |
|
311 unsigned int pdo_pos, |
|
312 unsigned int entry_pos, |
|
313 ec_domain_t *domain, |
|
314 unsigned int *bit_position |
|
315 ) |
|
316 { |
|
317 ec_ioctl_reg_pdo_pos_t io; |
|
318 int ret; |
|
319 |
|
320 io.config_index = sc->index; |
|
321 io.sync_index = sync_index; |
|
322 io.pdo_pos = pdo_pos; |
|
323 io.entry_pos = entry_pos; |
|
324 io.domain_index = domain->index; |
|
325 |
|
326 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_POS, &io); |
|
327 if (EC_IOCTL_IS_ERROR(ret)) { |
|
328 fprintf(stderr, "Failed to register PDO entry: %s\n", |
|
329 strerror(EC_IOCTL_ERRNO(ret))); |
|
330 return -EC_IOCTL_ERRNO(ret); |
|
331 } |
|
332 |
|
333 if (bit_position) { |
|
334 *bit_position = io.bit_position; |
|
335 } else { |
|
336 if (io.bit_position) { |
|
337 fprintf(stderr, "PDO entry %u/%u/%u does not byte-align " |
|
338 "in config %u:%u.\n", sync_index, pdo_pos, entry_pos, |
|
339 sc->alias, sc->position); |
|
340 return -EFAULT; |
|
341 } |
|
342 } |
|
343 |
|
344 return ret; |
|
345 } |
|
346 |
|
347 /*****************************************************************************/ |
|
348 |
307 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate, |
349 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate, |
308 uint32_t sync0_cycle_time, uint32_t sync0_shift_time, |
350 uint32_t sync0_cycle_time, int32_t sync0_shift_time, |
309 uint32_t sync1_cycle_time, uint32_t sync1_shift_time) |
351 uint32_t sync1_cycle_time, int32_t sync1_shift_time) |
310 { |
352 { |
311 ec_ioctl_config_t data; |
353 ec_ioctl_config_t data; |
|
354 int ret; |
312 |
355 |
313 data.config_index = sc->index; |
356 data.config_index = sc->index; |
314 data.dc_assign_activate = assign_activate; |
357 data.dc_assign_activate = assign_activate; |
315 data.dc_sync[0].cycle_time = sync0_cycle_time; |
358 data.dc_sync[0].cycle_time = sync0_cycle_time; |
316 data.dc_sync[0].shift_time = sync0_shift_time; |
359 data.dc_sync[0].shift_time = sync0_shift_time; |
317 data.dc_sync[1].cycle_time = sync1_cycle_time; |
360 data.dc_sync[1].cycle_time = sync1_cycle_time; |
318 data.dc_sync[1].shift_time = sync1_shift_time; |
361 data.dc_sync[1].shift_time = sync1_shift_time; |
319 |
362 |
320 if (ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data) == -1) { |
363 ret = ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data); |
321 fprintf(stderr, "Failed to set assign_activate word.\n"); |
364 if (EC_IOCTL_IS_ERROR(ret)) { |
|
365 fprintf(stderr, "Failed to set DC parameters: %s\n", |
|
366 strerror(EC_IOCTL_ERRNO(ret))); |
322 } |
367 } |
323 } |
368 } |
324 |
369 |
325 /*****************************************************************************/ |
370 /*****************************************************************************/ |
326 |
371 |
327 int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, |
372 int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, |
328 uint8_t subindex, const uint8_t *sdo_data, size_t size) |
373 uint8_t subindex, const uint8_t *sdo_data, size_t size) |
329 { |
374 { |
330 ec_ioctl_sc_sdo_t data; |
375 ec_ioctl_sc_sdo_t data; |
|
376 int ret; |
331 |
377 |
332 data.config_index = sc->index; |
378 data.config_index = sc->index; |
333 data.index = index; |
379 data.index = index; |
334 data.subindex = subindex; |
380 data.subindex = subindex; |
335 data.data = sdo_data; |
381 data.data = sdo_data; |
336 data.size = size; |
382 data.size = size; |
337 data.complete_access = 0; |
383 data.complete_access = 0; |
338 |
384 |
339 if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data) == -1) { |
385 ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data); |
340 fprintf(stderr, "Failed to configure SDO.\n"); |
386 if (EC_IOCTL_IS_ERROR(ret)) { |
341 return -1; // FIXME |
387 fprintf(stderr, "Failed to configure SDO: %s\n", |
|
388 strerror(EC_IOCTL_ERRNO(ret))); |
|
389 return -EC_IOCTL_ERRNO(ret); |
342 } |
390 } |
343 |
391 |
344 return 0; |
392 return 0; |
345 } |
393 } |
346 |
394 |
395 { |
446 { |
396 uint8_t data[4]; |
447 uint8_t data[4]; |
397 |
448 |
398 EC_WRITE_U32(data, value); |
449 EC_WRITE_U32(data, value); |
399 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
450 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
|
451 } |
|
452 |
|
453 /*****************************************************************************/ |
|
454 |
|
455 int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements) |
|
456 { |
|
457 ec_ioctl_sc_emerg_t io; |
|
458 int ret; |
|
459 |
|
460 io.config_index = sc->index; |
|
461 io.size = elements; |
|
462 |
|
463 ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_SIZE, &io); |
|
464 if (EC_IOCTL_IS_ERROR(ret)) { |
|
465 fprintf(stderr, "Failed to set emergency ring size: %s\n", |
|
466 strerror(EC_IOCTL_ERRNO(ret))); |
|
467 return -EC_IOCTL_ERRNO(ret); |
|
468 } |
|
469 |
|
470 return 0; |
|
471 } |
|
472 |
|
473 /*****************************************************************************/ |
|
474 |
|
475 int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target) |
|
476 { |
|
477 ec_ioctl_sc_emerg_t io; |
|
478 int ret; |
|
479 |
|
480 io.config_index = sc->index; |
|
481 io.target = target; |
|
482 |
|
483 ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_POP, &io); |
|
484 if (EC_IOCTL_IS_ERROR(ret)) { |
|
485 if (EC_IOCTL_ERRNO(ret) != ENOENT) { |
|
486 fprintf(stderr, "Failed to get emergency message: %s\n", |
|
487 strerror(EC_IOCTL_ERRNO(ret))); |
|
488 } |
|
489 return -EC_IOCTL_ERRNO(ret); |
|
490 } |
|
491 |
|
492 return 0; |
|
493 } |
|
494 |
|
495 /*****************************************************************************/ |
|
496 |
|
497 int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc) |
|
498 { |
|
499 ec_ioctl_sc_emerg_t io; |
|
500 int ret; |
|
501 |
|
502 io.config_index = sc->index; |
|
503 |
|
504 ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_CLEAR, &io); |
|
505 if (EC_IOCTL_IS_ERROR(ret)) { |
|
506 fprintf(stderr, "Failed to clear emergency ring: %s\n", |
|
507 strerror(EC_IOCTL_ERRNO(ret))); |
|
508 return -EC_IOCTL_ERRNO(ret); |
|
509 } |
|
510 |
|
511 return 0; |
|
512 } |
|
513 |
|
514 /*****************************************************************************/ |
|
515 |
|
516 int ecrt_slave_config_emerg_overruns(ec_slave_config_t *sc) |
|
517 { |
|
518 ec_ioctl_sc_emerg_t io; |
|
519 int ret; |
|
520 |
|
521 io.config_index = sc->index; |
|
522 |
|
523 ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_OVERRUNS, &io); |
|
524 if (EC_IOCTL_IS_ERROR(ret)) { |
|
525 fprintf(stderr, "Failed to get emergency overruns: %s\n", |
|
526 strerror(EC_IOCTL_ERRNO(ret))); |
|
527 return -EC_IOCTL_ERRNO(ret); |
|
528 } |
|
529 |
|
530 return io.overruns; |
400 } |
531 } |
401 |
532 |
402 /*****************************************************************************/ |
533 /*****************************************************************************/ |
403 |
534 |
404 void ec_slave_config_add_sdo_request(ec_slave_config_t *sc, |
535 void ec_slave_config_add_sdo_request(ec_slave_config_t *sc, |
467 return req; |
600 return req; |
468 } |
601 } |
469 |
602 |
470 /*****************************************************************************/ |
603 /*****************************************************************************/ |
471 |
604 |
|
605 void ec_slave_config_add_reg_request(ec_slave_config_t *sc, |
|
606 ec_reg_request_t *reg) |
|
607 { |
|
608 if (sc->first_reg_request) { |
|
609 ec_reg_request_t *r = sc->first_reg_request; |
|
610 while (r->next) { |
|
611 r = r->next; |
|
612 } |
|
613 r->next = reg; |
|
614 } else { |
|
615 sc->first_reg_request = reg; |
|
616 } |
|
617 } |
|
618 |
|
619 /*****************************************************************************/ |
|
620 |
|
621 ec_reg_request_t *ecrt_slave_config_create_reg_request(ec_slave_config_t *sc, |
|
622 size_t size) |
|
623 { |
|
624 ec_ioctl_reg_request_t io; |
|
625 ec_reg_request_t *reg; |
|
626 int ret; |
|
627 |
|
628 reg = malloc(sizeof(ec_reg_request_t)); |
|
629 if (!reg) { |
|
630 fprintf(stderr, "Failed to allocate memory.\n"); |
|
631 return NULL; |
|
632 } |
|
633 |
|
634 if (size) { |
|
635 reg->data = malloc(size); |
|
636 if (!reg->data) { |
|
637 fprintf(stderr, "Failed to allocate %zu bytes of register data" |
|
638 " memory.\n", size); |
|
639 free(reg); |
|
640 return 0; |
|
641 } |
|
642 } else { |
|
643 reg->data = NULL; |
|
644 } |
|
645 |
|
646 io.config_index = sc->index; |
|
647 io.mem_size = size; |
|
648 |
|
649 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_REQUEST, &io); |
|
650 if (EC_IOCTL_IS_ERROR(ret)) { |
|
651 fprintf(stderr, "Failed to create register request: %s\n", |
|
652 strerror(EC_IOCTL_ERRNO(ret))); |
|
653 ec_reg_request_clear(reg); |
|
654 free(reg); |
|
655 return NULL; |
|
656 } |
|
657 |
|
658 reg->next = NULL; |
|
659 reg->config = sc; |
|
660 reg->index = io.request_index; |
|
661 reg->mem_size = size; |
|
662 |
|
663 ec_slave_config_add_reg_request(sc, reg); |
|
664 |
|
665 return reg; |
|
666 } |
|
667 |
|
668 /*****************************************************************************/ |
|
669 |
472 void ec_slave_config_add_voe_handler(ec_slave_config_t *sc, |
670 void ec_slave_config_add_voe_handler(ec_slave_config_t *sc, |
473 ec_voe_handler_t *voe) |
671 ec_voe_handler_t *voe) |
474 { |
672 { |
475 if (sc->first_voe_handler) { |
673 if (sc->first_voe_handler) { |
476 ec_voe_handler_t *v = sc->first_voe_handler; |
674 ec_voe_handler_t *v = sc->first_voe_handler; |
536 |
735 |
537 void ecrt_slave_config_state(const ec_slave_config_t *sc, |
736 void ecrt_slave_config_state(const ec_slave_config_t *sc, |
538 ec_slave_config_state_t *state) |
737 ec_slave_config_state_t *state) |
539 { |
738 { |
540 ec_ioctl_sc_state_t data; |
739 ec_ioctl_sc_state_t data; |
|
740 int ret; |
541 |
741 |
542 data.config_index = sc->index; |
742 data.config_index = sc->index; |
543 data.state = state; |
743 data.state = state; |
544 |
744 |
545 if (ioctl(sc->master->fd, EC_IOCTL_SC_STATE, &data) == -1) { |
745 ret = ioctl(sc->master->fd, EC_IOCTL_SC_STATE, &data); |
|
746 if (EC_IOCTL_IS_ERROR(ret)) { |
546 fprintf(stderr, "Failed to get slave configuration state: %s\n", |
747 fprintf(stderr, "Failed to get slave configuration state: %s\n", |
547 strerror(errno)); |
748 strerror(EC_IOCTL_ERRNO(ret))); |
548 } |
749 } |
549 } |
750 } |
550 |
751 |
551 /*****************************************************************************/ |
752 /*****************************************************************************/ |
552 |
753 |
553 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, |
754 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, |
554 uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size) |
755 uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size) |
555 { |
756 { |
556 ec_ioctl_sc_idn_t io; |
757 ec_ioctl_sc_idn_t io; |
|
758 int ret; |
557 |
759 |
558 io.config_index = sc->index; |
760 io.config_index = sc->index; |
559 io.drive_no = drive_no; |
761 io.drive_no = drive_no; |
560 io.idn = idn; |
762 io.idn = idn; |
561 io.al_state = al_state; |
763 io.al_state = al_state; |
562 io.data = data; |
764 io.data = data; |
563 io.size = size; |
765 io.size = size; |
564 |
766 |
565 if (ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io) == -1) { |
767 ret = ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io); |
566 fprintf(stderr, "Failed to configure IDN.\n"); |
768 if (EC_IOCTL_IS_ERROR(ret)) { |
567 return -1; // FIXME |
769 fprintf(stderr, "Failed to configure IDN: %s\n", |
568 } |
770 strerror(EC_IOCTL_ERRNO(ret))); |
569 |
771 return -EC_IOCTL_ERRNO(ret); |
570 return 0; |
772 } |
571 } |
773 |
572 |
774 return 0; |
573 /*****************************************************************************/ |
775 } |
|
776 |
|
777 /*****************************************************************************/ |