35 |
35 |
36 #include "ioctl.h" |
36 #include "ioctl.h" |
37 #include "slave_config.h" |
37 #include "slave_config.h" |
38 #include "domain.h" |
38 #include "domain.h" |
39 #include "sdo_request.h" |
39 #include "sdo_request.h" |
|
40 #include "reg_request.h" |
40 #include "voe_handler.h" |
41 #include "voe_handler.h" |
41 #include "master.h" |
42 #include "master.h" |
42 |
43 |
43 /*****************************************************************************/ |
44 /*****************************************************************************/ |
44 |
45 |
45 void ec_slave_config_clear(ec_slave_config_t *sc) |
46 void ec_slave_config_clear(ec_slave_config_t *sc) |
46 { |
47 { |
47 ec_sdo_request_t *r, *next_r; |
48 ec_sdo_request_t *r, *next_r; |
|
49 ec_reg_request_t *e, *next_e; |
48 ec_voe_handler_t *v, *next_v; |
50 ec_voe_handler_t *v, *next_v; |
49 |
51 |
50 r = sc->first_sdo_request; |
52 r = sc->first_sdo_request; |
51 while (r) { |
53 while (r) { |
52 next_r = r->next; |
54 next_r = r->next; |
53 ec_sdo_request_clear(r); |
55 ec_sdo_request_clear(r); |
54 r = next_r; |
56 r = next_r; |
55 } |
57 } |
56 |
58 |
|
59 e = sc->first_reg_request; |
|
60 while (e) { |
|
61 next_e = e->next; |
|
62 ec_reg_request_clear(e); |
|
63 e = next_e; |
|
64 } |
57 |
65 |
58 v = sc->first_voe_handler; |
66 v = sc->first_voe_handler; |
59 while (v) { |
67 while (v) { |
60 next_v = v->next; |
68 next_v = v->next; |
61 ec_voe_handler_clear(v); |
69 ec_voe_handler_clear(v); |
549 return req; |
557 return req; |
550 } |
558 } |
551 |
559 |
552 /*****************************************************************************/ |
560 /*****************************************************************************/ |
553 |
561 |
|
562 void ec_slave_config_add_reg_request(ec_slave_config_t *sc, |
|
563 ec_reg_request_t *reg) |
|
564 { |
|
565 if (sc->first_reg_request) { |
|
566 ec_reg_request_t *r = sc->first_reg_request; |
|
567 while (r->next) { |
|
568 r = r->next; |
|
569 } |
|
570 r->next = reg; |
|
571 } else { |
|
572 sc->first_reg_request = reg; |
|
573 } |
|
574 } |
|
575 |
|
576 /*****************************************************************************/ |
|
577 |
|
578 ec_reg_request_t *ecrt_slave_config_create_reg_request(ec_slave_config_t *sc, |
|
579 size_t size) |
|
580 { |
|
581 ec_ioctl_reg_request_t io; |
|
582 ec_reg_request_t *reg; |
|
583 int ret; |
|
584 |
|
585 reg = malloc(sizeof(ec_reg_request_t)); |
|
586 if (!reg) { |
|
587 fprintf(stderr, "Failed to allocate memory.\n"); |
|
588 return NULL; |
|
589 } |
|
590 |
|
591 if (size) { |
|
592 reg->data = malloc(size); |
|
593 if (!reg->data) { |
|
594 fprintf(stderr, "Failed to allocate %u bytes of register data" |
|
595 " memory.\n", size); |
|
596 free(reg); |
|
597 return 0; |
|
598 } |
|
599 } else { |
|
600 reg->data = NULL; |
|
601 } |
|
602 |
|
603 io.config_index = sc->index; |
|
604 io.mem_size = size; |
|
605 |
|
606 ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_REQUEST, &io); |
|
607 if (EC_IOCTL_IS_ERROR(ret)) { |
|
608 fprintf(stderr, "Failed to create register request: %s\n", |
|
609 strerror(EC_IOCTL_ERRNO(ret))); |
|
610 ec_reg_request_clear(reg); |
|
611 free(reg); |
|
612 return NULL; |
|
613 } |
|
614 |
|
615 reg->next = NULL; |
|
616 reg->config = sc; |
|
617 reg->index = io.request_index; |
|
618 reg->mem_size = size; |
|
619 |
|
620 ec_slave_config_add_reg_request(sc, reg); |
|
621 |
|
622 return reg; |
|
623 } |
|
624 |
|
625 /*****************************************************************************/ |
|
626 |
554 void ec_slave_config_add_voe_handler(ec_slave_config_t *sc, |
627 void ec_slave_config_add_voe_handler(ec_slave_config_t *sc, |
555 ec_voe_handler_t *voe) |
628 ec_voe_handler_t *voe) |
556 { |
629 { |
557 if (sc->first_voe_handler) { |
630 if (sc->first_voe_handler) { |
558 ec_voe_handler_t *v = sc->first_voe_handler; |
631 ec_voe_handler_t *v = sc->first_voe_handler; |