!605 添加 clock_nanosleep 支持

Merge pull request !605 from kang/rt-smart

components/libc/time/clock_time.c

@@ -137,13 +137,13 @@ int clock_gettime(clockid_t clockid, struct timespec *tp)
     case CLOCK_CPUTIME_ID:
         {
             float unit = 0;
-            long long cpu_tick;
+            uint64_t cpu_tick;
 
             unit = clock_cpu_getres();
             cpu_tick = clock_cpu_gettime();
 
-            tp->tv_sec  = ((int)(cpu_tick * unit)) / NANOSECOND_PER_SECOND;
-            tp->tv_nsec = ((int)(cpu_tick * unit)) % NANOSECOND_PER_SECOND;
+            tp->tv_sec = ((uint64_t)(cpu_tick * unit)) / NANOSECOND_PER_SECOND;
+            tp->tv_nsec = ((uint64_t)(cpu_tick * unit)) % NANOSECOND_PER_SECOND;
         }
         break;
 #endif
@@ -191,3 +191,112 @@ int clock_settime(clockid_t clockid, const struct timespec *tp)
     return 0;
 }
 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)
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+#ifdef RT_USING_CPUTIME
+    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;
+    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);
+#else
+    rt_tick_t tick;
+    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;
+        /* get the passed time */
+        rmtp->tv_sec = tick / RT_TICK_PER_SECOND;
+        rmtp->tv_nsec = (tick % RT_TICK_PER_SECOND) * (1000000000 / RT_TICK_PER_SECOND);
+    }
+#endif
+    return 0;
+}
+RTM_EXPORT(nanosleep);

components/lwp/lwp_syscall.c

@@ -909,63 +909,28 @@ int sys_unlink(const char *pathname)
 /* syscall: "nanosleep" ret: "int" args: "const struct timespec *" "struct timespec *" */
 int sys_nanosleep(const struct timespec *rqtp, struct timespec *rmtp)
 {
-    rt_tick_t tick;
-#ifdef RT_USING_USERSPACE
-    struct timespec rqtp_k;
-    struct timespec rmtp_k;
-
+    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);
-
-    tick = rqtp_k.tv_sec * RT_TICK_PER_SECOND + ((uint64_t)rqtp_k.tv_nsec * RT_TICK_PER_SECOND) / 1000000000;
-    rt_thread_delay(tick);
-
-    if (rmtp)
-    {
-        if (!lwp_user_accessable((void *)rmtp, sizeof *rmtp))
-        {
-            return -EFAULT;
-        }
-
-        tick = rt_tick_get() - tick;
-        /* get the passed time */
-        rmtp_k.tv_sec = tick / RT_TICK_PER_SECOND;
-        rmtp_k.tv_nsec = (tick % RT_TICK_PER_SECOND) * (1000000000 / RT_TICK_PER_SECOND);
-
+    ret = nanosleep(&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
-    dbg_log(DBG_LOG, "sys_nanosleep\n");
-
-    if (!lwp_user_accessable((void *)rqtp, sizeof *rqtp))
-    {
-        return -EFAULT;
-    }
-
-    tick = rqtp->tv_sec * RT_TICK_PER_SECOND + ((uint64_t)rqtp->tv_nsec * RT_TICK_PER_SECOND) / 1000000000;
-    rt_thread_delay(tick);
-
     if (rmtp)
     {
         if (!lwp_user_accessable((void *)rmtp, sizeof *rmtp))
-        {
             return -EFAULT;
-        }
-
-        tick = rt_tick_get() - tick;
-        /* get the passed time */
-        rmtp->tv_sec = tick / RT_TICK_PER_SECOND;
-        rmtp->tv_nsec = (tick % RT_TICK_PER_SECOND) * (1000000000 / RT_TICK_PER_SECOND);
+        ret = nanosleep(rqtp, rmtp);
     }
 #endif
-
-    return 0;
+    return (ret < 0 ? GET_ERRNO() : ret);
 }
 
 /* syscall: "gettimeofday" ret: "int" args: "struct timeval *" "struct timezone *" */
@@ -3813,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;
@@ -4311,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)