equal
deleted
inserted
replaced
51 } |
51 } |
52 } |
52 } |
53 |
53 |
54 /*****************************************************************************/ |
54 /*****************************************************************************/ |
55 |
55 |
|
56 void ec_master_clear_config(ec_master_t *master) |
|
57 { |
|
58 ec_domain_t *d, *next_d; |
|
59 ec_slave_config_t *c, *next_c; |
|
60 |
|
61 d = master->first_domain; |
|
62 while (d) { |
|
63 next_d = d->next; |
|
64 ec_domain_clear(d); |
|
65 d = next_d; |
|
66 } |
|
67 master->first_domain = NULL; |
|
68 |
|
69 c = master->first_config; |
|
70 while (c) { |
|
71 next_c = c->next; |
|
72 ec_slave_config_clear(c); |
|
73 c = next_c; |
|
74 } |
|
75 master->first_config = NULL; |
|
76 } |
|
77 |
|
78 /*****************************************************************************/ |
|
79 |
|
80 void ec_master_clear(ec_master_t *master) |
|
81 { |
|
82 if (master->process_data) { |
|
83 munmap(master->process_data, master->process_data_size); |
|
84 } |
|
85 |
|
86 ec_master_clear_config(master); |
|
87 |
|
88 if (master->fd != -1) { |
|
89 close(master->fd); |
|
90 } |
|
91 } |
|
92 |
|
93 /*****************************************************************************/ |
|
94 |
|
95 void ec_master_add_domain(ec_master_t *master, ec_domain_t *domain) |
|
96 { |
|
97 if (master->first_domain) { |
|
98 ec_domain_t *d = master->first_domain; |
|
99 while (d->next) { |
|
100 d = d->next; |
|
101 } |
|
102 d->next = domain; |
|
103 } else { |
|
104 master->first_domain = domain; |
|
105 } |
|
106 } |
|
107 |
|
108 /*****************************************************************************/ |
|
109 |
56 ec_domain_t *ecrt_master_create_domain(ec_master_t *master) |
110 ec_domain_t *ecrt_master_create_domain(ec_master_t *master) |
57 { |
111 { |
58 ec_domain_t *domain; |
112 ec_domain_t *domain; |
59 int index; |
113 int index; |
60 |
114 |
69 fprintf(stderr, "Failed to create domain: %s\n", strerror(errno)); |
123 fprintf(stderr, "Failed to create domain: %s\n", strerror(errno)); |
70 free(domain); |
124 free(domain); |
71 return 0; |
125 return 0; |
72 } |
126 } |
73 |
127 |
|
128 domain->next = NULL; |
74 domain->index = (unsigned int) index; |
129 domain->index = (unsigned int) index; |
75 domain->master = master; |
130 domain->master = master; |
76 domain->process_data = NULL; |
131 domain->process_data = NULL; |
|
132 |
|
133 ec_master_add_domain(master, domain); |
|
134 |
77 return domain; |
135 return domain; |
|
136 } |
|
137 |
|
138 /*****************************************************************************/ |
|
139 |
|
140 void ec_master_add_slave_config(ec_master_t *master, ec_slave_config_t *sc) |
|
141 { |
|
142 if (master->first_config) { |
|
143 ec_slave_config_t *c = master->first_config; |
|
144 while (c->next) { |
|
145 c = c->next; |
|
146 } |
|
147 c->next = sc; |
|
148 } else { |
|
149 master->first_config = sc; |
|
150 } |
78 } |
151 } |
79 |
152 |
80 /*****************************************************************************/ |
153 /*****************************************************************************/ |
81 |
154 |
82 ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master, |
155 ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master, |
103 strerror(errno)); |
176 strerror(errno)); |
104 free(sc); |
177 free(sc); |
105 return 0; |
178 return 0; |
106 } |
179 } |
107 |
180 |
|
181 sc->next = NULL; |
108 sc->master = master; |
182 sc->master = master; |
109 sc->index = data.config_index; |
183 sc->index = data.config_index; |
110 sc->alias = alias; |
184 sc->alias = alias; |
111 sc->position = position; |
185 sc->position = position; |
|
186 sc->first_sdo_request = NULL; |
|
187 sc->first_voe_handler = NULL; |
|
188 |
|
189 ec_master_add_slave_config(master, sc); |
|
190 |
112 return sc; |
191 return sc; |
113 } |
192 } |
114 |
193 |
115 /*****************************************************************************/ |
194 /*****************************************************************************/ |
116 |
195 |
312 } |
391 } |
313 |
392 |
314 /*****************************************************************************/ |
393 /*****************************************************************************/ |
315 |
394 |
316 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, |
395 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, |
317 uint16_t idn, uint8_t *data, size_t data_size, uint32_t *error_code) |
396 uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size, |
|
397 uint16_t *error_code) |
318 { |
398 { |
319 ec_ioctl_slave_soe_write_t io; |
399 ec_ioctl_slave_soe_write_t io; |
320 |
400 |
321 io.slave_position = slave_position; |
401 io.slave_position = slave_position; |
|
402 io.drive_no = drive_no; |
322 io.idn = idn; |
403 io.idn = idn; |
323 io.data_size = data_size; |
404 io.data_size = data_size; |
324 io.data = data; |
405 io.data = data; |
325 |
406 |
326 if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_WRITE, &io) == -1) { |
407 if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_WRITE, &io) == -1) { |
335 } |
416 } |
336 |
417 |
337 /*****************************************************************************/ |
418 /*****************************************************************************/ |
338 |
419 |
339 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, |
420 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, |
340 uint16_t idn, uint8_t *target, size_t target_size, |
421 uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size, |
341 size_t *result_size, uint32_t *error_code) |
422 size_t *result_size, uint16_t *error_code) |
342 { |
423 { |
343 ec_ioctl_slave_soe_read_t io; |
424 ec_ioctl_slave_soe_read_t io; |
344 |
425 |
345 io.slave_position = slave_position; |
426 io.slave_position = slave_position; |
|
427 io.drive_no = drive_no; |
346 io.idn = idn; |
428 io.idn = idn; |
347 io.mem_size = target_size; |
429 io.mem_size = target_size; |
348 io.data = target; |
430 io.data = target; |
349 |
431 |
350 if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_READ, &io) == -1) { |
432 if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_READ, &io) == -1) { |
393 { |
475 { |
394 if (ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL) == -1) { |
476 if (ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL) == -1) { |
395 fprintf(stderr, "Failed to deactivate master: %s\n", strerror(errno)); |
477 fprintf(stderr, "Failed to deactivate master: %s\n", strerror(errno)); |
396 return; |
478 return; |
397 } |
479 } |
398 } |
480 |
399 |
481 ec_master_clear_config(master); |
|
482 } |
400 |
483 |
401 /*****************************************************************************/ |
484 /*****************************************************************************/ |
402 |
485 |
403 int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us) |
486 int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us) |
404 { |
487 { |
409 return -1; // FIXME |
492 return -1; // FIXME |
410 } |
493 } |
411 return 0; |
494 return 0; |
412 } |
495 } |
413 |
496 |
414 |
|
415 /*****************************************************************************/ |
497 /*****************************************************************************/ |
416 |
498 |
417 void ecrt_master_send(ec_master_t *master) |
499 void ecrt_master_send(ec_master_t *master) |
418 { |
500 { |
419 if (ioctl(master->fd, EC_IOCTL_SEND, NULL) == -1) { |
501 if (ioctl(master->fd, EC_IOCTL_SEND, NULL) == -1) { |