170 data.alias = alias; |
176 data.alias = alias; |
171 data.position = position; |
177 data.position = position; |
172 data.vendor_id = vendor_id; |
178 data.vendor_id = vendor_id; |
173 data.product_code = product_code; |
179 data.product_code = product_code; |
174 |
180 |
175 if (ioctl(master->fd, EC_IOCTL_CREATE_SLAVE_CONFIG, &data) == -1) { |
181 ret = ioctl(master->fd, EC_IOCTL_CREATE_SLAVE_CONFIG, &data); |
|
182 if (EC_IOCTL_IS_ERROR(ret)) { |
176 fprintf(stderr, "Failed to create slave config: %s\n", |
183 fprintf(stderr, "Failed to create slave config: %s\n", |
177 strerror(errno)); |
184 strerror(EC_IOCTL_ERRNO(ret))); |
178 free(sc); |
185 free(sc); |
179 return 0; |
186 return 0; |
180 } |
187 } |
181 |
188 |
182 sc->next = NULL; |
189 sc->next = NULL; |
183 sc->master = master; |
190 sc->master = master; |
184 sc->index = data.config_index; |
191 sc->index = data.config_index; |
185 sc->alias = alias; |
192 sc->alias = alias; |
186 sc->position = position; |
193 sc->position = position; |
187 sc->first_sdo_request = NULL; |
194 sc->first_sdo_request = NULL; |
|
195 sc->first_reg_request = NULL; |
188 sc->first_voe_handler = NULL; |
196 sc->first_voe_handler = NULL; |
189 |
197 |
190 ec_master_add_slave_config(master, sc); |
198 ec_master_add_slave_config(master, sc); |
191 |
199 |
192 return sc; |
200 return sc; |
193 } |
201 } |
194 |
202 |
195 /*****************************************************************************/ |
203 /*****************************************************************************/ |
196 |
204 |
197 int ecrt_master(ec_master_t* master, ec_master_info_t *master_info) |
205 int ecrt_master_select_reference_clock(ec_master_t *master, |
|
206 ec_slave_config_t *sc) |
|
207 { |
|
208 uint32_t config_index; |
|
209 int ret; |
|
210 |
|
211 if (sc) { |
|
212 config_index = sc->index; |
|
213 } |
|
214 else { |
|
215 config_index = 0xFFFFFFFF; |
|
216 } |
|
217 |
|
218 ret = ioctl(master->fd, EC_IOCTL_SELECT_REF_CLOCK, config_index); |
|
219 if (EC_IOCTL_IS_ERROR(ret)) { |
|
220 fprintf(stderr, "Failed to select reference clock: %s\n", |
|
221 strerror(EC_IOCTL_ERRNO(ret))); |
|
222 return -EC_IOCTL_ERRNO(ret); |
|
223 } |
|
224 |
|
225 return 0; |
|
226 } |
|
227 |
|
228 /****************************************************************************/ |
|
229 |
|
230 int ecrt_master(ec_master_t *master, ec_master_info_t *master_info) |
198 { |
231 { |
199 ec_ioctl_master_t data; |
232 ec_ioctl_master_t data; |
200 |
233 int ret; |
201 if (ioctl(master->fd, EC_IOCTL_MASTER, &data) < 0) { |
234 |
202 fprintf(stderr, "Failed to get master info: %s\n", strerror(errno)); |
235 ret = ioctl(master->fd, EC_IOCTL_MASTER, &data); |
203 return -1; |
236 if (EC_IOCTL_IS_ERROR(ret)) { |
|
237 fprintf(stderr, "Failed to get master info: %s\n", |
|
238 strerror(EC_IOCTL_ERRNO(ret))); |
|
239 return -EC_IOCTL_ERRNO(ret); |
204 } |
240 } |
205 |
241 |
206 master_info->slave_count = data.slave_count; |
242 master_info->slave_count = data.slave_count; |
207 master_info->link_up = data.devices[0].link_state; |
243 master_info->link_up = data.devices[0].link_state; |
208 master_info->scan_busy = data.scan_busy; |
244 master_info->scan_busy = data.scan_busy; |
209 master_info->app_time = data.app_time; |
245 master_info->app_time = data.app_time; |
210 return 0; |
246 return 0; |
211 } |
247 } |
212 |
248 |
213 /*****************************************************************************/ |
249 /****************************************************************************/ |
214 |
250 |
215 int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, |
251 int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position, |
216 ec_slave_info_t *slave_info) |
252 ec_slave_info_t *slave_info) |
217 { |
253 { |
218 ec_ioctl_slave_t data; |
254 ec_ioctl_slave_t data; |
219 int index, i; |
255 int ret, i; |
220 |
256 |
221 data.position = slave_position; |
257 data.position = slave_position; |
222 |
258 |
223 if (ioctl(master->fd, EC_IOCTL_SLAVE, &data) == -1) { |
259 ret = ioctl(master->fd, EC_IOCTL_SLAVE, &data); |
224 fprintf(stderr, "Failed to get slave info: %s\n", strerror(errno)); |
260 if (EC_IOCTL_IS_ERROR(ret)) { |
225 return -1; |
261 fprintf(stderr, "Failed to get slave info: %s\n", |
|
262 strerror(EC_IOCTL_ERRNO(ret))); |
|
263 return -EC_IOCTL_ERRNO(ret); |
226 } |
264 } |
227 |
265 |
228 slave_info->position = data.position; |
266 slave_info->position = data.position; |
229 slave_info->vendor_id = data.vendor_id; |
267 slave_info->vendor_id = data.vendor_id; |
230 slave_info->product_code = data.product_code; |
268 slave_info->product_code = data.product_code; |
231 slave_info->revision_number = data.revision_number; |
269 slave_info->revision_number = data.revision_number; |
232 slave_info->serial_number = data.serial_number; |
270 slave_info->serial_number = data.serial_number; |
233 slave_info->alias = data.alias; |
271 slave_info->alias = data.alias; |
234 slave_info->current_on_ebus = data.current_on_ebus; |
272 slave_info->current_on_ebus = data.current_on_ebus; |
235 for ( i = 0; i < EC_MAX_PORTS; i++ ) { |
273 for (i = 0; i < EC_MAX_PORTS; i++) { |
236 slave_info->ports[i].desc = data.ports[i].desc; |
274 slave_info->ports[i].desc = data.ports[i].desc; |
237 slave_info->ports[i].link.link_up = data.ports[i].link.link_up; |
275 slave_info->ports[i].link.link_up = data.ports[i].link.link_up; |
238 slave_info->ports[i].link.loop_closed = |
276 slave_info->ports[i].link.loop_closed = |
239 data.ports[i].link.loop_closed; |
277 data.ports[i].link.loop_closed; |
240 slave_info->ports[i].link.signal_detected = |
278 slave_info->ports[i].link.signal_detected = |
241 data.ports[i].link.signal_detected; |
279 data.ports[i].link.signal_detected; |
242 slave_info->ports[i].receive_time = data.ports[i].receive_time; |
280 slave_info->ports[i].receive_time = data.ports[i].receive_time; |
243 slave_info->ports[i].next_slave = data.ports[i].next_slave; |
281 slave_info->ports[i].next_slave = data.ports[i].next_slave; |
244 slave_info->ports[i].delay_to_next_dc = |
282 slave_info->ports[i].delay_to_next_dc = |
245 data.ports[i].delay_to_next_dc; |
283 data.ports[i].delay_to_next_dc; |
246 } |
284 } |
247 slave_info->al_state = data.al_state; |
285 slave_info->al_state = data.al_state; |
248 slave_info->error_flag = data.error_flag; |
286 slave_info->error_flag = data.error_flag; |
249 slave_info->sync_count = data.sync_count; |
287 slave_info->sync_count = data.sync_count; |
250 slave_info->sdo_count = data.sdo_count; |
288 slave_info->sdo_count = data.sdo_count; |
251 strncpy(slave_info->name, data.name, EC_MAX_STRING_LENGTH); |
289 strncpy(slave_info->name, data.name, EC_MAX_STRING_LENGTH); |
252 return 0; |
290 return 0; |
253 } |
291 } |
254 |
292 |
255 /*****************************************************************************/ |
293 /****************************************************************************/ |
256 |
294 |
257 int ecrt_master_get_sync_manager(ec_master_t *master, uint16_t slave_position, |
295 int ecrt_master_get_sync_manager(ec_master_t *master, uint16_t slave_position, |
258 uint8_t sync_index, ec_sync_info_t *sync) |
296 uint8_t sync_index, ec_sync_info_t *sync) |
259 { |
297 { |
260 ec_ioctl_slave_sync_t data; |
298 ec_ioctl_slave_sync_t data; |
261 |
299 int ret; |
262 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
300 |
|
301 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
263 return -ENOENT; |
302 return -ENOENT; |
|
303 } |
264 |
304 |
265 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_t)); |
305 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_t)); |
266 data.slave_position = slave_position; |
306 data.slave_position = slave_position; |
267 data.sync_index = sync_index; |
307 data.sync_index = sync_index; |
268 |
308 |
269 if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC, &data) == -1) { |
309 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC, &data); |
|
310 if (EC_IOCTL_IS_ERROR(ret)) { |
270 fprintf(stderr, "Failed to get sync manager information: %s\n", |
311 fprintf(stderr, "Failed to get sync manager information: %s\n", |
271 strerror(errno)); |
312 strerror(EC_IOCTL_ERRNO(ret))); |
272 return -1; // FIXME |
313 return -EC_IOCTL_ERRNO(ret); |
273 } |
314 } |
274 |
315 |
275 sync->index = sync_index; |
316 sync->index = sync_index; |
276 sync->dir = EC_READ_BIT(&data.control_register, 2) ? |
317 sync->dir = EC_READ_BIT(&data.control_register, 2) ? |
277 EC_DIR_OUTPUT : EC_DIR_INPUT; |
318 EC_DIR_OUTPUT : EC_DIR_INPUT; |
281 EC_WD_ENABLE : EC_WD_DISABLE; |
322 EC_WD_ENABLE : EC_WD_DISABLE; |
282 |
323 |
283 return 0; |
324 return 0; |
284 } |
325 } |
285 |
326 |
286 /*****************************************************************************/ |
327 /****************************************************************************/ |
287 |
328 |
288 int ecrt_master_get_pdo(ec_master_t *master, uint16_t slave_position, |
329 int ecrt_master_get_pdo(ec_master_t *master, uint16_t slave_position, |
289 uint8_t sync_index, uint16_t pos, ec_pdo_info_t *pdo) |
330 uint8_t sync_index, uint16_t pos, ec_pdo_info_t *pdo) |
290 { |
331 { |
291 ec_ioctl_slave_sync_pdo_t data; |
332 ec_ioctl_slave_sync_pdo_t data; |
|
333 int ret; |
292 |
334 |
293 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
335 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
294 return -ENOENT; |
336 return -ENOENT; |
295 |
337 |
296 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_t)); |
338 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_t)); |
297 data.slave_position = slave_position; |
339 data.slave_position = slave_position; |
298 data.sync_index = sync_index; |
340 data.sync_index = sync_index; |
299 data.pdo_pos = pos; |
341 data.pdo_pos = pos; |
300 |
342 |
301 if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO, &data) == -1) { |
343 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO, &data); |
|
344 if (EC_IOCTL_IS_ERROR(ret)) { |
302 fprintf(stderr, "Failed to get pdo information: %s\n", |
345 fprintf(stderr, "Failed to get pdo information: %s\n", |
303 strerror(errno)); |
346 strerror(EC_IOCTL_ERRNO(ret))); |
304 return -1; // FIXME |
347 return -EC_IOCTL_ERRNO(ret); |
305 } |
348 } |
306 |
349 |
307 pdo->index = data.index; |
350 pdo->index = data.index; |
308 pdo->n_entries = data.entry_count; |
351 pdo->n_entries = data.entry_count; |
309 pdo->entries = NULL; |
352 pdo->entries = NULL; |
310 |
353 |
311 return 0; |
354 return 0; |
312 } |
355 } |
313 |
356 |
314 /*****************************************************************************/ |
357 /****************************************************************************/ |
315 |
358 |
316 int ecrt_master_get_pdo_entry(ec_master_t *master, uint16_t slave_position, |
359 int ecrt_master_get_pdo_entry(ec_master_t *master, uint16_t slave_position, |
317 uint8_t sync_index, uint16_t pdo_pos, uint16_t entry_pos, |
360 uint8_t sync_index, uint16_t pdo_pos, uint16_t entry_pos, |
318 ec_pdo_entry_info_t *entry) |
361 ec_pdo_entry_info_t *entry) |
319 { |
362 { |
320 ec_ioctl_slave_sync_pdo_entry_t data; |
363 ec_ioctl_slave_sync_pdo_entry_t data; |
|
364 int ret; |
321 |
365 |
322 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
366 if (sync_index >= EC_MAX_SYNC_MANAGERS) |
323 return -ENOENT; |
367 return -ENOENT; |
324 |
368 |
325 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_entry_t)); |
369 memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_entry_t)); |
326 data.slave_position = slave_position; |
370 data.slave_position = slave_position; |
327 data.sync_index = sync_index; |
371 data.sync_index = sync_index; |
328 data.pdo_pos = pdo_pos; |
372 data.pdo_pos = pdo_pos; |
329 data.entry_pos = entry_pos; |
373 data.entry_pos = entry_pos; |
330 |
374 |
331 if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, &data) == -1) { |
375 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, &data); |
|
376 if (EC_IOCTL_IS_ERROR(ret)) { |
332 fprintf(stderr, "Failed to get pdo entry information: %s\n", |
377 fprintf(stderr, "Failed to get pdo entry information: %s\n", |
333 strerror(errno)); |
378 strerror(EC_IOCTL_ERRNO(ret))); |
334 return -1; // FIXME |
379 return -EC_IOCTL_ERRNO(ret); |
335 } |
380 } |
336 |
381 |
337 entry->index = data.index; |
382 entry->index = data.index; |
338 entry->subindex = data.subindex; |
383 entry->subindex = data.subindex; |
339 entry->bit_length = data.bit_length; |
384 entry->bit_length = data.bit_length; |
340 |
385 |
341 return 0; |
386 return 0; |
342 } |
387 } |
343 |
388 |
344 /*****************************************************************************/ |
389 /****************************************************************************/ |
345 |
390 |
346 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, |
391 int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position, |
347 uint16_t index, uint8_t subindex, uint8_t *data, |
392 uint16_t index, uint8_t subindex, uint8_t *data, |
348 size_t data_size, uint32_t *abort_code) |
393 size_t data_size, uint32_t *abort_code) |
349 { |
394 { |
350 ec_ioctl_slave_sdo_download_t download; |
395 ec_ioctl_slave_sdo_download_t download; |
|
396 int ret; |
351 |
397 |
352 download.slave_position = slave_position; |
398 download.slave_position = slave_position; |
353 download.sdo_index = index; |
399 download.sdo_index = index; |
354 download.sdo_entry_subindex = subindex; |
400 download.sdo_entry_subindex = subindex; |
355 download.complete_access = 0; |
401 download.complete_access = 0; |
356 download.data_size = data_size; |
402 download.data_size = data_size; |
357 download.data = data; |
403 download.data = data; |
358 |
404 |
359 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download) == -1) { |
405 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download); |
360 if (errno == EIO && abort_code) { |
406 if (EC_IOCTL_IS_ERROR(ret)) { |
|
407 if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) { |
361 *abort_code = download.abort_code; |
408 *abort_code = download.abort_code; |
362 } |
409 } |
363 fprintf(stderr, "Failed to execute SDO download: %s\n", |
410 fprintf(stderr, "Failed to execute SDO download: %s\n", |
364 strerror(errno)); |
411 strerror(EC_IOCTL_ERRNO(ret))); |
365 return -1; |
412 return -EC_IOCTL_ERRNO(ret); |
366 } |
413 } |
367 |
414 |
368 return 0; |
415 return 0; |
369 } |
416 } |
370 |
417 |
371 /*****************************************************************************/ |
418 /****************************************************************************/ |
372 |
419 |
373 int ecrt_master_sdo_download_complete(ec_master_t *master, |
420 int ecrt_master_sdo_download_complete(ec_master_t *master, |
374 uint16_t slave_position, uint16_t index, uint8_t *data, |
421 uint16_t slave_position, uint16_t index, uint8_t *data, |
375 size_t data_size, uint32_t *abort_code) |
422 size_t data_size, uint32_t *abort_code) |
376 { |
423 { |
377 ec_ioctl_slave_sdo_download_t download; |
424 ec_ioctl_slave_sdo_download_t download; |
|
425 int ret; |
378 |
426 |
379 download.slave_position = slave_position; |
427 download.slave_position = slave_position; |
380 download.sdo_index = index; |
428 download.sdo_index = index; |
381 download.sdo_entry_subindex = 0; |
429 download.sdo_entry_subindex = 0; |
382 download.complete_access = 1; |
430 download.complete_access = 1; |
383 download.data_size = data_size; |
431 download.data_size = data_size; |
384 download.data = data; |
432 download.data = data; |
385 |
433 |
386 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download) == -1) { |
434 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download); |
387 if (errno == EIO && abort_code) { |
435 if (EC_IOCTL_IS_ERROR(ret)) { |
|
436 if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) { |
388 *abort_code = download.abort_code; |
437 *abort_code = download.abort_code; |
389 } |
438 } |
390 fprintf(stderr, "Failed to execute SDO download: %s\n", |
439 fprintf(stderr, "Failed to execute SDO download: %s\n", |
391 strerror(errno)); |
440 strerror(EC_IOCTL_ERRNO(ret))); |
392 return -1; |
441 return -EC_IOCTL_ERRNO(ret); |
393 } |
442 } |
394 |
443 |
395 return 0; |
444 return 0; |
396 } |
445 } |
397 |
446 |
398 /*****************************************************************************/ |
447 /****************************************************************************/ |
399 |
448 |
400 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, |
449 int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position, |
401 uint16_t index, uint8_t subindex, uint8_t *target, |
450 uint16_t index, uint8_t subindex, uint8_t *target, |
402 size_t target_size, size_t *result_size, uint32_t *abort_code) |
451 size_t target_size, size_t *result_size, uint32_t *abort_code) |
403 { |
452 { |
404 ec_ioctl_slave_sdo_upload_t upload; |
453 ec_ioctl_slave_sdo_upload_t upload; |
|
454 int ret; |
405 |
455 |
406 upload.slave_position = slave_position; |
456 upload.slave_position = slave_position; |
407 upload.sdo_index = index; |
457 upload.sdo_index = index; |
408 upload.sdo_entry_subindex = subindex; |
458 upload.sdo_entry_subindex = subindex; |
409 upload.target_size = target_size; |
459 upload.target_size = target_size; |
410 upload.target = target; |
460 upload.target = target; |
411 |
461 |
412 if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &upload) == -1) { |
462 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &upload); |
413 if (errno == EIO && abort_code) { |
463 if (EC_IOCTL_IS_ERROR(ret)) { |
|
464 if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) { |
414 *abort_code = upload.abort_code; |
465 *abort_code = upload.abort_code; |
415 } |
466 } |
416 fprintf(stderr, "Failed to execute SDO upload: %s\n", |
467 fprintf(stderr, "Failed to execute SDO upload: %s\n", |
417 strerror(errno)); |
468 strerror(EC_IOCTL_ERRNO(ret))); |
418 return -1; |
469 return -EC_IOCTL_ERRNO(ret); |
419 } |
470 } |
420 |
471 |
421 *result_size = upload.data_size; |
472 *result_size = upload.data_size; |
422 return 0; |
473 return 0; |
423 } |
474 } |
424 |
475 |
425 /*****************************************************************************/ |
476 /****************************************************************************/ |
426 |
477 |
427 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, |
478 int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position, |
428 uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size, |
479 uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size, |
429 uint16_t *error_code) |
480 uint16_t *error_code) |
430 { |
481 { |
431 ec_ioctl_slave_soe_write_t io; |
482 ec_ioctl_slave_soe_write_t io; |
|
483 int ret; |
432 |
484 |
433 io.slave_position = slave_position; |
485 io.slave_position = slave_position; |
434 io.drive_no = drive_no; |
486 io.drive_no = drive_no; |
435 io.idn = idn; |
487 io.idn = idn; |
436 io.data_size = data_size; |
488 io.data_size = data_size; |
437 io.data = data; |
489 io.data = data; |
438 |
490 |
439 if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_WRITE, &io) == -1) { |
491 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SOE_WRITE, &io); |
440 if (errno == EIO && error_code) { |
492 if (EC_IOCTL_IS_ERROR(ret)) { |
|
493 if (EC_IOCTL_ERRNO(ret) == EIO && error_code) { |
441 *error_code = io.error_code; |
494 *error_code = io.error_code; |
442 } |
495 } |
443 fprintf(stderr, "Failed to write IDN: %s\n", strerror(errno)); |
496 fprintf(stderr, "Failed to write IDN: %s\n", |
444 return -1; |
497 strerror(EC_IOCTL_ERRNO(ret))); |
445 } |
498 return -EC_IOCTL_ERRNO(ret); |
446 |
499 } |
447 return 0; |
500 |
448 } |
501 return 0; |
449 |
502 } |
450 /*****************************************************************************/ |
503 |
|
504 /****************************************************************************/ |
451 |
505 |
452 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, |
506 int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position, |
453 uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size, |
507 uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size, |
454 size_t *result_size, uint16_t *error_code) |
508 size_t *result_size, uint16_t *error_code) |
455 { |
509 { |
456 ec_ioctl_slave_soe_read_t io; |
510 ec_ioctl_slave_soe_read_t io; |
|
511 int ret; |
457 |
512 |
458 io.slave_position = slave_position; |
513 io.slave_position = slave_position; |
459 io.drive_no = drive_no; |
514 io.drive_no = drive_no; |
460 io.idn = idn; |
515 io.idn = idn; |
461 io.mem_size = target_size; |
516 io.mem_size = target_size; |
462 io.data = target; |
517 io.data = target; |
463 |
518 |
464 if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_READ, &io) == -1) { |
519 ret = ioctl(master->fd, EC_IOCTL_SLAVE_SOE_READ, &io); |
465 if (errno == EIO && error_code) { |
520 if (EC_IOCTL_IS_ERROR(ret)) { |
|
521 if (EC_IOCTL_ERRNO(ret) == EIO && error_code) { |
466 *error_code = io.error_code; |
522 *error_code = io.error_code; |
467 } |
523 } |
468 fprintf(stderr, "Failed to read IDN: %s\n", strerror(errno)); |
524 fprintf(stderr, "Failed to read IDN: %s\n", |
469 return -1; |
525 strerror(EC_IOCTL_ERRNO(ret))); |
|
526 return -EC_IOCTL_ERRNO(ret); |
470 } |
527 } |
471 |
528 |
472 *result_size = io.data_size; |
529 *result_size = io.data_size; |
473 return 0; |
530 return 0; |
474 } |
531 } |
475 |
532 |
476 /*****************************************************************************/ |
533 /****************************************************************************/ |
477 |
534 |
478 int ecrt_master_activate(ec_master_t *master) |
535 int ecrt_master_activate(ec_master_t *master) |
479 { |
536 { |
480 if (ioctl(master->fd, EC_IOCTL_ACTIVATE, |
537 ec_ioctl_master_activate_t io; |
481 &master->process_data_size) == -1) { |
538 int ret; |
|
539 |
|
540 ret = ioctl(master->fd, EC_IOCTL_ACTIVATE, &io); |
|
541 if (EC_IOCTL_IS_ERROR(ret)) { |
482 fprintf(stderr, "Failed to activate master: %s\n", |
542 fprintf(stderr, "Failed to activate master: %s\n", |
483 strerror(errno)); |
543 strerror(EC_IOCTL_ERRNO(ret))); |
484 return -1; // FIXME |
544 return -EC_IOCTL_ERRNO(ret); |
485 } |
545 } |
|
546 |
|
547 master->process_data_size = io.process_data_size; |
486 |
548 |
487 if (master->process_data_size) { |
549 if (master->process_data_size) { |
|
550 #ifdef USE_RTDM |
|
551 /* memory-mapping was already done in kernel. The user-space addess is |
|
552 * provided in the ioctl data. |
|
553 */ |
|
554 master->process_data = io.process_data; |
|
555 #else |
488 master->process_data = mmap(0, master->process_data_size, |
556 master->process_data = mmap(0, master->process_data_size, |
489 PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0); |
557 PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0); |
490 if (master->process_data == MAP_FAILED) { |
558 if (master->process_data == MAP_FAILED) { |
491 fprintf(stderr, "Failed to map process data: %s", strerror(errno)); |
559 fprintf(stderr, "Failed to map process data: %s\n", |
|
560 strerror(errno)); |
492 master->process_data = NULL; |
561 master->process_data = NULL; |
493 master->process_data_size = 0; |
562 master->process_data_size = 0; |
494 return -1; // FIXME |
563 return -errno; |
495 } |
564 } |
|
565 #endif |
496 |
566 |
497 // Access the mapped region to cause the initial page fault |
567 // Access the mapped region to cause the initial page fault |
498 memset(master->process_data, 0, master->process_data_size); |
568 master->process_data[0] = 0x00; |
499 } |
569 } |
500 |
570 |
501 return 0; |
571 return 0; |
|
572 } |
|
573 |
|
574 /****************************************************************************/ |
|
575 |
|
576 void ecrt_master_deactivate(ec_master_t *master) |
|
577 { |
|
578 int ret; |
|
579 |
|
580 ret = ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL); |
|
581 if (EC_IOCTL_IS_ERROR(ret)) { |
|
582 fprintf(stderr, "Failed to deactivate master: %s\n", |
|
583 strerror(EC_IOCTL_ERRNO(ret))); |
|
584 return; |
|
585 } |
|
586 |
|
587 ec_master_clear_config(master); |
|
588 } |
|
589 |
|
590 /****************************************************************************/ |
|
591 |
|
592 int ecrt_master_set_send_interval(ec_master_t *master, |
|
593 size_t send_interval_us) |
|
594 { |
|
595 int ret; |
|
596 |
|
597 ret = ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL, &send_interval_us); |
|
598 if (EC_IOCTL_IS_ERROR(ret)) { |
|
599 fprintf(stderr, "Failed to set send interval: %s\n", |
|
600 strerror(EC_IOCTL_ERRNO(ret))); |
|
601 return -EC_IOCTL_ERRNO(ret); |
|
602 } |
|
603 |
|
604 return 0; |
|
605 } |
|
606 |
|
607 /****************************************************************************/ |
|
608 |
|
609 void ecrt_master_send(ec_master_t *master) |
|
610 { |
|
611 int ret; |
|
612 |
|
613 ret = ioctl(master->fd, EC_IOCTL_SEND, NULL); |
|
614 if (EC_IOCTL_IS_ERROR(ret)) { |
|
615 fprintf(stderr, "Failed to send: %s\n", |
|
616 strerror(EC_IOCTL_ERRNO(ret))); |
|
617 } |
|
618 } |
|
619 |
|
620 /****************************************************************************/ |
|
621 |
|
622 void ecrt_master_receive(ec_master_t *master) |
|
623 { |
|
624 int ret; |
|
625 |
|
626 ret = ioctl(master->fd, EC_IOCTL_RECEIVE, NULL); |
|
627 if (EC_IOCTL_IS_ERROR(ret)) { |
|
628 fprintf(stderr, "Failed to receive: %s\n", |
|
629 strerror(EC_IOCTL_ERRNO(ret))); |
|
630 } |
|
631 } |
|
632 |
|
633 /****************************************************************************/ |
|
634 |
|
635 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state) |
|
636 { |
|
637 int ret; |
|
638 |
|
639 ret = ioctl(master->fd, EC_IOCTL_MASTER_STATE, state); |
|
640 if (EC_IOCTL_IS_ERROR(ret)) { |
|
641 fprintf(stderr, "Failed to get master state: %s\n", |
|
642 strerror(EC_IOCTL_ERRNO(ret))); |
|
643 } |
|
644 } |
|
645 |
|
646 /****************************************************************************/ |
|
647 |
|
648 int ecrt_master_link_state(const ec_master_t *master, unsigned int dev_idx, |
|
649 ec_master_link_state_t *state) |
|
650 { |
|
651 ec_ioctl_link_state_t io; |
|
652 int ret; |
|
653 |
|
654 io.dev_idx = dev_idx; |
|
655 io.state = state; |
|
656 |
|
657 ret = ioctl(master->fd, EC_IOCTL_MASTER_LINK_STATE, &io); |
|
658 if (EC_IOCTL_IS_ERROR(ret)) { |
|
659 fprintf(stderr, "Failed to get link state: %s\n", |
|
660 strerror(EC_IOCTL_ERRNO(ret))); |
|
661 return -EC_IOCTL_ERRNO(ret); |
|
662 } |
|
663 |
|
664 return 0; |
|
665 } |
|
666 |
|
667 /****************************************************************************/ |
|
668 |
|
669 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time) |
|
670 { |
|
671 ec_ioctl_app_time_t data; |
|
672 int ret; |
|
673 |
|
674 data.app_time = app_time; |
|
675 |
|
676 ret = ioctl(master->fd, EC_IOCTL_APP_TIME, &data); |
|
677 if (EC_IOCTL_IS_ERROR(ret)) { |
|
678 fprintf(stderr, "Failed to set application time: %s\n", |
|
679 strerror(EC_IOCTL_ERRNO(ret))); |
|
680 } |
|
681 } |
|
682 |
|
683 /****************************************************************************/ |
|
684 |
|
685 void ecrt_master_sync_reference_clock(ec_master_t *master) |
|
686 { |
|
687 int ret; |
|
688 |
|
689 ret = ioctl(master->fd, EC_IOCTL_SYNC_REF, NULL); |
|
690 if (EC_IOCTL_IS_ERROR(ret)) { |
|
691 fprintf(stderr, "Failed to sync reference clock: %s\n", |
|
692 strerror(EC_IOCTL_ERRNO(ret))); |
|
693 } |
|
694 } |
|
695 |
|
696 /****************************************************************************/ |
|
697 |
|
698 void ecrt_master_sync_slave_clocks(ec_master_t *master) |
|
699 { |
|
700 int ret; |
|
701 |
|
702 ret = ioctl(master->fd, EC_IOCTL_SYNC_SLAVES, NULL); |
|
703 if (EC_IOCTL_IS_ERROR(ret)) { |
|
704 fprintf(stderr, "Failed to sync slave clocks: %s\n", |
|
705 strerror(EC_IOCTL_ERRNO(ret))); |
|
706 } |
502 } |
707 } |
503 |
708 |
504 /*****************************************************************************/ |
709 /*****************************************************************************/ |
505 |
710 |
506 void ecrt_master_deactivate(ec_master_t *master) |
711 int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) |
507 { |
712 { |
508 if (ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL) == -1) { |
713 int ret; |
509 fprintf(stderr, "Failed to deactivate master: %s\n", strerror(errno)); |
714 |
510 return; |
715 ret = ioctl(master->fd, EC_IOCTL_REF_CLOCK_TIME, time); |
511 } |
716 if (EC_IOCTL_IS_ERROR(ret)) { |
512 |
717 fprintf(stderr, "Failed to get reference clock time: %s\n", |
513 ec_master_clear_config(master); |
718 strerror(EC_IOCTL_ERRNO(ret))); |
514 } |
719 } |
515 |
720 |
516 /*****************************************************************************/ |
721 return ret; |
517 |
722 } |
518 int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us) |
723 |
519 { |
724 /****************************************************************************/ |
520 if (ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL, |
|
521 &send_interval_us) == -1) { |
|
522 fprintf(stderr, "Failed to set send interval: %s\n", |
|
523 strerror(errno)); |
|
524 return -1; // FIXME |
|
525 } |
|
526 return 0; |
|
527 } |
|
528 |
|
529 /*****************************************************************************/ |
|
530 |
|
531 void ecrt_master_send(ec_master_t *master) |
|
532 { |
|
533 if (ioctl(master->fd, EC_IOCTL_SEND, NULL) == -1) { |
|
534 fprintf(stderr, "Failed to send: %s\n", strerror(errno)); |
|
535 } |
|
536 } |
|
537 |
|
538 /*****************************************************************************/ |
|
539 |
|
540 void ecrt_master_receive(ec_master_t *master) |
|
541 { |
|
542 if (ioctl(master->fd, EC_IOCTL_RECEIVE, NULL) == -1) { |
|
543 fprintf(stderr, "Failed to receive: %s\n", strerror(errno)); |
|
544 } |
|
545 } |
|
546 |
|
547 /*****************************************************************************/ |
|
548 |
|
549 void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state) |
|
550 { |
|
551 if (ioctl(master->fd, EC_IOCTL_MASTER_STATE, state) == -1) { |
|
552 fprintf(stderr, "Failed to get master state: %s\n", strerror(errno)); |
|
553 } |
|
554 } |
|
555 |
|
556 /*****************************************************************************/ |
|
557 |
|
558 void ecrt_master_configured_slaves_state(const ec_master_t *master, |
|
559 ec_master_state_t *state) |
|
560 { |
|
561 if (ioctl(master->fd, EC_IOCTL_MASTER_SC_STATE, state) == -1) { |
|
562 fprintf(stderr, "Failed to get master state: %s\n", strerror(errno)); |
|
563 } |
|
564 } |
|
565 |
|
566 /*****************************************************************************/ |
|
567 |
|
568 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time) |
|
569 { |
|
570 ec_ioctl_app_time_t data; |
|
571 |
|
572 data.app_time = app_time; |
|
573 |
|
574 if (ioctl(master->fd, EC_IOCTL_APP_TIME, &data) == -1) { |
|
575 fprintf(stderr, "Failed to set application time: %s\n", |
|
576 strerror(errno)); |
|
577 } |
|
578 } |
|
579 |
|
580 /*****************************************************************************/ |
|
581 |
|
582 void ecrt_master_sync_reference_clock(ec_master_t *master) |
|
583 { |
|
584 if (ioctl(master->fd, EC_IOCTL_SYNC_REF, NULL) == -1) { |
|
585 fprintf(stderr, "Failed to sync reference clock: %s\n", |
|
586 strerror(errno)); |
|
587 } |
|
588 } |
|
589 |
|
590 /*****************************************************************************/ |
|
591 |
|
592 void ecrt_master_sync_slave_clocks(ec_master_t *master) |
|
593 { |
|
594 if (ioctl(master->fd, EC_IOCTL_SYNC_SLAVES, NULL) == -1) { |
|
595 fprintf(stderr, "Failed to sync slave clocks: %s\n", strerror(errno)); |
|
596 } |
|
597 } |
|
598 |
|
599 /*****************************************************************************/ |
|
600 |
725 |
601 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
726 void ecrt_master_sync_monitor_queue(ec_master_t *master) |
602 { |
727 { |
603 if (ioctl(master->fd, EC_IOCTL_SYNC_MON_QUEUE, NULL) == -1) { |
728 int ret; |
|
729 |
|
730 ret = ioctl(master->fd, EC_IOCTL_SYNC_MON_QUEUE, NULL); |
|
731 if (EC_IOCTL_IS_ERROR(ret)) { |
604 fprintf(stderr, "Failed to queue sync monitor datagram: %s\n", |
732 fprintf(stderr, "Failed to queue sync monitor datagram: %s\n", |
605 strerror(errno)); |
733 strerror(EC_IOCTL_ERRNO(ret))); |
606 } |
734 } |
607 } |
735 } |
608 |
736 |
609 /*****************************************************************************/ |
737 /****************************************************************************/ |
610 |
738 |
611 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |
739 uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) |
612 { |
740 { |
613 uint32_t time_diff; |
741 uint32_t time_diff; |
614 |
742 int ret; |
615 if (ioctl(master->fd, EC_IOCTL_SYNC_MON_PROCESS, &time_diff) == -1) { |
743 |
|
744 ret = ioctl(master->fd, EC_IOCTL_SYNC_MON_PROCESS, &time_diff); |
|
745 if (EC_IOCTL_IS_ERROR(ret)) { |
616 time_diff = 0xffffffff; |
746 time_diff = 0xffffffff; |
617 fprintf(stderr, "Failed to process sync monitor datagram: %s\n", |
747 fprintf(stderr, "Failed to process sync monitor datagram: %s\n", |
618 strerror(errno)); |
748 strerror(EC_IOCTL_ERRNO(ret))); |
619 } |
749 } |
620 |
750 |
621 return time_diff; |
751 return time_diff; |
622 } |
752 } |
623 |
753 |
624 /*****************************************************************************/ |
754 /****************************************************************************/ |
625 |
755 |
626 void ecrt_master_reset(ec_master_t *master) |
756 void ecrt_master_reset(ec_master_t *master) |
627 { |
757 { |
628 if (ioctl(master->fd, EC_IOCTL_RESET, NULL) == -1) { |
758 int ret; |
629 fprintf(stderr, "Failed to reset master: %s\n", strerror(errno)); |
759 |
630 } |
760 ret = ioctl(master->fd, EC_IOCTL_RESET, NULL); |
631 } |
761 if (EC_IOCTL_IS_ERROR(ret)) { |
632 |
762 fprintf(stderr, "Failed to reset master: %s\n", |
633 /*****************************************************************************/ |
763 strerror(EC_IOCTL_ERRNO(ret))); |
|
764 } |
|
765 } |
|
766 |
|
767 /****************************************************************************/ |