55 void ec_slave_clear(struct kobject *); |
55 void ec_slave_clear(struct kobject *); |
56 void ec_slave_sdos_clear(struct kobject *); |
56 void ec_slave_sdos_clear(struct kobject *); |
57 ssize_t ec_show_slave_attribute(struct kobject *, struct attribute *, char *); |
57 ssize_t ec_show_slave_attribute(struct kobject *, struct attribute *, char *); |
58 ssize_t ec_store_slave_attribute(struct kobject *, struct attribute *, |
58 ssize_t ec_store_slave_attribute(struct kobject *, struct attribute *, |
59 const char *, size_t); |
59 const char *, size_t); |
|
60 char *ec_slave_sii_string(ec_slave_t *, unsigned int); |
60 |
61 |
61 /*****************************************************************************/ |
62 /*****************************************************************************/ |
62 |
63 |
63 /** \cond */ |
64 /** \cond */ |
64 |
65 |
145 slave->sii_image = NULL; |
146 slave->sii_image = NULL; |
146 slave->sii_order = NULL; |
147 slave->sii_order = NULL; |
147 slave->sii_name = NULL; |
148 slave->sii_name = NULL; |
148 slave->sii_current_on_ebus = 0; |
149 slave->sii_current_on_ebus = 0; |
149 |
150 |
150 INIT_LIST_HEAD(&slave->sii_strings); |
151 slave->sii_strings = NULL; |
|
152 slave->sii_string_count = 0; |
151 slave->sii_syncs = NULL; |
153 slave->sii_syncs = NULL; |
152 slave->sii_sync_count = 0; |
154 slave->sii_sync_count = 0; |
153 INIT_LIST_HEAD(&slave->sii_pdos); |
155 INIT_LIST_HEAD(&slave->sii_pdos); |
154 INIT_LIST_HEAD(&slave->sdo_dictionary); |
156 INIT_LIST_HEAD(&slave->sdo_dictionary); |
155 INIT_LIST_HEAD(&slave->sdo_confs); |
157 INIT_LIST_HEAD(&slave->sdo_confs); |
237 */ |
239 */ |
238 |
240 |
239 void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */) |
241 void ec_slave_clear(struct kobject *kobj /**< kobject of the slave */) |
240 { |
242 { |
241 ec_slave_t *slave; |
243 ec_slave_t *slave; |
242 ec_sii_string_t *string, *next_str; |
|
243 ec_sii_pdo_t *pdo, *next_pdo; |
244 ec_sii_pdo_t *pdo, *next_pdo; |
244 ec_sii_pdo_entry_t *entry, *next_ent; |
245 ec_sii_pdo_entry_t *entry, *next_ent; |
245 ec_sdo_data_t *sdodata, *next_sdodata; |
246 ec_sdo_data_t *sdodata, *next_sdodata; |
|
247 unsigned int i; |
246 |
248 |
247 slave = container_of(kobj, ec_slave_t, kobj); |
249 slave = container_of(kobj, ec_slave_t, kobj); |
248 |
250 |
249 // free all string objects |
251 // free all strings |
250 list_for_each_entry_safe(string, next_str, &slave->sii_strings, list) { |
252 if (slave->sii_strings) { |
251 list_del(&string->list); |
253 for (i = 0; i < slave->sii_string_count; i++) |
252 kfree(string); |
254 kfree(slave->sii_strings[i]); |
|
255 kfree(slave->sii_strings); |
253 } |
256 } |
254 |
257 |
255 // free all sync managers |
258 // free all sync managers |
256 if (slave->sii_syncs) kfree(slave->sii_syncs); |
259 if (slave->sii_syncs) kfree(slave->sii_syncs); |
257 |
260 |
258 // free all PDOs |
261 // free all PDOs |
259 list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { |
262 list_for_each_entry_safe(pdo, next_pdo, &slave->sii_pdos, list) { |
260 list_del(&pdo->list); |
263 list_del(&pdo->list); |
261 if (pdo->name) kfree(pdo->name); |
|
262 |
264 |
263 // free all PDO entries |
265 // free all PDO entries |
264 list_for_each_entry_safe(entry, next_ent, &pdo->entries, list) { |
266 list_for_each_entry_safe(entry, next_ent, &pdo->entries, list) { |
265 list_del(&entry->list); |
267 list_del(&entry->list); |
266 if (entry->name) kfree(entry->name); |
|
267 kfree(entry); |
268 kfree(entry); |
268 } |
269 } |
269 |
270 |
270 kfree(pdo); |
271 kfree(pdo); |
271 } |
272 } |
272 |
|
273 if (slave->sii_group) kfree(slave->sii_group); |
|
274 if (slave->sii_image) kfree(slave->sii_image); |
|
275 if (slave->sii_order) kfree(slave->sii_order); |
|
276 if (slave->sii_name) kfree(slave->sii_name); |
|
277 |
273 |
278 // free all SDO configurations |
274 // free all SDO configurations |
279 list_for_each_entry_safe(sdodata, next_sdodata, &slave->sdo_confs, list) { |
275 list_for_each_entry_safe(sdodata, next_sdodata, &slave->sdo_confs, list) { |
280 list_del(&sdodata->list); |
276 list_del(&sdodata->list); |
281 kfree(sdodata->data); |
277 kfree(sdodata->data); |
403 int ec_slave_fetch_sii_strings( |
399 int ec_slave_fetch_sii_strings( |
404 ec_slave_t *slave, /**< EtherCAT slave */ |
400 ec_slave_t *slave, /**< EtherCAT slave */ |
405 const uint8_t *data /**< category data */ |
401 const uint8_t *data /**< category data */ |
406 ) |
402 ) |
407 { |
403 { |
408 unsigned int string_count, i; |
404 int i; |
409 size_t size; |
405 size_t size; |
410 off_t offset; |
406 off_t offset; |
411 ec_sii_string_t *string; |
407 |
412 |
408 slave->sii_string_count = data[0]; |
413 string_count = data[0]; |
409 |
|
410 if (!slave->sii_string_count) |
|
411 return 0; |
|
412 |
|
413 if (!(slave->sii_strings = |
|
414 kmalloc(sizeof(char *) * slave->sii_string_count, |
|
415 GFP_KERNEL))) { |
|
416 EC_ERR("Failed to allocate string array memory.\n"); |
|
417 goto out_zero; |
|
418 } |
|
419 |
414 offset = 1; |
420 offset = 1; |
415 for (i = 0; i < string_count; i++) { |
421 for (i = 0; i < slave->sii_string_count; i++) { |
416 size = data[offset]; |
422 size = data[offset]; |
417 // allocate memory for string structure and data at a single blow |
423 // allocate memory for string structure and data at a single blow |
418 if (!(string = (ec_sii_string_t *) |
424 if (!(slave->sii_strings[i] = |
419 kmalloc(sizeof(ec_sii_string_t) + size + 1, GFP_ATOMIC))) { |
425 kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { |
420 EC_ERR("Failed to allocate string memory.\n"); |
426 EC_ERR("Failed to allocate string memory.\n"); |
421 return -1; |
427 goto out_free; |
422 } |
428 } |
423 string->size = size; |
429 memcpy(slave->sii_strings[i], data + offset + 1, size); |
424 // string memory appended to string structure |
430 slave->sii_strings[i][size] = 0x00; // append binary zero |
425 string->data = (char *) string + sizeof(ec_sii_string_t); |
|
426 memcpy(string->data, data + offset + 1, size); |
|
427 string->data[size] = 0x00; |
|
428 list_add_tail(&string->list, &slave->sii_strings); |
|
429 offset += 1 + size; |
431 offset += 1 + size; |
430 } |
432 } |
431 |
433 |
432 return 0; |
434 return 0; |
|
435 |
|
436 out_free: |
|
437 for (i--; i >= 0; i--) kfree(slave->sii_strings[i]); |
|
438 kfree(slave->sii_strings); |
|
439 slave->sii_strings = NULL; |
|
440 out_zero: |
|
441 slave->sii_string_count = 0; |
|
442 return -1; |
433 } |
443 } |
434 |
444 |
435 /*****************************************************************************/ |
445 /*****************************************************************************/ |
436 |
446 |
437 /** |
447 /** |
444 const uint8_t *data /**< category data */ |
454 const uint8_t *data /**< category data */ |
445 ) |
455 ) |
446 { |
456 { |
447 unsigned int i; |
457 unsigned int i; |
448 |
458 |
449 ec_slave_locate_sii_string(slave, data[0], &slave->sii_group); |
459 slave->sii_group = ec_slave_sii_string(slave, data[0]); |
450 ec_slave_locate_sii_string(slave, data[1], &slave->sii_image); |
460 slave->sii_image = ec_slave_sii_string(slave, data[1]); |
451 ec_slave_locate_sii_string(slave, data[2], &slave->sii_order); |
461 slave->sii_order = ec_slave_sii_string(slave, data[2]); |
452 ec_slave_locate_sii_string(slave, data[3], &slave->sii_name); |
462 slave->sii_name = ec_slave_sii_string(slave, data[3]); |
453 |
463 |
454 for (i = 0; i < 4; i++) |
464 for (i = 0; i < 4; i++) |
455 slave->sii_physical_layer[i] = |
465 slave->sii_physical_layer[i] = |
456 (data[4] & (0x03 << (i * 2))) >> (i * 2); |
466 (data[4] & (0x03 << (i * 2))) >> (i * 2); |
457 |
467 |
527 pdo->type = pdo_type; |
537 pdo->type = pdo_type; |
528 |
538 |
529 pdo->index = EC_READ_U16(data); |
539 pdo->index = EC_READ_U16(data); |
530 entry_count = EC_READ_U8(data + 2); |
540 entry_count = EC_READ_U8(data + 2); |
531 pdo->sync_index = EC_READ_U8(data + 3); |
541 pdo->sync_index = EC_READ_U8(data + 3); |
532 pdo->name = NULL; |
542 pdo->name = ec_slave_sii_string(slave, EC_READ_U8(data + 5)); |
533 ec_slave_locate_sii_string(slave, EC_READ_U8(data + 5), &pdo->name); |
|
534 |
543 |
535 list_add_tail(&pdo->list, &slave->sii_pdos); |
544 list_add_tail(&pdo->list, &slave->sii_pdos); |
536 |
545 |
537 word_count -= 4; |
546 word_count -= 4; |
538 data += 8; |
547 data += 8; |
544 return -1; |
553 return -1; |
545 } |
554 } |
546 |
555 |
547 entry->index = EC_READ_U16(data); |
556 entry->index = EC_READ_U16(data); |
548 entry->subindex = EC_READ_U8(data + 2); |
557 entry->subindex = EC_READ_U8(data + 2); |
549 entry->name = NULL; |
558 entry->name = ec_slave_sii_string(slave, EC_READ_U8(data + 3)); |
550 ec_slave_locate_sii_string( |
|
551 slave, EC_READ_U8(data + 3), &entry->name); |
|
552 entry->bit_length = EC_READ_U8(data + 5); |
559 entry->bit_length = EC_READ_U8(data + 5); |
553 |
560 |
554 list_add_tail(&entry->list, &pdo->entries); |
561 list_add_tail(&entry->list, &pdo->entries); |
555 |
562 |
556 word_count -= 4; |
563 word_count -= 4; |
567 Searches the string list for an index and allocates a new string. |
574 Searches the string list for an index and allocates a new string. |
568 \return 0 in case of success, else < 0 |
575 \return 0 in case of success, else < 0 |
569 \todo documentation |
576 \todo documentation |
570 */ |
577 */ |
571 |
578 |
572 int ec_slave_locate_sii_string( |
579 char *ec_slave_sii_string( |
573 ec_slave_t *slave, /**< EtherCAT slave */ |
580 ec_slave_t *slave, /**< EtherCAT slave */ |
574 unsigned int index, /**< string index */ |
581 unsigned int index /**< string index */ |
575 char **ptr /**< Address of the string pointer */ |
|
576 ) |
582 ) |
577 { |
583 { |
578 ec_sii_string_t *string; |
584 if (!index--) |
579 char *err_string; |
585 return NULL; |
580 |
586 |
581 // Erst alten Speicher freigeben |
587 if (index >= slave->sii_string_count) { |
582 if (*ptr) { |
588 if (slave->master->debug_level) |
583 kfree(*ptr); |
589 EC_WARN("String %i not found in slave %i.\n", |
584 *ptr = NULL; |
590 index, slave->ring_position); |
585 } |
591 return NULL; |
586 |
592 } |
587 // Index 0 bedeutet "nicht belegt" |
593 |
588 if (!index) return 0; |
594 return slave->sii_strings[index]; |
589 |
|
590 // EEPROM-String mit Index finden und kopieren |
|
591 list_for_each_entry(string, &slave->sii_strings, list) { |
|
592 if (--index) continue; |
|
593 |
|
594 if (!(*ptr = (char *) kmalloc(string->size + 1, GFP_ATOMIC))) { |
|
595 EC_ERR("Unable to allocate string memory.\n"); |
|
596 return -1; |
|
597 } |
|
598 memcpy(*ptr, string->data, string->size + 1); |
|
599 return 0; |
|
600 } |
|
601 |
|
602 if (slave->master->debug_level) |
|
603 EC_WARN("String %i not found in slave %i.\n", |
|
604 index, slave->ring_position); |
|
605 |
|
606 err_string = "(string not found)"; |
|
607 |
|
608 if (!(*ptr = (char *) kmalloc(strlen(err_string) + 1, GFP_ATOMIC))) { |
|
609 EC_WARN("Unable to allocate string memory.\n"); |
|
610 return -1; |
|
611 } |
|
612 |
|
613 memcpy(*ptr, err_string, strlen(err_string) + 1); |
|
614 return 0; |
|
615 } |
595 } |
616 |
596 |
617 /*****************************************************************************/ |
597 /*****************************************************************************/ |
618 |
598 |
619 /** |
599 /** |