diff --git a/TODO b/TODO index ae4eec10..448b1bb4 100644 --- a/TODO +++ b/TODO @@ -13,8 +13,7 @@ Version 1.5.0: * Distributed clocks: - Delay calculation. - Synchronize the reference clock to the master clock. - - Reference clock zero. - - Configuration must exist, config state machine. + - Output reference clock and application time in 'ethercat master'. * Read alias from register 0x0012 instead of SII. * Finish library implementation. * Re-work EoE code. diff --git a/master/fsm_master.c b/master/fsm_master.c index b1de6f9a..66f22ca7 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -828,6 +828,9 @@ void ec_fsm_master_state_scan_slave( master->scan_busy = 0; wake_up_interruptible(&master->scan_queue); + // find DC reference clock + ec_master_find_dc_ref_clock(master); + // Attach slave configurations ec_master_attach_slave_configs(master); diff --git a/master/master.c b/master/master.c index 99efbdf3..7a641d4f 100644 --- a/master/master.c +++ b/master/master.c @@ -128,6 +128,8 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ INIT_LIST_HEAD(&master->configs); + master->app_time = 0ULL; + master->scan_busy = 0; master->allow_scan = 1; init_MUTEX(&master->scan_sem); @@ -212,12 +214,13 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ // init sync datagram ec_datagram_init(&master->sync_datagram); snprintf(master->sync_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync"); - ret = ec_datagram_armw(&master->sync_datagram, 0 /* FIXME */, 0x0910, 4); + ret = ec_datagram_prealloc(&master->sync_datagram, 4); if (ret < 0) { ec_datagram_clear(&master->sync_datagram); EC_ERR("Failed to allocate synchronisation datagram.\n"); goto out_clear_ref_sync; } + ec_master_find_dc_ref_clock(master); // init character device ret = ec_cdev_init(&master->cdev, master, device_number); @@ -1344,6 +1347,30 @@ int ec_master_debug_level( return 0; } +/*****************************************************************************/ + +/** Finds the DC reference clock. + */ +void ec_master_find_dc_ref_clock( + ec_master_t *master /**< EtherCAT master. */ + ) +{ + ec_slave_t *slave; + uint16_t ref_clock_addr = 0xffff; + + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + if (slave->base_dc_supported) { + ref_clock_addr = slave->station_address; + break; + } + } + + // This call always succeeds, because the datagram has been pre-allocated. + ec_datagram_frmw(&master->sync_datagram, ref_clock_addr, 0x0910, 4); +} + /****************************************************************************** * Application interface *****************************************************************************/ diff --git a/master/master.h b/master/master.h index 3bd91edc..b7659f08 100644 --- a/master/master.h +++ b/master/master.h @@ -233,6 +233,8 @@ ec_domain_t *ecrt_master_create_domain_err(ec_master_t *); ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *, uint16_t, uint16_t, uint32_t, uint32_t); +void ec_master_find_dc_ref_clock(ec_master_t *); + /*****************************************************************************/ #endif