添加 nanosleep 函数

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,45 @@ int clock_settime(clockid_t clockid, const struct timespec *tp)
     return 0;
 }
 RTM_EXPORT(clock_settime);
+
+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,23 @@ 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 *" */