317 |
317 |
318 slave->base_type = EC_READ_U8 (datagram->data); |
318 slave->base_type = EC_READ_U8 (datagram->data); |
319 slave->base_revision = EC_READ_U8 (datagram->data + 1); |
319 slave->base_revision = EC_READ_U8 (datagram->data + 1); |
320 slave->base_build = EC_READ_U16(datagram->data + 2); |
320 slave->base_build = EC_READ_U16(datagram->data + 2); |
321 slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); |
321 slave->base_fmmu_count = EC_READ_U8 (datagram->data + 4); |
322 slave->base_sync_count = EC_READ_U8 (datagram->data + 5); |
|
323 |
322 |
324 if (slave->base_fmmu_count > EC_MAX_FMMUS) |
323 if (slave->base_fmmu_count > EC_MAX_FMMUS) |
325 slave->base_fmmu_count = EC_MAX_FMMUS; |
324 slave->base_fmmu_count = EC_MAX_FMMUS; |
326 |
325 |
327 // read data link status |
326 // read data link status |
659 void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *fsm /**< slave state machine */) |
658 void ec_fsm_slave_conf_enter_sync(ec_fsm_slave_t *fsm /**< slave state machine */) |
660 { |
659 { |
661 ec_master_t *master = fsm->slave->master; |
660 ec_master_t *master = fsm->slave->master; |
662 ec_slave_t *slave = fsm->slave; |
661 ec_slave_t *slave = fsm->slave; |
663 ec_datagram_t *datagram = fsm->datagram; |
662 ec_datagram_t *datagram = fsm->datagram; |
664 const ec_sii_sync_t *sync; |
663 unsigned int i; |
665 ec_sii_sync_t mbox_sync; |
|
666 |
664 |
667 // slave is now in INIT |
665 // slave is now in INIT |
668 if (slave->current_state == slave->requested_state) { |
666 if (slave->current_state == slave->requested_state) { |
669 fsm->state = ec_fsm_slave_state_end; // successful |
667 fsm->state = ec_fsm_slave_state_end; // successful |
670 if (master->debug_level) { |
668 if (master->debug_level) { |
672 slave->ring_position); |
670 slave->ring_position); |
673 } |
671 } |
674 return; |
672 return; |
675 } |
673 } |
676 |
674 |
677 if (!slave->base_sync_count) { // no sync managers |
675 if (!slave->sii_mailbox_protocols || slave->sii_sync_count < 2) { |
|
676 // no mailbox sync managers to be configured |
678 ec_fsm_slave_conf_enter_preop(fsm); |
677 ec_fsm_slave_conf_enter_preop(fsm); |
679 return; |
678 return; |
680 } |
679 } |
681 |
680 |
682 if (master->debug_level) { |
681 if (master->debug_level) { |
683 EC_DBG("Configuring sync managers of slave %i.\n", |
682 EC_DBG("Configuring mailbox sync managers of slave %i.\n", |
684 slave->ring_position); |
683 slave->ring_position); |
685 } |
684 } |
686 |
685 |
687 // configure sync managers |
686 // configure sync managers |
688 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
687 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
689 EC_SYNC_SIZE * slave->base_sync_count); |
688 EC_SYNC_SIZE * slave->sii_sync_count); |
690 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
689 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->sii_sync_count); |
691 |
690 |
692 if (list_empty(&slave->sii_syncs)) { |
691 for (i = 0; i < 2; i++) { |
693 if (slave->sii_rx_mailbox_offset && slave->sii_tx_mailbox_offset) { |
692 ec_slave_sync_config(slave, &slave->sii_syncs[i], |
694 if (slave->master->debug_level) |
693 datagram->data + EC_SYNC_SIZE * i); |
695 EC_DBG("Guessing sync manager settings for slave %i.\n", |
|
696 slave->ring_position); |
|
697 mbox_sync.index = 0; |
|
698 mbox_sync.physical_start_address = slave->sii_tx_mailbox_offset; |
|
699 mbox_sync.length = slave->sii_tx_mailbox_size; |
|
700 mbox_sync.control_register = 0x26; |
|
701 mbox_sync.enable = 0x01; |
|
702 mbox_sync.est_length = 0; |
|
703 ec_slave_sync_config(slave, &mbox_sync, |
|
704 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
|
705 mbox_sync.index = 1; |
|
706 mbox_sync.physical_start_address = slave->sii_rx_mailbox_offset; |
|
707 mbox_sync.length = slave->sii_rx_mailbox_size; |
|
708 mbox_sync.control_register = 0x22; |
|
709 mbox_sync.enable = 0x01; |
|
710 mbox_sync.est_length = 0; |
|
711 ec_slave_sync_config(slave, &mbox_sync, |
|
712 datagram->data + EC_SYNC_SIZE * mbox_sync.index); |
|
713 } |
|
714 } |
|
715 else if (slave->sii_mailbox_protocols) { // mailboxes present |
|
716 list_for_each_entry(sync, &slave->sii_syncs, list) { |
|
717 // only configure mailbox sync-managers |
|
718 if (sync->index != 0 && sync->index != 1) continue; |
|
719 ec_slave_sync_config(slave, sync, |
|
720 datagram->data + EC_SYNC_SIZE * sync->index); |
|
721 } |
|
722 } |
694 } |
723 |
695 |
724 ec_master_queue_datagram(fsm->slave->master, datagram); |
696 ec_master_queue_datagram(fsm->slave->master, datagram); |
725 fsm->retries = EC_FSM_RETRIES; |
697 fsm->retries = EC_FSM_RETRIES; |
726 fsm->state = ec_fsm_slave_conf_state_sync; |
698 fsm->state = ec_fsm_slave_conf_state_sync; |
818 |
790 |
819 void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *fsm /**< slave state machine */) |
791 void ec_fsm_slave_conf_enter_sync2(ec_fsm_slave_t *fsm /**< slave state machine */) |
820 { |
792 { |
821 ec_slave_t *slave = fsm->slave; |
793 ec_slave_t *slave = fsm->slave; |
822 ec_datagram_t *datagram = fsm->datagram; |
794 ec_datagram_t *datagram = fsm->datagram; |
823 ec_sii_sync_t *sync; |
795 unsigned int i; |
824 |
796 |
825 if (list_empty(&slave->sii_syncs)) { |
797 if (!slave->sii_sync_count) { |
826 ec_fsm_slave_conf_enter_fmmu(fsm); |
798 ec_fsm_slave_conf_enter_fmmu(fsm); |
827 return; |
799 return; |
828 } |
800 } |
829 |
801 |
830 // configure sync managers for process data |
802 // configure sync managers for process data |
831 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
803 ec_datagram_npwr(datagram, slave->station_address, 0x0800, |
832 EC_SYNC_SIZE * slave->base_sync_count); |
804 EC_SYNC_SIZE * slave->sii_sync_count); |
833 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count); |
805 memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->sii_sync_count); |
834 |
806 |
835 list_for_each_entry(sync, &slave->sii_syncs, list) { |
807 for (i = 0; i < slave->sii_sync_count; i++) { |
836 ec_slave_sync_config(slave, sync, |
808 ec_slave_sync_config(slave, &slave->sii_syncs[i], |
837 datagram->data + EC_SYNC_SIZE * sync->index); |
809 datagram->data + EC_SYNC_SIZE * i); |
838 } |
810 } |
839 |
811 |
840 ec_master_queue_datagram(fsm->slave->master, datagram); |
812 ec_master_queue_datagram(fsm->slave->master, datagram); |
841 fsm->retries = EC_FSM_RETRIES; |
813 fsm->retries = EC_FSM_RETRIES; |
842 fsm->state = ec_fsm_slave_conf_state_sync2; |
814 fsm->state = ec_fsm_slave_conf_state_sync2; |