[PATCH RFC V1 net-next 3/4] net: Let the active time stamping layer be selectable.

[Richard Cochran <richardcochran@gmail.com>]

Make the sysfs knob writable, and add checks in the ioctl and time
stamping paths to respect the currently selected time stamping layer.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 .../ABI/testing/sysfs-class-net-timestamping  |  5 +-
 net/core/dev_ioctl.c                          | 44 ++++++++++++++--
 net/core/net-sysfs.c                          | 50 +++++++++++++++++--
 net/core/timestamping.c                       |  6 +++
 net/ethtool/common.c                          | 18 +++++--
 5 files changed, 111 insertions(+), 12 deletions(-)

diff --git a/Documentation/ABI/testing/sysfs-class-net-timestamping b/Documentation/ABI/testing/sysfs-class-net-timestamping
index 529c3a6eb607..6dfd59740cad 100644
--- a/Documentation/ABI/testing/sysfs-class-net-timestamping
+++ b/Documentation/ABI/testing/sysfs-class-net-timestamping
@@ -11,7 +11,10 @@ What:		/sys/class/net/<iface>/current_timestamping_provider
 Date:		January 2022
 Contact:	Richard Cochran <richardcochran@gmail.com>
 Description:
-		Show the current SO_TIMESTAMPING provider.
+		Shows or sets the current SO_TIMESTAMPING provider.
+		When changing the value, some packets in the kernel
+		networking stack may still be delivered with time
+		stamps from the previous provider.
 		The possible values are:
 		- "mac"  The MAC provides time stamping.
 		- "phy"  The PHY or MII device provides time stamping.
diff --git a/net/core/dev_ioctl.c b/net/core/dev_ioctl.c
index 1b807d119da5..269068ce3a51 100644
--- a/net/core/dev_ioctl.c
+++ b/net/core/dev_ioctl.c
@@ -260,6 +260,43 @@ static int dev_eth_ioctl(struct net_device *dev,
 	return err;
 }
 
