Merge pull request #4094 from pan-/fix_hal_ticker

[HAL] Add support for 64 bit us timestamp

TESTS/mbed_hal/lp_ticker/main.cpp

@@ -28,7 +28,8 @@
 
 using namespace utest::v1;
 
-volatile static bool complete;
+static volatile bool complete;
+static volatile timestamp_t complete_timestamp;
 static ticker_event_t delay_event;
 static const ticker_data_t *lp_ticker_data = get_lp_ticker_data();
 
@@ -38,6 +39,12 @@ static const ticker_data_t *lp_ticker_data = get_lp_ticker_data();
 #define SHORT_TIMEOUT (600)
 
 void cb_done(uint32_t id) {
+    complete_timestamp = us_ticker_read();
+    complete = true;
+}
+
+void cb_done_deepsleep(uint32_t id) {
+    complete_timestamp = lp_ticker_read();
     complete = true;
 }
 
@@ -53,7 +60,7 @@ void lp_ticker_delay_us(uint32_t delay_us, uint32_t tolerance)
     timestamp_t start = us_ticker_read();
     ticker_insert_event(lp_ticker_data, &delay_event, delay_ts, (uint32_t)&delay_event);
     while (!complete);
-    timestamp_t end = us_ticker_read();
+    timestamp_t end = complete_timestamp;
 
     TEST_ASSERT_UINT32_WITHIN(tolerance, delay_us, end - start);
     TEST_ASSERT_TRUE(complete);
@@ -75,7 +82,7 @@ void lp_ticker_1s_deepsleep()
      */
     wait_ms(10);
 
-    ticker_set_handler(lp_ticker_data, cb_done);
+    ticker_set_handler(lp_ticker_data, cb_done_deepsleep);
     ticker_remove_event(lp_ticker_data, &delay_event);
     delay_ts = lp_ticker_read() + 1000000;
 
@@ -87,7 +94,7 @@ void lp_ticker_1s_deepsleep()
     ticker_insert_event(lp_ticker_data, &delay_event, delay_ts, (uint32_t)&delay_event);
     deepsleep();
     while (!complete);
-    timestamp_t end = lp_ticker_read();
+    timestamp_t end = complete_timestamp;
 
     TEST_ASSERT_UINT32_WITHIN(LONG_TIMEOUT, 1000000, end - start);
     TEST_ASSERT_TRUE(complete);
@@ -106,7 +113,7 @@ void lp_ticker_1s_sleep()
     ticker_insert_event(lp_ticker_data, &delay_event, delay_ts, (uint32_t)&delay_event);
     sleep();
     while (!complete);
-    timestamp_t end = us_ticker_read();
+    timestamp_t end = complete_timestamp;
 
     TEST_ASSERT_UINT32_WITHIN(LONG_TIMEOUT, 1000000, end - start);
     TEST_ASSERT_TRUE(complete);

drivers/Ticker.cpp

@@ -29,16 +29,16 @@ void Ticker::detach() {
     core_util_critical_section_exit();
 }
 
