Diff: SOEM/osal/osal.cpp

Revision:

0:7077d8f28b3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SOEM/osal/osal.cpp	Tue Jun 11 10:19:08 2019 +0000
@@ -0,0 +1,117 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+#include "mbed.h"
+
+#include <osal.h>
+#include <config.h>
+
+
+uint64_t tick = 0;
+uint64_t last_tick = 0;
+
+
+#define  timercmp(a, b, CMP)                                \
+  (((a)->tv_sec == (b)->tv_sec) ?                           \
+   ((a)->tv_usec CMP (b)->tv_usec) :                        \
+   ((a)->tv_sec CMP (b)->tv_sec))
+
+
+#define  timeradd(a, b, result)                             \
+  do{                                                       \
+    (result)->tv_sec = (a)->tv_sec + (b)->tv_sec;           \
+    (result)->tv_usec = (a)->tv_usec + (b)->tv_usec;        \
+    if ((result)->tv_usec >= 1000000)                       \
+    {                                                       \
+       ++(result)->tv_sec;                                  \
+       (result)->tv_usec -= 1000000;                        \
+    }                                                       \
+  } while (0)
+#define  timersub(a, b, result)                             \
+  do {                                                      \
+    (result)->tv_sec = (a)->tv_sec - (b)->tv_sec;           \
+    (result)->tv_usec = (a)->tv_usec - (b)->tv_usec;        \
+    if ((result)->tv_usec < 0) {                            \
+      --(result)->tv_sec;                                   \
+      (result)->tv_usec += 1000000;                         \
+    }                                                       \
+  } while (0)
+
+
+#define CFG_TICKS_PER_SECOND 1000000            
+#define USECS_PER_SEC   1000000
+#define USECS_PER_TICK  (USECS_PER_SEC / CFG_TICKS_PER_SECOND)
+
+
+
+int gettimeofday_(struct timeval *tp, void *tzp)
+{
+    //tick_t tick = tick_get();     
+    uint32_t actual_tick = us_ticker_read();
+    tick += (actual_tick - last_tick);
+    last_tick = actual_tick;
+    
+    //tick_t ticks_left;           
+    uint64_t ticks_left;
+  
+    tp->tv_sec = tick / CFG_TICKS_PER_SECOND;
+    ticks_left = tick % CFG_TICKS_PER_SECOND;
+    tp->tv_usec = ticks_left * USECS_PER_TICK;
+
+    return 0;
+}
+
+int osal_usleep (uint32 usec)
+{
+    wait_us(usec); 
+    return 0;
+}
+
+
+int osal_gettimeofday(struct timeval *tv, struct timezone *tz)
+{
+    return gettimeofday_(tv, tz);
+}
+
+
+ec_timet osal_current_time (void)
+{
+    struct timeval current_time;
+    ec_timet return_value;
+
+    gettimeofday_ (&current_time, 0);
+    return_value.sec = current_time.tv_sec;
+    return_value.usec = current_time.tv_usec;
+    return return_value;
+}
+
+void osal_timer_start (osal_timert * self, uint32 timeout_usec)
+{
+    struct timeval start_time;
+    struct timeval timeout;
+    struct timeval stop_time;
+
+    gettimeofday_ (&start_time, 0);
+    timeout.tv_sec = timeout_usec / USECS_PER_SEC;
+    timeout.tv_usec = timeout_usec % USECS_PER_SEC;
+    timeradd (&start_time, &timeout, &stop_time);
+
+    self->stop_time.sec = stop_time.tv_sec;
+    self->stop_time.usec = stop_time.tv_usec;
+}
+
+boolean osal_timer_is_expired (osal_timert * self)
+{
+    struct timeval current_time;
+    struct timeval stop_time;
+    int is_not_yet_expired;
+
+    gettimeofday_ (&current_time, 0);
+    stop_time.tv_sec = self->stop_time.sec;
+    stop_time.tv_usec = self->stop_time.usec;
+    is_not_yet_expired = timercmp (&current_time, &stop_time, <);
+
+    return is_not_yet_expired == false;
+}