vdso_sys.c 2.2 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495
  1. /*
  2. * Copyright (c) 2006-2024 RT-Thread Development Team
  3. *
  4. * SPDX-License-Identifier: Apache-2.0
  5. *
  6. * Change Logs:
  7. * Date Author Notes
  8. * 2024-07-04 rcitach init ver.
  9. */
  10. #include <stdio.h>
  11. #include <time.h>
  12. #include <errno.h>
  13. #include <stdbool.h>
  14. #include <vdso_sys.h>
  15. #ifndef rt_vdso_cycles_ready
  16. static inline bool rt_vdso_cycles_ready(uint64_t cycles)
  17. {
  18. return true;
  19. }
  20. #endif
  21. #ifndef rt_vdso_get_ns
  22. static inline
  23. uint64_t rt_vdso_get_ns(uint64_t cycles, uint64_t last)
  24. {
  25. return (cycles - last) * NSEC_PER_SEC / __arch_get_hw_frq();
  26. }
  27. #endif
  28. static int
  29. __rt_vdso_getcoarse(struct timespec *ts, clockid_t clock, const struct vdso_data *vdns)
  30. {
  31. const struct vdso_data *vd;
  32. const struct timespec *vdso_ts;
  33. uint32_t seq;
  34. uint64_t sec, last, ns, cycles;
  35. if (clock != CLOCK_MONOTONIC_RAW)
  36. vd = &vdns[CS_HRES_COARSE];
  37. else
  38. vd = &vdns[CS_RAW];
  39. vdso_ts = &vd->basetime[clock];
  40. do {
  41. seq = rt_vdso_read_begin(vd);
  42. cycles = __arch_get_hw_counter(vd->clock_mode, vd);
  43. if (unlikely(!rt_vdso_cycles_ready(cycles)))
  44. return -1;
  45. ns = vdso_ts->tv_nsec;
  46. last = vd->cycle_last;
  47. ns += rt_vdso_get_ns(cycles, last);
  48. sec = vdso_ts->tv_sec;
  49. } while (unlikely(rt_vdso_read_retry(vd, seq)));
  50. ts->tv_sec = sec + __iter_div_u64_rem(ns, NSEC_PER_SEC, &ns);
  51. ts->tv_nsec = ns;
  52. return 0;
  53. }
  54. static inline int
  55. __vdso_clock_gettime_common(const struct vdso_data *vd, clockid_t clock,
  56. struct timespec *ts)
  57. {
  58. u_int32_t msk;
  59. if (unlikely((u_int32_t) clock >= MAX_CLOCKS))
  60. return -1;
  61. msk = 1U << clock;
  62. if (likely(msk & VDSO_REALTIME))
  63. return __rt_vdso_getcoarse(ts,CLOCK_REALTIME,vd);
  64. else if (msk & VDSO_MONOTIME)
  65. return __rt_vdso_getcoarse(ts,CLOCK_MONOTONIC,vd);
  66. else
  67. return ENOENT;
  68. }
  69. static __maybe_unused int
  70. rt_vdso_clock_gettime_data(const struct vdso_data *vd, clockid_t clock,
  71. struct timespec *ts)
  72. {
  73. int ret = 0;
  74. ret = __vdso_clock_gettime_common(vd, clock, ts);
  75. return ret;
  76. }
  77. int
  78. __kernel_clock_gettime(clockid_t clock, struct timespec *ts)
  79. {
  80. return rt_vdso_clock_gettime_data(__arch_get_vdso_data(), clock, ts);
  81. }