1 /****************************************************************************** |
1 /****************************************************************************** |
2 * |
2 * |
3 * $Id$ |
3 * $Id$ |
4 * |
4 * |
5 * Copyright (C) 2006-2009 Florian Pose, Ingenieurgemeinschaft IgH |
5 * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH |
6 * |
6 * |
7 * This file is part of the IgH EtherCAT master userspace library. |
7 * This file is part of the IgH EtherCAT master userspace library. |
8 * |
8 * |
9 * The IgH EtherCAT master userspace library is free software; you can |
9 * The IgH EtherCAT master userspace library is free software; you can |
10 * redistribute it and/or modify it under the terms of the GNU Lesser General |
10 * redistribute it and/or modify it under the terms of the GNU Lesser General |
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 "voe_handler.h" |
40 #include "voe_handler.h" |
41 #include "master.h" |
41 #include "master.h" |
42 #include "master/ioctl.h" |
|
43 |
42 |
44 /*****************************************************************************/ |
43 /*****************************************************************************/ |
45 |
44 |
46 void ec_slave_config_clear(ec_slave_config_t *sc) |
45 void ec_slave_config_clear(ec_slave_config_t *sc) |
47 { |
46 { |
68 |
67 |
69 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
68 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) |
69 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
71 { |
70 { |
72 ec_ioctl_config_t data; |
71 ec_ioctl_config_t data; |
|
72 int ret; |
73 |
73 |
74 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
74 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
75 return -ENOENT; |
75 return -ENOENT; |
76 |
76 |
77 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
77 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
78 data.config_index = sc->index; |
78 data.config_index = sc->index; |
79 data.syncs[sync_index].dir = dir; |
79 data.syncs[sync_index].dir = dir; |
80 data.syncs[sync_index].watchdog_mode = watchdog_mode; |
80 data.syncs[sync_index].watchdog_mode = watchdog_mode; |
81 data.syncs[sync_index].config_this = 1; |
81 data.syncs[sync_index].config_this = 1; |
82 |
82 |
83 if (ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data) == -1) { |
83 ret = ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data); |
|
84 if (EC_IOCTL_IS_ERROR(ret)) { |
84 fprintf(stderr, "Failed to config sync manager: %s\n", |
85 fprintf(stderr, "Failed to config sync manager: %s\n", |
85 strerror(errno)); |
86 strerror(EC_IOCTL_ERRNO(ret))); |
86 return -1; // FIXME |
87 return -EC_IOCTL_ERRNO(ret); |
87 } |
88 } |
88 |
89 |
89 return 0; |
90 return 0; |
90 } |
91 } |
91 |
92 |
93 |
94 |
94 void ecrt_slave_config_watchdog(ec_slave_config_t *sc, |
95 void ecrt_slave_config_watchdog(ec_slave_config_t *sc, |
95 uint16_t divider, uint16_t intervals) |
96 uint16_t divider, uint16_t intervals) |
96 { |
97 { |
97 ec_ioctl_config_t data; |
98 ec_ioctl_config_t data; |
|
99 int ret; |
98 |
100 |
99 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
101 memset(&data, 0x00, sizeof(ec_ioctl_config_t)); |
100 data.config_index = sc->index; |
102 data.config_index = sc->index; |
101 data.watchdog_divider = divider; |
103 data.watchdog_divider = divider; |
102 data.watchdog_intervals = intervals; |
104 data.watchdog_intervals = intervals; |
103 |
105 |
104 if (ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data) == -1) { |
106 ret = ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data); |
|
107 if (EC_IOCTL_IS_ERROR(ret)) { |
105 fprintf(stderr, "Failed to config watchdog: %s\n", |
108 fprintf(stderr, "Failed to config watchdog: %s\n", |
106 strerror(errno)); |
109 strerror(EC_IOCTL_ERRNO(ret))); |
107 } |
110 } |
108 } |
111 } |
109 |
112 |
110 /*****************************************************************************/ |
113 /*****************************************************************************/ |
111 |
114 |
112 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
115 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
113 uint8_t sync_index, uint16_t pdo_index) |
116 uint8_t sync_index, uint16_t pdo_index) |
114 { |
117 { |
115 ec_ioctl_config_pdo_t data; |
118 ec_ioctl_config_pdo_t data; |
|
119 int ret; |
116 |
120 |
117 data.config_index = sc->index; |
121 data.config_index = sc->index; |
118 data.sync_index = sync_index; |
122 data.sync_index = sync_index; |
119 data.index = pdo_index; |
123 data.index = pdo_index; |
120 |
124 |
121 if (ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data) == -1) { |
125 ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data); |
|
126 if (EC_IOCTL_IS_ERROR(ret)) { |
122 fprintf(stderr, "Failed to add PDO: %s\n", |
127 fprintf(stderr, "Failed to add PDO: %s\n", |
123 strerror(errno)); |
128 strerror(EC_IOCTL_ERRNO(ret))); |
124 return -1; // FIXME |
129 return -EC_IOCTL_ERRNO(ret); |
125 } |
130 } |
126 |
131 |
127 return 0; |
132 return 0; |
128 } |
133 } |
129 |
134 |
131 |
136 |
132 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
137 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
133 uint8_t sync_index) |
138 uint8_t sync_index) |
134 { |
139 { |
135 ec_ioctl_config_pdo_t data; |
140 ec_ioctl_config_pdo_t data; |
|
141 int ret; |
136 |
142 |
137 data.config_index = sc->index; |
143 data.config_index = sc->index; |
138 data.sync_index = sync_index; |
144 data.sync_index = sync_index; |
139 |
145 |
140 if (ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data) == -1) { |
146 ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data); |
|
147 if (EC_IOCTL_IS_ERROR(ret)) { |
141 fprintf(stderr, "Failed to clear PDOs: %s\n", |
148 fprintf(stderr, "Failed to clear PDOs: %s\n", |
142 strerror(errno)); |
149 strerror(EC_IOCTL_ERRNO(ret))); |
143 } |
150 } |
144 } |
151 } |
145 |
152 |
146 /*****************************************************************************/ |
153 /*****************************************************************************/ |
147 |
154 |
148 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
155 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
149 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
156 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
150 uint8_t entry_bit_length) |
157 uint8_t entry_bit_length) |
151 { |
158 { |
152 ec_ioctl_add_pdo_entry_t data; |
159 ec_ioctl_add_pdo_entry_t data; |
|
160 int ret; |
153 |
161 |
154 data.config_index = sc->index; |
162 data.config_index = sc->index; |
155 data.pdo_index = pdo_index; |
163 data.pdo_index = pdo_index; |
156 data.entry_index = entry_index; |
164 data.entry_index = entry_index; |
157 data.entry_subindex = entry_subindex; |
165 data.entry_subindex = entry_subindex; |
158 data.entry_bit_length = entry_bit_length; |
166 data.entry_bit_length = entry_bit_length; |
159 |
167 |
160 if (ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data) == -1) { |
168 ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data); |
|
169 if (EC_IOCTL_IS_ERROR(ret)) { |
161 fprintf(stderr, "Failed to add PDO entry: %s\n", |
170 fprintf(stderr, "Failed to add PDO entry: %s\n", |
162 strerror(errno)); |
171 strerror(EC_IOCTL_ERRNO(ret))); |
163 return -1; // FIXME |
172 return -EC_IOCTL_ERRNO(ret); |
164 } |
173 } |
165 |
174 |
166 return 0; |
175 return 0; |
167 } |
176 } |
168 |
177 |
170 |
179 |
171 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
180 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
172 uint16_t pdo_index) |
181 uint16_t pdo_index) |
173 { |
182 { |
174 ec_ioctl_config_pdo_t data; |
183 ec_ioctl_config_pdo_t data; |
|
184 int ret; |
175 |
185 |
176 data.config_index = sc->index; |
186 data.config_index = sc->index; |
177 data.index = pdo_index; |
187 data.index = pdo_index; |
178 |
188 |
179 if (ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data) == -1) { |
189 ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data); |
|
190 if (EC_IOCTL_IS_ERROR(ret)) { |
180 fprintf(stderr, "Failed to clear PDO entries: %s\n", |
191 fprintf(stderr, "Failed to clear PDO entries: %s\n", |
181 strerror(errno)); |
192 strerror(EC_IOCTL_ERRNO(ret))); |
182 } |
193 } |
183 } |
194 } |
184 |
195 |
185 /*****************************************************************************/ |
196 /*****************************************************************************/ |
186 |
197 |
262 data.entry_index = index; |
273 data.entry_index = index; |
263 data.entry_subindex = subindex; |
274 data.entry_subindex = subindex; |
264 data.domain_index = domain->index; |
275 data.domain_index = domain->index; |
265 |
276 |
266 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data); |
277 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data); |
267 if (ret == -1) { |
278 if (EC_IOCTL_IS_ERROR(ret)) { |
268 fprintf(stderr, "Failed to register PDO entry: %s\n", |
279 fprintf(stderr, "Failed to register PDO entry: %s\n", |
269 strerror(errno)); |
280 strerror(EC_IOCTL_ERRNO(ret))); |
270 return -2; // FIXME |
281 return -EC_IOCTL_ERRNO(ret); |
271 } |
282 } |
272 |
283 |
273 if (bit_position) { |
284 if (bit_position) { |
274 *bit_position = data.bit_position; |
285 *bit_position = data.bit_position; |
275 } else { |
286 } else { |
276 if (data.bit_position) { |
287 if (data.bit_position) { |
277 fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align " |
288 fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align " |
278 "in config %u:%u.\n", index, subindex, |
289 "in config %u:%u.\n", index, subindex, |
279 sc->alias, sc->position); |
290 sc->alias, sc->position); |
280 return -3; // FIXME |
291 return -EFAULT; |
281 } |
292 } |
282 } |
293 } |
283 |
294 |
284 return ret; |
295 return ret; |
285 } |
296 } |
289 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate, |
300 void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate, |
290 uint32_t sync0_cycle_time, uint32_t sync0_shift_time, |
301 uint32_t sync0_cycle_time, uint32_t sync0_shift_time, |
291 uint32_t sync1_cycle_time, uint32_t sync1_shift_time) |
302 uint32_t sync1_cycle_time, uint32_t sync1_shift_time) |
292 { |
303 { |
293 ec_ioctl_config_t data; |
304 ec_ioctl_config_t data; |
|
305 int ret; |
294 |
306 |
295 data.config_index = sc->index; |
307 data.config_index = sc->index; |
296 data.dc_assign_activate = assign_activate; |
308 data.dc_assign_activate = assign_activate; |
297 data.dc_sync[0].cycle_time = sync0_cycle_time; |
309 data.dc_sync[0].cycle_time = sync0_cycle_time; |
298 data.dc_sync[0].shift_time = sync0_shift_time; |
310 data.dc_sync[0].shift_time = sync0_shift_time; |
299 data.dc_sync[1].cycle_time = sync1_cycle_time; |
311 data.dc_sync[1].cycle_time = sync1_cycle_time; |
300 data.dc_sync[1].shift_time = sync1_shift_time; |
312 data.dc_sync[1].shift_time = sync1_shift_time; |
301 |
313 |
302 if (ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data) == -1) { |
314 ret = ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data); |
303 fprintf(stderr, "Failed to set assign_activate word.\n"); |
315 if (EC_IOCTL_IS_ERROR(ret)) { |
|
316 fprintf(stderr, "Failed to set assign_activate word: %s\n", |
|
317 strerror(EC_IOCTL_ERRNO(ret))); |
304 } |
318 } |
305 } |
319 } |
306 |
320 |
307 /*****************************************************************************/ |
321 /*****************************************************************************/ |
308 |
322 |
309 int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, |
323 int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index, |
310 uint8_t subindex, const uint8_t *sdo_data, size_t size) |
324 uint8_t subindex, const uint8_t *sdo_data, size_t size) |
311 { |
325 { |
312 ec_ioctl_sc_sdo_t data; |
326 ec_ioctl_sc_sdo_t data; |
|
327 int ret; |
313 |
328 |
314 data.config_index = sc->index; |
329 data.config_index = sc->index; |
315 data.index = index; |
330 data.index = index; |
316 data.subindex = subindex; |
331 data.subindex = subindex; |
317 data.data = sdo_data; |
332 data.data = sdo_data; |
318 data.size = size; |
333 data.size = size; |
319 data.complete_access = 0; |
334 data.complete_access = 0; |
320 |
335 |
321 if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data) == -1) { |
336 ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data); |
322 fprintf(stderr, "Failed to configure SDO.\n"); |
337 if (EC_IOCTL_IS_ERROR(ret)) { |
323 return -1; // FIXME |
338 fprintf(stderr, "Failed to configure SDO: %s\n", |
|
339 strerror(EC_IOCTL_ERRNO(ret))); |
|
340 return -EC_IOCTL_ERRNO(ret); |
324 } |
341 } |
325 |
342 |
326 return 0; |
343 return 0; |
327 } |
344 } |
328 |
345 |
330 |
347 |
331 int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index, |
348 int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index, |
332 const uint8_t *sdo_data, size_t size) |
349 const uint8_t *sdo_data, size_t size) |
333 { |
350 { |
334 ec_ioctl_sc_sdo_t data; |
351 ec_ioctl_sc_sdo_t data; |
|
352 int ret; |
335 |
353 |
336 data.config_index = sc->index; |
354 data.config_index = sc->index; |
337 data.index = index; |
355 data.index = index; |
338 data.subindex = 0; |
356 data.subindex = 0; |
339 data.data = sdo_data; |
357 data.data = sdo_data; |
340 data.size = size; |
358 data.size = size; |
341 data.complete_access = 1; |
359 data.complete_access = 1; |
342 |
360 |
343 if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data) == -1) { |
361 ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data); |
344 fprintf(stderr, "Failed to configure SDO.\n"); |
362 if (EC_IOCTL_IS_ERROR(ret)) { |
345 return -1; // FIXME |
363 fprintf(stderr, "Failed to configure SDO: %s\n", |
|
364 strerror(EC_IOCTL_ERRNO(ret))); |
|
365 return -EC_IOCTL_ERRNO(ret); |
346 } |
366 } |
347 |
367 |
348 return 0; |
368 return 0; |
349 } |
369 } |
350 |
370 |
402 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, |
422 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, |
403 uint16_t index, uint8_t subindex, size_t size) |
423 uint16_t index, uint8_t subindex, size_t size) |
404 { |
424 { |
405 ec_ioctl_sdo_request_t data; |
425 ec_ioctl_sdo_request_t data; |
406 ec_sdo_request_t *req; |
426 ec_sdo_request_t *req; |
|
427 int ret; |
407 |
428 |
408 req = malloc(sizeof(ec_sdo_request_t)); |
429 req = malloc(sizeof(ec_sdo_request_t)); |
409 if (!req) { |
430 if (!req) { |
410 fprintf(stderr, "Failed to allocate memory.\n"); |
431 fprintf(stderr, "Failed to allocate memory.\n"); |
411 return 0; |
432 return 0; |
426 data.config_index = sc->index; |
447 data.config_index = sc->index; |
427 data.sdo_index = index; |
448 data.sdo_index = index; |
428 data.sdo_subindex = subindex; |
449 data.sdo_subindex = subindex; |
429 data.size = size; |
450 data.size = size; |
430 |
451 |
431 if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data) == -1) { |
452 ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data); |
|
453 if (EC_IOCTL_IS_ERROR(ret)) { |
432 fprintf(stderr, "Failed to create SDO request: %s\n", |
454 fprintf(stderr, "Failed to create SDO request: %s\n", |
433 strerror(errno)); |
455 strerror(EC_IOCTL_ERRNO(ret))); |
434 ec_sdo_request_clear(req); |
456 ec_sdo_request_clear(req); |
435 free(req); |
457 free(req); |
436 return NULL; |
458 return NULL; |
437 } |
459 } |
438 |
460 |
492 } |
515 } |
493 |
516 |
494 data.config_index = sc->index; |
517 data.config_index = sc->index; |
495 data.size = size; |
518 data.size = size; |
496 |
519 |
497 if (ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data) == -1) { |
520 ret = ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data); |
|
521 if (EC_IOCTL_IS_ERROR(ret)) { |
498 fprintf(stderr, "Failed to create VoE handler: %s\n", |
522 fprintf(stderr, "Failed to create VoE handler: %s\n", |
499 strerror(errno)); |
523 strerror(EC_IOCTL_ERRNO(ret))); |
500 ec_voe_handler_clear(voe); |
524 ec_voe_handler_clear(voe); |
501 free(voe); |
525 free(voe); |
502 return NULL; |
526 return NULL; |
503 } |
527 } |
504 |
528 |
517 |
541 |
518 void ecrt_slave_config_state(const ec_slave_config_t *sc, |
542 void ecrt_slave_config_state(const ec_slave_config_t *sc, |
519 ec_slave_config_state_t *state) |
543 ec_slave_config_state_t *state) |
520 { |
544 { |
521 ec_ioctl_sc_state_t data; |
545 ec_ioctl_sc_state_t data; |
|
546 int ret; |
522 |
547 |
523 data.config_index = sc->index; |
548 data.config_index = sc->index; |
524 data.state = state; |
549 data.state = state; |
525 |
550 |
526 if (ioctl(sc->master->fd, EC_IOCTL_SC_STATE, &data) == -1) { |
551 ret = ioctl(sc->master->fd, EC_IOCTL_SC_STATE, &data); |
|
552 if (EC_IOCTL_IS_ERROR(ret)) { |
527 fprintf(stderr, "Failed to get slave configuration state: %s\n", |
553 fprintf(stderr, "Failed to get slave configuration state: %s\n", |
528 strerror(errno)); |
554 strerror(EC_IOCTL_ERRNO(ret))); |
529 } |
555 } |
530 } |
556 } |
531 |
557 |
532 /*****************************************************************************/ |
558 /*****************************************************************************/ |
533 |
559 |
534 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, |
560 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, |
535 uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size) |
561 uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size) |
536 { |
562 { |
537 ec_ioctl_sc_idn_t io; |
563 ec_ioctl_sc_idn_t io; |
|
564 int ret; |
538 |
565 |
539 io.config_index = sc->index; |
566 io.config_index = sc->index; |
540 io.drive_no = drive_no; |
567 io.drive_no = drive_no; |
541 io.idn = idn; |
568 io.idn = idn; |
542 io.al_state = al_state; |
569 io.al_state = al_state; |
543 io.data = data; |
570 io.data = data; |
544 io.size = size; |
571 io.size = size; |
545 |
572 |
546 if (ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io) == -1) { |
573 ret = ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io); |
547 fprintf(stderr, "Failed to configure IDN.\n"); |
574 if (EC_IOCTL_IS_ERROR(ret)) { |
548 return -1; // FIXME |
575 fprintf(stderr, "Failed to configure IDN: %s\n", |
549 } |
576 strerror(EC_IOCTL_ERRNO(ret))); |
550 |
577 return -EC_IOCTL_ERRNO(ret); |
551 return 0; |
578 } |
552 } |
579 |
553 |
580 return 0; |
554 /*****************************************************************************/ |
581 } |
|
582 |
|
583 /*****************************************************************************/ |