diff --git a/components/libc/time/clock_time.c b/components/libc/time/clock_time.c index 6b91359ffe65c1c8287c847ccd093d1eb249929c..9411835c23c1e1dddd1fa0a7938a52ee39d38f78 100644 --- a/components/libc/time/clock_time.c +++ b/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); \ No newline at end of file diff --git a/components/lwp/lwp_syscall.c b/components/lwp/lwp_syscall.c index ff97a47f1fa05d3cf5d04fc68181977d17b07aa0..8f7faf2d7ca47eea79841a7a5ced1910743f8f72 100644 --- a/components/lwp/lwp_syscall.c +++ b/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)