Implemented SDO reading with wait queues.
authorFlorian Pose <fp@igh-essen.com>
Fri, 09 Mar 2007 15:11:29 +0000
changeset 646 fbbd4e54e031
parent 645 473ec2246ec1
child 647 dc556a8c8fed
Implemented SDO reading with wait queues.
NEWS
TODO
master/canopen.c
master/canopen.h
master/fsm_master.c
master/master.c
master/master.h
--- a/NEWS	Fri Mar 09 14:00:32 2007 +0000
+++ b/NEWS	Fri Mar 09 15:11:29 2007 +0000
@@ -35,6 +35,7 @@
 * All EEPROM write operations from user space are now blocking until
   writing has finished and return appropriate error codes.
 * Implemented setting of secondary slave address (alias) via sysfs.
+* Implemented SDO reading in operation mode via sysfs.
 * Removed annoying eeprom_write_enable file. EEPROM writing always enabled.
 * Removed EtherCAT line comments from 8139too drivers.
 
--- a/TODO	Fri Mar 09 14:00:32 2007 +0000
+++ b/TODO	Fri Mar 09 15:11:29 2007 +0000
@@ -24,7 +24,7 @@
   - Calculate expected working counter for domains.
   - Optimize alignment of process data.
   - Evaluate EEPROM contents after writing.
-  - SDO dictionary and -access in operation mode.
+  - SDO dictionary fetching in operation mode.
   - SDO write access in sysfs.
   - Speed up IDLE state machine through fast mode with schedule().
   - Configure slave ports to automatically open on link detection.
--- a/master/canopen.c	Fri Mar 09 14:00:32 2007 +0000
+++ b/master/canopen.c	Fri Mar 09 15:11:29 2007 +0000
@@ -320,6 +320,9 @@
         off += sprintf(buffer + off, "%s\n", request->data);
     }
     else {
+        off += sprintf(buffer + off,
+                "Unknown data type %04X, bit size %u. Data:\n",
+                entry->data_type, entry->bit_length);
         for (i = 0; i < request->size; i++)
             off += sprintf(buffer + off, "%02X (%c)\n",
                            request->data[i], request->data[i]);
@@ -339,33 +342,34 @@
     off_t off = 0;
     ec_sdo_request_t request;
 
-    if (down_interruptible(&master->sdo_sem)) {
+    ec_sdo_request_init_read(&request, sdo, entry);
+
+    // schedule request.
+    down(&master->sdo_sem);
+    list_add_tail(&request.list, &master->sdo_requests);
+    up(&master->sdo_sem);
+
+    // wait for processing through FSM
+    if (wait_event_interruptible(master->sdo_queue,
+                request.state != EC_REQ_QUEUED)) {
         // interrupted by signal
-        return -ERESTARTSYS;
-    }
-
-    ec_sdo_request_init_read(&request, sdo, entry);
-
-    // this is necessary, because the completion object
-    // is completed by the ec_master_flush_sdo_requests() function.
-    INIT_COMPLETION(master->sdo_complete);
-
-    master->sdo_request = &request;
-    master->sdo_seq_user++;
-    master->sdo_timer.expires = jiffies + 10;
-    add_timer(&master->sdo_timer);
-
-    wait_for_completion(&master->sdo_complete);
-
-    master->sdo_request = NULL;
-    up(&master->sdo_sem);
-
-    if (request.return_code == 1 && request.data) {
-        off += ec_sdo_entry_format_data(entry, &request, buffer);
-    }
-    else {
-        off = -EINVAL;
-    }
+        down(&master->sdo_sem);
+        if (request.state == EC_REQ_QUEUED) {
+            list_del(&request.list);
+            up(&master->sdo_sem);
+            return -EINTR;
+        }
+        // request already processing: interrupt not possible.
+        up(&master->sdo_sem);
+    }
+
+    // wait until master FSM has finished processing
+    wait_event(master->sdo_queue, request.state != EC_REQ_BUSY);
+
+    if (request.state != EC_REQ_COMPLETED)
+        return -EIO;
+
+    off += ec_sdo_entry_format_data(entry, &request, buffer);
 
     ec_sdo_request_clear(&request);
     return off;
@@ -405,7 +409,7 @@
     req->entry = entry;
     req->data = NULL;
     req->size = 0;
-    req->return_code = 0;
+    req->state = EC_REQ_QUEUED;
 }
 
 /*****************************************************************************/
