|
|
@@ -0,0 +1,302 @@
|
|
|
+/*
|
|
|
+ * Copyright (c) 2006-2025 RT-Thread Development Team
|
|
|
+ *
|
|
|
+ * SPDX-License-Identifier: Apache-2.0
|
|
|
+ *
|
|
|
+ * Change Logs:
|
|
|
+ * Date Author Notes
|
|
|
+ * 2024-07-04 rcitach init ver.
|
|
|
+ * 2025-04-22 ScuDays Add VDSO functionality under the riscv64 architecture.
|
|
|
+ * 2026-04-21 rcitach Refactor vDSO runtime around the clock data page.
|
|
|
+ */
|
|
|
+
|
|
|
+#include <rtthread.h>
|
|
|
+#include <mmu.h>
|
|
|
+#include <elf.h>
|
|
|
+#include <time.h>
|
|
|
+#include <lwp_user_mm.h>
|
|
|
+#include <drivers/clock_time.h>
|
|
|
+#ifdef RT_USING_RTC
|
|
|
+#include <rtdevice.h>
|
|
|
+#endif
|
|
|
+
|
|
|
+#include <vdso_kernel.h>
|
|
|
+#include <vdso_kernel_internal.h>
|
|
|
+#define DBG_TAG "vdso"
|
|
|
+#define DBG_LVL DBG_INFO
|
|
|
+#include <rtdbg.h>
|
|
|
+
|
|
|
+enum rt_vdso_binary_id
|
|
|
+{
|
|
|
+ RT_VDSO_BINARY_COMMON,
|
|
|
+};
|
|
|
+
|
|
|
+struct rt_vdso_binary_info
|
|
|
+{
|
|
|
+ const char *name;
|
|
|
+ const char *image_start;
|
|
|
+ const char *image_end;
|
|
|
+ unsigned long page_count;
|
|
|
+};
|
|
|
+
|
|
|
+static struct rt_vdso_binary_info rt_vdso_binaries[] = {
|
|
|
+ [RT_VDSO_BINARY_COMMON] = {
|
|
|
+ .name = "rt_vdso_common",
|
|
|
+ .image_start = __rt_vdso_image_start,
|
|
|
+ .image_end = __rt_vdso_image_end,
|
|
|
+ },
|
|
|
+};
|
|
|
+
|
|
|
+static union {
|
|
|
+ struct rt_vdso_data_page data_page;
|
|
|
+ uint8_t raw[RT_VDSO_DATA_PAGE_COUNT * ARCH_PAGE_SIZE];
|
|
|
+} rt_vdso_data_page_store RT_VDSO_DATA_PAGE_ALIGNED;
|
|
|
+struct rt_vdso_data_page *rt_vdso_kernel_data_page = &rt_vdso_data_page_store.data_page;
|
|
|
+static int rt_vdso_runtime_status = RT_EOK;
|
|
|
+
|
|
|
+static struct timespec rt_vdso_realtime_offset;
|
|
|
+static rt_bool_t rt_vdso_realtime_offset_ready;
|
|
|
+
|
|
|
+#ifndef MMU_MAP_U_ROCB
|
|
|
+#define MMU_MAP_U_ROCB MMU_MAP_U_RWCB
|
|
|
+#endif
|
|
|
+
|
|
|
+#ifndef MMU_MAP_U_RWCB_XN
|
|
|
+#define MMU_MAP_U_RWCB_XN MMU_MAP_U_RWCB
|
|
|
+#endif
|
|
|
+
|
|
|
+#ifdef MMU_MAP_U_ROCB_XN
|
|
|
+#define RT_VDSO_DATA_PAGE_ATTR MMU_MAP_U_ROCB_XN
|
|
|
+#elif defined(MMU_MAP_U_RWCB_XN)
|
|
|
+#define RT_VDSO_DATA_PAGE_ATTR MMU_MAP_U_RWCB_XN
|
|
|
+#else
|
|
|
+#define RT_VDSO_DATA_PAGE_ATTR MMU_MAP_U_ROCB
|
|
|
+#endif
|
|
|
+
|
|
|
+#define RT_VDSO_IMAGE_PAGE_ATTR MMU_MAP_U_ROCB
|
|
|
+
|
|
|
+static void rt_vdso_normalize_timespec(struct timespec *ts)
|
|
|
+{
|
|
|
+ while (ts->tv_nsec >= RT_VDSO_NSEC_PER_SEC)
|
|
|
+ {
|
|
|
+ ts->tv_sec += 1;
|
|
|
+ ts->tv_nsec -= RT_VDSO_NSEC_PER_SEC;
|
|
|
+ }
|
|
|
+
|
|
|
+ while (ts->tv_nsec < 0)
|
|
|
+ {
|
|
|
+ ts->tv_sec -= 1;
|
|
|
+ ts->tv_nsec += RT_VDSO_NSEC_PER_SEC;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static struct timespec rt_vdso_add_timespec(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 int rt_vdso_read_monotonic_snapshot(struct timespec *monotonic_time,
|
|
|
+ rt_uint64_t *counter_value,
|
|
|
+ rt_uint64_t *counter_freq)
|
|
|
+{
|
|
|
+ if (rt_clock_boottime_get_ns(monotonic_time) != RT_EOK)
|
|
|
+ {
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+ *counter_value = rt_clock_time_get_counter();
|
|
|
+ *counter_freq = rt_clock_time_get_freq();
|
|
|
+
|
|
|
+ return RT_EOK;
|
|
|
+}
|
|
|
+
|
|
|
+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_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);
|
|
|
+}
|
|
|
+
|
|
|
+static void *rt_vdso_map_physical_pages(struct rt_lwp *lwp, void *user_va,
|
|
|
+ void *kernel_pa, size_t map_size,
|
|
|
+ rt_size_t attr)
|
|
|
+{
|
|
|
+ int err;
|
|
|
+ char *va;
|
|
|
+ size_t offset = 0;
|
|
|
+
|
|
|
+ if (!map_size)
|
|
|
+ {
|
|
|
+ return RT_NULL;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (user_va)
|
|
|
+ {
|
|
|
+ if (((size_t)user_va & ARCH_PAGE_MASK) !=
|
|
|
+ ((size_t)kernel_pa & ARCH_PAGE_MASK))
|
|
|
+ {
|
|
|
+ return RT_NULL;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ offset = (size_t)kernel_pa & ARCH_PAGE_MASK;
|
|
|
+ map_size += offset + ARCH_PAGE_SIZE - 1;
|
|
|
+ map_size &= ~ARCH_PAGE_MASK;
|
|
|
+ kernel_pa = (void *)((size_t)kernel_pa & ~ARCH_PAGE_MASK);
|
|
|
+ struct rt_mm_va_hint hint = {
|
|
|
+ .flags = 0,
|
|
|
+ .limit_range_size = lwp->aspace->size,
|
|
|
+ .limit_start = lwp->aspace->start,
|
|
|
+ .prefer = user_va,
|
|
|
+ .map_size = map_size,
|
|
|
+ };
|
|
|
+
|
|
|
+ if (user_va != RT_NULL)
|
|
|
+ {
|
|
|
+ hint.flags |= MMF_MAP_FIXED;
|
|
|
+ }
|
|
|
+
|
|
|
+ err = rt_aspace_map_phy(lwp->aspace, &hint, attr, MM_PA_TO_OFF(kernel_pa),
|
|
|
+ (void **)&va);
|
|
|
+ if (err != RT_EOK)
|
|
|
+ {
|
|
|
+ return RT_NULL;
|
|
|
+ }
|
|
|
+
|
|
|
+ return va + offset;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+static int rt_vdso_map_binary_pages(enum rt_vdso_binary_id binary_id,
|
|
|
+ struct rt_lwp *lwp)
|
|
|
+{
|
|
|
+ void *data_page_base = RT_NULL;
|
|
|
+ void *image_base = RT_NULL;
|
|
|
+ unsigned long data_page_len;
|
|
|
+ unsigned long image_len;
|
|
|
+
|
|
|
+ RT_ASSERT(lwp != RT_NULL);
|
|
|
+
|
|
|
+ data_page_len = RT_VDSO_DATA_PAGE_COUNT * ARCH_PAGE_SIZE;
|
|
|
+ image_len = rt_vdso_binaries[binary_id].page_count << ARCH_PAGE_SHIFT;
|
|
|
+
|
|
|
+ data_page_base = rt_vdso_map_physical_pages(
|
|
|
+ lwp, RT_NULL, rt_kmem_v2p((void *)rt_vdso_get_kernel_data_page()),
|
|
|
+ data_page_len, RT_VDSO_DATA_PAGE_ATTR);
|
|
|
+ if (data_page_base == RT_NULL)
|
|
|
+ {
|
|
|
+ lwp->vdso_vbase = RT_NULL;
|
|
|
+ return RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+ image_base = (uint8_t *)data_page_base + data_page_len;
|
|
|
+ image_base = rt_vdso_map_physical_pages(
|
|
|
+ lwp, image_base,
|
|
|
+ rt_kmem_v2p((void *)rt_vdso_binaries[binary_id].image_start),
|
|
|
+ image_len, RT_VDSO_IMAGE_PAGE_ATTR);
|
|
|
+ if (image_base == RT_NULL)
|
|
|
+ {
|
|
|
+ lwp_unmap_user_phy(lwp, data_page_base);
|
|
|
+ lwp->vdso_vbase = RT_NULL;
|
|
|
+ return RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+ lwp->vdso_vbase = image_base;
|
|
|
+ return RT_EOK;
|
|
|
+}
|
|
|
+
|
|
|
+int rt_vdso_map_process_image(struct rt_lwp *lwp)
|
|
|
+{
|
|
|
+ if (rt_vdso_runtime_status != RT_EOK)
|
|
|
+ {
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+ return rt_vdso_map_binary_pages(RT_VDSO_BINARY_COMMON, lwp);
|
|
|
+}
|
|
|
+
|
|
|
+void rt_vdso_sync_clock_data(void)
|
|
|
+{
|
|
|
+ struct timespec monotonic;
|
|
|
+ rt_uint64_t counter;
|
|
|
+ rt_uint64_t freq;
|
|
|
+
|
|
|
+ if (rt_vdso_runtime_status != RT_EOK)
|
|
|
+ {
|
|
|
+ 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_store_clock_snapshot(&monotonic, counter, freq);
|
|
|
+ rt_vdso_data_page_write_end(rt_vdso_kernel_data_page);
|
|
|
+}
|
|
|
+
|
|
|
+static int rt_vdso_validate_image(void)
|
|
|
+{
|
|
|
+ const Elf64_Ehdr *ehdr =
|
|
|
+ (const Elf64_Ehdr *)rt_vdso_binaries[RT_VDSO_BINARY_COMMON].image_start;
|
|
|
+
|
|
|
+ rt_memset(rt_vdso_data_page_store.raw, 0, sizeof(rt_vdso_data_page_store.raw));
|
|
|
+ rt_vdso_realtime_offset_ready = RT_FALSE;
|
|
|
+
|
|
|
+ if (rt_memcmp(rt_vdso_binaries[RT_VDSO_BINARY_COMMON].image_start,
|
|
|
+ RT_VDSO_IMAGE_ELF_MAGIC, RT_VDSO_IMAGE_ELF_MAGIC_LEN))
|
|
|
+ {
|
|
|
+ LOG_E("vDSO is not a valid ELF object!");
|
|
|
+ rt_vdso_runtime_status = -RT_ERROR;
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (ehdr->e_ident[EI_CLASS] != ELFCLASS64 || ehdr->e_type != ET_DYN)
|
|
|
+ {
|
|
|
+ LOG_E("vDSO ELF class/type mismatch!");
|
|
|
+ rt_vdso_runtime_status = -RT_ERROR;
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+#if defined(__aarch64__)
|
|
|
+ if (ehdr->e_machine != EM_AARCH64)
|
|
|
+ {
|
|
|
+ LOG_E("vDSO machine mismatch: expect AArch64");
|
|
|
+ rt_vdso_runtime_status = -RT_ERROR;
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+#elif defined(__riscv)
|
|
|
+ if (ehdr->e_machine != EM_RISCV)
|
|
|
+ {
|
|
|
+ LOG_E("vDSO machine mismatch: expect RISC-V");
|
|
|
+ rt_vdso_runtime_status = -RT_ERROR;
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+ rt_vdso_binaries[RT_VDSO_BINARY_COMMON].page_count =
|
|
|
+ (rt_vdso_binaries[RT_VDSO_BINARY_COMMON].image_end -
|
|
|
+ rt_vdso_binaries[RT_VDSO_BINARY_COMMON].image_start) >>
|
|
|
+ ARCH_PAGE_SHIFT;
|
|
|
+ if (rt_vdso_binaries[RT_VDSO_BINARY_COMMON].page_count == 0)
|
|
|
+ {
|
|
|
+ LOG_E("vDSO image is empty!");
|
|
|
+ rt_vdso_runtime_status = -RT_ERROR;
|
|
|
+ return -RT_ERROR;
|
|
|
+ }
|
|
|
+
|
|
|
+ return RT_EOK;
|
|
|
+}
|
|
|
+INIT_COMPONENT_EXPORT(rt_vdso_validate_image);
|