master/slave_config.c
changeset 854 f4f53be425ac
parent 844 8839ba8bfeb4
child 858 69122084d066
equal deleted inserted replaced
853:726326d0aef4 854:f4f53be425ac
   157  */
   157  */
   158 void ec_slave_config_clear(struct kobject *kobj /**< kobject of the config. */)
   158 void ec_slave_config_clear(struct kobject *kobj /**< kobject of the config. */)
   159 {
   159 {
   160     ec_slave_config_t *sc;
   160     ec_slave_config_t *sc;
   161     ec_direction_t dir;
   161     ec_direction_t dir;
   162     ec_sdo_data_t *sdodata, *next_sdodata;
   162     ec_sdo_request_t *req, *next_req;
   163 
   163 
   164     sc = container_of(kobj, ec_slave_config_t, kobj);
   164     sc = container_of(kobj, ec_slave_config_t, kobj);
   165 
   165 
   166     // Free Pdo mappings
   166     // Free Pdo mappings
   167     for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++)
   167     for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++)
   168         ec_pdo_mapping_clear(&sc->mapping[dir]);
   168         ec_pdo_mapping_clear(&sc->mapping[dir]);
   169 
   169 
   170     // free all Sdo configurations
   170     // free all Sdo configurations
   171     list_for_each_entry_safe(sdodata, next_sdodata, &sc->sdo_configs, list) {
   171     list_for_each_entry_safe(req, next_req, &sc->sdo_configs, list) {
   172         list_del(&sdodata->list);
   172         list_del(&req->list);
   173         kfree(sdodata->data);
   173         ec_sdo_request_clear(req);
   174         kfree(sdodata);
   174         kfree(req);
   175     }
   175     }
   176 
       
   177     /** \todo */
       
   178 
   176 
   179     kfree(sc);
   177     kfree(sc);
   180 }
   178 }
   181 
   179 
   182 /*****************************************************************************/
   180 /*****************************************************************************/
   232     ec_direction_t dir;
   230     ec_direction_t dir;
   233     const ec_pdo_mapping_t *map;
   231     const ec_pdo_mapping_t *map;
   234     const ec_pdo_t *pdo;
   232     const ec_pdo_t *pdo;
   235     const ec_pdo_entry_t *entry;
   233     const ec_pdo_entry_t *entry;
   236     char str[20];
   234     char str[20];
   237     ec_sdo_data_t *sdodata;
   235     const ec_sdo_request_t *req;
   238 
   236 
   239     buf += sprintf(buf, "Alias: 0x%04X (%u)\n", sc->alias, sc->alias);
   237     buf += sprintf(buf, "Alias: 0x%04X (%u)\n", sc->alias, sc->alias);
   240     buf += sprintf(buf, "Position: %u\n", sc->position);
   238     buf += sprintf(buf, "Position: %u\n", sc->position);
   241 
   239 
   242     for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) {
   240     for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) {
   263     
   261     
   264     // type-cast to avoid warnings on some compilers
   262     // type-cast to avoid warnings on some compilers
   265     if (!list_empty((struct list_head *) &sc->sdo_configs)) {
   263     if (!list_empty((struct list_head *) &sc->sdo_configs)) {
   266         buf += sprintf(buf, "\nSdo configurations:\n");
   264         buf += sprintf(buf, "\nSdo configurations:\n");
   267 
   265 
   268         list_for_each_entry(sdodata, &sc->sdo_configs, list) {
   266         list_for_each_entry(req, &sc->sdo_configs, list) {
   269             switch (sdodata->size) {
   267             switch (req->data_size) {
   270                 case 1: sprintf(str, "%u", EC_READ_U8(sdodata->data)); break;
   268                 case 1: sprintf(str, "%u", EC_READ_U8(req->data)); break;
   271                 case 2: sprintf(str, "%u", EC_READ_U16(sdodata->data)); break;
   269                 case 2: sprintf(str, "%u", EC_READ_U16(req->data)); break;
   272                 case 4: sprintf(str, "%u", EC_READ_U32(sdodata->data)); break;
   270                 case 4: sprintf(str, "%u", EC_READ_U32(req->data)); break;
   273                 default: sprintf(str, "(invalid size)"); break;
   271                 default: sprintf(str, "(invalid size)"); break;
   274             }
   272             }
   275             buf += sprintf(buf, "  0x%04X:%-3i -> %s\n",
   273             buf += sprintf(buf, "  0x%04X:%-3i -> %s\n",
   276                     sdodata->index, sdodata->subindex, str);
   274                     req->index, req->subindex, str);
   277         }
   275         }
   278         buf += sprintf(buf, "\n");
   276         buf += sprintf(buf, "\n");
   279     }
   277     }
   280 
   278 
   281     return buf - buffer;
   279     return buf - buffer;
   308  */
   306  */
   309 int ec_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
   307 int ec_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
   310         uint8_t subindex, const uint8_t *data, size_t size)
   308         uint8_t subindex, const uint8_t *data, size_t size)
   311 {
   309 {
   312     ec_slave_t *slave = sc->slave;
   310     ec_slave_t *slave = sc->slave;
   313     ec_sdo_data_t *sdodata;
   311     ec_sdo_request_t *req;
   314 
   312 
   315     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
   313     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
   316         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
   314         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
   317         return -1;
   315         return -1;
   318     }
   316     }
   319 
   317 
   320     if (!(sdodata = (ec_sdo_data_t *)
   318     if (!(req = (ec_sdo_request_t *)
   321           kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) {
   319           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   322         EC_ERR("Failed to allocate memory for Sdo configuration object!\n");
   320         EC_ERR("Failed to allocate memory for Sdo configuration!\n");
   323         return -1;
   321         return -1;
   324     }
   322     }
   325 
   323 
   326     if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
   324     ec_sdo_request_init(req);
   327         EC_ERR("Failed to allocate memory for Sdo configuration data!\n");
   325     ec_sdo_request_address(req, index, subindex);
   328         kfree(sdodata);
   326 
       
   327     if (ec_sdo_request_copy_data(req, data, size)) {
       
   328         ec_sdo_request_clear(req);
       
   329         kfree(req);
   329         return -1;
   330         return -1;
   330     }
   331     }
   331 
   332         
   332     sdodata->index = index;
   333     list_add_tail(&req->list, &sc->sdo_configs);
   333     sdodata->subindex = subindex;
       
   334     memcpy(sdodata->data, data, size);
       
   335     sdodata->size = size;
       
   336 
       
   337     list_add_tail(&sdodata->list, &sc->sdo_configs);
       
   338     return 0;
   334     return 0;
   339 }
   335 }
   340 
   336 
   341 /*****************************************************************************/
   337 /*****************************************************************************/
   342 
   338