[PATCH v1] ptp: add PTP_MULTI_CLOCK_GET ioctl

[Sagi Maimon <maimon.sagi@gmail.com>]

        Some user space applications need to read some clocks.
        Each read requires moving from user space to kernel space.
        This asymmetry causes the measured offset to have a significant error.

        Introduce a new ioctl, which can be used to measure the offset between
        multiple clocks, from variety of types: PHC, virtual PHC and system time
        (CLOCK_REALTIME). The offset includes the total time that the driver needs
        to read the clock timestamp. Similar to the capability already introduced
        by PTP_SYS_OFFSET ioctl (read PHC and system clock), with flexibility
        regard clock types and number of required clocks.

        New ioctl allows the reading of a list of clocks - up to PTP_MAX_CLOCKS.
        Supported clocks IDs: PHC, virtual PHC and system time (CLOCK_REALTIME).
        Up to PTP_MAX_SAMPLES times (per clock) in a single ioctl read.
        The ioctl returns n_clocks timestamps for each measurement:
        - clock 0 timestamp or system time
        - ...
        - clock n timestamp or system time

Signed-off-by: Sagi Maimon <maimon.sagi@gmail.com>
---
 drivers/ptp/ptp_chardev.c      | 38 +++++++++++++++++++++++++++++++++-
 include/linux/posix-clock.h    | 10 +++++++++
 include/uapi/linux/ptp_clock.h | 13 ++++++++++++
 kernel/time/posix-clock.c      |  6 ++++++
 4 files changed, 66 insertions(+), 1 deletion(-)

diff --git a/drivers/ptp/ptp_chardev.c b/drivers/ptp/ptp_chardev.c
index 3f7a74788802..c5318f7c1f75 100644
--- a/drivers/ptp/ptp_chardev.c
+++ b/drivers/ptp/ptp_chardev.c
@@ -170,7 +170,9 @@ long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd,
 	struct ptp_clock_request req;
 	struct ptp_clock_caps caps;
 	struct ptp_clock_time *pct;
-	unsigned int i, pin_index;
+	struct ptp_multi_clock_get *multi_clk_get = NULL;
+
+	unsigned int i, pin_index, j;
 	struct ptp_pin_desc pd;
 	struct timespec64 ts;
 	int enable, err = 0;
@@ -491,6 +493,40 @@ long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd,
 		set_bit(i, tsevq->mask);
 		break;
 
+	case PTP_MULTI_CLOCK_GET:
+		multi_clk_get = memdup_user((void __user *)arg, sizeof(*multi_clk_get));
+		if (IS_ERR(multi_clk_get)) {
+			err = PTR_ERR(multi_clk_get);
+			multi_clk_get = NULL;
+			break;
+		}
+		if (multi_clk_get->n_samples > PTP_MAX_SAMPLES) {
+			err = -EINVAL;
+			break;
+		}
+		if (multi_clk_get->n_clocks > PTP_MAX_CLOCKS) {
+			err = -EINVAL;
+			break;
+		}
+		for (j = 0; j < multi_clk_get->n_samples; j++) {
+			for (i = 0; i < multi_clk_get->n_clocks; i++) {
+				if (multi_clk_get->clkid_arr[i] == CLOCK_REALTIME) {
+					ktime_get_real_ts64(&ts);
+				} else {
+					err = pc_clock_gettime_wrapper(multi_clk_get->clkid_arr[i],
+								       &ts);
+					if (err)
+						goto out;
+				}
+				multi_clk_get->ts[j][i].sec = ts.tv_sec;
+				multi_clk_get->ts[j][i].nsec = ts.tv_nsec;
+			}
+		}
+		if (copy_to_user((void __user *)arg, multi_clk_get,
+				 sizeof(*multi_clk_get)))
+			err = -EFAULT;
+
+		break;
 	default:
 		err = -ENOTTY;
 		break;
diff --git a/include/linux/posix-clock.h b/include/linux/posix-clock.h
index ef8619f48920..dd88e8d8a66c 100644
--- a/include/linux/posix-clock.h
+++ b/include/linux/posix-clock.h
@@ -135,4 +135,14 @@ int posix_clock_register(struct posix_clock *clk, struct device *dev);
  */
 void posix_clock_unregister(struct posix_clock *clk);
 
+/**
+ * pc_clock_gettime_warrper() - retrieve time of a clock specified by clock ID
+ * @id: Clock ID.
+ * @ts: clock time stamp.
+ *
+ * Returns the current time of a clock specified by clock ID, errno in case of
+ * error
+ */
+int pc_clock_gettime_wrapper(clockid_t id, struct timespec64 *ts);
+
 #endif
diff --git a/include/uapi/linux/ptp_clock.h b/include/uapi/linux/ptp_clock.h
index da700999cad4..ef21cc97e9bf 100644
--- a/include/uapi/linux/ptp_clock.h
+++ b/include/uapi/linux/ptp_clock.h
@@ -165,6 +165,18 @@ struct ptp_sys_offset_precise {
 	unsigned int rsv[4];    /* Reserved for future use. */
 };
 
+#define PTP_MAX_CLOCKS 20 /* Maximum number of clocks */
+
+struct ptp_multi_clock_get {
+	unsigned int n_clocks; /* Desired number of clocks. */
+	unsigned int n_samples; /* Desired number of measurements per clock. */
+	clockid_t  clkid_arr[PTP_MAX_CLOCKS]; /* list of clock IDs */
+	/*
+	 * Array of list of n_clocks clocks time samples n_samples times.
+	 */
+	struct ptp_clock_time ts[PTP_MAX_SAMPLES][PTP_MAX_CLOCKS];
+};
+
 enum ptp_pin_function {
 	PTP_PF_NONE,
 	PTP_PF_EXTTS,
@@ -226,6 +238,7 @@ struct ptp_pin_desc {
 	_IOWR(PTP_CLK_MAGIC, 18, struct ptp_sys_offset_extended)
 #define PTP_MASK_CLEAR_ALL  _IO(PTP_CLK_MAGIC, 19)
 #define PTP_MASK_EN_SINGLE  _IOW(PTP_CLK_MAGIC, 20, unsigned int)
+#define PTP_MULTI_CLOCK_GET _IOWR(PTP_CLK_MAGIC, 21, struct ptp_multi_clock_get)
 
 struct ptp_extts_event {
 	struct ptp_clock_time t; /* Time event occured. */
diff --git a/kernel/time/posix-clock.c b/kernel/time/posix-clock.c
index 9de66bbbb3d1..8df0ba79ebc2 100644
--- a/kernel/time/posix-clock.c
+++ b/kernel/time/posix-clock.c
@@ -284,6 +284,12 @@ static int pc_clock_gettime(clockid_t id, struct timespec64 *ts)
 	return err;
 }
 
+int pc_clock_gettime_wrapper(clockid_t id, struct timespec64 *ts)
+{
+	return pc_clock_gettime(id, ts);
+}
+EXPORT_SYMBOL_GPL(pc_clock_gettime_wrapper);
+
 static int pc_clock_getres(clockid_t id, struct timespec64 *ts)
 {
 	struct posix_clock_desc cd;
-- 
2.26.3