添加 clock_nanosleep 支持

components/libc/time/clock_time.c

@@ -192,6 +192,73 @@ int clock_settime(clockid_t clockid, const struct timespec *tp)
 }
 RTM_EXPORT(clock_settime);
 
+int clock_nanosleep(clockid_t clockid, int flags, const struct timespec *rqtp, struct timespec *rmtp)
+{
+    if (rqtp->tv_sec < 0 || rqtp->tv_nsec < 0 || rqtp->tv_nsec >= 1000000000)
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+    switch (clockid)
+    {
+    case CLOCK_REALTIME:
+    {
+        rt_tick_t tick;
+        if (flags & TIMER_ABSTIME == TIMER_ABSTIME)
+        {
+            tick = (rqtp->tv_sec - _timevalue.tv_sec) * RT_TICK_PER_SECOND + ((uint64_t)(rqtp->tv_nsec - _timevalue.tv_usec) * RT_TICK_PER_SECOND) / 1000000000;
+            rt_tick_t rt_tick = rt_tick_get();
+            tick = tick < rt_tick ? 0 : tick - rt_tick;
+        }
+        else
+        {
+            tick = rqtp->tv_sec * RT_TICK_PER_SECOND + ((uint64_t)(rqtp->tv_nsec) * RT_TICK_PER_SECOND) / 1000000000;
+        }
+        rt_thread_delay(tick);
+        if (rmtp)
+        {
+            tick = rt_tick_get() - tick;
+            rmtp->tv_sec = tick / RT_TICK_PER_SECOND;
+            rmtp->tv_nsec = (tick % RT_TICK_PER_SECOND) * (1000000000 / RT_TICK_PER_SECOND);
+        }
+    }
+    break;
+
+#ifdef RT_USING_CPUTIME
+    case CLOCK_MONOTONIC:
+    case CLOCK_CPUTIME_ID:
+    {
+        uint64_t cpu_tick, cpu_tick_old;
+        cpu_tick_old = clock_cpu_gettime();
+        rt_tick_t tick;
+        float unit = clock_cpu_getres();
+
+        cpu_tick = (rqtp->tv_sec * NANOSECOND_PER_SECOND + ((uint64_t)rqtp->tv_nsec * NANOSECOND_PER_SECOND) / 1000000000) / unit;
+        if (flags & TIMER_ABSTIME == TIMER_ABSTIME)
+            cpu_tick = cpu_tick < cpu_tick_old ? 0 : cpu_tick - cpu_tick_old;
+        tick = cpu_tick / (NANOSECOND_PER_SECOND / RT_TICK_PER_SECOND);
+        rt_thread_delay(tick);
+
+        if (rmtp)
+        {
+            uint64_t rmtp_cpu_tick = clock_cpu_gettime() - tick * (NANOSECOND_PER_SECOND / RT_TICK_PER_SECOND);
+            rmtp->tv_sec = ((int)(rmtp_cpu_tick * unit)) / NANOSECOND_PER_SECOND;
+            rmtp->tv_nsec = ((int)(rmtp_cpu_tick * unit)) % NANOSECOND_PER_SECOND;
+        }
+
+        while (clock_cpu_gettime() - cpu_tick_old < cpu_tick);
+    }
+    break;
+#endif
+    default:
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    return 0;
+}
+RTM_EXPORT(clock_nanosleep);
+
 int nanosleep(const struct timespec *rqtp, struct timespec *rmtp)
 {
     if (rqtp->tv_sec < 0 || rqtp->tv_nsec < 0 || rqtp->tv_nsec >= 1000000000)

components/lwp/lwp_syscall.c

@@ -923,7 +923,12 @@ int sys_nanosleep(const struct timespec *rqtp, struct timespec *rmtp)
     if (ret != -1 && rmtp && lwp_user_accessable((void *)rmtp, sizeof *rmtp))
         lwp_put_to_user(rmtp, (void *)&rmtp_k, sizeof rmtp_k);
 #else
-    ret = nanosleep(rqtp, rmtp);
+    if (rmtp)
+    {
+        if (!lwp_user_accessable((void *)rmtp, sizeof *rmtp))
+            return -EFAULT;
+        ret = nanosleep(rqtp, rmtp);
+    }
 #endif
     return (ret < 0 ? GET_ERRNO() : ret);
 }
@@ -3773,6 +3778,32 @@ int sys_clock_gettime(clockid_t clk, struct timespec *ts)
     return (ret < 0 ? GET_ERRNO() : ret);
 }
 
+int sys_clock_nanosleep(clockid_t clk, int flags, const struct timespec *rqtp, struct timespec *rmtp)
+{
+    int ret = 0;
+    dbg_log(DBG_LOG, "sys_nanosleep\n");
+    if (!lwp_user_accessable((void *)rqtp, sizeof *rqtp))
+        return -EFAULT;
+
+#ifdef RT_USING_USERSPACE
+    struct timespec rqtp_k;
+    struct timespec rmtp_k;
+
+    lwp_get_from_user(&rqtp_k, (void *)rqtp, sizeof rqtp_k);
+    ret = clock_nanosleep(clk, flags, &rqtp_k, &rmtp_k);
+    if (ret != -1 && rmtp && lwp_user_accessable((void *)rmtp, sizeof *rmtp))
+        lwp_put_to_user(rmtp, (void *)&rmtp_k, sizeof rmtp_k);
+#else
+    if (rmtp)
+    {
+        if (!lwp_user_accessable((void *)rmtp, sizeof *rmtp))
+            return -EFAULT;
+        ret = clock_nanosleep(clk, flags, rqtp, rmtp);
+    }
+#endif
+    return (ret < 0 ? GET_ERRNO() : ret);
+}
+
 int sys_clock_getres(clockid_t clk, struct timespec *ts)
 {
     int ret = 0;
@@ -4271,7 +4302,8 @@ const static void* func_table[] =
     SYSCALL_SIGN(sys_sched_setscheduler),
     SYSCALL_SIGN(sys_sched_getscheduler),
     SYSCALL_SIGN(sys_setaffinity),
-    SYSCALL_SIGN(sys_fsync)
+    SYSCALL_SIGN(sys_fsync),
+    SYSCALL_SIGN(sys_clock_nanosleep),
 };
 
 const void *lwp_get_sys_api(rt_uint32_t number)