Extend os_get_current_time_us bits by helper

include/osal_utils.h

@@ -0,0 +1,74 @@
+/*********************************************************************
+ *        _       _         _
+ *  _ __ | |_  _ | |  __ _ | |__   ___
+ * | '__|| __|(_)| | / _` || '_ \ / __|
+ * | |   | |_  _ | || (_| || |_) |\__ \
+ * |_|    \__|(_)|_| \__,_||_.__/ |___/
+ *
+ * www.rt-labs.com
+ * Copyright 2017 rt-labs AB, Sweden.
+ *
+ * This software is licensed under the terms of the BSD 3-clause
+ * license. See the file LICENSE distributed with this software for
+ * full license information.
+ ********************************************************************/
+
+#ifndef OSAL_LOG_H
+#define OSAL_LOG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <inttypes.h>
+
+/**
+ * \brief Tick state for time updates
+ *
+ */
+typedef struct os_tick_state
+{
+    uint32_t last;
+    uint64_t base;
+} os_tick_state_t;
+
+
+/**
+ * \brief Extend 32 bit tick to 64 bits without wrap
+ *
+ * \return Number of ticks extended to 64 bit
+ */
+static uint64_t os_tick_update (volatile os_tick_state_t * state_ptr, uint32_t tick)
+{
+   const uint64_t wrap = 1ull + UINT32_MAX;
+   os_tick_state_t state;
+
+   state.base = state_ptr->base;
+   state.last = state_ptr->last;
+   if (tick < state.last)
+   {
+      /* The tick wrapped around */
+      state.base += wrap;
+   }
+   else if ((tick - state.last) > (UINT32_MAX / 2))
+   {
+      /* The tick is from before wrap, this can occur
+         if the call was preempted after getting
+         the current tick value and before evaulating
+         previous tick relation. We undo the wrap
+         wrap around, it will be re-done on next call */
+      state.base -= wrap;
+   }
+   state.last = tick;
+   state_ptr->base = state.base;
+   state_ptr->last = state.last;
+
+   return state.base + tick;
+}
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* OSAL_LOG_H */

src/freertos/osal.c

@@ -14,12 +14,15 @@
  ********************************************************************/
 
 #include "osal.h"
+#include "osal_utils.h"
 
 #include <stdlib.h>
 
 #define TMO_TO_TICKS(ms) \
    ((ms == OS_WAIT_FOREVER) ? portMAX_DELAY : (ms) / portTICK_PERIOD_MS)
 
+static volatile os_tick_state_t os_tick_state;
+
 void * os_malloc (size_t size)
 {
    return malloc (size);
@@ -83,7 +86,18 @@ void os_usleep (uint32_t us)
 
 uint32_t os_get_current_time_us (void)
 {
-   return 1000 * (xTaskGetTickCount() / portTICK_PERIOD_MS);
+   uint32_t tick;
+   uint64_t time;
+
+   tick = xTaskGetTickCount();
+
+   vTaskSuspendAll()
+   time = os_tick_update(&os_tick_state, tick);
+   vTaskResumeAll()
+
+   time *= 1000ull;
+   time /= portTICK_PERIOD_MS;
+   return (uint32_t)time;
 }
 
 os_sem_t * os_sem_create (size_t count)

src/rt-kernel/osal.c

@@ -14,6 +14,13 @@
  ********************************************************************/
 
 #include "osal.h"
+#include "kern/types.h"
+#include "kern/int.h"
+#include "osal_utils.h"
+
+extern const os_cfg_t os_cfg;
+static volatile os_tick_state_t os_tick_state;
+
 
 void * os_malloc (size_t size)
 {
@@ -62,7 +69,18 @@ void os_usleep (uint32_t us)
 
 uint32_t os_get_current_time_us (void)
 {
-   return 1000 * tick_to_ms (tick_get());
+   tick_t tick;
+   uint64_t time;
+
+   tick = tick_get();
+
+   int_lock();
+   time = os_tick_update(&os_tick_state, tick);
+   int_unlock();
+
+   time *= 10000000ull;
+   time /= os_cfg.ticks_per_second;
+   return (uint32_t)time;
 }
 
 os_sem_t * os_sem_create (size_t count)

test/test_osal.cpp

@@ -14,6 +14,7 @@
  ********************************************************************/
 
 #include "osal.h"
+#include "osal_utils.h"
 #include <gtest/gtest.h>
 
 static int expired_calls;
@@ -209,3 +210,26 @@ TEST_F (Osal, CurrentTime)
 
    EXPECT_NEAR (100 * 1000, t1 - t0, 1000);
 }
+
+TEST_F (Osal, TimeWrapping)
+{
+   volatile os_tick_state_t state = {};
+   EXPECT_EQ(os_tick_update(&state, 0x00000000u), 0x000000000ull);
+   EXPECT_EQ(os_tick_update(&state, 0x40000000u), 0x040000000ull);
+   EXPECT_EQ(os_tick_update(&state, 0x80000000u), 0x080000000ull);
+
+   /* right before wrap */
+   EXPECT_EQ(os_tick_update(&state, 0xFFFFFFFFu), 0x0FFFFFFFFull);
+
+   /* trigger wrap */
+   EXPECT_EQ(os_tick_update(&state, 0x00000000u), 0x100000000ull);
+
+   /* check that old tick before wrap still works */
+   EXPECT_EQ(os_tick_update(&state, 0xFFFFFFFFu), 0x0FFFFFFFFull);
+
+   /* continue after wrap */
+   EXPECT_EQ(os_tick_update(&state, 0x00000010u), 0x100000010ull);
+   EXPECT_EQ(os_tick_update(&state, 0x40000000u), 0x140000000ull);
+   EXPECT_EQ(os_tick_update(&state, 0x80000000u), 0x180000000ull);
+   EXPECT_EQ(os_tick_update(&state, 0x00000000u), 0x200000000ull);
+}