153 |
153 |
154 INIT_LIST_HEAD(&slave->sii_strings); |
154 INIT_LIST_HEAD(&slave->sii_strings); |
155 INIT_LIST_HEAD(&slave->sii_syncs); |
155 INIT_LIST_HEAD(&slave->sii_syncs); |
156 INIT_LIST_HEAD(&slave->sii_pdos); |
156 INIT_LIST_HEAD(&slave->sii_pdos); |
157 INIT_LIST_HEAD(&slave->sdo_dictionary); |
157 INIT_LIST_HEAD(&slave->sdo_dictionary); |
|
158 INIT_LIST_HEAD(&slave->sdo_confs); |
158 INIT_LIST_HEAD(&slave->varsize_fields); |
159 INIT_LIST_HEAD(&slave->varsize_fields); |
159 |
160 |
160 for (i = 0; i < 4; i++) { |
161 for (i = 0; i < 4; i++) { |
161 slave->dl_link[i] = 0; |
162 slave->dl_link[i] = 0; |
162 slave->dl_loop[i] = 0; |
163 slave->dl_loop[i] = 0; |
180 ec_sii_sync_t *sync, *next_sync; |
181 ec_sii_sync_t *sync, *next_sync; |
181 ec_sii_pdo_t *pdo, *next_pdo; |
182 ec_sii_pdo_t *pdo, *next_pdo; |
182 ec_sii_pdo_entry_t *entry, *next_ent; |
183 ec_sii_pdo_entry_t *entry, *next_ent; |
183 ec_sdo_t *sdo, *next_sdo; |
184 ec_sdo_t *sdo, *next_sdo; |
184 ec_sdo_entry_t *en, *next_en; |
185 ec_sdo_entry_t *en, *next_en; |
|
186 ec_sdo_data_t *sdodata, *next_sdodata; |
185 ec_varsize_t *var, *next_var; |
187 ec_varsize_t *var, *next_var; |
186 |
188 |
187 slave = container_of(kobj, ec_slave_t, kobj); |
189 slave = container_of(kobj, ec_slave_t, kobj); |
188 |
190 |
189 // free all string objects |
191 // free all string objects |
227 list_for_each_entry_safe(en, next_en, &sdo->entries, list) { |
229 list_for_each_entry_safe(en, next_en, &sdo->entries, list) { |
228 list_del(&en->list); |
230 list_del(&en->list); |
229 kfree(en); |
231 kfree(en); |
230 } |
232 } |
231 kfree(sdo); |
233 kfree(sdo); |
|
234 } |
|
235 |
|
236 // free all SDO configurations |
|
237 list_for_each_entry_safe(sdodata, next_sdodata, &slave->sdo_confs, list) { |
|
238 list_del(&sdodata->list); |
|
239 kfree(sdodata->data); |
|
240 kfree(sdodata); |
232 } |
241 } |
233 |
242 |
234 // free information about variable sized data fields |
243 // free information about variable sized data fields |
235 list_for_each_entry_safe(var, next_var, &slave->varsize_fields, list) { |
244 list_for_each_entry_safe(var, next_var, &slave->varsize_fields, list) { |
236 list_del(&var->list); |
245 list_del(&var->list); |
510 off += sprintf(buffer + off, "\nName: "); |
519 off += sprintf(buffer + off, "\nName: "); |
511 |
520 |
512 if (slave->sii_name) |
521 if (slave->sii_name) |
513 off += sprintf(buffer + off, "%s", slave->sii_name); |
522 off += sprintf(buffer + off, "%s", slave->sii_name); |
514 |
523 |
515 off += sprintf(buffer + off, "\n\nVendor ID: 0x%08X\n", |
524 off += sprintf(buffer + off, "\nVendor ID: 0x%08X\n", |
516 slave->sii_vendor_id); |
525 slave->sii_vendor_id); |
517 off += sprintf(buffer + off, "Product code: 0x%08X\n\n", |
526 off += sprintf(buffer + off, "Product code: 0x%08X\n\n", |
518 slave->sii_product_code); |
527 slave->sii_product_code); |
519 |
528 |
520 off += sprintf(buffer + off, "Ring position: %i\n", slave->ring_position); |
529 off += sprintf(buffer + off, "State: "); |
|
530 off += ec_state_string(slave->current_state, buffer + off); |
|
531 off += sprintf(buffer + off, "\nRing position: %i\n", |
|
532 slave->ring_position); |
521 off += sprintf(buffer + off, "Advanced position: %i:%i\n\n", |
533 off += sprintf(buffer + off, "Advanced position: %i:%i\n\n", |
522 slave->coupler_index, slave->coupler_subindex); |
534 slave->coupler_index, slave->coupler_subindex); |
523 |
|
524 off += sprintf(buffer + off, "State: "); |
|
525 off += ec_state_string(slave->current_state, buffer + off); |
|
526 off += sprintf(buffer + off, "\n\n"); |
|
527 |
535 |
528 off += sprintf(buffer + off, "Data link status:\n"); |
536 off += sprintf(buffer + off, "Data link status:\n"); |
529 for (i = 0; i < 4; i++) { |
537 for (i = 0; i < 4; i++) { |
530 off += sprintf(buffer + off, " Port %i (", i); |
538 off += sprintf(buffer + off, " Port %i (", i); |
531 switch (slave->sii_physical_layer[i]) { |
539 switch (slave->sii_physical_layer[i]) { |
600 if (slave->sii_group) |
608 if (slave->sii_group) |
601 off += sprintf(buffer + off, " Group: %s\n", slave->sii_group); |
609 off += sprintf(buffer + off, " Group: %s\n", slave->sii_group); |
602 if (slave->sii_image) |
610 if (slave->sii_image) |
603 off += sprintf(buffer + off, " Image: %s\n", slave->sii_image); |
611 off += sprintf(buffer + off, " Image: %s\n", slave->sii_image); |
604 if (slave->sii_order) |
612 if (slave->sii_order) |
605 off += sprintf(buffer + off, " Order#: %s\n", slave->sii_order); |
613 off += sprintf(buffer + off, " Order number: %s\n", slave->sii_order); |
606 |
614 |
607 if (!list_empty(&slave->sii_syncs)) |
615 if (!list_empty(&slave->sii_syncs)) |
608 off += sprintf(buffer + off, "\nSync-Managers:\n"); |
616 off += sprintf(buffer + off, "\nSync-Managers:\n"); |
609 |
617 |
610 list_for_each_entry(sync, &slave->sii_syncs, list) { |
618 list_for_each_entry(sync, &slave->sii_syncs, list) { |
618 if (!list_empty(&slave->sii_pdos)) |
626 if (!list_empty(&slave->sii_pdos)) |
619 off += sprintf(buffer + off, "\nPDOs:\n"); |
627 off += sprintf(buffer + off, "\nPDOs:\n"); |
620 |
628 |
621 list_for_each_entry(pdo, &slave->sii_pdos, list) { |
629 list_for_each_entry(pdo, &slave->sii_pdos, list) { |
622 off += sprintf(buffer + off, |
630 off += sprintf(buffer + off, |
623 " %s \"%s\" (0x%04X), -> Sync-Manager %i\n", |
631 " %s \"%s\" (0x%04X), Sync-Manager %i\n", |
624 pdo->type == EC_RX_PDO ? "RXPDO" : "TXPDO", |
632 pdo->type == EC_RX_PDO ? "RXPDO" : "TXPDO", |
625 pdo->name ? pdo->name : "???", |
633 pdo->name ? pdo->name : "???", |
626 pdo->index, pdo->sync_index); |
634 pdo->index, pdo->sync_index); |
627 |
635 |
628 list_for_each_entry(pdo_entry, &pdo->entries, list) { |
636 list_for_each_entry(pdo_entry, &pdo->entries, list) { |
629 off += sprintf(buffer + off, " \"%s\" 0x%04X:%X, %i Bit\n", |
637 off += sprintf(buffer + off, " \"%s\" 0x%04X:%X, %i bit\n", |
630 pdo_entry->name ? pdo_entry->name : "???", |
638 pdo_entry->name ? pdo_entry->name : "???", |
631 pdo_entry->index, pdo_entry->subindex, |
639 pdo_entry->index, pdo_entry->subindex, |
632 pdo_entry->bit_length); |
640 pdo_entry->bit_length); |
633 } |
641 } |
634 } |
642 } |
838 ) |
846 ) |
839 { |
847 { |
840 ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj); |
848 ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj); |
841 |
849 |
842 if (attr == &attr_state) { |
850 if (attr == &attr_state) { |
|
851 char state[25]; |
843 if (!strcmp(buffer, "INIT\n")) |
852 if (!strcmp(buffer, "INIT\n")) |
844 slave->requested_state = EC_SLAVE_STATE_INIT; |
853 slave->requested_state = EC_SLAVE_STATE_INIT; |
845 else if (!strcmp(buffer, "PREOP\n")) |
854 else if (!strcmp(buffer, "PREOP\n")) |
846 slave->requested_state = EC_SLAVE_STATE_PREOP; |
855 slave->requested_state = EC_SLAVE_STATE_PREOP; |
847 else if (!strcmp(buffer, "SAVEOP\n")) |
856 else if (!strcmp(buffer, "SAVEOP\n")) |
851 else { |
860 else { |
852 EC_ERR("Invalid slave state \"%s\"!\n", buffer); |
861 EC_ERR("Invalid slave state \"%s\"!\n", buffer); |
853 return -EINVAL; |
862 return -EINVAL; |
854 } |
863 } |
855 |
864 |
|
865 ec_state_string(slave->requested_state, state); |
856 EC_INFO("Accepted new state %s for slave %i.\n", |
866 EC_INFO("Accepted new state %s for slave %i.\n", |
857 buffer, slave->ring_position); |
867 state, slave->ring_position); |
858 slave->error_flag = 0; |
868 slave->error_flag = 0; |
859 return size; |
869 return size; |
860 } |
870 } |
861 else if (attr == &attr_eeprom) { |
871 else if (attr == &attr_eeprom) { |
862 if (!ec_slave_write_eeprom(slave, buffer, size)) |
872 if (!ec_slave_write_eeprom(slave, buffer, size)) |
911 // TODO: Better bus coupler criterion |
921 // TODO: Better bus coupler criterion |
912 return slave->sii_vendor_id == 0x00000002 |
922 return slave->sii_vendor_id == 0x00000002 |
913 && slave->sii_product_code == 0x044C2C52; |
923 && slave->sii_product_code == 0x044C2C52; |
914 } |
924 } |
915 |
925 |
|
926 /*****************************************************************************/ |
|
927 |
|
928 /** |
|
929 \return 0 in case of success, else < 0 |
|
930 */ |
|
931 |
|
932 int ec_slave_conf_sdo(ec_slave_t *slave, /**< EtherCAT slave */ |
|
933 uint16_t sdo_index, /**< SDO index */ |
|
934 uint8_t sdo_subindex, /**< SDO subindex */ |
|
935 const uint8_t *data, /**< SDO data */ |
|
936 size_t size /**< SDO size in bytes */ |
|
937 ) |
|
938 { |
|
939 ec_sdo_data_t *sdodata; |
|
940 |
|
941 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
|
942 EC_ERR("Slave %i does not support CoE!\n", slave->ring_position); |
|
943 return -1; |
|
944 } |
|
945 |
|
946 if (!(sdodata = (ec_sdo_data_t *) |
|
947 kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) { |
|
948 EC_ERR("Failed to allocate memory for SDO configuration object!\n"); |
|
949 return -1; |
|
950 } |
|
951 |
|
952 if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) { |
|
953 EC_ERR("Failed to allocate memory for SDO configuration data!\n"); |
|
954 kfree(sdodata); |
|
955 return -1; |
|
956 } |
|
957 |
|
958 sdodata->index = sdo_index; |
|
959 sdodata->subindex = sdo_subindex; |
|
960 memcpy(sdodata->data, data, size); |
|
961 sdodata->size = size; |
|
962 |
|
963 list_add_tail(&sdodata->list, &slave->sdo_confs); |
|
964 return 0; |
|
965 } |
|
966 |
916 /****************************************************************************** |
967 /****************************************************************************** |
917 * Realtime interface |
968 * Realtime interface |
918 *****************************************************************************/ |
969 *****************************************************************************/ |
|
970 |
|
971 /** |
|
972 \return 0 in case of success, else < 0 |
|
973 \ingroup RealtimeInterface |
|
974 */ |
|
975 |
|
976 int ecrt_slave_conf_sdo8(ec_slave_t *slave, /**< EtherCAT slave */ |
|
977 uint16_t sdo_index, /**< SDO index */ |
|
978 uint8_t sdo_subindex, /**< SDO subindex */ |
|
979 uint8_t value /**< new SDO value */ |
|
980 ) |
|
981 { |
|
982 uint8_t data[1]; |
|
983 EC_WRITE_U8(data, value); |
|
984 return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 1); |
|
985 } |
|
986 |
|
987 /*****************************************************************************/ |
|
988 |
|
989 /** |
|
990 \return 0 in case of success, else < 0 |
|
991 \ingroup RealtimeInterface |
|
992 */ |
|
993 |
|
994 int ecrt_slave_conf_sdo16(ec_slave_t *slave, /**< EtherCAT slave */ |
|
995 uint16_t sdo_index, /**< SDO index */ |
|
996 uint8_t sdo_subindex, /**< SDO subindex */ |
|
997 uint16_t value /**< new SDO value */ |
|
998 ) |
|
999 { |
|
1000 uint8_t data[2]; |
|
1001 EC_WRITE_U16(data, value); |
|
1002 return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 2); |
|
1003 } |
|
1004 |
|
1005 /*****************************************************************************/ |
|
1006 |
|
1007 /** |
|
1008 \return 0 in case of success, else < 0 |
|
1009 \ingroup RealtimeInterface |
|
1010 */ |
|
1011 |
|
1012 int ecrt_slave_conf_sdo32(ec_slave_t *slave, /**< EtherCAT slave */ |
|
1013 uint16_t sdo_index, /**< SDO index */ |
|
1014 uint8_t sdo_subindex, /**< SDO subindex */ |
|
1015 uint32_t value /**< new SDO value */ |
|
1016 ) |
|
1017 { |
|
1018 uint8_t data[4]; |
|
1019 EC_WRITE_U32(data, value); |
|
1020 return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 4); |
|
1021 } |
|
1022 |
|
1023 /*****************************************************************************/ |
919 |
1024 |
920 /** |
1025 /** |
921 \return 0 in case of success, else < 0 |
1026 \return 0 in case of success, else < 0 |
922 \ingroup RealtimeInterface |
1027 \ingroup RealtimeInterface |
923 */ |
1028 */ |