159 return 0; |
159 return 0; |
160 } |
160 } |
161 |
161 |
162 /*****************************************************************************/ |
162 /*****************************************************************************/ |
163 |
163 |
|
164 int ecrt_master_get_sync_manager(ec_master_t *master, uint16_t slave_position, |
|
165 uint8_t sync_index, ec_sync_info_t *sync) |
|
166 { |
|
167 ec_ioctl_slave_sync_t data; |
|
168 |
|
169 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
|
170 return -ENOENT; |
|
171 |
|
172 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_t)); |
|
173 data.slave_position = slave_position; |
|
174 data.sync_index = sync_index; |
|
175 |
|
176 if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC, &data) == -1) { |
|
177 fprintf(stderr, "Failed to get sync manager information: %s\n", |
|
178 strerror(errno)); |
|
179 return -1; // FIXME |
|
180 } |
|
181 |
|
182 sync->index = sync_index; |
|
183 sync->dir = EC_READ_BIT(&data.control_register, 2) ? |
|
184 EC_DIR_OUTPUT : EC_DIR_INPUT; |
|
185 sync->n_pdos = data.pdo_count; |
|
186 sync->pdos = NULL; |
|
187 sync->watchdog_mode = EC_READ_BIT(&data.control_register, 6) ? |
|
188 EC_WD_ENABLE : EC_WD_DISABLE; |
|
189 |
|
190 return 0; |
|
191 } |
|
192 |
|
193 /*****************************************************************************/ |
|
194 |
|
195 int ecrt_master_get_pdo(ec_master_t *master, uint16_t slave_position, |
|
196 uint8_t sync_index, uint16_t pos, ec_pdo_info_t *pdo) |
|
197 { |
|
198 ec_ioctl_slave_sync_pdo_t data; |
|
199 |
|
200 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
|
201 return -ENOENT; |
|
202 |
|
203 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_t)); |
|
204 data.slave_position = slave_position; |
|
205 data.sync_index = sync_index; |
|
206 data.pdo_pos = pos; |
|
207 |
|
208 if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO, &data) == -1) { |
|
209 fprintf(stderr, "Failed to get pdo information: %s\n", |
|
210 strerror(errno)); |
|
211 return -1; // FIXME |
|
212 } |
|
213 |
|
214 pdo->index = data.index; |
|
215 pdo->n_entries = data.entry_count; |
|
216 pdo->entries = NULL; |
|
217 |
|
218 return 0; |
|
219 } |
|
220 |
|
221 /*****************************************************************************/ |
|
222 |
|
223 int ecrt_master_get_pdo_entry(ec_master_t *master, uint16_t slave_position, |
|
224 uint8_t sync_index, uint16_t pdo_pos, uint16_t entry_pos, |
|
225 ec_pdo_entry_info_t *entry) |
|
226 { |
|
227 ec_ioctl_slave_sync_pdo_entry_t data; |
|
228 |
|
229 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
|
230 return -ENOENT; |
|
231 |
|
232 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_entry_t)); |
|
233 data.slave_position = slave_position; |
|
234 data.sync_index = sync_index; |
|
235 data.pdo_pos = pdo_pos; |
|
236 data.entry_pos = entry_pos; |
|
237 |
|
238 if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, &data) == -1) { |
|
239 fprintf(stderr, "Failed to get pdo entry information: %s\n", |
|
240 strerror(errno)); |
|
241 return -1; // FIXME |
|
242 } |
|
243 |
|
244 entry->index = data.index; |
|
245 entry->subindex = data.subindex; |
|
246 entry->bit_length = data.bit_length; |
|
247 |
|
248 return 0; |
|
249 } |
|
250 |
|
251 /*****************************************************************************/ |
|
252 |
|
253 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, |
|
254 uint16_t index, uint8_t subindex, uint8_t *data, |
|
255 size_t data_size, uint32_t *abort_code) |
|
256 { |
|
257 ec_ioctl_slave_sdo_download_t download; |
|
258 |
|
259 download.slave_position = slave_position; |
|
260 download.sdo_index = index; |
|
261 download.sdo_entry_subindex = subindex; |
|
262 download.data_size = data_size; |
|
263 download.data = data; |
|
264 |
|
265 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download) == -1) { |
|
266 if (errno == -EIO) { |
|
267 if (abort_code) { |
|
268 *abort_code = download.abort_code; |
|
269 } |
|
270 } |
|
271 fprintf(stderr, "Failed to execute SDO download: %s\n", |
|
272 strerror(errno)); |
|
273 return -1; |
|
274 } |
|
275 |
|
276 return 0; |
|
277 } |
|
278 |
|
279 /*****************************************************************************/ |
|
280 |
|
281 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, |
|
282 uint16_t index, uint8_t subindex, uint8_t *target, |
|
283 size_t target_size, size_t *result_size, uint32_t *abort_code) |
|
284 { |
|
285 ec_ioctl_slave_sdo_upload_t upload; |
|
286 |
|
287 upload.slave_position = slave_position; |
|
288 upload.sdo_index = index; |
|
289 upload.sdo_entry_subindex = subindex; |
|
290 upload.target_size = target_size; |
|
291 upload.target = target; |
|
292 |
|
293 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &upload) == -1) { |
|
294 if (errno == -EIO) { |
|
295 if (abort_code) { |
|
296 *abort_code = upload.abort_code; |
|
297 } |
|
298 } |
|
299 fprintf(stderr, "Failed to execute SDO upload: %s\n", |
|
300 strerror(errno)); |
|
301 return -1; |
|
302 } |
|
303 |
|
304 *result_size = upload.data_size; |
|
305 return 0; |
|
306 } |
|
307 |
|
308 /*****************************************************************************/ |
|
309 |
164 int ecrt_master_activate(ec_master_t *master) |
310 int ecrt_master_activate(ec_master_t *master) |
165 { |
311 { |
166 if (ioctl(master->fd, EC_IOCTL_ACTIVATE, |
312 if (ioctl(master->fd, EC_IOCTL_ACTIVATE, |
167 &master->process_data_size) == -1) { |
313 &master->process_data_size) == -1) { |
168 fprintf(stderr, "Failed to activate master: %s\n", |
314 fprintf(stderr, "Failed to activate master: %s\n", |
246 fprintf(stderr, "Failed to sync slave clocks: %s\n", strerror(errno)); |
392 fprintf(stderr, "Failed to sync slave clocks: %s\n", strerror(errno)); |
247 } |
393 } |
248 } |
394 } |
249 |
395 |
250 /*****************************************************************************/ |
396 /*****************************************************************************/ |
251 |
|
252 int ecrt_slave_sdo_download(ec_master_t* master, uint16_t slave_position, |
|
253 uint16_t index, uint8_t subindex, uint8_t *data, |
|
254 size_t data_size, uint32_t *abort_code) |
|
255 { |
|
256 ec_ioctl_slave_sdo_download_t download; |
|
257 |
|
258 download.slave_position = slave_position; |
|
259 download.sdo_index = index; |
|
260 download.sdo_entry_subindex = subindex; |
|
261 download.data_size = data_size; |
|
262 download.data = data; |
|
263 |
|
264 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download) == -1) { |
|
265 if (errno == -EIO) { |
|
266 if (abort_code) { |
|
267 *abort_code = download.abort_code; |
|
268 } |
|
269 } |
|
270 fprintf(stderr, "Failed to execute SDO download: %s\n", |
|
271 strerror(errno)); |
|
272 return -1; |
|
273 } |
|
274 |
|
275 return 0; |
|
276 } |
|
277 |
|
278 /*****************************************************************************/ |
|
279 |
|
280 int ecrt_slave_sdo_upload(ec_master_t* master, uint16_t slave_position, |
|
281 uint16_t index, uint8_t subindex, uint8_t *target, |
|
282 size_t target_size, size_t *result_size, uint32_t *abort_code) |
|
283 { |
|
284 ec_ioctl_slave_sdo_upload_t upload; |
|
285 |
|
286 upload.slave_position = slave_position; |
|
287 upload.sdo_index = index; |
|
288 upload.sdo_entry_subindex = subindex; |
|
289 upload.target_size = target_size; |
|
290 upload.target = target; |
|
291 |
|
292 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &upload) == -1) { |
|
293 if (errno == -EIO) { |
|
294 if (abort_code) { |
|
295 *abort_code = upload.abort_code; |
|
296 } |
|
297 } |
|
298 fprintf(stderr, "Failed to execute SDO upload: %s\n", |
|
299 strerror(errno)); |
|
300 return -1; |
|
301 } |
|
302 |
|
303 *result_size = upload.data_size; |
|
304 return 0; |
|
305 } |
|
306 |
|
307 /*****************************************************************************/ |
|