68 sc->position = position; |
68 sc->position = position; |
69 sc->vendor_id = vendor_id; |
69 sc->vendor_id = vendor_id; |
70 sc->product_code = product_code; |
70 sc->product_code = product_code; |
71 sc->watchdog_divider = 0; // use default |
71 sc->watchdog_divider = 0; // use default |
72 sc->watchdog_intervals = 0; // use default |
72 sc->watchdog_intervals = 0; // use default |
73 |
73 sc->allow_overlapping_pdos = 0; // default not allowed |
74 sc->slave = NULL; |
74 sc->slave = NULL; |
75 |
75 |
76 for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) |
76 for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++) |
77 ec_sync_config_init(&sc->sync_configs[i]); |
77 ec_sync_config_init(&sc->sync_configs[i]); |
78 |
78 |
162 ec_direction_t dir /**< PDO direction. */ |
162 ec_direction_t dir /**< PDO direction. */ |
163 ) |
163 ) |
164 { |
164 { |
165 unsigned int i; |
165 unsigned int i; |
166 ec_fmmu_config_t *fmmu; |
166 ec_fmmu_config_t *fmmu; |
|
167 ec_fmmu_config_t *prev_fmmu; |
|
168 uint32_t fmmu_logical_start_address; |
|
169 size_t tx_size, old_prev_tx_size; |
167 |
170 |
168 // FMMU configuration already prepared? |
171 // FMMU configuration already prepared? |
169 for (i = 0; i < sc->used_fmmus; i++) { |
172 for (i = 0; i < sc->used_fmmus; i++) { |
170 fmmu = &sc->fmmu_configs[i]; |
173 fmmu = &sc->fmmu_configs[i]; |
171 if (fmmu->domain == domain && fmmu->sync_index == sync_index) |
174 if (fmmu->domain == domain && fmmu->sync_index == sync_index) |
172 return fmmu->logical_start_address; |
175 return fmmu->domain_address; |
173 } |
176 } |
174 |
177 |
175 if (sc->used_fmmus == EC_MAX_FMMUS) { |
178 if (sc->used_fmmus == EC_MAX_FMMUS) { |
176 EC_CONFIG_ERR(sc, "FMMU limit reached!\n"); |
179 EC_CONFIG_ERR(sc, "FMMU limit reached!\n"); |
177 return -EOVERFLOW; |
180 return -EOVERFLOW; |
178 } |
181 } |
179 |
182 |
180 fmmu = &sc->fmmu_configs[sc->used_fmmus++]; |
183 fmmu = &sc->fmmu_configs[sc->used_fmmus]; |
181 |
184 |
182 down(&sc->master->master_sem); |
185 ec_mutex_lock(&sc->master->master_mutex); |
183 ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir); |
186 ec_fmmu_config_init(fmmu, sc, sync_index, dir); |
184 up(&sc->master->master_sem); |
187 fmmu_logical_start_address = domain->tx_size; |
185 |
188 tx_size = fmmu->data_size; |
186 return fmmu->logical_start_address; |
189 if (sc->allow_overlapping_pdos && sc->used_fmmus > 0) { |
|
190 prev_fmmu = &sc->fmmu_configs[sc->used_fmmus-1]; |
|
191 if (fmmu->dir != prev_fmmu->dir && prev_fmmu->tx_size != 0) { |
|
192 // prev fmmu has opposite direction |
|
193 // and is not already paired with prev-prev fmmu |
|
194 old_prev_tx_size = prev_fmmu->tx_size; |
|
195 prev_fmmu->tx_size = max(fmmu->data_size,prev_fmmu->data_size); |
|
196 domain->tx_size += prev_fmmu->tx_size - old_prev_tx_size; |
|
197 tx_size = 0; |
|
198 fmmu_logical_start_address = prev_fmmu->logical_start_address; |
|
199 } |
|
200 } |
|
201 ec_fmmu_config_domain(fmmu,domain,fmmu_logical_start_address,tx_size); |
|
202 ec_mutex_unlock(&sc->master->master_mutex); |
|
203 |
|
204 ++sc->used_fmmus; |
|
205 return fmmu->domain_address; |
187 } |
206 } |
188 |
207 |
189 /*****************************************************************************/ |
208 /*****************************************************************************/ |
190 |
209 |
191 /** Attaches the configuration to the addressed slave object. |
210 /** Attaches the configuration to the addressed slave object. |
492 sc->watchdog_intervals = intervals; |
509 sc->watchdog_intervals = intervals; |
493 } |
510 } |
494 |
511 |
495 /*****************************************************************************/ |
512 /*****************************************************************************/ |
496 |
513 |
|
514 void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc, |
|
515 uint8_t allow_overlapping_pdos ) |
|
516 { |
|
517 if (sc->master->debug_level) |
|
518 EC_DBG("%s(sc = 0x%p, allow_overlapping_pdos = %u)\n", |
|
519 __func__, sc, allow_overlapping_pdos); |
|
520 |
|
521 sc->allow_overlapping_pdos = allow_overlapping_pdos; |
|
522 } |
|
523 |
|
524 /*****************************************************************************/ |
|
525 |
497 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
526 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
498 uint8_t sync_index, uint16_t pdo_index) |
527 uint8_t sync_index, uint16_t pdo_index) |
499 { |
528 { |
500 ec_pdo_t *pdo; |
529 ec_pdo_t *pdo; |
501 |
530 |
505 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
534 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
506 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
535 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
507 return -EINVAL; |
536 return -EINVAL; |
508 } |
537 } |
509 |
538 |
510 down(&sc->master->master_sem); |
539 ec_mutex_lock(&sc->master->master_mutex); |
511 |
540 |
512 pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index); |
541 pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index); |
513 if (IS_ERR(pdo)) { |
542 if (IS_ERR(pdo)) { |
514 up(&sc->master->master_sem); |
543 ec_mutex_unlock(&sc->master->master_mutex); |
515 return PTR_ERR(pdo); |
544 return PTR_ERR(pdo); |
516 } |
545 } |
517 pdo->sync_index = sync_index; |
546 pdo->sync_index = sync_index; |
518 |
547 |
519 ec_slave_config_load_default_mapping(sc, pdo); |
548 ec_slave_config_load_default_mapping(sc, pdo); |
520 |
549 |
521 up(&sc->master->master_sem); |
550 ec_mutex_unlock(&sc->master->master_mutex); |
522 return 0; |
551 return 0; |
523 } |
552 } |
524 |
553 |
525 /*****************************************************************************/ |
554 /*****************************************************************************/ |
526 |
555 |
533 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
562 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
534 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
563 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
535 return; |
564 return; |
536 } |
565 } |
537 |
566 |
538 down(&sc->master->master_sem); |
567 ec_mutex_lock(&sc->master->master_mutex); |
539 ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); |
568 ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); |
540 up(&sc->master->master_sem); |
569 ec_mutex_unlock(&sc->master->master_mutex); |
541 } |
570 } |
542 |
571 |
543 /*****************************************************************************/ |
572 /*****************************************************************************/ |
544 |
573 |
545 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
574 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
561 if ((pdo = ec_pdo_list_find_pdo( |
590 if ((pdo = ec_pdo_list_find_pdo( |
562 &sc->sync_configs[sync_index].pdos, pdo_index))) |
591 &sc->sync_configs[sync_index].pdos, pdo_index))) |
563 break; |
592 break; |
564 |
593 |
565 if (pdo) { |
594 if (pdo) { |
566 down(&sc->master->master_sem); |
595 ec_mutex_lock(&sc->master->master_mutex); |
567 entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex, |
596 entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex, |
568 entry_bit_length); |
597 entry_bit_length); |
569 up(&sc->master->master_sem); |
598 ec_mutex_unlock(&sc->master->master_mutex); |
570 if (IS_ERR(entry)) |
599 if (IS_ERR(entry)) |
571 retval = PTR_ERR(entry); |
600 retval = PTR_ERR(entry); |
572 } else { |
601 } else { |
573 EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index); |
602 EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index); |
574 retval = -ENOENT; |
603 retval = -ENOENT; |
592 if ((pdo = ec_pdo_list_find_pdo( |
621 if ((pdo = ec_pdo_list_find_pdo( |
593 &sc->sync_configs[sync_index].pdos, pdo_index))) |
622 &sc->sync_configs[sync_index].pdos, pdo_index))) |
594 break; |
623 break; |
595 |
624 |
596 if (pdo) { |
625 if (pdo) { |
597 down(&sc->master->master_sem); |
626 ec_mutex_lock(&sc->master->master_mutex); |
598 ec_pdo_clear_entries(pdo); |
627 ec_pdo_clear_entries(pdo); |
599 up(&sc->master->master_sem); |
628 ec_mutex_unlock(&sc->master->master_mutex); |
600 } else { |
629 } else { |
601 EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index); |
630 EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index); |
602 } |
631 } |
603 } |
632 } |
604 |
633 |
682 const ec_sync_config_t *sync_config; |
711 const ec_sync_config_t *sync_config; |
683 unsigned int bit_offset, bit_pos; |
712 unsigned int bit_offset, bit_pos; |
684 ec_pdo_t *pdo; |
713 ec_pdo_t *pdo; |
685 ec_pdo_entry_t *entry; |
714 ec_pdo_entry_t *entry; |
686 int sync_offset; |
715 int sync_offset; |
687 |
|
688 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
|
689 "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n", |
|
690 __func__, sc, index, subindex, domain, bit_position); |
|
691 |
716 |
692 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { |
717 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { |
693 sync_config = &sc->sync_configs[sync_index]; |
718 sync_config = &sc->sync_configs[sync_index]; |
694 bit_offset = 0; |
719 bit_offset = 0; |
695 |
720 |
710 sync_offset = ec_slave_config_prepare_fmmu( |
735 sync_offset = ec_slave_config_prepare_fmmu( |
711 sc, domain, sync_index, sync_config->dir); |
736 sc, domain, sync_index, sync_config->dir); |
712 if (sync_offset < 0) |
737 if (sync_offset < 0) |
713 return sync_offset; |
738 return sync_offset; |
714 |
739 |
715 return sync_offset + bit_offset / 8; |
740 EC_CONFIG_DBG(sc, 1, "%s(index = 0x%04X, " |
|
741 "subindex = 0x%02X, domain = %u, bytepos=%u, bitpos=%u)\n", |
|
742 __func__,index, subindex, |
|
743 domain->index, sync_offset + bit_offset / 8, bit_pos); |
|
744 return sync_offset + bit_offset / 8; |
716 } |
745 } |
717 } |
746 } |
718 } |
747 } |
719 } |
748 } |
720 |
749 |
900 |
929 |
901 // prepare data for optional writing |
930 // prepare data for optional writing |
902 memset(req->data, 0x00, size); |
931 memset(req->data, 0x00, size); |
903 req->data_size = size; |
932 req->data_size = size; |
904 |
933 |
905 down(&sc->master->master_sem); |
934 ec_mutex_lock(&sc->master->master_mutex); |
906 list_add_tail(&req->list, &sc->sdo_requests); |
935 list_add_tail(&req->list, &sc->sdo_requests); |
907 up(&sc->master->master_sem); |
936 ec_mutex_unlock(&sc->master->master_mutex); |
908 |
937 |
909 return req; |
938 return req; |
910 } |
939 } |
911 |
940 |
912 /*****************************************************************************/ |
941 /*****************************************************************************/ |
1025 ec_soe_request_clear(req); |
1054 ec_soe_request_clear(req); |
1026 kfree(req); |
1055 kfree(req); |
1027 return ret; |
1056 return ret; |
1028 } |
1057 } |
1029 |
1058 |
1030 down(&sc->master->master_sem); |
1059 ec_mutex_lock(&sc->master->master_mutex); |
1031 list_add_tail(&req->list, &sc->soe_configs); |
1060 list_add_tail(&req->list, &sc->soe_configs); |
1032 up(&sc->master->master_sem); |
1061 ec_mutex_unlock(&sc->master->master_mutex); |
1033 return 0; |
1062 return 0; |
1034 } |
1063 } |
1035 |
1064 |
1036 /*****************************************************************************/ |
1065 /*****************************************************************************/ |
1037 |
1066 |
1038 /** \cond */ |
1067 /** \cond */ |
1039 |
1068 |
1040 EXPORT_SYMBOL(ecrt_slave_config_sync_manager); |
1069 EXPORT_SYMBOL(ecrt_slave_config_sync_manager); |
1041 EXPORT_SYMBOL(ecrt_slave_config_watchdog); |
1070 EXPORT_SYMBOL(ecrt_slave_config_watchdog); |
|
1071 EXPORT_SYMBOL(ecrt_slave_config_overlapping_pdos); |
1042 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add); |
1072 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_add); |
1043 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear); |
1073 EXPORT_SYMBOL(ecrt_slave_config_pdo_assign_clear); |
1044 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add); |
1074 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_add); |
1045 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear); |
1075 EXPORT_SYMBOL(ecrt_slave_config_pdo_mapping_clear); |
1046 EXPORT_SYMBOL(ecrt_slave_config_pdos); |
1076 EXPORT_SYMBOL(ecrt_slave_config_pdos); |