149 sc->alias, sc->position); |
149 sc->alias, sc->position); |
150 return -1; |
150 return -1; |
151 } |
151 } |
152 |
152 |
153 fmmu = &sc->fmmu_configs[sc->used_fmmus++]; |
153 fmmu = &sc->fmmu_configs[sc->used_fmmus++]; |
|
154 |
|
155 down(&sc->master->master_sem); |
154 ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir); |
156 ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir); |
|
157 up(&sc->master->master_sem); |
|
158 |
155 return fmmu->logical_start_address; |
159 return fmmu->logical_start_address; |
156 } |
160 } |
157 |
161 |
158 /*****************************************************************************/ |
162 /*****************************************************************************/ |
159 |
163 |
386 if (sync_index >= EC_MAX_SYNCS) { |
390 if (sync_index >= EC_MAX_SYNCS) { |
387 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
391 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
388 return -1; |
392 return -1; |
389 } |
393 } |
390 |
394 |
391 if (!(pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index))) |
395 down(&sc->master->master_sem); |
392 return -1; |
396 |
|
397 if (!(pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, |
|
398 pdo_index))) { |
|
399 up(&sc->master->master_sem); |
|
400 return -1; |
|
401 } |
393 pdo->sync_index = sync_index; |
402 pdo->sync_index = sync_index; |
394 |
403 |
395 ec_slave_config_load_default_mapping(sc, pdo); |
404 ec_slave_config_load_default_mapping(sc, pdo); |
|
405 |
|
406 up(&sc->master->master_sem); |
396 return 0; |
407 return 0; |
397 } |
408 } |
398 |
409 |
399 /*****************************************************************************/ |
410 /*****************************************************************************/ |
400 |
411 |
408 if (sync_index >= EC_MAX_SYNCS) { |
419 if (sync_index >= EC_MAX_SYNCS) { |
409 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
420 EC_ERR("Invalid sync manager index %u!\n", sync_index); |
410 return; |
421 return; |
411 } |
422 } |
412 |
423 |
|
424 down(&sc->master->master_sem); |
413 ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); |
425 ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos); |
|
426 up(&sc->master->master_sem); |
414 } |
427 } |
415 |
428 |
416 /*****************************************************************************/ |
429 /*****************************************************************************/ |
417 |
430 |
418 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
431 int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc, |
419 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
432 uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex, |
420 uint8_t entry_bit_length) |
433 uint8_t entry_bit_length) |
421 { |
434 { |
422 uint8_t sync_index; |
435 uint8_t sync_index; |
423 ec_pdo_t *pdo = NULL; |
436 ec_pdo_t *pdo = NULL; |
|
437 int retval = -1; |
424 |
438 |
425 if (sc->master->debug_level) |
439 if (sc->master->debug_level) |
426 EC_DBG("ecrt_slave_config_pdo_mapping_add(sc = 0x%x, " |
440 EC_DBG("ecrt_slave_config_pdo_mapping_add(sc = 0x%x, " |
427 "pdo_index = 0x%04X, entry_index = 0x%04X, " |
441 "pdo_index = 0x%04X, entry_index = 0x%04X, " |
428 "entry_subindex = 0x%02X, entry_bit_length = %u)\n", |
442 "entry_subindex = 0x%02X, entry_bit_length = %u)\n", |
432 for (sync_index = 0; sync_index < EC_MAX_SYNCS; sync_index++) |
446 for (sync_index = 0; sync_index < EC_MAX_SYNCS; sync_index++) |
433 if ((pdo = ec_pdo_list_find_pdo( |
447 if ((pdo = ec_pdo_list_find_pdo( |
434 &sc->sync_configs[sync_index].pdos, pdo_index))) |
448 &sc->sync_configs[sync_index].pdos, pdo_index))) |
435 break; |
449 break; |
436 |
450 |
437 if (!pdo) { |
451 if (pdo) { |
|
452 down(&sc->master->master_sem); |
|
453 retval = ec_pdo_add_entry(pdo, entry_index, entry_subindex, |
|
454 entry_bit_length) ? 0 : -1; |
|
455 up(&sc->master->master_sem); |
|
456 } else { |
438 EC_ERR("Pdo 0x%04X is not assigned in config %u:%u.\n", |
457 EC_ERR("Pdo 0x%04X is not assigned in config %u:%u.\n", |
439 pdo_index, sc->alias, sc->position); |
458 pdo_index, sc->alias, sc->position); |
440 return -1; |
459 } |
441 } |
460 |
442 |
461 return retval; |
443 return ec_pdo_add_entry(pdo, entry_index, entry_subindex, |
|
444 entry_bit_length) ? 0 : -1; |
|
445 } |
462 } |
446 |
463 |
447 /*****************************************************************************/ |
464 /*****************************************************************************/ |
448 |
465 |
449 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
466 void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc, |
459 for (sync_index = 0; sync_index < EC_MAX_SYNCS; sync_index++) |
476 for (sync_index = 0; sync_index < EC_MAX_SYNCS; sync_index++) |
460 if ((pdo = ec_pdo_list_find_pdo( |
477 if ((pdo = ec_pdo_list_find_pdo( |
461 &sc->sync_configs[sync_index].pdos, pdo_index))) |
478 &sc->sync_configs[sync_index].pdos, pdo_index))) |
462 break; |
479 break; |
463 |
480 |
464 if (!pdo) { |
481 if (pdo) { |
|
482 down(&sc->master->master_sem); |
|
483 ec_pdo_clear_entries(pdo); |
|
484 up(&sc->master->master_sem); |
|
485 } else { |
465 EC_WARN("Pdo 0x%04X is not assigned in config %u:%u.\n", |
486 EC_WARN("Pdo 0x%04X is not assigned in config %u:%u.\n", |
466 pdo_index, sc->alias, sc->position); |
487 pdo_index, sc->alias, sc->position); |
467 return; |
488 } |
468 } |
|
469 |
|
470 ec_pdo_clear_entries(pdo); |
|
471 } |
489 } |
472 |
490 |
473 /*****************************************************************************/ |
491 /*****************************************************************************/ |
474 |
492 |
475 int ecrt_slave_config_sync_managers(ec_slave_config_t *sc, |
493 int ecrt_slave_config_sync_managers(ec_slave_config_t *sc, |
561 list_for_each_entry(pdo, &sync_config->pdos.list, list) { |
579 list_for_each_entry(pdo, &sync_config->pdos.list, list) { |
562 list_for_each_entry(entry, &pdo->entries, list) { |
580 list_for_each_entry(entry, &pdo->entries, list) { |
563 if (entry->index != index || entry->subindex != subindex) { |
581 if (entry->index != index || entry->subindex != subindex) { |
564 bit_offset += entry->bit_length; |
582 bit_offset += entry->bit_length; |
565 } else { |
583 } else { |
566 sync_offset = ec_slave_config_prepare_fmmu( |
|
567 sc, domain, sync_index, sync_config->dir); |
|
568 if (sync_offset < 0) |
|
569 return -2; |
|
570 |
|
571 bit_pos = bit_offset % 8; |
584 bit_pos = bit_offset % 8; |
572 if (bit_position) { |
585 if (bit_position) { |
573 *bit_position = bit_pos; |
586 *bit_position = bit_pos; |
574 } else if (bit_pos) { |
587 } else if (bit_pos) { |
575 EC_ERR("Pdo entry 0x%04X:%02X does not byte-align " |
588 EC_ERR("Pdo entry 0x%04X:%02X does not byte-align " |
576 "in config %u:%u.\n", index, subindex, |
589 "in config %u:%u.\n", index, subindex, |
577 sc->alias, sc->position); |
590 sc->alias, sc->position); |
578 return -3; |
591 return -3; |
579 } |
592 } |
580 |
593 |
|
594 sync_offset = ec_slave_config_prepare_fmmu( |
|
595 sc, domain, sync_index, sync_config->dir); |
|
596 if (sync_offset < 0) |
|
597 return -2; |
|
598 |
581 return sync_offset + bit_offset / 8; |
599 return sync_offset + bit_offset / 8; |
582 } |
600 } |
583 } |
601 } |
584 } |
602 } |
585 } |
603 } |
676 |
697 |
677 // prepare data for optional writing |
698 // prepare data for optional writing |
678 memset(req->data, 0x00, size); |
699 memset(req->data, 0x00, size); |
679 req->data_size = size; |
700 req->data_size = size; |
680 |
701 |
|
702 down(&sc->master->master_sem); |
681 list_add_tail(&req->list, &sc->sdo_requests); |
703 list_add_tail(&req->list, &sc->sdo_requests); |
|
704 up(&sc->master->master_sem); |
|
705 |
682 return req; |
706 return req; |
683 } |
707 } |
684 |
708 |
685 /*****************************************************************************/ |
709 /*****************************************************************************/ |
686 |
710 |