[PATCH net-next v2 3/3] ptp: support event queue reader channel masks

[Xabier Marquiegui <reibax@gmail.com>]

Implement ioctl to support filtering of external timestamp event queue
channels per reader based on the process PID accessing the timestamp
queue.

Can be tested using testptp test binary. Use lsof to figure out readers
of the DUT. LSB of the timestamp channel mask is channel 0.

eg: To allow PID 3000 to access only ts channel 0:
```
 # testptp -F 3000,0x1
```

eg: To allow PID 3000 to access any channel:
```
 # testptp -F 3000,0xFFFFFFFF
```

Signed-off-by: Xabier Marquiegui <reibax@gmail.com>
Suggested-by: Richard Cochran <richardcochran@gmail.com>
---
v2:
  - fix testptp compilation error: unknown type name 'pid_t'
  - rename mask variable for easier code traceability
  - more detailed commit message with two examples
v1: https://lore.kernel.org/netdev/20230906104754.1324412-4-reibax@gmail.com/

 drivers/ptp/ptp_chardev.c             | 17 +++++++++++++++++
 drivers/ptp/ptp_clock.c               |  4 +++-
 drivers/ptp/ptp_private.h             |  1 +
 include/uapi/linux/ptp_clock.h        |  7 +++++++
 tools/testing/selftests/ptp/testptp.c | 26 +++++++++++++++++++++++++-
 5 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/drivers/ptp/ptp_chardev.c b/drivers/ptp/ptp_chardev.c
index c9da0f27d204..007f7710421e 100644
--- a/drivers/ptp/ptp_chardev.c
+++ b/drivers/ptp/ptp_chardev.c
@@ -109,6 +109,7 @@ int ptp_open(struct posix_clock *pc, fmode_t fmode)
 	queue = kzalloc(sizeof(*queue), GFP_KERNEL);
 	if (!queue)
 		return -EINVAL;
+	queue->tsevqmask = 0xFFFFFFFF;
 	queue->reader_pid = task_pid_nr(current);
 	list_add_tail(&queue->qlist, &ptp->tsevqs);
 
