--- 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 */