master/slave.c
changeset 814 a51f857b1b2d
parent 813 bfc3f1ab52de
child 831 ded9519c8d6e
equal deleted inserted replaced
813:bfc3f1ab52de 814:a51f857b1b2d
   176     if (kobject_add(&slave->kobj)) {
   176     if (kobject_add(&slave->kobj)) {
   177         EC_ERR("Failed to add slave's kobject.\n");
   177         EC_ERR("Failed to add slave's kobject.\n");
   178         goto out_slave_put;
   178         goto out_slave_put;
   179     }
   179     }
   180 
   180 
   181     // init SDO kobject and add it to the hierarchy
   181     // init Sdo kobject and add it to the hierarchy
   182     memset(&slave->sdo_kobj, 0x00, sizeof(struct kobject));
   182     memset(&slave->sdo_kobj, 0x00, sizeof(struct kobject));
   183     kobject_init(&slave->sdo_kobj);
   183     kobject_init(&slave->sdo_kobj);
   184     slave->sdo_kobj.ktype = &ktype_ec_slave_sdos;
   184     slave->sdo_kobj.ktype = &ktype_ec_slave_sdos;
   185     slave->sdo_kobj.parent = &slave->kobj;
   185     slave->sdo_kobj.parent = &slave->kobj;
   186     if (kobject_set_name(&slave->sdo_kobj, "sdos")) {
   186     if (kobject_set_name(&slave->sdo_kobj, "sdos")) {
   187         EC_ERR("Failed to set kobject name.\n");
   187         EC_ERR("Failed to set kobject name.\n");
   188         goto out_sdo_put;
   188         goto out_sdo_put;
   189     }
   189     }
   190     if (kobject_add(&slave->sdo_kobj)) {
   190     if (kobject_add(&slave->sdo_kobj)) {
   191         EC_ERR("Failed to add SDOs kobject.\n");
   191         EC_ERR("Failed to add Sdos kobject.\n");
   192         goto out_sdo_put;
   192         goto out_sdo_put;
   193     }
   193     }
   194 
   194 
   195     return 0;
   195     return 0;
   196 
   196 
   214     ec_sdo_t *sdo, *next_sdo;
   214     ec_sdo_t *sdo, *next_sdo;
   215 
   215 
   216     if (slave->config)
   216     if (slave->config)
   217         ec_slave_config_detach(slave->config);
   217         ec_slave_config_detach(slave->config);
   218 
   218 
   219     // free all SDOs
   219     // free all Sdos
   220     list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
   220     list_for_each_entry_safe(sdo, next_sdo, &slave->sdo_dictionary, list) {
   221         list_del(&sdo->list);
   221         list_del(&sdo->list);
   222         ec_sdo_destroy(sdo);
   222         ec_sdo_destroy(sdo);
   223     }
   223     }
   224 
   224 
   225     // free SDO kobject
   225     // free Sdo kobject
   226     kobject_del(&slave->sdo_kobj);
   226     kobject_del(&slave->sdo_kobj);
   227     kobject_put(&slave->sdo_kobj);
   227     kobject_put(&slave->sdo_kobj);
   228 
   228 
   229     // destroy self
   229     // destroy self
   230     kobject_del(&slave->kobj);
   230     kobject_del(&slave->kobj);
   260             ec_sync_clear(&slave->sii_syncs[i]);
   260             ec_sync_clear(&slave->sii_syncs[i]);
   261         }
   261         }
   262         kfree(slave->sii_syncs);
   262         kfree(slave->sii_syncs);
   263     }
   263     }
   264 
   264 
   265     // free all SII PDOs
   265     // free all SII Pdos
   266     list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) {
   266     list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) {
   267         list_del(&pdo->list);
   267         list_del(&pdo->list);
   268         ec_pdo_clear(pdo);
   268         ec_pdo_clear(pdo);
   269         kfree(pdo);
   269         kfree(pdo);
   270     }
   270     }
   275 }
   275 }
   276 
   276 
   277 /*****************************************************************************/
   277 /*****************************************************************************/
   278 
   278 
   279 /**
   279 /**
   280  * SDO kobject clear method.
   280  * Sdo kobject clear method.
   281  */
   281  */
   282 
   282 
   283 void ec_slave_sdos_clear(struct kobject *kobj /**< kobject for SDOs */)
   283 void ec_slave_sdos_clear(struct kobject *kobj /**< kobject for Sdos */)
   284 {
   284 {
   285 }
   285 }
   286 
   286 
   287 /*****************************************************************************/
   287 /*****************************************************************************/
   288 
   288 
   488 }
   488 }
   489 
   489 
   490 /*****************************************************************************/
   490 /*****************************************************************************/
   491 
   491 
   492 /**
   492 /**
   493    Fetches data from a [RT]XPDO category.
   493    Fetches data from a [RT]XPdo category.
   494    \return 0 in case of success, else < 0
   494    \return 0 in case of success, else < 0
   495 */
   495 */
   496 
   496 
   497 int ec_slave_fetch_sii_pdos(
   497 int ec_slave_fetch_sii_pdos(
   498         ec_slave_t *slave, /**< EtherCAT slave */
   498         ec_slave_t *slave, /**< EtherCAT slave */
   499         const uint8_t *data, /**< category data */
   499         const uint8_t *data, /**< category data */
   500         size_t data_size, /**< number of bytes */
   500         size_t data_size, /**< number of bytes */
   501         ec_direction_t dir /**< PDO direction. */
   501         ec_direction_t dir /**< Pdo direction. */
   502         )
   502         )
   503 {
   503 {
   504     ec_pdo_t *pdo;
   504     ec_pdo_t *pdo;
   505     ec_pdo_entry_t *entry;
   505     ec_pdo_entry_t *entry;
   506     unsigned int entry_count, i;
   506     unsigned int entry_count, i;
   507 
   507 
   508     while (data_size >= 8) {
   508     while (data_size >= 8) {
   509         if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
   509         if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
   510             EC_ERR("Failed to allocate PDO memory.\n");
   510             EC_ERR("Failed to allocate Pdo memory.\n");
   511             return -1;
   511             return -1;
   512         }
   512         }
   513 
   513 
   514         ec_pdo_init(pdo);
   514         ec_pdo_init(pdo);
   515         pdo->dir = dir;
   515         pdo->dir = dir;
   527         data_size -= 8;
   527         data_size -= 8;
   528         data += 8;
   528         data += 8;
   529 
   529 
   530         for (i = 0; i < entry_count; i++) {
   530         for (i = 0; i < entry_count; i++) {
   531             if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
   531             if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
   532                 EC_ERR("Failed to allocate PDO entry memory.\n");
   532                 EC_ERR("Failed to allocate Pdo entry memory.\n");
   533                 return -1;
   533                 return -1;
   534             }
   534             }
   535 
   535 
   536             ec_pdo_entry_init(entry);
   536             ec_pdo_entry_init(entry);
   537             entry->index = EC_READ_U16(data);
   537             entry->index = EC_READ_U16(data);
   547 
   547 
   548             data_size -= 8;
   548             data_size -= 8;
   549             data += 8;
   549             data += 8;
   550         }
   550         }
   551 
   551 
   552         // if sync manager index is positive, the PDO is mapped by default
   552         // if sync manager index is positive, the Pdo is mapped by default
   553         if (pdo->sync_index >= 0) {
   553         if (pdo->sync_index >= 0) {
   554             ec_sync_t *sync;
   554             ec_sync_t *sync;
   555 
   555 
   556             if (pdo->sync_index >= slave->sii_sync_count) {
   556             if (pdo->sync_index >= slave->sii_sync_count) {
   557                 EC_ERR("Invalid SM index %i for PDO 0x%04X in slave %u.",
   557                 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.",
   558                         pdo->sync_index, pdo->index, slave->ring_position);
   558                         pdo->sync_index, pdo->index, slave->ring_position);
   559                 return -1;
   559                 return -1;
   560             }
   560             }
   561             sync = &slave->sii_syncs[pdo->sync_index];
   561             sync = &slave->sii_syncs[pdo->sync_index];
   562 
   562 
   720             buf += sprintf(buf, "  Name: %s\n", slave->sii_name);
   720             buf += sprintf(buf, "  Name: %s\n", slave->sii_name);
   721         buf += sprintf(buf, "\n");
   721         buf += sprintf(buf, "\n");
   722     }
   722     }
   723 
   723 
   724     if (slave->sii_sync_count) {
   724     if (slave->sii_sync_count) {
   725         buf += sprintf(buf, "Sync managers / PDO mapping:\n");
   725         buf += sprintf(buf, "Sync managers / Pdo mapping:\n");
   726 
   726 
   727         for (i = 0; i < slave->sii_sync_count; i++) {
   727         for (i = 0; i < slave->sii_sync_count; i++) {
   728             sync = &slave->sii_syncs[i];
   728             sync = &slave->sii_syncs[i];
   729             buf += sprintf(buf,
   729             buf += sprintf(buf,
   730                     "  SM%u: addr 0x%04X, size %u, control 0x%02X, %s\n",
   730                     "  SM%u: addr 0x%04X, size %u, control 0x%02X, %s\n",
   731                     sync->index, sync->physical_start_address,
   731                     sync->index, sync->physical_start_address,
   732                     sync->length, sync->control_register,
   732                     sync->length, sync->control_register,
   733                     sync->enable ? "enable" : "disable");
   733                     sync->enable ? "enable" : "disable");
   734 
   734 
   735             if (list_empty(&sync->mapping.pdos)) {
   735             if (list_empty(&sync->mapping.pdos)) {
   736                 buf += sprintf(buf, "    No PDOs mapped.\n");
   736                 buf += sprintf(buf, "    No Pdos mapped.\n");
   737             } else if (sync->mapping_source != EC_SYNC_MAPPING_NONE) {
   737             } else if (sync->mapping_source != EC_SYNC_MAPPING_NONE) {
   738                 buf += sprintf(buf,
   738                 buf += sprintf(buf,
   739                         "    PDO mapping information from %s.\n",
   739                         "    Pdo mapping information from %s.\n",
   740                         sync->mapping_source == EC_SYNC_MAPPING_SII
   740                         sync->mapping_source == EC_SYNC_MAPPING_SII
   741                         ? "SII" : "CoE");
   741                         ? "SII" : "CoE");
   742             }
   742             }
   743 
   743 
   744             list_for_each_entry(pdo, &sync->mapping.pdos, list) {
   744             list_for_each_entry(pdo, &sync->mapping.pdos, list) {
   758         buf += sprintf(buf, "\n");
   758         buf += sprintf(buf, "\n");
   759     }
   759     }
   760 
   760 
   761     // type-cast to avoid warnings on some compilers
   761     // type-cast to avoid warnings on some compilers
   762     if (!list_empty((struct list_head *) &slave->sii_pdos)) {
   762     if (!list_empty((struct list_head *) &slave->sii_pdos)) {
   763         buf += sprintf(buf, "Available PDOs from SII:\n");
   763         buf += sprintf(buf, "Available Pdos from SII:\n");
   764 
   764 
   765         list_for_each_entry(pdo, &slave->sii_pdos, list) {
   765         list_for_each_entry(pdo, &slave->sii_pdos, list) {
   766             buf += sprintf(buf, "  %s 0x%04X \"%s\"",
   766             buf += sprintf(buf, "  %s 0x%04X \"%s\"",
   767                     pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo",
   767                     pdo->dir == EC_DIR_OUTPUT ? "RxPdo" : "TxPdo",
   768                     pdo->index, pdo->name ? pdo->name : "???");
   768                     pdo->index, pdo->name ? pdo->name : "???");
  1109 }
  1109 }
  1110 
  1110 
  1111 /*****************************************************************************/
  1111 /*****************************************************************************/
  1112 
  1112 
  1113 /**
  1113 /**
  1114  * Get the sync manager for either Rx- or Tx-PDOs.
  1114  * Get the sync manager for either Rx- or Tx-Pdos.
  1115  * \return pointer to sync manager, or NULL.
  1115  * \return pointer to sync manager, or NULL.
  1116  */
  1116  */
  1117 
  1117 
  1118 ec_sync_t *ec_slave_get_pdo_sync(
  1118 ec_sync_t *ec_slave_get_pdo_sync(
  1119         ec_slave_t *slave, /**< EtherCAT slave */
  1119         ec_slave_t *slave, /**< EtherCAT slave */
  1159 }
  1159 }
  1160 
  1160 
  1161 /*****************************************************************************/
  1161 /*****************************************************************************/
  1162 
  1162 
  1163 /**
  1163 /**
  1164    Counts the total number of SDOs and entries in the dictionary.
  1164    Counts the total number of Sdos and entries in the dictionary.
  1165 */
  1165 */
  1166 
  1166 
  1167 void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */
  1167 void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */
  1168                             unsigned int *sdo_count, /**< number of SDOs */
  1168                             unsigned int *sdo_count, /**< number of Sdos */
  1169                             unsigned int *entry_count /**< total number of
  1169                             unsigned int *entry_count /**< total number of
  1170                                                          entries */
  1170                                                          entries */
  1171                             )
  1171                             )
  1172 {
  1172 {
  1173     unsigned int sdos = 0, entries = 0;
  1173     unsigned int sdos = 0, entries = 0;
  1186 }
  1186 }
  1187 
  1187 
  1188 /*****************************************************************************/
  1188 /*****************************************************************************/
  1189 
  1189 
  1190 /**
  1190 /**
  1191  * Get an SDO from the dictionary.
  1191  * Get an Sdo from the dictionary.
  1192  * \returns The desired SDO, or NULL.
  1192  * \returns The desired Sdo, or NULL.
  1193  */
  1193  */
  1194 
  1194 
  1195 ec_sdo_t *ec_slave_get_sdo(
  1195 ec_sdo_t *ec_slave_get_sdo(
  1196         ec_slave_t *slave /**< EtherCAT slave */,
  1196         ec_slave_t *slave /**< EtherCAT slave */,
  1197         uint16_t index /**< SDO index */
  1197         uint16_t index /**< Sdo index */
  1198         )
  1198         )
  1199 {
  1199 {
  1200     ec_sdo_t *sdo;
  1200     ec_sdo_t *sdo;
  1201 
  1201 
  1202     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
  1202     list_for_each_entry(sdo, &slave->sdo_dictionary, list) {