41 #include "master.h" |
41 #include "master.h" |
42 #include "master/ioctl.h" |
42 #include "master/ioctl.h" |
43 |
43 |
44 /*****************************************************************************/ |
44 /*****************************************************************************/ |
45 |
45 |
|
46 void ec_slave_config_clear(ec_slave_config_t *sc) |
|
47 { |
|
48 ec_sdo_request_t *r, *next_r; |
|
49 ec_voe_handler_t *v, *next_v; |
|
50 |
|
51 r = sc->first_sdo_request; |
|
52 while (r) { |
|
53 next_r = r->next; |
|
54 ec_sdo_request_clear(r); |
|
55 r = next_r; |
|
56 } |
|
57 |
|
58 |
|
59 v = sc->first_voe_handler; |
|
60 while (v) { |
|
61 next_v = v->next; |
|
62 ec_voe_handler_clear(v); |
|
63 v = next_v; |
|
64 } |
|
65 } |
|
66 |
|
67 /*****************************************************************************/ |
|
68 |
46 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
69 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
47 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
70 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
48 { |
71 { |
49 ec_ioctl_config_t data; |
72 ec_ioctl_config_t data; |
50 unsigned int i; |
73 unsigned int i; |
376 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
399 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
377 } |
400 } |
378 |
401 |
379 /*****************************************************************************/ |
402 /*****************************************************************************/ |
380 |
403 |
|
404 void ec_slave_config_add_sdo_request(ec_slave_config_t *sc, |
|
405 ec_sdo_request_t *req) |
|
406 { |
|
407 if (sc->first_sdo_request) { |
|
408 ec_sdo_request_t *r = sc->first_sdo_request; |
|
409 while (r->next) { |
|
410 r = r->next; |
|
411 } |
|
412 r->next = req; |
|
413 } else { |
|
414 sc->first_sdo_request = req; |
|
415 } |
|
416 } |
|
417 |
|
418 /*****************************************************************************/ |
|
419 |
381 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, |
420 ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc, |
382 uint16_t index, uint8_t subindex, size_t size) |
421 uint16_t index, uint8_t subindex, size_t size) |
383 { |
422 { |
384 ec_ioctl_sdo_request_t data; |
423 ec_ioctl_sdo_request_t data; |
385 ec_sdo_request_t *req; |
424 ec_sdo_request_t *req; |
408 data.size = size; |
447 data.size = size; |
409 |
448 |
410 if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data) == -1) { |
449 if (ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data) == -1) { |
411 fprintf(stderr, "Failed to create SDO request: %s\n", |
450 fprintf(stderr, "Failed to create SDO request: %s\n", |
412 strerror(errno)); |
451 strerror(errno)); |
413 if (req->data) |
452 ec_sdo_request_clear(req); |
414 free(req->data); |
|
415 free(req); |
453 free(req); |
416 return NULL; |
454 return NULL; |
417 } |
455 } |
418 |
456 |
|
457 req->next = NULL; |
419 req->config = sc; |
458 req->config = sc; |
420 req->index = data.request_index; |
459 req->index = data.request_index; |
421 req->sdo_index = data.sdo_index; |
460 req->sdo_index = data.sdo_index; |
422 req->sdo_subindex = data.sdo_subindex; |
461 req->sdo_subindex = data.sdo_subindex; |
423 req->data_size = size; |
462 req->data_size = size; |
424 req->mem_size = size; |
463 req->mem_size = size; |
|
464 |
|
465 ec_slave_config_add_sdo_request(sc, req); |
|
466 |
425 return req; |
467 return req; |
|
468 } |
|
469 |
|
470 /*****************************************************************************/ |
|
471 |
|
472 void ec_slave_config_add_voe_handler(ec_slave_config_t *sc, |
|
473 ec_voe_handler_t *voe) |
|
474 { |
|
475 if (sc->first_voe_handler) { |
|
476 ec_voe_handler_t *v = sc->first_voe_handler; |
|
477 while (v->next) { |
|
478 v = v->next; |
|
479 } |
|
480 v->next = voe; |
|
481 } else { |
|
482 sc->first_voe_handler = voe; |
|
483 } |
426 } |
484 } |
427 |
485 |
428 /*****************************************************************************/ |
486 /*****************************************************************************/ |
429 |
487 |
430 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc, |
488 ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc, |
456 data.size = size; |
514 data.size = size; |
457 |
515 |
458 if (ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data) == -1) { |
516 if (ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data) == -1) { |
459 fprintf(stderr, "Failed to create VoE handler: %s\n", |
517 fprintf(stderr, "Failed to create VoE handler: %s\n", |
460 strerror(errno)); |
518 strerror(errno)); |
461 if (voe->data) |
519 ec_voe_handler_clear(voe); |
462 free(voe->data); |
|
463 free(voe); |
520 free(voe); |
464 return NULL; |
521 return NULL; |
465 } |
522 } |
466 |
523 |
|
524 voe->next = NULL; |
467 voe->config = sc; |
525 voe->config = sc; |
468 voe->index = data.voe_index; |
526 voe->index = data.voe_index; |
469 voe->data_size = size; |
527 voe->data_size = size; |
470 voe->mem_size = size; |
528 voe->mem_size = size; |
|
529 |
|
530 ec_slave_config_add_voe_handler(sc, voe); |
|
531 |
471 return voe; |
532 return voe; |
472 } |
533 } |
473 |
534 |
474 /*****************************************************************************/ |
535 /*****************************************************************************/ |
475 |
536 |
487 } |
548 } |
488 } |
549 } |
489 |
550 |
490 /*****************************************************************************/ |
551 /*****************************************************************************/ |
491 |
552 |
492 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn, |
553 int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no, |
493 const uint8_t *data, size_t size) |
554 uint16_t idn, ec_al_state_t al_state, const uint8_t *data, size_t size) |
494 { |
555 { |
495 ec_ioctl_sc_idn_t io; |
556 ec_ioctl_sc_idn_t io; |
496 |
557 |
497 io.config_index = sc->index; |
558 io.config_index = sc->index; |
|
559 io.drive_no = drive_no; |
498 io.idn = idn; |
560 io.idn = idn; |
|
561 io.al_state = al_state; |
499 io.data = data; |
562 io.data = data; |
500 io.size = size; |
563 io.size = size; |
501 |
564 |
502 if (ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io) == -1) { |
565 if (ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io) == -1) { |
503 fprintf(stderr, "Failed to configure IDN.\n"); |
566 fprintf(stderr, "Failed to configure IDN.\n"); |