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 |