From 1eb0fafaa9865b729509a7d47ecf1f05c2c0595c Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@nbd.name>
Date: Mon, 2 Aug 2021 22:48:44 +0200
Subject: [PATCH] device: add support for configuring device link speed/duplex

The 'speed' option can be set to the speed in Mbps
The 'duplex' option can be 1 or 0 for full or half duplex

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 device.c       | 14 ++++++++++++++
 device.h       |  6 ++++++
 system-linux.c | 52 ++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 72 insertions(+)

diff --git a/device.c b/device.c
index f9ec635..521d9a6 100644
--- a/device.c
+++ b/device.c
@@ -61,6 +61,8 @@ static const struct blobmsg_policy dev_attrs[__DEV_ATTR_MAX] = {
 	[DEV_ATTR_DROP_UNSOLICITED_NA] = { .name = "drop_unsolicited_na", .type = BLOBMSG_TYPE_BOOL },
 	[DEV_ATTR_ARP_ACCEPT] = { .name = "arp_accept", .type = BLOBMSG_TYPE_BOOL },
 	[DEV_ATTR_AUTH] = { .name = "auth", .type = BLOBMSG_TYPE_BOOL },
+	[DEV_ATTR_SPEED] = { .name = "speed", .type = BLOBMSG_TYPE_INT32 },
+	[DEV_ATTR_DUPLEX] = { .name = "duplex", .type = BLOBMSG_TYPE_BOOL },
 };
 
 const struct uci_blob_param_list device_attr_list = {
@@ -276,6 +278,8 @@ device_merge_settings(struct device *dev, struct device_settings *n)
 	n->arp_accept = s->flags & DEV_OPT_ARP_ACCEPT ?
 		s->arp_accept : os->arp_accept;
 	n->auth = s->flags & DEV_OPT_AUTH ? s->auth : os->auth;
+	n->speed = s->flags & DEV_OPT_SPEED ? s->speed : os->speed;
+	n->duplex = s->flags & DEV_OPT_DUPLEX ? s->duplex : os->duplex;
 	n->flags = s->flags | os->flags | os->valid_flags;
 }
 
@@ -450,6 +454,16 @@ device_init_settings(struct device *dev, struct blob_attr **tb)
 		s->flags |= DEV_OPT_AUTH;
 	}
 
+	if ((cur = tb[DEV_ATTR_SPEED])) {
+		s->speed = blobmsg_get_u32(cur);
+		s->flags |= DEV_OPT_SPEED;
+	}
+
+	if ((cur = tb[DEV_ATTR_DUPLEX])) {
+		s->duplex = blobmsg_get_bool(cur);
+		s->flags |= DEV_OPT_DUPLEX;
+	}
+
 	device_set_disabled(dev, disabled);
 }
 
diff --git a/device.h b/device.h
index dcae4c7..0968c98 100644
--- a/device.h
+++ b/device.h
@@ -60,6 +60,8 @@ enum {
 	DEV_ATTR_DROP_UNSOLICITED_NA,
 	DEV_ATTR_ARP_ACCEPT,
 	DEV_ATTR_AUTH,
+	DEV_ATTR_SPEED,
+	DEV_ATTR_DUPLEX,
 	__DEV_ATTR_MAX,
 };
 
@@ -121,6 +123,8 @@ enum {
 	DEV_OPT_DROP_GRATUITOUS_ARP	= (1ULL << 27),
 	DEV_OPT_DROP_UNSOLICITED_NA	= (1ULL << 28),
 	DEV_OPT_ARP_ACCEPT		= (1ULL << 29),
+	DEV_OPT_SPEED			= (1ULL << 30),
+	DEV_OPT_DUPLEX			= (1ULL << 31),
 };
 
 /* events broadcasted to all users of a device */
@@ -196,6 +200,8 @@ struct device_settings {
 	bool drop_unsolicited_na;
 	bool arp_accept;
 	bool auth;
+	unsigned int speed;
+	bool duplex;
 };
 
 /*
diff --git a/system-linux.c b/system-linux.c
index 65c62df..c0ba279 100644
--- a/system-linux.c
+++ b/system-linux.c
@@ -1578,6 +1578,57 @@ int system_vlandev_del(struct device *vlandev)
 	return system_link_del(vlandev->ifname);
 }
 
+static void
+system_set_ethtool_settings(struct device *dev, struct device_settings *s)
+{
+	struct ethtool_cmd ecmd = {
+		.cmd = ETHTOOL_GSET,
+	};
+	struct ifreq ifr = {
+		.ifr_data = (caddr_t)&ecmd,
+	};
+	static const struct {
+		int speed;
+		uint8_t bit_half;
+		uint8_t bit_full;
+	} speed_mask[] = {
+		{ 10, ETHTOOL_LINK_MODE_10baseT_Half_BIT, ETHTOOL_LINK_MODE_10baseT_Full_BIT },
+		{ 100, ETHTOOL_LINK_MODE_100baseT_Half_BIT, ETHTOOL_LINK_MODE_100baseT_Full_BIT },
+		{ 1000, ETHTOOL_LINK_MODE_1000baseT_Half_BIT, ETHTOOL_LINK_MODE_1000baseT_Full_BIT },
+	};
+	uint32_t adv;
+	int i;
+
+	strncpy(ifr.ifr_name, dev->ifname, sizeof(ifr.ifr_name) - 1);
+
+	if (ioctl(sock_ioctl, SIOCETHTOOL, &ifr) != 0)
+		return;
+
+	adv = ecmd.supported;
+	for (i = 0; i < ARRAY_SIZE(speed_mask); i++) {
+		if (s->flags & DEV_OPT_DUPLEX) {
+			int bit = s->duplex ? speed_mask[i].bit_half : speed_mask[i].bit_full;
+			adv &= ~(1 << bit);
+		}
+
+		if (!(s->flags & DEV_OPT_SPEED) ||
+		    s->speed == speed_mask[i].speed)
+			continue;
+
+		adv &= ~(1 << speed_mask[i].bit_full);
+		adv &= ~(1 << speed_mask[i].bit_half);
+	}
+
+
+	if (ecmd.autoneg && ecmd.advertising == adv)
+		return;
+
+	ecmd.autoneg = 1;
+	ecmd.advertising = adv;
+	ecmd.cmd = ETHTOOL_SSET;
+	ioctl(sock_ioctl, SIOCETHTOOL, &ifr);
+}
+
 void
 system_if_get_settings(struct device *dev, struct device_settings *s)
 {
@@ -1801,6 +1852,7 @@ system_if_apply_settings(struct device *dev, struct device_settings *s, uint64_t
 		system_set_drop_unsolicited_na(dev, s->drop_unsolicited_na ? "1" : "0");
 	if (apply_mask & DEV_OPT_ARP_ACCEPT)
 		system_set_arp_accept(dev, s->arp_accept ? "1" : "0");
+	system_set_ethtool_settings(dev, s);
 }
 
 int system_if_up(struct device *dev)
-- 
2.30.2