[RFC PATCH v3 3/5] Add ndo_hwtstamp_get/set support to vlan code path

[Maxim Georgiev <glipus@gmail.com>]

This patch makes VLAN subsystem to use the newly introduced
ndo_hwtstamp_get/set API to pass hw timestamp requests to
underlying NIC drivers in case if these drivers implement
ndo_hwtstamp_get/set functions. Otherwise VLAN·subsystem
falls back to calling ndo_eth_ioctl.

Suggested-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: Maxim Georgiev <glipus@gmail.com>
---
 net/8021q/vlan_dev.c | 42 +++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 41 insertions(+), 1 deletion(-)

diff --git a/net/8021q/vlan_dev.c b/net/8021q/vlan_dev.c
index 5920544e93e8..66d54c610aa5 100644
--- a/net/8021q/vlan_dev.c
+++ b/net/8021q/vlan_dev.c
@@ -353,6 +353,44 @@ static int vlan_dev_set_mac_address(struct net_device *dev, void *p)
 	return 0;
 }
 
+static int vlan_dev_hwtstamp(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	const struct net_device_ops *ops = dev->netdev_ops;
+	struct kernel_hwtstamp_config kernel_config = {};
+	struct hwtstamp_config config;
+	int err;
+
+	if (!netif_device_present(dev))
+		return -ENODEV;
+
+	if ((cmd == SIOCSHWTSTAMP && !ops->ndo_hwtstamp_set) ||
+	    (cmd == SIOCGHWTSTAMP && !ops->ndo_hwtstamp_get)) {
+		if (ops->ndo_eth_ioctl) {
+			return ops->ndo_eth_ioctl(real_dev, &ifr, cmd);
+		else
+			return -EOPNOTSUPP;
+	}
+
+	kernel_config.ifr = ifr;
+	if (cmd == SIOCSHWTSTAMP) {
+		if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+			return -EFAULT;
+
+		hwtstamp_config_to_kernel(&kernel_config, &config);
+		err = ops->ndo_hwtstamp_set(dev, &kernel_config, NULL);
+	} else if (cmd == SIOCGHWTSTAMP) {
+		err = ops->ndo_hwtstamp_get(dev, &kernel_config, NULL);
+	}
+
+	if (err)
+		return err;
+
+	hwtstamp_kernel_to_config(&config, &kernel_config);
+	if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
+		return -EFAULT;
+	return 0;
+}
+
 static int vlan_dev_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
 	struct net_device *real_dev = vlan_dev_priv(dev)->real_dev;
@@ -368,10 +406,12 @@ static int vlan_dev_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 		if (!net_eq(dev_net(dev), dev_net(real_dev)))
 			break;
 		fallthrough;
+	case SIOCGHWTSTAMP:
+		err = vlan_dev_hwtstamp(real_dev, &ifrr, cmd);
+		break;
 	case SIOCGMIIPHY:
 	case SIOCGMIIREG:
 	case SIOCSMIIREG:
-	case SIOCGHWTSTAMP:
 		if (netif_device_present(real_dev) && ops->ndo_eth_ioctl)
 			err = ops->ndo_eth_ioctl(real_dev, &ifrr, cmd);
 		break;
-- 
2.39.2