--- a/master/canopen.h	Fri Mar 09 14:00:32 2007 +0000
+++ b/master/canopen.h	Fri Mar 09 15:11:29 2007 +0000
@@ -108,12 +108,12 @@
 
 typedef struct
 {
-    struct list_head queue; /**< list item */
+    struct list_head list; /**< list item */
     ec_sdo_t *sdo;
     ec_sdo_entry_t *entry;
     uint8_t *data; /**< pointer to SDO data */
     size_t size; /**< size of SDO data */
-    int return_code;
+    ec_request_state_t state;
 }
 ec_sdo_request_t;
 
--- a/master/fsm_master.c	Fri Mar 09 14:00:32 2007 +0000
+++ b/master/fsm_master.c	Fri Mar 09 15:11:29 2007 +0000
@@ -316,6 +316,63 @@
 /*****************************************************************************/
 
 /**
+ * Check for pending SDO requests and process one.
+ * \return non-zero, if an SDO request is processed.
+ */
+
+int ec_fsm_master_action_process_sdo(
+        ec_fsm_master_t *fsm /**< master state machine */
+        )
+{
+    ec_master_t *master = fsm->master;
+    ec_sdo_request_t *request;
+    ec_slave_t *slave;
+
+    // search the first request to be processed
+    while (1) {
+        down(&master->sdo_sem);
+        if (list_empty(&master->sdo_requests)) {
+            up(&master->sdo_sem);
+            break;
+        }
+        // get first request
+        request =
+            list_entry(master->sdo_requests.next, ec_sdo_request_t, list);
+        list_del_init(&request->list); // dequeue
+        request->state = EC_REQ_BUSY;
+        up(&master->sdo_sem);
+
+        slave = request->sdo->slave;
+        if (slave->current_state == EC_SLAVE_STATE_INIT ||
+                slave->online_state == EC_SLAVE_OFFLINE ||
+                slave->error_flag) {
+            EC_ERR("Discarding SDO request, slave %i not ready.\n",
+                    slave->ring_position);
+            request->state = EC_REQ_ERROR;
+            wake_up(&master->sdo_queue);
+            continue;
+        }
+
+        // found pending SDO request. execute it!
+        if (master->debug_level)
+            EC_DBG("Processing SDO request for slave %i...\n",
+                    slave->ring_position);
+
+        // start uploading SDO
+        fsm->slave = slave;
+        fsm->sdo_request = request;
+        fsm->state = ec_fsm_master_state_sdo_request;
+        ec_fsm_coe_upload(&fsm->fsm_coe, slave, fsm->sdo_request);
+        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
+        return 1;
+    }
+
+    return 0;
+}
+
+/*****************************************************************************/
+
+/**
    Master action: PROC_STATES.
    Processes the slave states.
 */
@@ -359,31 +416,12 @@
     // Check, if EoE processing has to be started
     ec_master_eoe_start(master);
 
+    // Check for a pending SDO request
+    if (ec_fsm_master_action_process_sdo(fsm))
+        return;
+
     if (master->mode == EC_MASTER_MODE_IDLE) {
 
-        // Check for a pending SDO request
-        if (master->sdo_seq_master != master->sdo_seq_user) {
-            if (master->debug_level)
-                EC_DBG("Processing SDO request...\n");
-            slave = master->sdo_request->sdo->slave;
-            if (slave->current_state == EC_SLAVE_STATE_INIT
-                || slave->online_state == EC_SLAVE_OFFLINE) {
-                EC_ERR("Failed to process SDO request, slave %i not ready.\n",
-                       slave->ring_position);
-                master->sdo_request->return_code = -1;
-                master->sdo_seq_master++;
-            }
-            else {
-                // start uploading SDO
-                fsm->slave = slave;
-                fsm->sdo_request = master->sdo_request;
-                fsm->state = ec_fsm_master_state_sdo_request;
-                ec_fsm_coe_upload(&fsm->fsm_coe, slave, fsm->sdo_request);
-                ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
-                return;
-            }
-        }
-
         // check, if slaves have an SDO dictionary to read out.
         list_for_each_entry(slave, &master->slaves, list) {
             if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)
@@ -827,16 +865,25 @@
     if (ec_fsm_coe_exec(&fsm->fsm_coe)) return;
 
     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
-        request->return_code = -1;
-        master->sdo_seq_master++;
-        fsm->state = ec_fsm_master_state_error;
-        return;
-    }
-
-    // SDO dictionary fetching finished
-
-    request->return_code = 1;
-    master->sdo_seq_master++;
+        EC_DBG("Failed to process SDO request for slave %i.\n",
+                fsm->slave->ring_position);
+        request->state = EC_REQ_ERROR;
+        wake_up(&master->sdo_queue);
+        fsm->state = ec_fsm_master_state_error;
+        return;
+    }
+
+    // SDO request finished 
+    request->state = EC_REQ_COMPLETED;
+    wake_up(&master->sdo_queue);
+
+    if (master->debug_level)
+        EC_DBG("Finished SDO request for slave %i.\n",
+                fsm->slave->ring_position);
+
+    // check for another SDO request
+    if (ec_fsm_master_action_process_sdo(fsm))
+        return; // processing another request
 
     fsm->state = ec_fsm_master_state_end;
 }