+static int dev_hwtstamp_ioctl(struct net_device *dev,
+			      struct ifreq *ifr, unsigned int cmd)
+{
+	const struct net_device_ops *ops = dev->netdev_ops;
+	int err;
+
+	err = dsa_ndo_eth_ioctl(dev, ifr, cmd);
+	if (err == 0 || err != -EOPNOTSUPP)
+		return err;
+
+	if (!netif_device_present(dev))
+		return -ENODEV;
+
+	switch (dev->selected_timestamping_layer) {
+
+	case MAC_TIMESTAMPING:
+		if (ops->ndo_do_ioctl == phy_do_ioctl) {
+			/* Some drivers set .ndo_do_ioctl to phy_do_ioctl. */
+			err = -EOPNOTSUPP;
+		} else {
+			err = ops->ndo_eth_ioctl(dev, ifr, cmd);
+		}
+		break;
+
+	case PHY_TIMESTAMPING:
+		if (phy_has_hwtstamp(dev->phydev)) {
+			err = phy_mii_ioctl(dev->phydev, ifr, cmd);
+		} else {
+			err = -ENODEV;
+			WARN_ON(1);
+		}
+		break;
+	}
+
+	return err;
+}
+
 static int dev_siocbond(struct net_device *dev,
 			struct ifreq *ifr, unsigned int cmd)
 {
@@ -395,6 +432,9 @@ static int dev_ifsioc(struct net *net, struct ifreq *ifr, void __user *data,
 			return err;
 		fallthrough;
 
+	case SIOCGHWTSTAMP:
+		return dev_hwtstamp_ioctl(dev, ifr, cmd);
+
 	/*
 	 *	Unknown or private ioctl
 	 */
@@ -405,9 +445,7 @@ static int dev_ifsioc(struct net *net, struct ifreq *ifr, void __user *data,
 
 		if (cmd == SIOCGMIIPHY ||
 		    cmd == SIOCGMIIREG ||
-		    cmd == SIOCSMIIREG ||
-		    cmd == SIOCSHWTSTAMP ||
-		    cmd == SIOCGHWTSTAMP) {
+		    cmd == SIOCSMIIREG) {
 			err = dev_eth_ioctl(dev, ifr, cmd);
 		} else if (cmd == SIOCBONDENSLAVE ||
 		    cmd == SIOCBONDRELEASE ||
diff --git a/net/core/net-sysfs.c b/net/core/net-sysfs.c
index 4ff7ef417c38..c27f01a1a285 100644
--- a/net/core/net-sysfs.c
+++ b/net/core/net-sysfs.c
@@ -664,17 +664,59 @@ static ssize_t current_timestamping_provider_show(struct device *dev,
 	if (!rtnl_trylock())
 		return restart_syscall();
 
-	if (phy_has_tsinfo(phydev)) {
-		ret = sprintf(buf, "%s\n", "phy");
-	} else {
+	switch (netdev->selected_timestamping_layer) {
+	case MAC_TIMESTAMPING:
 		ret = sprintf(buf, "%s\n", "mac");
+		break;
+	case PHY_TIMESTAMPING:
+		ret = sprintf(buf, "%s\n", "phy");
+		break;
 	}
 
 	rtnl_unlock();
 
 	return ret;
 }
-static DEVICE_ATTR_RO(current_timestamping_provider);
+
+static ssize_t current_timestamping_provider_store(struct device *dev,
+						   struct device_attribute *attr,
+						   const char *buf, size_t len)
+{
+	struct net_device *netdev = to_net_dev(dev);
+	struct net *net = dev_net(netdev);
+	enum timestamping_layer flavor;
+
+	if (!ns_capable(net->user_ns, CAP_NET_ADMIN))
+		return -EPERM;
+
+	if (!strcmp(buf, "mac\n"))
+		flavor = MAC_TIMESTAMPING;
+	else if (!strcmp(buf, "phy\n"))
+		flavor = PHY_TIMESTAMPING;
+	else
+		return -EINVAL;
+
+	if (!rtnl_trylock())
+		return restart_syscall();
+
+	if (!dev_isalive(netdev))
+		goto out;
+
+	if (netdev->selected_timestamping_layer != flavor) {
+		const struct net_device_ops *ops = netdev->netdev_ops;
+		struct ifreq ifr = {0};
+
+		/* Disable time stamping in the current layer. */
+		if (netif_device_present(netdev) && ops->ndo_eth_ioctl)
+			ops->ndo_eth_ioctl(netdev, &ifr, SIOCSHWTSTAMP);
+
+		netdev->selected_timestamping_layer = flavor;
+	}
+out:
+	rtnl_unlock();
+	return len;
+}
+static DEVICE_ATTR_RW(current_timestamping_provider);
 
 static struct attribute *net_class_attrs[] __ro_after_init = {
 	&dev_attr_netdev_group.attr,
diff --git a/net/core/timestamping.c b/net/core/timestamping.c
index 04840697fe79..31c3142787b7 100644
--- a/net/core/timestamping.c
+++ b/net/core/timestamping.c
@@ -28,6 +28,9 @@ void skb_clone_tx_timestamp(struct sk_buff *skb)
 	if (!skb->sk)
 		return;
 
+	if (skb->dev->selected_timestamping_layer != PHY_TIMESTAMPING)
+		return;
+
 	type = classify(skb);
 	if (type == PTP_CLASS_NONE)
 		return;
@@ -50,6 +53,9 @@ bool skb_defer_rx_timestamp(struct sk_buff *skb)
 	if (!skb->dev || !skb->dev->phydev || !skb->dev->phydev->mii_ts)
 		return false;
 
+	if (skb->dev->selected_timestamping_layer != PHY_TIMESTAMPING)
+		return false;
+
 	if (skb_headroom(skb) < ETH_HLEN)
 		return false;
 
diff --git a/net/ethtool/common.c b/net/ethtool/common.c
index 651d18eef589..7b50820c1d1d 100644
--- a/net/ethtool/common.c
+++ b/net/ethtool/common.c
@@ -545,10 +545,20 @@ int __ethtool_get_ts_info(struct net_device *dev, struct ethtool_ts_info *info)
 	memset(info, 0, sizeof(*info));
 	info->cmd = ETHTOOL_GET_TS_INFO;
 
-	if (phy_has_tsinfo(phydev))
-		return phy_ts_info(phydev, info);
-	if (ops->get_ts_info)
-		return ops->get_ts_info(dev, info);
+	switch (dev->selected_timestamping_layer) {
+
+	case MAC_TIMESTAMPING:
+		if (ops->get_ts_info)
+			return ops->get_ts_info(dev, info);
+		break;
+
+	case PHY_TIMESTAMPING:
+		if (phy_has_tsinfo(phydev)) {
+			return phy_ts_info(phydev, info);
+		}
+		WARN_ON(1);
+		return -ENODEV;
+	}
 
 	info->so_timestamping = SOF_TIMESTAMPING_RX_SOFTWARE |
 				SOF_TIMESTAMPING_SOFTWARE;
-- 
2.20.1