添加 vdso readtime 更新逻辑

Author: rcitach

components/libc/compilers/common/ctime.c

@@ -52,6 +52,10 @@
 
 #define _WARNING_NO_RTC "Cannot find a RTC device!"
 
+#if defined(RT_USING_SMART) && defined(RT_USING_VDSO)
+#include <vdso_kernel.h>
+#endif
+
 /* days per month -- nonleap! */
 static const short __spm[13] =
 {
@@ -519,12 +523,26 @@ int settimeofday(const struct timeval *tv, const struct timezone *tz)
     {
         if (_control_rtc(RT_DEVICE_CTRL_RTC_SET_TIMEVAL, (void *)tv) == RT_EOK)
         {
+#if defined(RT_USING_SMART) && defined(RT_USING_VDSO)
+            struct timespec ts = {
+                .tv_sec = tv->tv_sec,
+                .tv_nsec = tv->tv_usec * 1000L,
+            };
+            rt_vdso_set_realtime(&ts);
+#endif
             return 0;
         }
         else
         {
             if (_control_rtc(RT_DEVICE_CTRL_RTC_SET_TIME, (void *)&tv->tv_sec) == RT_EOK)
             {
+#if defined(RT_USING_SMART) && defined(RT_USING_VDSO)
+                struct timespec ts = {
+                    .tv_sec = tv->tv_sec,
+                    .tv_nsec = 0,
+                };
+                rt_vdso_set_realtime(&ts);
+#endif
                 return 0;
             }
         }
@@ -736,7 +754,16 @@ int clock_settime(clockid_t clockid, const struct timespec *tp)
     {
 #ifdef RT_USING_RTC
         case CLOCK_REALTIME:
-            return _control_rtc(RT_DEVICE_CTRL_RTC_SET_TIMESPEC, (void *)tp);
+        {
+            int ret = _control_rtc(RT_DEVICE_CTRL_RTC_SET_TIMESPEC, (void *)tp);
+            if (ret == RT_EOK)
+            {
+#if defined(RT_USING_SMART) && defined(RT_USING_VDSO)
+                rt_vdso_set_realtime(tp);
+#endif
+            }
+            return ret;
+        }
 #endif /* RT_USING_RTC */
 
         case CLOCK_REALTIME_COARSE:

components/lwp/arch/common/vdso_kernel.c

@@ -101,6 +101,18 @@ static void rt_vdso_counter_to_timespec(rt_uint64_t counter_value,
     ts->tv_nsec = monotonic_ns % RT_VDSO_NSEC_PER_SEC;
 }
 
+static struct timespec rt_vdso_timespec_subtract(const struct timespec *lhs,
+                                                 const struct timespec *rhs)
+{
+    struct timespec ret;
+
+    ret.tv_sec = lhs->tv_sec - rhs->tv_sec;
+    ret.tv_nsec = lhs->tv_nsec - rhs->tv_nsec;
+    rt_vdso_normalize_timespec(&ret);
+
+    return ret;
+}
+
 static struct timespec rt_vdso_add_timespec(const struct timespec *lhs,
                                             const struct timespec *rhs)
 {
@@ -129,15 +141,58 @@ static int rt_vdso_read_monotonic_snapshot(struct timespec *monotonic_time,
     return RT_EOK;
 }
 
+static void rt_vdso_update_data_page_flags(void)
+{
+    rt_uint32_t flags = 0;
+
+    if (rt_vdso_realtime_offset_ready)
+    {
+        flags |= RT_VDSO_FLAG_REALTIME_VALID;
+    }
+
+    rt_vdso_kernel_data_page->flags = flags;
+}
+
 static void rt_vdso_store_clock_snapshot(const struct timespec *monotonic_time,
                                          rt_uint64_t counter_value,
                                          rt_uint64_t counter_freq)
 {
     rt_vdso_kernel_data_page->counter_last = counter_value;
     rt_vdso_kernel_data_page->counter_freq = counter_freq;
+    rt_vdso_update_data_page_flags();
     rt_vdso_kernel_data_page->base_time[RT_VDSO_CLOCK_MONOTONIC_INDEX] = *monotonic_time;
-    rt_vdso_kernel_data_page->base_time[RT_VDSO_CLOCK_REALTIME_INDEX] =
-        rt_vdso_add_timespec(monotonic_time, &rt_vdso_realtime_offset);
+    if (rt_vdso_realtime_offset_ready)
+    {
+        rt_vdso_kernel_data_page->base_time[RT_VDSO_CLOCK_REALTIME_INDEX] =
+            rt_vdso_add_timespec(monotonic_time, &rt_vdso_realtime_offset);
+    }
+    else
+    {
+        rt_vdso_kernel_data_page->base_time[RT_VDSO_CLOCK_REALTIME_INDEX] = *monotonic_time;
+    }
+}
+
+void rt_vdso_set_realtime(const struct timespec *realtime)
+{
+    struct timespec monotonic;
+    rt_uint64_t counter;
+    rt_uint64_t freq;
+
+    if (rt_vdso_runtime_status != RT_EOK || realtime == RT_NULL)
+    {
+        return;
+    }
+
+    if (rt_vdso_read_monotonic_snapshot(&monotonic, &counter, &freq) != RT_EOK)
+    {
+        return;
+    }
+
+    rt_vdso_data_page_write_begin(rt_vdso_kernel_data_page);
+    rt_vdso_realtime_offset = rt_vdso_timespec_subtract(realtime, &monotonic);
+    rt_vdso_realtime_offset_ready = RT_TRUE;
+    rt_vdso_store_clock_snapshot(&monotonic, counter, freq);
+    rt_vdso_data_page_write_end(rt_vdso_kernel_data_page);
 }
 
 static void *rt_vdso_map_physical_pages(struct rt_lwp *lwp, void *user_va,
@@ -266,6 +321,7 @@ static int rt_vdso_validate_image(void)
 
     rt_memset(rt_vdso_data_page_store.raw, 0, sizeof(rt_vdso_data_page_store.raw));
     rt_vdso_realtime_offset_ready = RT_FALSE;
+    rt_memset(&rt_vdso_realtime_offset, 0, sizeof(rt_vdso_realtime_offset));
 
     if (rt_memcmp(rt_vdso_binaries[RT_VDSO_BINARY_COMMON].image_start,
                   RT_VDSO_IMAGE_ELF_MAGIC, RT_VDSO_IMAGE_ELF_MAGIC_LEN))

components/lwp/vdso/user/common/vdso_clock_gettime.c

@@ -55,6 +55,11 @@ int rt_vdso_clock_gettime_impl(clockid_t clock, struct timespec *ts)
     do
     {
         seq = rt_vdso_data_read_begin(data_page);
+        if (index == RT_VDSO_CLOCK_REALTIME_INDEX &&
+            !(data_page->flags & RT_VDSO_FLAG_REALTIME_VALID))
+        {
+            return -ENOSYS;
+        }
         base_time = data_page->base_time[index];
         last = data_page->counter_last;
         freq = data_page->counter_freq;

components/lwp/vdso/vdso_data_page.h

@@ -22,6 +22,7 @@ extern "C" {
 
 #define RT_VDSO_CLOCK_ID_MAX 16
 #define RT_VDSO_NSEC_PER_SEC 1000000000ULL
+#define RT_VDSO_FLAG_REALTIME_VALID (1U << 0)
 
 enum rt_vdso_clock_data_index
 {