174 if (fmmu->domain == domain && fmmu->sync_index == sync_index) |
174 if (fmmu->domain == domain && fmmu->sync_index == sync_index) |
175 return fmmu->domain_address; |
175 return fmmu->domain_address; |
176 } |
176 } |
177 |
177 |
178 if (sc->used_fmmus == EC_MAX_FMMUS) { |
178 if (sc->used_fmmus == EC_MAX_FMMUS) { |
179 EC_ERR("FMMU limit reached for slave configuration %u:%u!\n", |
179 EC_CONFIG_ERR(sc, "FMMU limit reached!\n"); |
180 sc->alias, sc->position); |
|
181 return -EOVERFLOW; |
180 return -EOVERFLOW; |
182 } |
181 } |
183 |
182 |
184 fmmu = &sc->fmmu_configs[sc->used_fmmus]; |
183 fmmu = &sc->fmmu_configs[sc->used_fmmus]; |
185 |
184 |
218 if (sc->slave) |
217 if (sc->slave) |
219 return 0; // already attached |
218 return 0; // already attached |
220 |
219 |
221 if (!(slave = ec_master_find_slave( |
220 if (!(slave = ec_master_find_slave( |
222 sc->master, sc->alias, sc->position))) { |
221 sc->master, sc->alias, sc->position))) { |
223 if (sc->master->debug_level) |
222 EC_CONFIG_DBG(sc, 1, "Failed to find slave for configuration.\n"); |
224 EC_DBG("Failed to find slave for configuration %u:%u.\n", |
|
225 sc->alias, sc->position); |
|
226 return -ENOENT; |
223 return -ENOENT; |
227 } |
224 } |
228 |
225 |
229 if (slave->config) { |
226 if (slave->config) { |
230 if (sc->master->debug_level) |
227 EC_CONFIG_DBG(sc, 1, "Failed to attach configuration. Slave %u" |
231 EC_DBG("Failed to attach slave configuration %u:%u. Slave %u" |
228 " already has a configuration!\n", slave->ring_position); |
232 " already has a configuration!\n", sc->alias, |
|
233 sc->position, slave->ring_position); |
|
234 return -EEXIST; |
229 return -EEXIST; |
235 } |
230 } |
236 |
231 |
237 if (slave->sii.vendor_id != sc->vendor_id |
232 if (slave->sii.vendor_id != sc->vendor_id |
238 || slave->sii.product_code != sc->product_code) { |
233 || slave->sii.product_code != sc->product_code) { |
239 if (sc->master->debug_level) |
234 EC_CONFIG_DBG(sc, 1, "Slave %u has an invalid type (0x%08X/0x%08X)" |
240 EC_DBG("Slave %u has an invalid type (0x%08X/0x%08X) for" |
235 " for configuration (0x%08X/0x%08X).\n", |
241 " configuration %u:%u (0x%08X/0x%08X).\n", |
236 slave->ring_position, slave->sii.vendor_id, |
242 slave->ring_position, slave->sii.vendor_id, |
237 slave->sii.product_code, sc->vendor_id, sc->product_code); |
243 slave->sii.product_code, sc->alias, sc->position, |
|
244 sc->vendor_id, sc->product_code); |
|
245 return -EINVAL; |
238 return -EINVAL; |
246 } |
239 } |
247 |
240 |
248 // attach slave |
241 // attach slave |
249 slave->config = sc; |
242 slave->config = sc; |
250 sc->slave = slave; |
243 sc->slave = slave; |
251 |
244 |
252 ec_slave_request_state(slave, EC_SLAVE_STATE_OP); |
245 ec_slave_request_state(slave, EC_SLAVE_STATE_OP); |
253 |
246 |
254 if (sc->master->debug_level) |
247 EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position); |
255 EC_DBG("Attached slave %u to config %u:%u.\n", |
|
256 slave->ring_position, sc->alias, sc->position); |
|
257 |
248 |
258 return 0; |
249 return 0; |
259 } |
250 } |
260 |
251 |
261 /*****************************************************************************/ |
252 /*****************************************************************************/ |
288 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { |
279 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { |
289 sync_config = &sc->sync_configs[sync_index]; |
280 sync_config = &sc->sync_configs[sync_index]; |
290 if ((sync = ec_slave_get_sync(sc->slave, sync_index))) { |
281 if ((sync = ec_slave_get_sync(sc->slave, sync_index))) { |
291 sync_config->dir = ec_sync_default_direction(sync); |
282 sync_config->dir = ec_sync_default_direction(sync); |
292 if (sync_config->dir == EC_DIR_INVALID) |
283 if (sync_config->dir == EC_DIR_INVALID) |
293 EC_WARN("SM%u of slave %u has an invalid direction field!\n", |
284 EC_SLAVE_WARN(sc->slave, |
294 sync_index, sc->slave->ring_position); |
285 "SM%u has an invalid direction field!\n", sync_index); |
295 ec_pdo_list_copy(&sync_config->pdos, &sync->pdos); |
286 ec_pdo_list_copy(&sync_config->pdos, &sync->pdos); |
296 } |
287 } |
297 } |
288 } |
298 } |
289 } |
299 |
290 |
311 const ec_pdo_t *default_pdo; |
302 const ec_pdo_t *default_pdo; |
312 |
303 |
313 if (!sc->slave) |
304 if (!sc->slave) |
314 return; |
305 return; |
315 |
306 |
316 if (sc->master->debug_level) |
307 EC_CONFIG_DBG(sc, 1, "Loading default mapping for PDO 0x%04X.\n", |
317 EC_DBG("Loading default mapping for PDO 0x%04X in config %u:%u.\n", |
308 pdo->index); |
318 pdo->index, sc->alias, sc->position); |
|
319 |
309 |
320 // find PDO in any sync manager (it could be reassigned later) |
310 // find PDO in any sync manager (it could be reassigned later) |
321 for (i = 0; i < sc->slave->sii.sync_count; i++) { |
311 for (i = 0; i < sc->slave->sii.sync_count; i++) { |
322 sync = &sc->slave->sii.syncs[i]; |
312 sync = &sc->slave->sii.syncs[i]; |
323 |
313 |
324 list_for_each_entry(default_pdo, &sync->pdos.list, list) { |
314 list_for_each_entry(default_pdo, &sync->pdos.list, list) { |
325 if (default_pdo->index != pdo->index) |
315 if (default_pdo->index != pdo->index) |
326 continue; |
316 continue; |
327 |
317 |
328 if (default_pdo->name) { |
318 if (default_pdo->name) { |
329 if (sc->master->debug_level) |
319 EC_CONFIG_DBG(sc, 1, "Found PDO name \"%s\".\n", |
330 EC_DBG("Found PDO name \"%s\".\n", default_pdo->name); |
320 default_pdo->name); |
331 |
321 |
332 // take PDO name from assigned one |
322 // take PDO name from assigned one |
333 ec_pdo_set_name(pdo, default_pdo->name); |
323 ec_pdo_set_name(pdo, default_pdo->name); |
334 } |
324 } |
335 |
325 |
338 return; |
328 return; |
339 |
329 |
340 if (sc->master->debug_level) { |
330 if (sc->master->debug_level) { |
341 const ec_pdo_entry_t *entry; |
331 const ec_pdo_entry_t *entry; |
342 list_for_each_entry(entry, &pdo->entries, list) { |
332 list_for_each_entry(entry, &pdo->entries, list) { |
343 EC_DBG("Entry 0x%04X:%02X.\n", |
333 EC_CONFIG_DBG(sc, 1, "Entry 0x%04X:%02X.\n", |
344 entry->index, entry->subindex); |
334 entry->index, entry->subindex); |
345 } |
335 } |
346 } |
336 } |
347 |
337 |
348 return; |
338 return; |
349 } |
339 } |
350 } |
340 } |
351 |
341 |
352 if (sc->master->debug_level) |
342 EC_CONFIG_DBG(sc, 1, "No default mapping found.\n"); |
353 EC_DBG("No default mapping found.\n"); |
|
354 } |
343 } |
355 |
344 |
356 /*****************************************************************************/ |
345 /*****************************************************************************/ |
357 |
346 |
358 /** Get the number of SDO configurations. |
347 /** Get the number of SDO configurations. |
442 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
431 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index, |
443 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
432 ec_direction_t dir, ec_watchdog_mode_t watchdog_mode) |
444 { |
433 { |
445 ec_sync_config_t *sync_config; |
434 ec_sync_config_t *sync_config; |
446 |
435 |
447 if (sc->master->debug_level) |
436 EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p," |
448 EC_DBG("ecrt_slave_config_sync_manager(sc = 0x%p, sync_index = %u, " |
437 " sync_index = %u, dir = %i, watchdog_mode = %i)\n", |
449 "dir = %i, watchdog_mode = %i)\n", |
438 sc, sync_index, dir, watchdog_mode); |
450 sc, sync_index, dir, watchdog_mode); |
|
451 |
439 |
452 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
440 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
453 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
441 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
454 return -ENOENT; |
442 return -ENOENT; |
455 } |
443 } |
456 |
444 |
457 if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) { |
445 if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) { |
458 EC_ERR("Invalid direction %u!\n", (u32) dir); |
446 EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (u32) dir); |
459 return -EINVAL; |
447 return -EINVAL; |
460 } |
448 } |
461 |
449 |
462 sync_config = &sc->sync_configs[sync_index]; |
450 sync_config = &sc->sync_configs[sync_index]; |
463 sync_config->dir = dir; |
451 sync_config->dir = dir; |
468 /*****************************************************************************/ |
456 /*****************************************************************************/ |
469 |
457 |
470 void ecrt_slave_config_watchdog(ec_slave_config_t *sc, |
458 void ecrt_slave_config_watchdog(ec_slave_config_t *sc, |
471 uint16_t divider, uint16_t intervals) |
459 uint16_t divider, uint16_t intervals) |
472 { |
460 { |
473 if (sc->master->debug_level) |
461 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, divider = %u, intervals = %u)\n", |
474 EC_DBG("%s(sc = 0x%p, divider = %u, intervals = %u)\n", |
462 __func__, sc, divider, intervals); |
475 __func__, sc, divider, intervals); |
|
476 |
463 |
477 sc->watchdog_divider = divider; |
464 sc->watchdog_divider = divider; |
478 sc->watchdog_intervals = intervals; |
465 sc->watchdog_intervals = intervals; |
479 } |
466 } |
480 |
467 |
495 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
482 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc, |
496 uint8_t sync_index, uint16_t pdo_index) |
483 uint8_t sync_index, uint16_t pdo_index) |
497 { |
484 { |
498 ec_pdo_t *pdo; |
485 ec_pdo_t *pdo; |
499 |
486 |
500 if (sc->master->debug_level) |
487 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, " |
501 EC_DBG("ecrt_slave_config_pdo_assign_add(sc = 0x%p, sync_index = %u, " |
488 "pdo_index = 0x%04X)\n", __func__, sc, sync_index, pdo_index); |
502 "pdo_index = 0x%04X)\n", sc, sync_index, pdo_index); |
|
503 |
489 |
504 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
490 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
505 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
491 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
506 return -EINVAL; |
492 return -EINVAL; |
507 } |
493 } |
508 |
494 |
509 down(&sc->master->master_sem); |
495 down(&sc->master->master_sem); |
510 |
496 |
524 /*****************************************************************************/ |
510 /*****************************************************************************/ |
525 |
511 |
526 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
512 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc, |
527 uint8_t sync_index) |
513 uint8_t sync_index) |
528 { |
514 { |
529 if (sc->master->debug_level) |
515 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u)\n", |
530 EC_DBG("ecrt_slave_config_pdo_assign_clear(sc = 0x%p, " |
516 __func__, sc, sync_index); |
531 "sync_index = %u)\n", sc, sync_index); |
|
532 |
517 |
533 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
518 if (sync_index >= EC_MAX_SYNC_MANAGERS) { |
534 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
519 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index); |
535 return; |
520 return; |
536 } |
521 } |
537 |
522 |
538 down(&sc->master->master_sem); |
523 down(&sc->master->master_sem); |
539 ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); |
524 ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); |
549 uint8_t sync_index; |
534 uint8_t sync_index; |
550 ec_pdo_t *pdo = NULL; |
535 ec_pdo_t *pdo = NULL; |
551 ec_pdo_entry_t *entry; |
536 ec_pdo_entry_t *entry; |
552 int retval = 0; |
537 int retval = 0; |
553 |
538 |
554 if (sc->master->debug_level) |
539 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, " |
555 EC_DBG("ecrt_slave_config_pdo_mapping_add(sc = 0x%p, " |
540 "pdo_index = 0x%04X, entry_index = 0x%04X, " |
556 "pdo_index = 0x%04X, entry_index = 0x%04X, " |
541 "entry_subindex = 0x%02X, entry_bit_length = %u)\n", |
557 "entry_subindex = 0x%02X, entry_bit_length = %u)\n", |
542 __func__, sc, pdo_index, entry_index, entry_subindex, |
558 sc, pdo_index, entry_index, entry_subindex, |
543 entry_bit_length); |
559 entry_bit_length); |
|
560 |
544 |
561 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) |
545 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) |
562 if ((pdo = ec_pdo_list_find_pdo( |
546 if ((pdo = ec_pdo_list_find_pdo( |
563 &sc->sync_configs[sync_index].pdos, pdo_index))) |
547 &sc->sync_configs[sync_index].pdos, pdo_index))) |
564 break; |
548 break; |
585 uint16_t pdo_index) |
568 uint16_t pdo_index) |
586 { |
569 { |
587 uint8_t sync_index; |
570 uint8_t sync_index; |
588 ec_pdo_t *pdo = NULL; |
571 ec_pdo_t *pdo = NULL; |
589 |
572 |
590 if (sc->master->debug_level) |
573 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n", |
591 EC_DBG("ecrt_slave_config_pdo_mapping_clear(sc = 0x%p, " |
574 __func__, sc, pdo_index); |
592 "pdo_index = 0x%04X)\n", sc, pdo_index); |
|
593 |
575 |
594 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) |
576 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) |
595 if ((pdo = ec_pdo_list_find_pdo( |
577 if ((pdo = ec_pdo_list_find_pdo( |
596 &sc->sync_configs[sync_index].pdos, pdo_index))) |
578 &sc->sync_configs[sync_index].pdos, pdo_index))) |
597 break; |
579 break; |
599 if (pdo) { |
581 if (pdo) { |
600 down(&sc->master->master_sem); |
582 down(&sc->master->master_sem); |
601 ec_pdo_clear_entries(pdo); |
583 ec_pdo_clear_entries(pdo); |
602 up(&sc->master->master_sem); |
584 up(&sc->master->master_sem); |
603 } else { |
585 } else { |
604 EC_WARN("PDO 0x%04X is not assigned in config %u:%u.\n", |
586 EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index); |
605 pdo_index, sc->alias, sc->position); |
|
606 } |
587 } |
607 } |
588 } |
608 |
589 |
609 /*****************************************************************************/ |
590 /*****************************************************************************/ |
610 |
591 |
615 unsigned int i, j, k; |
596 unsigned int i, j, k; |
616 const ec_sync_info_t *sync_info; |
597 const ec_sync_info_t *sync_info; |
617 const ec_pdo_info_t *pdo_info; |
598 const ec_pdo_info_t *pdo_info; |
618 const ec_pdo_entry_info_t *entry_info; |
599 const ec_pdo_entry_info_t *entry_info; |
619 |
600 |
620 if (sc->master->debug_level) |
601 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, n_syncs = %u, syncs = 0x%p)\n", |
621 EC_DBG("ecrt_slave_config_pdos(sc = 0x%p, n_syncs = %u, " |
602 __func__, sc, n_syncs, syncs); |
622 "syncs = 0x%p)\n", sc, n_syncs, syncs); |
|
623 |
603 |
624 if (!syncs) |
604 if (!syncs) |
625 return 0; |
605 return 0; |
626 |
606 |
627 for (i = 0; i < n_syncs; i++) { |
607 for (i = 0; i < n_syncs; i++) { |
629 |
609 |
630 if (sync_info->index == (uint8_t) EC_END) |
610 if (sync_info->index == (uint8_t) EC_END) |
631 break; |
611 break; |
632 |
612 |
633 if (sync_info->index >= EC_MAX_SYNC_MANAGERS) { |
613 if (sync_info->index >= EC_MAX_SYNC_MANAGERS) { |
634 EC_ERR("Invalid sync manager index %u!\n", sync_info->index); |
614 EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", |
|
615 sync_info->index); |
635 return -ENOENT; |
616 return -ENOENT; |
636 } |
617 } |
637 |
618 |
638 ret = ecrt_slave_config_sync_manager(sc, sync_info->index, |
619 ret = ecrt_slave_config_sync_manager(sc, sync_info->index, |
639 sync_info->dir, sync_info->watchdog_mode); |
620 sync_info->dir, sync_info->watchdog_mode); |
687 unsigned int bit_offset, bit_pos; |
668 unsigned int bit_offset, bit_pos; |
688 ec_pdo_t *pdo; |
669 ec_pdo_t *pdo; |
689 ec_pdo_entry_t *entry; |
670 ec_pdo_entry_t *entry; |
690 int sync_offset; |
671 int sync_offset; |
691 |
672 |
692 if (sc->master->debug_level) |
673 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
693 EC_DBG("ecrt_slave_config_reg_pdo_entry(sc = 0x%p, index = 0x%04X, " |
674 "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n", |
694 "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n", |
675 __func__, sc, index, subindex, domain, bit_position); |
695 sc, index, subindex, domain, bit_position); |
|
696 |
676 |
697 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { |
677 for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) { |
698 sync_config = &sc->sync_configs[sync_index]; |
678 sync_config = &sc->sync_configs[sync_index]; |
699 bit_offset = 0; |
679 bit_offset = 0; |
700 |
680 |
705 } else { |
685 } else { |
706 bit_pos = bit_offset % 8; |
686 bit_pos = bit_offset % 8; |
707 if (bit_position) { |
687 if (bit_position) { |
708 *bit_position = bit_pos; |
688 *bit_position = bit_pos; |
709 } else if (bit_pos) { |
689 } else if (bit_pos) { |
710 EC_ERR("PDO entry 0x%04X:%02X does not byte-align " |
690 EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does" |
711 "in config %u:%u.\n", index, subindex, |
691 " not byte-align.\n", index, subindex); |
712 sc->alias, sc->position); |
|
713 return -EFAULT; |
692 return -EFAULT; |
714 } |
693 } |
715 |
694 |
716 sync_offset = ec_slave_config_prepare_fmmu( |
695 sync_offset = ec_slave_config_prepare_fmmu( |
717 sc, domain, sync_index, sync_config->dir); |
696 sc, domain, sync_index, sync_config->dir); |
749 { |
728 { |
750 ec_slave_t *slave = sc->slave; |
729 ec_slave_t *slave = sc->slave; |
751 ec_sdo_request_t *req; |
730 ec_sdo_request_t *req; |
752 int ret; |
731 int ret; |
753 |
732 |
754 if (sc->master->debug_level) |
733 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
755 EC_DBG("ecrt_slave_config_sdo(sc = 0x%p, index = 0x%04X, " |
734 "subindex = 0x%02X, data = 0x%p, size = %zu)\n", |
756 "subindex = 0x%02X, data = 0x%p, size = %zu)\n", sc, |
735 __func__, sc, index, subindex, data, size); |
757 index, subindex, data, size); |
|
758 |
736 |
759 if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
737 if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
760 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
738 EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n"); |
761 return -EPROTONOSUPPORT; // protocol not supported |
|
762 } |
739 } |
763 |
740 |
764 if (!(req = (ec_sdo_request_t *) |
741 if (!(req = (ec_sdo_request_t *) |
765 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { |
742 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { |
766 EC_ERR("Failed to allocate memory for SDO configuration!\n"); |
743 EC_CONFIG_ERR(sc, "Failed to allocate memory for" |
|
744 " SDO configuration!\n"); |
767 return -ENOMEM; |
745 return -ENOMEM; |
768 } |
746 } |
769 |
747 |
770 ec_sdo_request_init(req); |
748 ec_sdo_request_init(req); |
771 ec_sdo_request_address(req, index, subindex); |
749 ec_sdo_request_address(req, index, subindex); |
788 int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index, |
766 int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index, |
789 uint8_t subindex, uint8_t value) |
767 uint8_t subindex, uint8_t value) |
790 { |
768 { |
791 uint8_t data[1]; |
769 uint8_t data[1]; |
792 |
770 |
793 if (sc->master->debug_level) |
771 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
794 EC_DBG("ecrt_slave_config_sdo8(sc = 0x%p, index = 0x%04X, " |
772 "subindex = 0x%02X, value = %u)\n", |
795 "subindex = 0x%02X, value = %u)\n", sc, |
773 __func__, sc, index, subindex, (u32) value); |
796 index, subindex, (u32) value); |
|
797 |
774 |
798 EC_WRITE_U8(data, value); |
775 EC_WRITE_U8(data, value); |
799 return ecrt_slave_config_sdo(sc, index, subindex, data, 1); |
776 return ecrt_slave_config_sdo(sc, index, subindex, data, 1); |
800 } |
777 } |
801 |
778 |
804 int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index, |
781 int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index, |
805 uint8_t subindex, uint16_t value) |
782 uint8_t subindex, uint16_t value) |
806 { |
783 { |
807 uint8_t data[2]; |
784 uint8_t data[2]; |
808 |
785 |
809 if (sc->master->debug_level) |
786 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
810 EC_DBG("ecrt_slave_config_sdo16(sc = 0x%p, index = 0x%04X, " |
787 "subindex = 0x%02X, value = %u)\n", |
811 "subindex = 0x%02X, value = %u)\n", sc, |
788 __func__, sc, index, subindex, value); |
812 index, subindex, value); |
|
813 |
789 |
814 EC_WRITE_U16(data, value); |
790 EC_WRITE_U16(data, value); |
815 return ecrt_slave_config_sdo(sc, index, subindex, data, 2); |
791 return ecrt_slave_config_sdo(sc, index, subindex, data, 2); |
816 } |
792 } |
817 |
793 |
820 int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index, |
796 int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index, |
821 uint8_t subindex, uint32_t value) |
797 uint8_t subindex, uint32_t value) |
822 { |
798 { |
823 uint8_t data[4]; |
799 uint8_t data[4]; |
824 |
800 |
825 if (sc->master->debug_level) |
801 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
826 EC_DBG("ecrt_slave_config_sdo32(sc = 0x%p, index = 0x%04X, " |
802 "subindex = 0x%02X, value = %u)\n", |
827 "subindex = 0x%02X, value = %u)\n", sc, |
803 __func__, sc, index, subindex, value); |
828 index, subindex, value); |
|
829 |
804 |
830 EC_WRITE_U32(data, value); |
805 EC_WRITE_U32(data, value); |
831 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
806 return ecrt_slave_config_sdo(sc, index, subindex, data, 4); |
832 } |
807 } |
833 |
808 |
838 { |
813 { |
839 ec_slave_t *slave = sc->slave; |
814 ec_slave_t *slave = sc->slave; |
840 ec_sdo_request_t *req; |
815 ec_sdo_request_t *req; |
841 int ret; |
816 int ret; |
842 |
817 |
843 if (sc->master->debug_level) |
818 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, " |
844 EC_DBG("ecrt_slave_config_complete_sdo(sc = 0x%p, index = 0x%04X, " |
819 "data = 0x%p, size = %zu)\n", __func__, sc, index, data, size); |
845 "data = 0x%p, size = %zu)\n", sc, index, data, size); |
|
846 |
820 |
847 if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
821 if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) { |
848 EC_ERR("Slave %u does not support CoE!\n", slave->ring_position); |
822 EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n"); |
849 return -EPROTONOSUPPORT; // protocol not supported |
|
850 } |
823 } |
851 |
824 |
852 if (!(req = (ec_sdo_request_t *) |
825 if (!(req = (ec_sdo_request_t *) |
853 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { |
826 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { |
854 EC_ERR("Failed to allocate memory for SDO configuration!\n"); |
827 EC_CONFIG_ERR(sc, "Failed to allocate memory for" |
|
828 " SDO configuration!\n"); |
855 return -ENOMEM; |
829 return -ENOMEM; |
856 } |
830 } |
857 |
831 |
858 ec_sdo_request_init(req); |
832 ec_sdo_request_init(req); |
859 ec_sdo_request_address(req, index, 0); |
833 ec_sdo_request_address(req, index, 0); |
881 ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size) |
855 ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size) |
882 { |
856 { |
883 ec_sdo_request_t *req; |
857 ec_sdo_request_t *req; |
884 int ret; |
858 int ret; |
885 |
859 |
886 if (sc->master->debug_level) |
860 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, " |
887 EC_DBG("ecrt_slave_config_create_sdo_request(sc = 0x%p, " |
861 "index = 0x%04X, subindex = 0x%02X, size = %zu)\n", |
888 "index = 0x%04X, subindex = 0x%02X, size = %zu)\n", sc, |
862 __func__, sc, index, subindex, size); |
889 index, subindex, size); |
|
890 |
863 |
891 if (!(req = (ec_sdo_request_t *) |
864 if (!(req = (ec_sdo_request_t *) |
892 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { |
865 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) { |
893 EC_ERR("Failed to allocate SDO request memory!\n"); |
866 EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n"); |
894 return ERR_PTR(-ENOMEM); |
867 return ERR_PTR(-ENOMEM); |
895 } |
868 } |
896 |
869 |
897 ec_sdo_request_init(req); |
870 ec_sdo_request_init(req); |
898 ec_sdo_request_address(req, index, subindex); |
871 ec_sdo_request_address(req, index, subindex); |
934 ec_slave_config_t *sc, size_t size) |
907 ec_slave_config_t *sc, size_t size) |
935 { |
908 { |
936 ec_voe_handler_t *voe; |
909 ec_voe_handler_t *voe; |
937 int ret; |
910 int ret; |
938 |
911 |
939 if (sc->master->debug_level) |
912 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n", __func__, sc, size); |
940 EC_DBG("ecrt_slave_config_create_voe_handler(sc = 0x%p, size = %zu)\n", |
|
941 sc, size); |
|
942 |
913 |
943 if (!(voe = (ec_voe_handler_t *) |
914 if (!(voe = (ec_voe_handler_t *) |
944 kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) { |
915 kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) { |
945 EC_ERR("Failed to allocate VoE request memory!\n"); |
916 EC_CONFIG_ERR(sc, "Failed to allocate VoE request memory!\n"); |
946 return ERR_PTR(-ENOMEM); |
917 return ERR_PTR(-ENOMEM); |
947 } |
918 } |
948 |
919 |
949 ret = ec_voe_handler_init(voe, sc, size); |
920 ret = ec_voe_handler_init(voe, sc, size); |
950 if (ret < 0) { |
921 if (ret < 0) { |
993 { |
964 { |
994 ec_slave_t *slave = sc->slave; |
965 ec_slave_t *slave = sc->slave; |
995 ec_soe_request_t *req; |
966 ec_soe_request_t *req; |
996 int ret; |
967 int ret; |
997 |
968 |
998 if (sc->master->debug_level) |
969 EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, " |
999 EC_DBG("ecrt_slave_config_idn(sc = 0x%p, idn = 0x%04X, " |
970 "data = 0x%p, size = %zu)\n", __func__, sc, idn, data, size); |
1000 "data = 0x%p, size = %zu)\n", sc, idn, data, size); |
|
1001 |
971 |
1002 if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
972 if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
1003 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
973 EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n"); |
1004 return -EPROTONOSUPPORT; // protocol not supported |
|
1005 } |
974 } |
1006 |
975 |
1007 if (!(req = (ec_soe_request_t *) |
976 if (!(req = (ec_soe_request_t *) |
1008 kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) { |
977 kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) { |
1009 EC_ERR("Failed to allocate memory for IDN configuration!\n"); |
978 EC_CONFIG_ERR(sc, "Failed to allocate memory for" |
|
979 " IDN configuration!\n"); |
1010 return -ENOMEM; |
980 return -ENOMEM; |
1011 } |
981 } |
1012 |
982 |
1013 ec_soe_request_init(req); |
983 ec_soe_request_init(req); |
1014 ec_soe_request_set_idn(req, idn); |
984 ec_soe_request_set_idn(req, idn); |