--- a/master/master.c	Fri Mar 09 14:00:32 2007 +0000
+++ b/master/master.c	Fri Mar 09 15:11:29 2007 +0000
@@ -157,14 +157,9 @@
     init_MUTEX(&master->eeprom_sem);
     init_waitqueue_head(&master->eeprom_queue);
 
-    master->sdo_request = NULL;
-    master->sdo_seq_user = 0;
-    master->sdo_seq_master = 0;
+    INIT_LIST_HEAD(&master->sdo_requests);
     init_MUTEX(&master->sdo_sem);
-    init_timer(&master->sdo_timer);
-    master->sdo_timer.function = ec_master_check_sdo;
-    master->sdo_timer.data = (unsigned long) master;
-    init_completion(&master->sdo_complete);
+    init_waitqueue_head(&master->sdo_queue);
 
     // init devices
     if (ec_device_init(&master->main_device, master))
@@ -301,21 +296,6 @@
 /*****************************************************************************/
 
 /**
-   Flushes the SDO request queue.
-*/
-
-void ec_master_flush_sdo_requests(ec_master_t *master)
-{
-    del_timer_sync(&master->sdo_timer);
-    complete(&master->sdo_complete);
-    master->sdo_request = NULL;
-    master->sdo_seq_user = 0;
-    master->sdo_seq_master = 0;
-}
-
-/*****************************************************************************/
-
-/**
    Internal locking callback.
 */
 
@@ -416,7 +396,6 @@
     
     ec_master_eoe_stop(master);
     ec_master_thread_stop(master);
-    ec_master_flush_sdo_requests(master);
     ec_master_destroy_slaves(master);
 }
 
@@ -1238,25 +1217,6 @@
 /*****************************************************************************/
 
 /**
-*/
-
-void ec_master_check_sdo(unsigned long data /**< master pointer */)
-{
-    ec_master_t *master = (ec_master_t *) data;
-
-    if (master->sdo_seq_master != master->sdo_seq_user) {
-        master->sdo_timer.expires = jiffies + 10;
-        add_timer(&master->sdo_timer);
-        return;
-    }
-
-    // master has processed the request
-    complete(&master->sdo_complete);
-}
-
-/*****************************************************************************/
-
-/**
    Measures the time, a frame is on the bus.
    \return 0 in case of success, else < 0
 */
--- a/master/master.h	Fri Mar 09 14:00:32 2007 +0000
+++ b/master/master.h	Fri Mar 09 15:11:29 2007 +0000
@@ -150,12 +150,11 @@
     wait_queue_head_t eeprom_queue; /**< wait queue for EEPROM
                                       write requests from user space */
 
-    ec_sdo_request_t *sdo_request; /**< pointer to the current SDO request */
-    unsigned int sdo_seq_user; /**< sequence number for user space */
-    unsigned int sdo_seq_master; /**< sequence number for master */
-    struct semaphore sdo_sem; /**< SDO semaphore */
-    struct timer_list sdo_timer; /**< timer for polling sdo processing */
-    struct completion sdo_complete; /**< SDO request completion object */
+    struct list_head sdo_requests; /**< SDO access requests */
+    struct semaphore sdo_sem; /**< semaphore protecting the list of
+                                   SDO access requests */
+    wait_queue_head_t sdo_queue; /**< wait queue for SDO access requests
+                                   from user space */
 };
 
 /*****************************************************************************/