diff -r 326d47aa986c -r 1baac79a40cf master/master.c --- a/master/master.c Thu Nov 19 14:39:10 2009 +0100 +++ b/master/master.c Thu Nov 19 14:44:57 2009 +0100 @@ -122,6 +122,7 @@ sema_init(&master->device_sem, 1); master->phase = EC_ORPHANED; + master->active = 0; master->injection_seq_fsm = 0; master->injection_seq_rt = 0; @@ -169,8 +170,10 @@ sema_init(&master->io_sem, 1); master->send_cb = NULL; master->receive_cb = NULL; + master->cb_data = NULL; master->app_send_cb = NULL; master->app_receive_cb = NULL; + master->app_cb_data = NULL; INIT_LIST_HEAD(&master->sii_requests); init_waitqueue_head(&master->sii_queue); @@ -225,12 +228,23 @@ EC_ERR("Failed to allocate synchronisation datagram.\n"); goto out_clear_ref_sync; } + + // init sync monitor datagram + ec_datagram_init(&master->sync_mon_datagram); + snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE, "syncmon"); + ret = ec_datagram_brd(&master->sync_mon_datagram, 0x092c, 4); + if (ret < 0) { + ec_datagram_clear(&master->sync_mon_datagram); + EC_ERR("Failed to allocate sync monitoring datagram.\n"); + goto out_clear_sync; + } + ec_master_find_dc_ref_clock(master); // init character device ret = ec_cdev_init(&master->cdev, master, device_number); if (ret) - goto out_clear_sync; + goto out_clear_sync_mon; #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) master->class_device = device_create(class, NULL, @@ -259,6 +273,8 @@ out_clear_cdev: ec_cdev_clear(&master->cdev); +out_clear_sync_mon: + ec_datagram_clear(&master->sync_mon_datagram); out_clear_sync: ec_datagram_clear(&master->sync_datagram); out_clear_ref_sync: @@ -297,6 +313,7 @@ ec_master_clear_slave_configs(master); ec_master_clear_slaves(master); + ec_datagram_clear(&master->sync_mon_datagram); ec_datagram_clear(&master->sync_datagram); ec_datagram_clear(&master->ref_sync_datagram); ec_fsm_master_clear(&master->fsm); @@ -349,22 +366,25 @@ master->dc_ref_clock = NULL; - // external requests are obsolete, so we wake pending waiters and remove them from the list - // SII + // external requests are obsolete, so we wake pending waiters and remove + // them from the list + // + // SII requests while (1) { ec_sii_write_request_t *request; if (list_empty(&master->sii_requests)) break; // get first request - request = list_entry(master->sii_requests.next, ec_sii_write_request_t, - list); + request = list_entry(master->sii_requests.next, + ec_sii_write_request_t, list); list_del_init(&request->list); // dequeue EC_INFO("Discarding SII request, slave %u does not exist anymore.\n", request->slave->ring_position); request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->sii_queue); } - // Reg + + // Register requests while (1) { ec_reg_request_t *request; if (list_empty(&master->reg_requests)) @@ -378,7 +398,8 @@ request->state = EC_INT_REQUEST_FAILURE; wake_up(&master->reg_queue); } - // SDO + + // SDO requests while (1) { ec_master_sdo_request_t *request; if (list_empty(&master->slave_sdo_requests)) @@ -392,7 +413,8 @@ request->req.state = EC_INT_REQUEST_FAILURE; wake_up(&master->sdo_queue); } - // FOE + + // FoE requests while (1) { ec_master_foe_request_t *request; if (list_empty(&master->foe_requests)) @@ -441,9 +463,10 @@ /** Internal sending callback. */ void ec_master_internal_send_cb( - ec_master_t *master /**< EtherCAT master. */ - ) -{ + void *cb_data /**< Callback data. */ + ) +{ + ec_master_t *master = (ec_master_t *) cb_data; down(&master->io_sem); ecrt_master_send_ext(master); up(&master->io_sem); @@ -454,9 +477,10 @@ /** Internal receiving callback. */ void ec_master_internal_receive_cb( - ec_master_t *master /**< EtherCAT master. */ - ) -{ + void *cb_data /**< Callback data. */ + ) +{ + ec_master_t *master = (ec_master_t *) cb_data; down(&master->io_sem); ecrt_master_receive(master); up(&master->io_sem); @@ -532,6 +556,7 @@ master->send_cb = ec_master_internal_send_cb; master->receive_cb = ec_master_internal_receive_cb; + master->cb_data = master; master->phase = EC_IDLE; ret = ec_master_thread_start(master, ec_master_idle_thread, @@ -633,6 +658,7 @@ master->phase = EC_OPERATION; master->app_send_cb = NULL; master->app_receive_cb = NULL; + master->app_cb_data = NULL; return ret; out_allow: @@ -645,66 +671,17 @@ /** Transition function from OPERATION to IDLE phase. */ -void ec_master_leave_operation_phase(ec_master_t *master - /**< EtherCAT master */) -{ - ec_slave_t *slave; -#ifdef EC_EOE - ec_eoe_t *eoe; -#endif +void ec_master_leave_operation_phase( + ec_master_t *master /**< EtherCAT master */ + ) +{ + if (master->active) + ecrt_master_deactivate(master); if (master->debug_level) EC_DBG("OPERATION -> IDLE.\n"); master->phase = EC_IDLE; - -#ifdef EC_EOE - ec_master_eoe_stop(master); -#endif - ec_master_thread_stop(master); - - master->send_cb = ec_master_internal_send_cb; - master->receive_cb = ec_master_internal_receive_cb; - - down(&master->master_sem); - ec_master_clear_domains(master); - ec_master_clear_slave_configs(master); - up(&master->master_sem); - - for (slave = master->slaves; - slave < master->slaves + master->slave_count; - slave++) { - - // set states for all slaves - ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); - - // mark for reconfiguration, because the master could have no - // possibility for a reconfiguration between two sequential operation - // phases. - slave->force_config = 1; - } - -#ifdef EC_EOE - // ... but leave EoE slaves in OP - list_for_each_entry(eoe, &master->eoe_handlers, list) { - if (ec_eoe_is_open(eoe)) - ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); - } -#endif - - master->app_time = 0ULL; - master->app_start_time = 0ULL; - master->has_start_time = 0; - - if (ec_master_thread_start(master, ec_master_idle_thread, - "EtherCAT-IDLE")) - EC_WARN("Failed to restart master thread!\n"); -#ifdef EC_EOE - ec_master_eoe_start(master); -#endif - - master->allow_scan = 1; - master->allow_config = 1; } /*****************************************************************************/ @@ -1189,7 +1166,7 @@ goto schedule; // receive datagrams - master->receive_cb(master); + master->receive_cb(master->cb_data); // actual EoE processing sth_to_send = 0; @@ -1209,7 +1186,7 @@ } // (try to) send datagrams down(&master->ext_queue_sem); - master->send_cb(master); + master->send_cb(master->cb_data); up(&master->ext_queue_sem); } @@ -1438,6 +1415,8 @@ /*****************************************************************************/ +#ifdef EC_EOE + /** Get the number of EoE handlers. * * \return Number of EoE handlers. @@ -1480,6 +1459,8 @@ return NULL; } +#endif + /*****************************************************************************/ /** Set the debug level. @@ -1683,6 +1664,11 @@ if (master->debug_level) EC_DBG("ecrt_master_activate(master = 0x%x)\n", (u32) master); + if (master->active) { + EC_WARN("%s: Master already active!\n", __func__); + return 0; + } + down(&master->master_sem); // finish all domains @@ -1718,6 +1704,7 @@ master->send_cb = master->app_send_cb; master->receive_cb = master->app_receive_cb; + master->cb_data = master->app_cb_data; ret = ec_master_thread_start(master, ec_master_operation_thread, "EtherCAT-OP"); @@ -1731,11 +1718,80 @@ master->allow_config = 1; // request the current configuration master->allow_scan = 1; // allow re-scanning on topology change + master->active = 1; return 0; } /*****************************************************************************/ +void ecrt_master_deactivate(ec_master_t *master) +{ + ec_slave_t *slave; +#ifdef EC_EOE + ec_eoe_t *eoe; +#endif + + if (master->debug_level) + EC_DBG("ecrt_master_deactivate(master = 0x%x)\n", (u32) master); + + if (!master->active) { + EC_WARN("%s: Master not active.\n", __func__); + return; + } + +#ifdef EC_EOE + ec_master_eoe_stop(master); +#endif + ec_master_thread_stop(master); + + master->send_cb = ec_master_internal_send_cb; + master->receive_cb = ec_master_internal_receive_cb; + master->cb_data = master; + + down(&master->master_sem); + ec_master_clear_domains(master); + ec_master_clear_slave_configs(master); + up(&master->master_sem); + + for (slave = master->slaves; + slave < master->slaves + master->slave_count; + slave++) { + + // set states for all slaves + ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); + + // mark for reconfiguration, because the master could have no + // possibility for a reconfiguration between two sequential operation + // phases. + slave->force_config = 1; + } + +#ifdef EC_EOE + // ... but leave EoE slaves in OP + list_for_each_entry(eoe, &master->eoe_handlers, list) { + if (ec_eoe_is_open(eoe)) + ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP); + } +#endif + + master->app_time = 0ULL; + master->app_start_time = 0ULL; + master->has_start_time = 0; + + if (ec_master_thread_start(master, ec_master_idle_thread, + "EtherCAT-IDLE")) + EC_WARN("Failed to restart master thread!\n"); +#ifdef EC_EOE + ec_master_eoe_start(master); +#endif + + master->allow_scan = 1; + master->allow_config = 1; + master->active = 0; +} + +/*****************************************************************************/ + void ecrt_master_send(ec_master_t *master) { ec_datagram_t *datagram, *n; @@ -1895,15 +1951,16 @@ /*****************************************************************************/ void ecrt_master_callbacks(ec_master_t *master, - void (*send_cb)(ec_master_t *), void (*receive_cb)(ec_master_t *)) + void (*send_cb)(void *), void (*receive_cb)(void *), void *cb_data) { if (master->debug_level) EC_DBG("ecrt_master_callbacks(master = 0x%x, send_cb = 0x%x, " - " receive_cb = 0x%x)\n", (u32) master, (u32) send_cb, - (u32) receive_cb); + " receive_cb = 0x%x, cb_data = 0x%x)\n", (u32) master, + (u32) send_cb, (u32) receive_cb, (u32) cb_data); master->app_send_cb = send_cb; master->app_receive_cb = receive_cb; + master->app_cb_data = cb_data; } /*****************************************************************************/ @@ -1945,10 +2002,30 @@ /*****************************************************************************/ +void ecrt_master_sync_monitor_queue(ec_master_t *master) +{ + ec_datagram_zero(&master->sync_mon_datagram); + ec_master_queue_datagram(master, &master->sync_mon_datagram); +} + +/*****************************************************************************/ + +uint32_t ecrt_master_sync_monitor_process(ec_master_t *master) +{ + if (master->sync_mon_datagram.state == EC_DATAGRAM_RECEIVED) { + return EC_READ_U32(master->sync_mon_datagram.data) & 0x7fffffff; + } else { + return 0xffffffff; + } +} + +/*****************************************************************************/ + /** \cond */ EXPORT_SYMBOL(ecrt_master_create_domain); EXPORT_SYMBOL(ecrt_master_activate); +EXPORT_SYMBOL(ecrt_master_deactivate); EXPORT_SYMBOL(ecrt_master_send); EXPORT_SYMBOL(ecrt_master_send_ext); EXPORT_SYMBOL(ecrt_master_receive); @@ -1958,6 +2035,8 @@ EXPORT_SYMBOL(ecrt_master_application_time); EXPORT_SYMBOL(ecrt_master_sync_reference_clock); EXPORT_SYMBOL(ecrt_master_sync_slave_clocks); +EXPORT_SYMBOL(ecrt_master_sync_monitor_queue); +EXPORT_SYMBOL(ecrt_master_sync_monitor_process); /** \endcond */