@@ -139,9 +140,11 @@ int ptp_release(struct posix_clock *pc)
 long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 {
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+	struct timestamp_event_queue *tsevq, *tsevq_alt;
 	struct ptp_sys_offset_extended *extoff = NULL;
 	struct ptp_sys_offset_precise precise_offset;
 	struct system_device_crosststamp xtstamp;
+	struct ptp_tsfilter_request tsfilter_req;
 	struct ptp_clock_info *ops = ptp->info;
 	struct ptp_sys_offset *sysoff = NULL;
 	struct ptp_system_timestamp sts;
@@ -451,6 +454,20 @@ long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 		mutex_unlock(&ptp->pincfg_mux);
 		break;
 
+	case PTP_FILTERTS_REQUEST:
+		if (copy_from_user(&tsfilter_req, (void __user *)arg,
+				   sizeof(tsfilter_req))) {
+			err = -EFAULT;
+			break;
+		}
+		list_for_each_entry_safe(tsevq, tsevq_alt, &ptp->tsevqs, qlist) {
+			if (tsevq->reader_pid == (pid_t)tsfilter_req.reader_pid) {
+				tsevq->tsevqmask = tsfilter_req.tsevqmask;
+				break;
+			}
+		}
+		break;
+
 	default:
 		err = -ENOTTY;
 		break;
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index d52fc23e20a8..9ebb78eb333f 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -247,6 +247,7 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
 	if (!queue)
 		goto no_memory_queue;
 	queue->reader_pid = 0;
+	queue->tsevqmask = 0xFFFFFFFF;
 	spin_lock_init(&queue->lock);
 	list_add_tail(&queue->qlist, &ptp->tsevqs);
 	mutex_init(&ptp->pincfg_mux);
@@ -406,7 +407,8 @@ void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
 	case PTP_CLOCK_EXTTS:
 		/* Enqueue timestamp on all other queues */
 		list_for_each_entry_safe(tsevq, tsevq_alt, &ptp->tsevqs, qlist) {
-			enqueue_external_timestamp(tsevq, event);
+			if (tsevq->tsevqmask & (0x1 << event->index))
+				enqueue_external_timestamp(tsevq, event);
 		}
 		wake_up_interruptible(&ptp->tsev_wq);
 		break;
diff --git a/drivers/ptp/ptp_private.h b/drivers/ptp/ptp_private.h
index 046d1482bcee..6888ecb26e82 100644
--- a/drivers/ptp/ptp_private.h
+++ b/drivers/ptp/ptp_private.h
@@ -28,6 +28,7 @@ struct timestamp_event_queue {
 	spinlock_t lock;
 	struct list_head qlist;
 	pid_t reader_pid;
+	int tsevqmask;
 };
 
 struct ptp_clock {
diff --git a/include/uapi/linux/ptp_clock.h b/include/uapi/linux/ptp_clock.h
index 05cc35fc94ac..a13ab8e27703 100644
--- a/include/uapi/linux/ptp_clock.h
+++ b/include/uapi/linux/ptp_clock.h
@@ -105,6 +105,11 @@ struct ptp_extts_request {
 	unsigned int rsv[2]; /* Reserved for future use. */
 };
 
+struct ptp_tsfilter_request {
+	unsigned int reader_pid; /* PID of process reading the timestamp event queue */
+	unsigned int tsevqmask; /* Channel mask. LSB = channel 0 */
+};
+
 struct ptp_perout_request {
 	union {
 		/*
@@ -224,6 +229,8 @@ struct ptp_pin_desc {
 	_IOWR(PTP_CLK_MAGIC, 17, struct ptp_sys_offset_precise)
 #define PTP_SYS_OFFSET_EXTENDED2 \
 	_IOWR(PTP_CLK_MAGIC, 18, struct ptp_sys_offset_extended)
+#define PTP_FILTERTS_REQUEST \
+	_IOW(PTP_CLK_MAGIC, 19, struct ptp_tsfilter_request)
 
 struct ptp_extts_event {
 	struct ptp_clock_time t; /* Time event occured. */
diff --git a/tools/testing/selftests/ptp/testptp.c b/tools/testing/selftests/ptp/testptp.c
index c9f6cca4feb4..e84ff689c6be 100644
--- a/tools/testing/selftests/ptp/testptp.c
+++ b/tools/testing/selftests/ptp/testptp.c
@@ -121,6 +121,7 @@ static void usage(char *progname)
 		" -d name    device to open\n"
 		" -e val     read 'val' external time stamp events\n"
 		" -f val     adjust the ptp clock frequency by 'val' ppb\n"
+		" -F pid,msk apply ts channel mask to queue open by pid\n"
 		" -g         get the ptp clock time\n"
 		" -h         prints this message\n"
 		" -i val     index for event/trigger\n"
@@ -162,6 +163,7 @@ int main(int argc, char *argv[])
 	struct ptp_sys_offset *sysoff;
 	struct ptp_sys_offset_extended *soe;
 	struct ptp_sys_offset_precise *xts;
+	struct ptp_tsfilter_request tsfilter_req;
 
 	char *progname;
 	unsigned int i;
@@ -194,9 +196,14 @@ int main(int argc, char *argv[])
 	int64_t pulsewidth = -1;
 	int64_t perout = -1;
 
+	tsfilter_req.reader_pid = 0;
+	tsfilter_req.tsevqmask = 0xFFFFFFFF;
+
 	progname = strrchr(argv[0], '/');
 	progname = progname ? 1+progname : argv[0];
-	while (EOF != (c = getopt(argc, argv, "cd:e:f:ghH:i:k:lL:n:o:p:P:sSt:T:w:x:Xz"))) {
+	while (EOF !=
+	       (c = getopt(argc, argv,
+			   "cd:e:f:F:ghH:i:k:lL:n:o:p:P:sSt:T:w:x:Xz"))) {
 		switch (c) {
 		case 'c':
 			capabilities = 1;
@@ -210,6 +217,14 @@ int main(int argc, char *argv[])
 		case 'f':
 			adjfreq = atoi(optarg);
 			break;
+		case 'F':
+			cnt = sscanf(optarg, "%d,%X", &tsfilter_req.reader_pid,
+				     &tsfilter_req.tsevqmask);
+			if (cnt != 2) {
+				usage(progname);
+				return -1;
+			}
+			break;
 		case 'g':
 			gettime = 1;
 			break;
@@ -604,6 +619,15 @@ int main(int argc, char *argv[])
 		free(xts);
 	}
 
+	if (tsfilter_req.reader_pid != 0) {
+		if (ioctl(fd, PTP_FILTERTS_REQUEST, &tsfilter_req)) {
+			perror("PTP_FILTERTS_REQUEST");
+		} else {
+			printf("Timestamp event queue mask 0x%X applied to reader with PID: %d\n",
+			       (int)tsfilter_req.tsevqmask, tsfilter_req.reader_pid);
+		}
+	}
+
 	close(fd);
 	return 0;
 }
-- 
2.34.1