Index: asp3_tinet_ecnl_arm/trunk/asp3_dcre/mbed/hal/mbed_ticker_api.c
===================================================================
--- asp3_tinet_ecnl_arm/trunk/asp3_dcre/mbed/hal/mbed_ticker_api.c	(revision 352)
+++ asp3_tinet_ecnl_arm/trunk/asp3_dcre/mbed/hal/mbed_ticker_api.c	(revision 374)
@@ -14,47 +14,342 @@
  * limitations under the License.
  */
+#include <stdio.h>
 #include <stddef.h>
 #include "hal/ticker_api.h"
 #include "platform/mbed_critical.h"
-
-void ticker_set_handler(const ticker_data_t *const data, ticker_event_handler handler) {
-    data->interface->init();
-
-    data->queue->event_handler = handler;
-}
-
-void ticker_irq_handler(const ticker_data_t *const data) {
-    data->interface->clear_interrupt();
+#include "platform/mbed_assert.h"
+
+static void schedule_interrupt(const ticker_data_t *const ticker);
+static void update_present_time(const ticker_data_t *const ticker);
+
+/*
+ * Initialize a ticker instance.
+ */
+static void initialize(const ticker_data_t *ticker)
+{
+    // return if the queue has already been initialized, in that case the
+    // interface used by the queue is already initialized.
+    if (ticker->queue->initialized) {
+        return;
+    }
+    if (ticker->queue->suspended) {
+        return;
+    }
+
+    ticker->interface->init();
+
+    const ticker_info_t *info = ticker->interface->get_info();
+    uint32_t frequency = info->frequency;
+    if (info->frequency == 0) {
+        MBED_ASSERT(0);
+        frequency = 1000000;
+    }
+
+    uint8_t frequency_shifts = 0;
+    for (uint8_t i = 31; i > 0; --i) {
+        if ((1 << i) == frequency) {
+            frequency_shifts = i;
+            break;
+        }
+    }
+
+    uint32_t bits = info->bits;
+    if ((info->bits > 32) || (info->bits < 4)) {
+        MBED_ASSERT(0);
+        bits = 32;
+    }
+    uint32_t max_delta = 0x7 << (bits - 4); // 7/16th
+    uint64_t max_delta_us =
+        ((uint64_t)max_delta * 1000000 + frequency - 1) / frequency;
+
+    ticker->queue->event_handler = NULL;
+    ticker->queue->head = NULL;
+    ticker->queue->tick_last_read = ticker->interface->read();
+    ticker->queue->tick_remainder = 0;
+    ticker->queue->frequency = frequency;
+    ticker->queue->frequency_shifts = frequency_shifts;
+    ticker->queue->bitmask = ((uint64_t)1 << bits) - 1;
+    ticker->queue->max_delta = max_delta;
+    ticker->queue->max_delta_us = max_delta_us;
+    ticker->queue->present_time = 0;
+    ticker->queue->dispatching = false;
+    ticker->queue->suspended = false;
+    ticker->queue->initialized = true;
+
+    update_present_time(ticker);
+    schedule_interrupt(ticker);
+}
+
+/**
+ * Set the event handler function of a ticker instance.
+ */
+static void set_handler(const ticker_data_t *const ticker, ticker_event_handler handler)
+{
+    ticker->queue->event_handler = handler;
+}
+
+/*
+ * Convert a 32 bit timestamp into a 64 bit timestamp.
+ *
+ * A 64 bit timestamp is used as the point of time of reference while the
+ * timestamp to convert is relative to this point of time.
+ *
+ * The lower 32 bits of the timestamp returned will be equal to the timestamp to
+ * convert.
+ *
+ * If the timestamp to convert is less than the lower 32 bits of the time
+ * reference then the timestamp to convert is seen as an overflowed value and
+ * the upper 32 bit of the timestamp returned will be equal to the upper 32 bit
+ * of the reference point + 1.
+ * Otherwise, the upper 32 bit returned will be equal to the upper 32 bit of the
+ * reference point.
+ *
+ * @param ref: The 64 bit timestamp of reference.
+ * @param timestamp: The timestamp to convert.
+ */
+static us_timestamp_t convert_timestamp(us_timestamp_t ref, timestamp_t timestamp)
+{
+    bool overflow = timestamp < ((timestamp_t) ref) ? true : false;
+
+    us_timestamp_t result = (ref & ~((us_timestamp_t)UINT32_MAX)) | timestamp;
+    if (overflow) {
+        result += (1ULL << 32);
+    }
+
+    return result;
+}
+
+/**
+ * Update the present timestamp value of a ticker.
+ */
+static void update_present_time(const ticker_data_t *const ticker)
+{
+    ticker_event_queue_t *queue = ticker->queue;
+    if (queue->suspended) {
+        return;
+    }
+    uint32_t ticker_time = ticker->interface->read();
+    if (ticker_time == ticker->queue->tick_last_read) {
+        // No work to do
+        return;
+    }
+
+    uint64_t elapsed_ticks = (ticker_time - queue->tick_last_read) & queue->bitmask;
+    queue->tick_last_read = ticker_time;
+
+    uint64_t elapsed_us;
+    if (1000000 == queue->frequency) {
+        // Optimized for 1MHz
+
+        elapsed_us = elapsed_ticks;
+    } else if (0 != queue->frequency_shifts) {
+        // Optimized for frequencies divisible by 2
+        uint64_t us_x_ticks = elapsed_ticks * 1000000;
+        elapsed_us = us_x_ticks >> queue->frequency_shifts;
+
+        // Update remainder
+        queue->tick_remainder += us_x_ticks - (elapsed_us << queue->frequency_shifts);
+        if (queue->tick_remainder >= queue->frequency) {
+            elapsed_us += 1;
+            queue->tick_remainder -= queue->frequency;
+        }
+    } else {
+        // General case
+
+        uint64_t us_x_ticks = elapsed_ticks * 1000000;
+        elapsed_us = us_x_ticks / queue->frequency;
+
+        // Update remainder
+        queue->tick_remainder += us_x_ticks - elapsed_us * queue->frequency;
+        if (queue->tick_remainder >= queue->frequency) {
+            elapsed_us += 1;
+            queue->tick_remainder -= queue->frequency;
+        }
+    }
+
+    // Update current time
+    queue->present_time += elapsed_us;
+}
+
+/**
+ * Given the absolute timestamp compute the hal tick timestamp rounded up.
+ */
+static timestamp_t compute_tick_round_up(const ticker_data_t *const ticker, us_timestamp_t timestamp)
+{
+    ticker_event_queue_t *queue = ticker->queue;
+    us_timestamp_t delta_us = timestamp - queue->present_time;
+
+    timestamp_t delta = ticker->queue->max_delta;
+    if (delta_us <=  ticker->queue->max_delta_us) {
+        // Checking max_delta_us ensures the operation will not overflow
+
+        if (1000000 == queue->frequency) {
+            // Optimized for 1MHz
+
+            delta = delta_us;
+            if (delta > ticker->queue->max_delta) {
+                delta = ticker->queue->max_delta;
+            }
+        } else if (0 != queue->frequency_shifts) {
+            // Optimized frequencies divisible by 2
+
+            delta = ((delta_us << ticker->queue->frequency_shifts) + 1000000 - 1) / 1000000;
+            if (delta > ticker->queue->max_delta) {
+                delta = ticker->queue->max_delta;
+            }
+        } else {
+            // General case
+
+            delta = (delta_us * queue->frequency + 1000000 - 1) / 1000000;
+            if (delta > ticker->queue->max_delta) {
+                delta = ticker->queue->max_delta;
+            }
+        }
+    }
+    return (queue->tick_last_read + delta) & queue->bitmask;
+}
+
+/**
+ * Return 1 if the tick has incremented to or past match_tick, otherwise 0.
+ */
+int _ticker_match_interval_passed(timestamp_t prev_tick, timestamp_t cur_tick, timestamp_t match_tick)
+{
+    if (match_tick > prev_tick) {
+        return (cur_tick >= match_tick) || (cur_tick < prev_tick);
+    } else {
+        return (cur_tick < prev_tick) && (cur_tick >= match_tick);
+    }
+}
+
+/**
+ * Compute the time when the interrupt has to be triggered and schedule it.
+ *
+ * If there is no event in the queue or the next event to execute is in more
+ * than ticker.queue.max_delta ticks from now then the ticker irq will be
+ * scheduled in ticker.queue.max_delta ticks. Otherwise the irq will be
+ * scheduled to happen when the running counter reach the timestamp of the
+ * first event in the queue.
+ *
+ * @note If there is no event in the queue then the interrupt is scheduled to
+ * in ticker.queue.max_delta. This is necessary to keep track
+ * of the timer overflow.
+ */
+static void schedule_interrupt(const ticker_data_t *const ticker)
+{
+    ticker_event_queue_t *queue = ticker->queue;
+    if (queue->suspended || ticker->queue->dispatching) {
+        // Don't schedule the next interrupt until dispatching is
+        // finished. This prevents repeated calls to interface->set_interrupt
+        return;
+    }
+
+    update_present_time(ticker);
+
+    if (ticker->queue->head) {
+        us_timestamp_t present = ticker->queue->present_time;
+        us_timestamp_t match_time = ticker->queue->head->timestamp;
+
+        // if the event at the head of the queue is in the past then schedule
+        // it immediately.
+        if (match_time <= present) {
+            ticker->interface->fire_interrupt();
+            return;
+        }
+
+        timestamp_t match_tick = compute_tick_round_up(ticker, match_time);
+
+        // The same tick should never occur since match_tick is rounded up.
+        // If the same tick is returned scheduling will not work correctly.
+        MBED_ASSERT(match_tick != queue->tick_last_read);
+
+        ticker->interface->set_interrupt(match_tick);
+        timestamp_t cur_tick = ticker->interface->read();
+
+        if (_ticker_match_interval_passed(queue->tick_last_read, cur_tick, match_tick)) {
+            ticker->interface->fire_interrupt();
+        }
+    } else {
+        uint32_t match_tick =
+            (queue->tick_last_read + queue->max_delta) & queue->bitmask;
+        ticker->interface->set_interrupt(match_tick);
+    }
+}
+
+void ticker_set_handler(const ticker_data_t *const ticker, ticker_event_handler handler)
+{
+    initialize(ticker);
+
+    core_util_critical_section_enter();
+    set_handler(ticker, handler);
+    core_util_critical_section_exit();
+}
+
+void ticker_irq_handler(const ticker_data_t *const ticker)
+{
+    core_util_critical_section_enter();
+
+    ticker->interface->clear_interrupt();
+    if (ticker->queue->suspended) {
+        core_util_critical_section_exit();
+        return;
+    }
 
     /* Go through all the pending TimerEvents */
+    ticker->queue->dispatching = true;
     while (1) {
-        if (data->queue->head == NULL) {
-            // There are no more TimerEvents left, so disable matches.
-            data->interface->disable_interrupt();
-            return;
-        }
-
-        if ((int)(data->queue->head->timestamp - data->interface->read()) <= 0) {
+        if (ticker->queue->head == NULL) {
+            break;
+        }
+
+        // update the current timestamp used by the queue
+        update_present_time(ticker);
+
+        if (ticker->queue->head->timestamp <= ticker->queue->present_time) {
             // This event was in the past:
             //      point to the following one and execute its handler
-            ticker_event_t *p = data->queue->head;
-            data->queue->head = data->queue->head->next;
-            if (data->queue->event_handler != NULL) {
-                (*data->queue->event_handler)(p->id); // NOTE: the handler can set new events
+            ticker_event_t *p = ticker->queue->head;
+            ticker->queue->head = ticker->queue->head->next;
+            if (ticker->queue->event_handler != NULL) {
+                (*ticker->queue->event_handler)(p->id); // NOTE: the handler can set new events
             }
             /* Note: We continue back to examining the head because calling the
              * event handler may have altered the chain of pending events. */
         } else {
-            // This event and the following ones in the list are in the future:
-            //      set it as next interrupt and return
-            data->interface->set_interrupt(data->queue->head->timestamp);
-            return;
-        }
-    }
-}
-
-void ticker_insert_event(const ticker_data_t *const data, ticker_event_t *obj, timestamp_t timestamp, uint32_t id) {
-    /* disable interrupts for the duration of the function */
-    core_util_critical_section_enter();
+            break;
+        }
+    }
+    ticker->queue->dispatching = false;
+
+    schedule_interrupt(ticker);
+
+    core_util_critical_section_exit();
+}
+
+void ticker_insert_event(const ticker_data_t *const ticker, ticker_event_t *obj, timestamp_t timestamp, uint32_t id)
+{
+    core_util_critical_section_enter();
+
+    // update the current timestamp
+    update_present_time(ticker);
+    us_timestamp_t absolute_timestamp = convert_timestamp(
+                                            ticker->queue->present_time,
+                                            timestamp
+                                        );
+
+    // defer to ticker_insert_event_us
+    ticker_insert_event_us(
+        ticker,
+        obj, absolute_timestamp, id
+    );
+
+    core_util_critical_section_exit();
+}
+
+void ticker_insert_event_us(const ticker_data_t *const ticker, ticker_event_t *obj, us_timestamp_t timestamp, uint32_t id)
+{
+    core_util_critical_section_enter();
+
+    // update the current timestamp
+    update_present_time(ticker);
 
     // initialise our data
@@ -65,8 +360,8 @@
        an element this should come before (which is possibly the
        head). */
-    ticker_event_t *prev = NULL, *p = data->queue->head;
+    ticker_event_t *prev = NULL, *p = ticker->queue->head;
     while (p != NULL) {
         /* check if we come before p */
-        if ((int)(timestamp - p->timestamp) < 0) {
+        if (timestamp < p->timestamp) {
             break;
         }
@@ -75,5 +370,5 @@
         p = p->next;
     }
-    
+
     /* if we're at the end p will be NULL, which is correct */
     obj->next = p;
@@ -81,6 +376,6 @@
     /* if prev is NULL we're at the head */
     if (prev == NULL) {
-        data->queue->head = obj;
-        data->interface->set_interrupt(timestamp);
+        ticker->queue->head = obj;
+        schedule_interrupt(ticker);
     } else {
         prev->next = obj;
@@ -90,19 +385,16 @@
 }
 
-void ticker_remove_event(const ticker_data_t *const data, ticker_event_t *obj) {
+void ticker_remove_event(const ticker_data_t *const ticker, ticker_event_t *obj)
+{
     core_util_critical_section_enter();
 
     // remove this object from the list
-    if (data->queue->head == obj) {
+    if (ticker->queue->head == obj) {
         // first in the list, so just drop me
-        data->queue->head = obj->next;
-        if (data->queue->head == NULL) {
-            data->interface->disable_interrupt();
-        } else {
-            data->interface->set_interrupt(data->queue->head->timestamp);
-        }
+        ticker->queue->head = obj->next;
+        schedule_interrupt(ticker);
     } else {
         // find the object before me, then drop me
-        ticker_event_t* p = data->queue->head;
+        ticker_event_t *p = ticker->queue->head;
         while (p != NULL) {
             if (p->next == obj) {
@@ -117,7 +409,18 @@
 }
 
-timestamp_t ticker_read(const ticker_data_t *const data)
-{
-    return data->interface->read();
+timestamp_t ticker_read(const ticker_data_t *const ticker)
+{
+    return ticker_read_us(ticker);
+}
+
+us_timestamp_t ticker_read_us(const ticker_data_t *const ticker)
+{
+    initialize(ticker);
+
+    core_util_critical_section_enter();
+    update_present_time(ticker);
+    core_util_critical_section_exit();
+
+    return ticker->queue->present_time;
 }
 
@@ -136,2 +439,28 @@
     return ret;
 }
+
+void ticker_suspend(const ticker_data_t *const ticker)
+{
+    core_util_critical_section_enter();
+
+    ticker->queue->suspended = true;
+
+    core_util_critical_section_exit();
+}
+
+void ticker_resume(const ticker_data_t *const ticker)
+{
+    core_util_critical_section_enter();
+
+    ticker->queue->suspended = false;
+    if (ticker->queue->initialized) {
+        ticker->queue->tick_last_read = ticker->interface->read();
+
+        update_present_time(ticker);
+        schedule_interrupt(ticker);
+    } else {
+        initialize(ticker);
+    }
+
+    core_util_critical_section_exit();
+}