-void Ticker::setup(timestamp_t t) {
+void Ticker::setup(us_timestamp_t t) {
     core_util_critical_section_enter();
     remove();
     _delay = t;
-    insert(_delay + ticker_read(_ticker_data));
+    insert_absolute(_delay + ticker_read_us(_ticker_data));
     core_util_critical_section_exit();
 }
 
 void Ticker::handler() {
-    insert(event.timestamp + _delay);
+    insert_absolute(event.timestamp + _delay);
     _function();
 }
 

drivers/Ticker.h

@@ -100,7 +100,7 @@ public:
      *  @param func pointer to the function to be called
      *  @param t the time between calls in micro-seconds
      */
-    void attach_us(Callback<void()> func, timestamp_t t) {
+    void attach_us(Callback<void()> func, us_timestamp_t t) {
         _function = func;
         setup(t);
     }
@@ -118,7 +118,7 @@ public:
     MBED_DEPRECATED_SINCE("mbed-os-5.1",
         "The attach_us function does not support cv-qualifiers. Replaced by "
         "attach_us(callback(obj, method), t).")
-    void attach_us(T *obj, M method, timestamp_t t) {
+    void attach_us(T *obj, M method, us_timestamp_t t) {
         attach_us(Callback<void()>(obj, method), t);
     }
 
@@ -131,12 +131,12 @@ public:
     void detach();
 
 protected:
-    void setup(timestamp_t t);
+    void setup(us_timestamp_t t);
     virtual void handler();
 
 protected:
-    timestamp_t         _delay;     /* Time delay (in microseconds) for re-setting the multi-shot callback. */
+    us_timestamp_t         _delay;  /**< Time delay (in microseconds) for re-setting the multi-shot callback. */
-    Callback<void()>    _function;  /* Callback. */
+    Callback<void()>    _function;  /**< Callback. */
 };
 
 } // namespace mbed

drivers/Timer.cpp

@@ -31,7 +31,7 @@ Timer::Timer(const ticker_data_t *data) : _running(), _start(), _time(), _ticker
 void Timer::start() {
     core_util_critical_section_enter();
     if (!_running) {
-        _start = ticker_read(_ticker_data);
+        _start = ticker_read_us(_ticker_data);
         _running = 1;
     }
     core_util_critical_section_exit();
@@ -45,10 +45,7 @@ void Timer::stop() {
 }
 
 int Timer::read_us() {
-    core_util_critical_section_enter();
+    return read_high_resolution_us();
-    int time = _time + slicetime();
-    core_util_critical_section_exit();
-    return time;
 }
 
 float Timer::read() {
@@ -56,14 +53,21 @@ float Timer::read() {
 }
 
 int Timer::read_ms() {
-    return read_us() / 1000;
+    return read_high_resolution_us() / 1000;
 }
 
-int Timer::slicetime() {
+us_timestamp_t Timer::read_high_resolution_us() {
+    core_util_critical_section_enter();
+    us_timestamp_t time = _time + slicetime();
+    core_util_critical_section_exit();
+    return time;
+}
+
+us_timestamp_t Timer::slicetime() {
+    us_timestamp_t ret = 0;
     core_util_critical_section_enter();
-    int ret = 0;
     if (_running) {
-        ret = ticker_read(_ticker_data) - _start;
+        ret = ticker_read_us(_ticker_data) - _start;
     }
     core_util_critical_section_exit();
     return ret;
@@ -71,7 +75,7 @@ int Timer::slicetime() {
 
 void Timer::reset() {
     core_util_critical_section_enter();
-    _start = ticker_read(_ticker_data);
+    _start = ticker_read_us(_ticker_data);
     _time = 0;
     core_util_critical_section_exit();
 }

drivers/Timer.h

@@ -82,11 +82,15 @@ public:
      */
     operator float();
 
+    /** Get in a high resolution type the time passed in micro-seconds.
+     */
+    us_timestamp_t read_high_resolution_us();
+
 protected:
-    int slicetime();
+    us_timestamp_t slicetime();
-    int _running;          // whether the timer is running
+    int _running;            // whether the timer is running
-    unsigned int _start;   // the start time of the latest slice
+    us_timestamp_t _start;   // the start time of the latest slice
-    int _time;             // any accumulated time from previous slices
+    us_timestamp_t _time;    // any accumulated time from previous slices
     const ticker_data_t *_ticker_data;
 };
 

drivers/TimerEvent.cpp

@@ -44,6 +44,10 @@ void TimerEvent::insert(timestamp_t timestamp) {
     ticker_insert_event(_ticker_data, &event, timestamp, (uint32_t)this);
 }
 
+void TimerEvent::insert_absolute(us_timestamp_t timestamp) {
+    ticker_insert_event_us(_ticker_data, &event, timestamp, (uint32_t)this);
+}
+
 void TimerEvent::remove() {
     ticker_remove_event(_ticker_data, &event);
 }

drivers/TimerEvent.h

@@ -44,9 +44,12 @@ protected:
     // The handler called to service the timer event of the derived class
     virtual void handler() = 0;
 
-    // insert in to linked list
+    // insert relative timestamp in to linked list
     void insert(timestamp_t timestamp);
 
+    // insert absolute timestamp into linked list
+    void insert_absolute(us_timestamp_t timestamp);
+
     // remove from linked list, if in it
     void remove();
 

hal/mbed_lp_ticker_api.c

@@ -17,7 +17,7 @@
 
 #if DEVICE_LOWPOWERTIMER
 
-static ticker_event_queue_t events;
+static ticker_event_queue_t events = { 0 };
 
 static const ticker_interface_t lp_interface = {
     .init = lp_ticker_init,

hal/mbed_ticker_api.c

@@ -13,50 +13,183 @@
  * See the License for the specific language governing permissions and
  * 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) {
+static void schedule_interrupt(const ticker_data_t *const ticker);
-    data->interface->init();
+static void update_present_time(const ticker_data_t *const ticker);
 
-    data->queue->event_handler = handler;
+/*
+ * 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;
+    }
+
+    ticker->interface->init();
+    
+    ticker->queue->event_handler = NULL;
+    ticker->queue->head = NULL;
+    ticker->queue->present_time = 0;
+    ticker->queue->initialized = true;
+    
+    update_present_time(ticker);
+    schedule_interrupt(ticker);
 }
 
-void ticker_irq_handler(const ticker_data_t *const data) {
+/**
-    data->interface->clear_interrupt();
+ * 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->queue->present_time = convert_timestamp(
+        ticker->queue->present_time, 
+        ticker->interface->read()
+    );
+}
+
+/**
+ * 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 MBED_TICKER_INTERRUPT_TIMESTAMP_MAX_DELTA us from now then the ticker 
+ * irq will be scheduled in MBED_TICKER_INTERRUPT_TIMESTAMP_MAX_DELTA us.
+ * 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 MBED_TICKER_INTERRUPT_TIMESTAMP_MAX_DELTA. This is necessary to keep track 
+ * of the timer overflow.
+ */
+static void schedule_interrupt(const ticker_data_t *const ticker)
+{
+    update_present_time(ticker);
+    uint32_t relative_timeout = MBED_TICKER_INTERRUPT_TIMESTAMP_MAX_DELTA;
+
+    if (ticker->queue->head) {
+        us_timestamp_t present = ticker->queue->present_time;
+        us_timestamp_t next_event_timestamp = ticker->queue->head->timestamp;
+
+        // if the event at the head of the queue is in the past then schedule
+        // it immediately.
+        if (next_event_timestamp < present) {
+            relative_timeout = 0;
+        } else if ((next_event_timestamp - present) < MBED_TICKER_INTERRUPT_TIMESTAMP_MAX_DELTA) {
+            relative_timeout = next_event_timestamp - present;
+        }
+    } 
+
+    ticker->interface->set_interrupt(ticker->queue->present_time + relative_timeout);
+}
+
+void ticker_set_handler(const ticker_data_t *const ticker, ticker_event_handler handler)
+{
+    initialize(ticker);
+    set_handler(ticker, handler);
+}
+
+void ticker_irq_handler(const ticker_data_t *const ticker)
+{
+    ticker->interface->clear_interrupt();
 
     /* Go through all the pending TimerEvents */
     while (1) {
-        if (data->queue->head == NULL) {
+        if (ticker->queue->head == NULL) {
-            // There are no more TimerEvents left, so disable matches.
+            break;
-            data->interface->disable_interrupt();
-            return;
         }
 
-        if ((int)(data->queue->head->timestamp - data->interface->read()) <= 0) {
+        // 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;
+            ticker_event_t *p = ticker->queue->head;
-            data->queue->head = data->queue->head->next;
+            ticker->queue->head = ticker->queue->head->next;
-            if (data->queue->event_handler != NULL) {
+            if (ticker->queue->event_handler != NULL) {
-                (*data->queue->event_handler)(p->id); // NOTE: the handler can set new events
+                (*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:
+            break;
-            //      set it as next interrupt and return
+        } 
-            data->interface->set_interrupt(data->queue->head->timestamp);
-            return;
-        }
     }
+
+    schedule_interrupt(ticker);
 }
 
-void ticker_insert_event(const ticker_data_t *const data, ticker_event_t *obj, timestamp_t timestamp, uint32_t id) {
+void ticker_insert_event(const ticker_data_t *const ticker, ticker_event_t *obj, timestamp_t timestamp, uint32_t id)
-    /* disable interrupts for the duration of the function */
+{
     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
+    );
+    core_util_critical_section_exit();
+
+    // defer to ticker_insert_event_us
+    ticker_insert_event_us(
+        ticker, 
+        obj, absolute_timestamp, id
+    );
+}
+
+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
     obj->timestamp = timestamp;
     obj->id = id;
@@ -64,10 +197,10 @@ void ticker_insert_event(const ticker_data_t *const data, ticker_event_t *obj, t
     /* Go through the list until we either reach the end, or find
        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;
         }
         /* go to the next element */
@@ -80,30 +213,28 @@ void ticker_insert_event(const ticker_data_t *const data, ticker_event_t *obj, t
 
     /* if prev is NULL we're at the head */
     if (prev == NULL) {
-        data->queue->head = obj;
+        ticker->queue->head = obj;
-        data->interface->set_interrupt(timestamp);
     } else {
         prev->next = obj;
     }
 
+    schedule_interrupt(ticker);
+
     core_util_critical_section_exit();
 }
 
-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;
+        ticker->queue->head = obj->next;
-        if (data->queue->head == NULL) {
+        schedule_interrupt(ticker);
-            data->interface->disable_interrupt();
-        } else {
-            data->interface->set_interrupt(data->queue->head->timestamp);
-        }
     } 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) {
                 p->next = obj->next;
@@ -116,9 +247,15 @@ void ticker_remove_event(const ticker_data_t *const data, ticker_event_t *obj) {
     core_util_critical_section_exit();
 }
 
-timestamp_t ticker_read(const ticker_data_t *const data)
+timestamp_t ticker_read(const ticker_data_t *const ticker)
 {
-    return data->interface->read();
+    return ticker_read_us(ticker);
+}
+
+us_timestamp_t ticker_read_us(const ticker_data_t *const ticker)
+{
+    update_present_time(ticker);
+    return ticker->queue->present_time;
 }
 
 int ticker_get_next_timestamp(const ticker_data_t *const data, timestamp_t *timestamp)

hal/mbed_us_ticker_api.c

@@ -15,7 +15,7 @@
  */
 #include "hal/us_ticker_api.h"
 
-static ticker_event_queue_t events;
+static ticker_event_queue_t events = { 0 };
 
 static const ticker_interface_t us_interface = {
     .init = us_ticker_init,
@@ -27,7 +27,7 @@ static const ticker_interface_t us_interface = {
 
 static const ticker_data_t us_data = {
     .interface = &us_interface,
-    .queue = &events,
+    .queue = &events
 };
 
 const ticker_data_t* get_us_ticker_data(void)

hal/ticker_api.h

@@ -20,14 +20,32 @@
 #define MBED_TICKER_API_H
 
 #include <stdint.h>
+#include <stdbool.h>
 #include "device.h"
 
+/**
+ * Maximum delta (in us) between too interrupts.
+ */
+#define MBED_TICKER_INTERRUPT_TIMESTAMP_MAX_DELTA   0x70000000ULL
+
+/**
+ * Legacy format representing a timestamp in us.
+ * Given it is modeled as a 32 bit integer, this type can represent timestamp
+ * up to 4294 seconds (71 minutes).
+ * Prefer using us_timestamp_t which store timestamp as 64 bits integer.
+ */
 typedef uint32_t timestamp_t;
 
+/**
+ * A us timestamp stored in a 64 bit integer.
+ * Can store timestamp up to 584810 years.
+ */
+typedef uint64_t us_timestamp_t;
+
 /** Ticker's event structure
  */
 typedef struct ticker_event_s {
-    timestamp_t            timestamp; /**< Event's timestamp */
+    us_timestamp_t         timestamp; /**< Event's timestamp */
     uint32_t               id;        /**< TimerEvent object */
     struct ticker_event_s *next;      /**< Next event in the queue */
 } ticker_event_t;
@@ -49,6 +67,8 @@ typedef struct {
 typedef struct {
     ticker_event_handler event_handler; /**< Event handler */
     ticker_event_t *head;               /**< A pointer to head */
+    us_timestamp_t present_time;        /**< Store the timestamp used for present time */
+    bool initialized;                   /**< Indicate if the instance is initialized */
 } ticker_event_queue_t;
 
 /** Ticker's data structure
@@ -69,46 +89,84 @@ extern "C" {
 
 /** Initialize a ticker and set the event handler
  *
- * @param data    The ticker's data
+ * @param ticker The ticker object.
  * @param handler A handler to be set
  */
-void ticker_set_handler(const ticker_data_t *const data, ticker_event_handler handler);
+void ticker_set_handler(const ticker_data_t *const ticker, ticker_event_handler handler);
 
 /** IRQ handler that goes through the events to trigger overdue events.
  *
- * @param data    The ticker's data
+ * @param ticker The ticker object.
  */
-void ticker_irq_handler(const ticker_data_t *const data);
+void ticker_irq_handler(const ticker_data_t *const ticker);
 
 /** Remove an event from the queue
  *
- * @param data The ticker's data
+ * @param ticker The ticker object.
  * @param obj  The event object to be removed from the queue
  */
-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);
 
 /** Insert an event to the queue
  *
- * @param data      The ticker's data
+ * The event will be executed in timestamp - ticker_read().
+ *
+ * @warning This function does not consider timestamp in the past. If an event
+ * is inserted with a timestamp less than the current timestamp then the event
+ * will be executed in timestamp - ticker_read() us.
+ * The internal counter wrap very quickly it is hard to decide weither an
+ * event is in the past or in 1 hour.
+ *
+ * @note prefer the use of ticker_insert_event_us which allows registration of
+ * absolute timestamp.
+ *
+ * @param ticker    The ticker object.
  * @param obj       The event object to be inserted to the queue
  * @param timestamp The event's timestamp
  * @param id        The event object
  */
-void ticker_insert_event(const ticker_data_t *const data, ticker_event_t *obj, timestamp_t timestamp, uint32_t id);
+void ticker_insert_event(const ticker_data_t *const ticker, ticker_event_t *obj, timestamp_t timestamp, uint32_t id);
 
-/** Read the current ticker's timestamp
+/** Insert an event to the queue
  *
- * @param data The ticker's data
+ * The event will be executed in timestamp - ticker_read_us() us.
+ *
+ * @warning If an event is inserted with a timestamp less than the current
+ * timestamp then the event will **not** be inserted.
+ *
+ * @param ticker    The ticker object.
+ * @param obj       The event object to be inserted to the queue
+ * @param timestamp The event's timestamp
+ * @param id        The event object
+ */
+void ticker_insert_event_us(const ticker_data_t *const ticker, ticker_event_t *obj, us_timestamp_t timestamp, uint32_t id);
+
+/** Read the current (relative) ticker's timestamp
+ *
+ * @warning Return a relative timestamp because the counter wrap every 4294
+ * seconds.
+ *
+ * @param ticker The ticker object.
  * @return The current timestamp
  */
-timestamp_t ticker_read(const ticker_data_t *const data);
+timestamp_t ticker_read(const ticker_data_t *const ticker);
+
+/** Read the current (absolute) ticker's timestamp
+ *
+ * @warning Return an absolute timestamp counting from the initialization of the
+ * ticker.
+ *
+ * @param ticker The ticker object.
+ * @return The current timestamp
+ */
+us_timestamp_t ticker_read_us(const ticker_data_t *const ticker);
 
 /** Read the next event's timestamp
  *
- * @param data The ticker's data
+ * @param ticker The ticker object.
  * @return 1 if timestamp is pending event, 0 if there's no event pending
  */
-int ticker_get_next_timestamp(const ticker_data_t *const data, timestamp_t *timestamp);
+int ticker_get_next_timestamp(const ticker_data_t *const ticker, timestamp_t *timestamp);
 
 /**@}*/
 

targets/TARGET_ONSEMI/TARGET_NCS36510/rtc.c

@@ -7,11 +7,11 @@
  * $Rev: 3525 $
  * $Date: 2015-07-20 15:24:25 +0530 (Mon, 20 Jul 2015) $
  ******************************************************************************
- * Copyright 2016 Semiconductor Components Industries LLC (d/b/a “ON Semiconductor”).
+ * Copyright 2016 Semiconductor Components Industries LLC (d/b/a <EFBFBD>ON Semiconductor<EFBFBD>).
  * All rights reserved.  This software and/or documentation is licensed by ON Semiconductor
  * under limited terms and conditions.  The terms and conditions pertaining to the software
  * and/or documentation are available at http://www.onsemi.com/site/pdf/ONSEMI_T&C.pdf
- * (“ON Semiconductor Standard Terms and Conditions of Sale, Section 8 Software”) and
+ * (<EFBFBD>ON Semiconductor Standard Terms and Conditions of Sale, Section 8 Software<EFBFBD>) and
  * if applicable the software license agreement.  Do not use this software and/or
  * documentation unless you have carefully read and you agree to the limited terms and
  * conditions.  By using this software and/or documentation, you agree to the limited
@@ -45,8 +45,25 @@
 #include "mbed_assert.h"
 #include "lp_ticker_api.h"
 
-static uint16_t SubSecond;
+static volatile uint64_t last_time_read;
-static uint64_t LastRtcTimeus;
+
+/**
+ * Convert sub seconds ticks to micro seconds.
+ * The clock running at 32kHz, a tick is 1/32768 of a second.
+ */
+static inline uint32_t ticks_to_us(uint16_t ticks) {
+	return (((uint64_t)ticks * RTC_SEC_TO_US) / RTC_CLOCK_HZ);
+}
+
+/**
+ * Convert us into sub seconds ticks.
+ * @note result might be troncated to be in the range [0 - RTC_SUB_SEC_MASK].
+ */
+static inline uint16_t us_to_ticks(uint32_t us) {
+	return (((uint64_t) us * RTC_CLOCK_HZ) / RTC_SEC_TO_US) & RTC_SUB_SEC_MASK;
+}
+
+#define RTC_TICK_THRESHOLD 5 
 
 /* See rtc.h for details */
 void fRtcInit(void)
@@ -55,115 +72,131 @@ void fRtcInit(void)
     CLOCKREG->CCR.BITS.RTCEN = True;     /* Enable RTC clock 32K */
 
     /* Reset RTC control register */
-    RTCREG->CONTROL.WORD     = False;
+    RTCREG->CONTROL.WORD = 0;
 
     /* Initialize all counters */
-    RTCREG->SECOND_COUNTER       = False;
+    RTCREG->SECOND_COUNTER = 0;
-    RTCREG->SUB_SECOND_COUNTER   = False;
+    RTCREG->SUB_SECOND_COUNTER = 0;
-    RTCREG->SECOND_ALARM         = False;
+    RTCREG->SECOND_ALARM = 0;
-    RTCREG->SUB_SECOND_ALARM     = False;
+    RTCREG->SUB_SECOND_ALARM = 0;
-    LastRtcTimeus = 0;
+    last_time_read = 0;
 
     /* Reset RTC Status register */
-    RTCREG->STATUS.WORD      = False;
+    RTCREG->STATUS.WORD = 0;
 
     /* Clear interrupt status */
-    RTCREG->INT_CLEAR.WORD   = False;
+    RTCREG->INT_CLEAR.WORD = (
+        (1 << RTC_INT_CLR_SUB_SEC_BIT_POS) | 
+        (1 << RTC_INT_CLR_SEC_BIT_POS)
+    );
 
+    /* Wait previous write to complete */
+    while(RTCREG->STATUS.BITS.BSY_CTRL_REG_WRT == True);
     /* Start sec & sub_sec counter */
-    while(RTCREG->STATUS.BITS.BSY_CTRL_REG_WRT == True);/* Wait previous write to complete */
+    RTCREG->CONTROL.WORD |= (
-    RTCREG->CONTROL.WORD |= ((True << RTC_CONTROL_SUBSEC_CNT_START_BIT_POS) |
+        (True << RTC_CONTROL_SUBSEC_CNT_START_BIT_POS) |
-                             (True << RTC_CONTROL_SEC_CNT_START_BIT_POS));
+        (True << RTC_CONTROL_SEC_CNT_START_BIT_POS)
+    );
 
     /* enable interruption associated with the rtc at NVIC level */
-    NVIC_SetVector(Rtc_IRQn,(uint32_t)fRtcHandler); /* TODO define lp_ticker_isr */
+    NVIC_SetVector(Rtc_IRQn,(uint32_t) fRtcHandler); /* TODO define lp_ticker_isr */
     NVIC_ClearPendingIRQ(Rtc_IRQn);
     NVIC_EnableIRQ(Rtc_IRQn);
 
-    while(RTCREG->STATUS.BITS.BSY_CTRL_REG_WRT == True); /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
+    /* Wait for RTC to finish writing register */
-
+    while(RTCREG->STATUS.BITS.BSY_CTRL_REG_WRT == True);
-    return;
 }
 
 /* See rtc.h for details */
 void fRtcFree(void)
 {
-    /* Reset RTC control register */
+    /* Disable interrupts and counter */
-    RTCREG->CONTROL.WORD = False;
+    RTCREG->CONTROL.WORD = 0;
 
     /* disable interruption associated with the rtc */
     NVIC_DisableIRQ(Rtc_IRQn);
 
-    while(RTCREG->STATUS.BITS.BSY_CTRL_REG_WRT == True); /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
+    /* Wait for RTC to finish writing register */
+    while(RTCREG->STATUS.BITS.BSY_CTRL_REG_WRT == True);
 }
 
 /* See rtc.h for details */
 void fRtcSetInterrupt(uint32_t timestamp)
 {
-    SubSecond             = False;
+	uint64_t current_time = fRtcRead();
-    uint32_t Second = False, EnableInterrupt = False;
-    uint8_t DividerAdjust = 1;
 
-    if(timestamp) {
+	/* compute delta between current time and timestamp.
-        if(timestamp >= RTC_SEC_TO_US) {
+	 * Note: the current time used to compute the delta is relative (truncated 
-            /* TimeStamp is big enough to set second alarm */
+     * to 32 bits).
-            Second =  ((timestamp / RTC_SEC_TO_US) & RTC_SEC_MASK); /* Convert micro second to second */
+	 */
-            RTCREG->SECOND_ALARM = Second; /* Write to alarm register */
+	int32_t delta = timestamp - (uint32_t) current_time;
+	if (delta <= 0) {
+		// event considered in the past, set the interrupt as pending.
+		NVIC_SetPendingIRQ(Rtc_IRQn);
+		return;
+	}
 
-            /* Enable second interrupt */
+	uint64_t full_timestamp = (current_time & ~UINT32_MAX) | timestamp;
-              EnableInterrupt = True << RTC_CONTROL_SEC_CNT_INT_BIT_POS;
+	if ( (uint32_t)current_time > timestamp) {
-        }
+		full_timestamp += ((uint64_t) UINT32_MAX) + 1;
-        timestamp = timestamp - Second * RTC_SEC_TO_US; /* Take out micro second for sub second alarm */
+	}
-        if(timestamp > False) {
-            /* We have some thing for sub second */
 
-            /* Convert micro second to sub_seconds(each count = 30.5 us) */
+	uint32_t target_seconds = full_timestamp / RTC_SEC_TO_US;
-            if(timestamp > 131000) {
+	uint16_t target_ticks = us_to_ticks(full_timestamp);
-                DividerAdjust = 100;
-            }
 
-            volatile uint64_t Temp = (timestamp / DividerAdjust * RTC_CLOCK_HZ);
+	/*
-            Temp = (uint64_t)(Temp / RTC_SEC_TO_US * DividerAdjust);
+	 * If the interrupt is in more than one second from now then use the
-            SubSecond = Temp & RTC_SUB_SEC_MASK;
+	 * second alarm, otherwise use the subsecond alarm.
+	 * In case of the second alarm is used, there is no need to preserve the
+	 * remaining subsecond because the irq handler should manage spurious
+	 * interrupts (like when the timestamp is in the past). In such case, irq
+	 * handler will schedule a new interrupt with the remaining us.
+	 */
+	NVIC_DisableIRQ(Rtc_IRQn);
+	if (target_seconds != RTCREG->SECOND_COUNTER) {
+		RTCREG->SECOND_ALARM = target_seconds;
 
-            if(SubSecond <= 5) {
+		uint32_t rtc_control = RTCREG->CONTROL.WORD;
-                SubSecond = 0;
+		rtc_control |= (1 << RTC_CONTROL_SEC_CNT_INT_BIT_POS); // enable seconds interrupt
-            }
+		rtc_control &= ~(1 << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS); // disable sub sec interrupt
+		RTCREG->CONTROL.WORD = rtc_control;
+	} else {
+		uint16_t current_ticks = RTCREG->SUB_SECOND_COUNTER;
+		if (current_ticks == target_ticks ||
+			((target_ticks > current_ticks) && ((target_ticks - current_ticks) < RTC_TICK_THRESHOLD)) ||
+			((target_ticks < current_ticks) && ((RTC_SUB_SEC_MASK - (current_ticks - target_ticks)) < RTC_TICK_THRESHOLD))) {
+			// target ticks too close; schedule the interrupt immediately
+			NVIC_SetPendingIRQ(Rtc_IRQn);
+		} else {
+			RTCREG->SUB_SECOND_ALARM = target_ticks;
 
-            if(SubSecond > False) {
+			uint32_t rtc_control = RTCREG->CONTROL.WORD;
-                /* Second interrupt not enabled */
+			rtc_control &= ~(1 << RTC_CONTROL_SEC_CNT_INT_BIT_POS); // disable seconds interrupt
+			rtc_control |= (1 << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS); // enable sub sec interrupt
+			RTCREG->CONTROL.WORD = rtc_control;
+		}
+	}
+	 NVIC_EnableIRQ(Rtc_IRQn);
 
-                /* Set SUB SEC_ALARM */
+     /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
-                RTCREG->SUB_SECOND_ALARM = SubSecond;    /* Write to sub second alarm */
+     while(RTCREG->STATUS.WORD &
-
+        (
-                /* Enable sub second interrupt */
+        	(True << RTC_STATUS_SUB_SEC_ALARM_WRT_BIT_POS) |
-                EnableInterrupt |= (True << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS);
+			(True << RTC_STATUS_SEC_ALARM_WRT_BIT_POS) |
-            }
+			(True << RTC_STATUS_CONTROL_WRT_BIT_POS)
-        }
+		)
-        
+	);
-        RTCREG->CONTROL.WORD |= EnableInterrupt;
-        /* Enable RTC interrupt */
-        NVIC_EnableIRQ(Rtc_IRQn);
-        
-        /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
-        while((RTCREG->STATUS.WORD & ((True << RTC_STATUS_SUB_SEC_ALARM_WRT_BIT_POS) |
-                                      (True << RTC_STATUS_SEC_ALARM_WRT_BIT_POS) |
-                                      (True << RTC_STATUS_CONTROL_WRT_BIT_POS))) == True); 
-    }
-    return;
 }
 
 /* See rtc.h for details */
 void fRtcDisableInterrupt(void)
 {
-    /* Disable RTC interrupt */
     NVIC_DisableIRQ(Rtc_IRQn);
 }
 
 /* See rtc.h for details */
 void fRtcEnableInterrupt(void)
 {
-    /* Enable RTC interrupt */
     NVIC_EnableIRQ(Rtc_IRQn);
 }
 
@@ -176,39 +209,36 @@ void fRtcClearInterrupt(void)
                               (True << RTC_INT_CLR_SEC_BIT_POS));
     
     while((RTCREG->STATUS.WORD & ((True << RTC_STATUS_SUB_SEC_INT_CLR_WRT_BIT_POS) |
-                                  (True << RTC_STATUS_SEC_INT_CLR_WRT_BIT_POS))) == True);  /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
+                                  (True << RTC_STATUS_SEC_INT_CLR_WRT_BIT_POS))));  /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
 }
 
 /* See rtc.h for details */
 uint64_t fRtcRead(void)
 {
-    uint32_t Second;
-    uint16_t SubSecond;
-
     /* Hardware Bug fix: The rollover of the sub-second counter initiates the increment of the second counter.
      * That means there is one cycle where the sub-second has rolled back to zero and the second counter has not incremented
      * and a read during that cycle will be incorrect.  That will occur for one RTC cycle and that is about 31us of exposure.
      * If you read a zero in the sub-second counter then increment the second counter by 1.
      * Alternatively, subtract 1 from the Sub-seconds counter to align the Second and Sub-Second rollover.
      */
+	uint32_t seconds = RTCREG->SECOND_COUNTER;
+    uint16_t ticks = (RTCREG->SUB_SECOND_COUNTER - 1) & SUB_SEC_MASK;
 
-    /* Read the Second and Sub-second counters, then read the Second counter again.
+    /*
-     * If it changed, then the Second rolled over while reading Sub-seconds, so go back and read them both again.
+     * If seconds has changed while reading ticks, read them both again.
      */
+    while (seconds != RTCREG->SECOND_COUNTER) {
+        seconds = RTCREG->SECOND_COUNTER;
+        ticks = (RTCREG->SUB_SECOND_COUNTER - 1) & SUB_SEC_MASK;
+    }
 
-    do {
+    uint64_t current_time = ((uint64_t) seconds *  RTC_SEC_TO_US) + ticks_to_us(ticks);
-        Second    = RTCREG->SECOND_COUNTER;                            /* Get SEC_COUNTER reg value */
-        SubSecond = (RTCREG->SUB_SECOND_COUNTER - 1) & SUB_SEC_MASK;   /* Get SUB_SEC_COUNTER reg value */
-    } while (Second != RTCREG->SECOND_COUNTER);                        /* Repeat if the second has changed */
-
-    //note: casting to float removed to avoid reduction in resolution
-    uint64_t RtcTimeus = ((uint64_t)SubSecond * RTC_SEC_TO_US / RTC_CLOCK_HZ) + ((uint64_t)Second * RTC_SEC_TO_US);
 
     /*check that the time did not go backwards */
-    MBED_ASSERT(RtcTimeus >= LastRtcTimeus);
+	MBED_ASSERT(current_time >= last_time_read);
-    LastRtcTimeus = RtcTimeus;
+    last_time_read = current_time;
 
-    return RtcTimeus;
+    return current_time;
 }
 
 /* See rtc.h for details */
@@ -244,43 +274,31 @@ void fRtcWrite(uint64_t RtcTimeus)
 /* See rtc.h for details */
 void fRtcHandler(void)
 {
-    /* SUB_SECOND/SECOND interrupt occured */
-    volatile uint32_t TempStatus = RTCREG->STATUS.WORD;
-
     /* Disable RTC interrupt */
     NVIC_DisableIRQ(Rtc_IRQn);
 
     /* Clear sec & sub_sec interrupts */
-    RTCREG->INT_CLEAR.WORD = ((True << RTC_INT_CLR_SUB_SEC_BIT_POS) |
+    RTCREG->INT_CLEAR.WORD = (
-                              (True << RTC_INT_CLR_SEC_BIT_POS));
+    	(True << RTC_INT_CLR_SUB_SEC_BIT_POS) |
+        (True << RTC_INT_CLR_SEC_BIT_POS)
+	);
 
-    /* TODO  ANDing SUB_SEC & SEC interrupt - work around for RTC issue - will be resolved in REV G */
+    /* Disable sub seconds and seconds interrupts */
-    if(TempStatus & RTC_SEC_INT_STATUS_MASK) {
+    RTCREG->CONTROL.WORD &= ~(
-        /* Second interrupt occured */
+    	(True << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS) |
-        if(SubSecond > False) {
+        (True << RTC_CONTROL_SEC_CNT_INT_BIT_POS)
-            /* Set SUB SEC_ALARM */
+	);
-            RTCREG->SUB_SECOND_ALARM = SubSecond + RTCREG->SUB_SECOND_COUNTER;
-            /* Enable sub second interrupt */
-            RTCREG->CONTROL.WORD |= (True << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS);
-        } else {
-            /* We reach here after second interrupt is occured */
-            RTCREG->CONTROL.WORD &= ~(True << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS) |
-                                    (True << RTC_CONTROL_SEC_CNT_INT_BIT_POS);
-        }
-    } else {
-        /* We reach here after sub_second or (Sub second + second) interrupt occured */
-		/* Disable Second and sub_second interrupt */
-        RTCREG->CONTROL.WORD &= ~(True << RTC_CONTROL_SUBSEC_CNT_INT_BIT_POS) |
-                                (True << RTC_CONTROL_SEC_CNT_INT_BIT_POS);
-    }
 
 	NVIC_EnableIRQ(Rtc_IRQn);
 	
-    /* Wait for RTC to finish writing register - RTC operates on 32K clock as compared to 32M core*/
+    /* Wait for RTC to finish writing registers */
-    while((RTCREG->STATUS.WORD & ((True << RTC_STATUS_SUB_SEC_ALARM_WRT_BIT_POS) |
+    while(RTCREG->STATUS.WORD &
-                                  (True << RTC_STATUS_CONTROL_WRT_BIT_POS) |
+    	(
-                                  (True << RTC_STATUS_SUB_SEC_INT_CLR_WRT_BIT_POS) |
+    		(True << RTC_STATUS_CONTROL_WRT_BIT_POS) |
-                                  (True << RTC_STATUS_SEC_INT_CLR_WRT_BIT_POS))) == True);  
+			(True << RTC_STATUS_SUB_SEC_INT_CLR_WRT_BIT_POS) |
+            (True << RTC_STATUS_SEC_INT_CLR_WRT_BIT_POS)
+		)
+	);
 	
 	lp_ticker_irq_handler();
 }