equal
deleted
inserted
replaced
34 * Vendor-specific-over-EtherCAT protocol handler functions. |
34 * Vendor-specific-over-EtherCAT protocol handler functions. |
35 */ |
35 */ |
36 |
36 |
37 /*****************************************************************************/ |
37 /*****************************************************************************/ |
38 |
38 |
|
39 #include <stdlib.h> |
|
40 #include <sys/ioctl.h> |
39 #include <stdio.h> |
41 #include <stdio.h> |
|
42 #include <errno.h> |
|
43 #include <string.h> |
40 |
44 |
41 #include "voe_handler.h" |
45 #include "voe_handler.h" |
42 #include "slave_config.h" |
46 #include "slave_config.h" |
43 #include "master.h" |
47 #include "master.h" |
44 #include "master/ioctl.h" |
48 #include "master/ioctl.h" |
53 data.config_index = voe->config->index; |
57 data.config_index = voe->config->index; |
54 data.voe_index = voe->index; |
58 data.voe_index = voe->index; |
55 data.vendor_id = &vendor_id; |
59 data.vendor_id = &vendor_id; |
56 data.vendor_type = &vendor_type; |
60 data.vendor_type = &vendor_type; |
57 |
61 |
58 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data) == -1) { |
62 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data) == -1) |
59 fprintf(stderr, "Failed to set VoE send header.\n"); |
63 fprintf(stderr, "Failed to set VoE send header: %s\n", |
60 } |
64 strerror(errno)); |
61 } |
65 } |
62 |
66 |
63 /*****************************************************************************/ |
67 /*****************************************************************************/ |
64 |
68 |
65 void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe, |
69 void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe, |
70 data.config_index = voe->config->index; |
74 data.config_index = voe->config->index; |
71 data.voe_index = voe->index; |
75 data.voe_index = voe->index; |
72 data.vendor_id = vendor_id; |
76 data.vendor_id = vendor_id; |
73 data.vendor_type = vendor_type; |
77 data.vendor_type = vendor_type; |
74 |
78 |
75 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data) == -1) { |
79 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data) == -1) |
76 fprintf(stderr, "Failed to get received VoE header.\n"); |
80 fprintf(stderr, "Failed to get received VoE header: %s\n", |
77 } |
81 strerror(errno)); |
78 } |
82 } |
79 |
83 |
80 /*****************************************************************************/ |
84 /*****************************************************************************/ |
81 |
85 |
82 uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe) |
86 uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe) |
98 ec_ioctl_voe_t data; |
102 ec_ioctl_voe_t data; |
99 |
103 |
100 data.config_index = voe->config->index; |
104 data.config_index = voe->config->index; |
101 data.voe_index = voe->index; |
105 data.voe_index = voe->index; |
102 |
106 |
103 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data) == -1) { |
107 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data) == -1) |
104 fprintf(stderr, "Failed to initiate VoE reading.\n"); |
108 fprintf(stderr, "Failed to initiate VoE reading: %s\n", |
105 } |
109 strerror(errno)); |
106 } |
110 } |
107 |
111 |
108 /*****************************************************************************/ |
112 /*****************************************************************************/ |
109 |
113 |
110 void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size) |
114 void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size) |
114 data.config_index = voe->config->index; |
118 data.config_index = voe->config->index; |
115 data.voe_index = voe->index; |
119 data.voe_index = voe->index; |
116 data.size = size; |
120 data.size = size; |
117 data.data = voe->data; |
121 data.data = voe->data; |
118 |
122 |
119 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data) == -1) { |
123 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data) == -1) |
120 fprintf(stderr, "Failed to initiate VoE reading.\n"); |
124 fprintf(stderr, "Failed to initiate VoE writing: %s\n", |
121 } |
125 strerror(errno)); |
122 } |
126 } |
123 |
127 |
124 /*****************************************************************************/ |
128 /*****************************************************************************/ |
125 |
129 |
126 ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe) |
130 ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe) |
130 data.config_index = voe->config->index; |
134 data.config_index = voe->config->index; |
131 data.voe_index = voe->index; |
135 data.voe_index = voe->index; |
132 data.size = 0; |
136 data.size = 0; |
133 |
137 |
134 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data) == -1) { |
138 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data) == -1) { |
135 fprintf(stderr, "Failed to initiate VoE reading.\n"); |
139 fprintf(stderr, "Failed to execute VoE handler: %s\n", |
|
140 strerror(errno)); |
136 return EC_REQUEST_ERROR; |
141 return EC_REQUEST_ERROR; |
137 } |
142 } |
138 |
143 |
139 if (data.size) { // new data waiting to be copied |
144 if (data.size) { // new data waiting to be copied |
140 if (voe->mem_size < data.size) { |
145 if (voe->mem_size < data.size) { |
147 return EC_REQUEST_ERROR; |
152 return EC_REQUEST_ERROR; |
148 } |
153 } |
149 voe->mem_size = data.size; |
154 voe->mem_size = data.size; |
150 } |
155 } |
151 |
156 |
|
157 data.data = voe->data; |
|
158 |
152 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data) == -1) { |
159 if (ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data) == -1) { |
153 fprintf(stderr, "Failed to get VoE data!\n"); |
160 fprintf(stderr, "Failed to get VoE data: %s\n", strerror(errno)); |
154 return EC_REQUEST_ERROR; |
161 return EC_REQUEST_ERROR; |
155 } |
162 } |
156 voe->data_size = data.size; |
163 voe->data_size = data.size; |
157 } |
164 } |
158 |
165 |