[dm][ptp] support Precision Time Protocol (PTP) IEEE 1588 clock

Signed-off-by: GuEe-GUI <[email protected]>

2 files changed, 164 insertions, 0 deletions

diff --git a/components/libc/compilers/common/ctime.c b/components/libc/compilers/common/ctime.c
index 3e1ea77fd7..60d5dc74a4 100644
--- a/components/libc/compilers/common/ctime.c
+++ b/components/libc/compilers/common/ctime.c
@@ -45,6 +45,13 @@
 #ifdef RT_USING_CLOCK_TIME
 #include <drivers/clock_time.h>
 #endif
+#ifdef RT_USING_PTP
+#include <drivers/ptp.h>
+#ifdef RT_USING_POSIX_FS
+#include <dfs.h>
+#include <dfs_file.h>
+#endif /* RT_USING_POSIX_FS */
+#endif /* RT_USING_PTP */
 
 #define DBG_TAG    "time"
 #define DBG_LVL    DBG_INFO
@@ -611,6 +618,134 @@ RTM_EXPORT(nanosleep);
 
 #if defined(RT_USING_POSIX_CLOCK) && defined(RT_USING_CLOCK_TIME)
 
+#ifdef RT_USING_PTP
+
+static rt_bool_t _clockid_is_ptp_fd(clockid_t clockid)
+{
+    return (clockid & 7) == CLOCKFD;
+}
+
+static struct rt_ptp_clock *_ptp_clock_from_clockid(clockid_t clockid)
+{
+    unsigned int fd;
+    struct rt_ptp_clock *ptp;
+    rt_device_t dev;
+
+#ifndef RT_USING_POSIX_FS
+    RT_UNUSED(fd);
+    RT_UNUSED(ptp);
+    RT_UNUSED(dev);
+    return RT_NULL;
+#else
+    struct dfs_file *file;
+
+    if (!_clockid_is_ptp_fd(clockid))
+    {
+        return RT_NULL;
+    }
+
+    fd = CLOCKID_TO_FD(clockid);
+    file = fd_get((int)fd);
+    if (!file || !file->vnode || !file->vnode->data)
+    {
+        return RT_NULL;
+    }
+
+    dev = (rt_device_t)file->vnode->data;
+    if (dev->type != RT_Device_Class_Char)
+    {
+        return RT_NULL;
+    }
+
+    if (rt_strncmp(rt_dm_dev_get_name(dev), "ptp", 3) != 0)
+    {
+        return RT_NULL;
+    }
+
+    ptp = rt_device_to_ptp_clock(dev);
+    if (!ptp->ops || !ptp->ops->gettime)
+    {
+        return RT_NULL;
+    }
+
+    return ptp;
+#endif /* RT_USING_POSIX_FS */
+}
+
+static int _clock_gettime_ptp(clockid_t clockid, struct timespec *tp)
+{
+    struct rt_ptp_clock *ptp;
+    struct rt_ptp_clock_time ts;
+    rt_err_t err;
+
+    ptp = _ptp_clock_from_clockid(clockid);
+    if (!ptp)
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    err = rt_ptp_gettime(ptp, &ts);
+    if (err != RT_EOK)
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    tp->tv_sec = (time_t)ts.sec;
+    tp->tv_nsec = ts.nsec;
+
+    return 0;
+}
+
+static int _clock_settime_ptp(clockid_t clockid, const struct timespec *tp)
+{
+    struct rt_ptp_clock *ptp;
+    struct rt_ptp_clock_time ts;
+    rt_err_t err;
+
+    ptp = _ptp_clock_from_clockid(clockid);
+    if (!ptp)
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    if (!ptp->ops->settime)
+    {
+        rt_set_errno(EPERM);
+        return -1;
+    }
+
+    ts.sec = tp->tv_sec;
+    ts.nsec = (rt_int32_t)tp->tv_nsec;
+
+    err = rt_ptp_settime(ptp, &ts);
+    if (err != RT_EOK)
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    return 0;
+}
+
+static int _clock_getres_ptp(clockid_t clockid, struct timespec *res)
+{
+    if (!_ptp_clock_from_clockid(clockid))
+    {
+        rt_set_errno(EINVAL);
+        return -1;
+    }
+
+    res->tv_sec = 0;
+    res->tv_nsec = 1;
+
+    return 0;
+}
+
+#endif /* RT_USING_PTP */
+
 int clock_getres(clockid_t clockid, struct timespec *res)
 {
     if (res == RT_NULL)
@@ -619,6 +754,13 @@ int clock_getres(clockid_t clockid, struct timespec *res)
         return -1;
     }
 
+#ifdef RT_USING_PTP
+    if (_clockid_is_ptp_fd(clockid))
+    {
+        return _clock_getres_ptp(clockid, res);
+    }
+#endif /* RT_USING_PTP */
+
     switch (clockid)
     {
         case CLOCK_REALTIME:  // use RTC
@@ -652,6 +794,13 @@ int clock_gettime(clockid_t clockid, struct timespec *tp)
         return -1;
     }
 
+#ifdef RT_USING_PTP
+    if (_clockid_is_ptp_fd(clockid))
+    {
+        return _clock_gettime_ptp(clockid, tp);
+    }
+#endif /* RT_USING_PTP */
+
     switch (clockid)
     {
         case CLOCK_REALTIME:  // use RTC
@@ -750,6 +899,13 @@ int clock_settime(clockid_t clockid, const struct timespec *tp)
         return -1;
     }
 
+#ifdef RT_USING_PTP
+    if (_clockid_is_ptp_fd(clockid))
+    {
+        return _clock_settime_ptp(clockid, tp);
+    }
+#endif /* RT_USING_PTP */
+
     switch (clockid)
     {
 #ifdef RT_USING_RTC
diff --git a/components/libc/compilers/common/include/sys/time.h b/components/libc/compilers/common/include/sys/time.h
index 817fe78676..8804433eeb 100644
--- a/components/libc/compilers/common/include/sys/time.h
+++ b/components/libc/compilers/common/include/sys/time.h
@@ -198,6 +198,14 @@ int nanosleep(const struct timespec *rqtp, struct timespec *rmtp);
 #define CLOCK_TAI 11  // newlib says they don't have this definition,  make the compiler happy
 #endif /* CLOCK_TAI */
 
+#ifdef RT_USING_PTP
+#ifndef CLOCKFD
+#define CLOCKFD 3
+#endif /* CLOCKFD */
+#define FD_TO_CLOCKID(fd)       ((clockid_t)((~(clockid_t)(fd) << 3) | CLOCKFD))
+#define CLOCKID_TO_FD(clk)      ((unsigned int)(~((unsigned int)(clk) >> 3)))
+#endif /* RT_USING_PTP */
+
 #endif /* defined(RT_USING_POSIX_CLOCK) || defined (RT_USING_POSIX_TIMER) */
 
 #ifdef RT_USING_POSIX_CLOCK