1 files changed, 125 insertions, 12 deletions

diff --git a/target/linux/generic/files/drivers/net/phy/swconfig_leds.c b/target/linux/generic/files/drivers/net/phy/swconfig_leds.c
index 20b9a12a54..92be3c7501 100644
--- a/target/linux/generic/files/drivers/net/phy/swconfig_leds.c
+++ b/target/linux/generic/files/drivers/net/phy/swconfig_leds.c
@@ -29,6 +29,15 @@
 					 SWCONFIG_LED_PORT_SPEED_100 | \
 					 SWCONFIG_LED_PORT_SPEED_1000)
 
+#define SWCONFIG_LED_MODE_LINK		0x01
+#define SWCONFIG_LED_MODE_TX		0x02
+#define SWCONFIG_LED_MODE_RX		0x04
+#define SWCONFIG_LED_MODE_TXRX		(SWCONFIG_LED_MODE_TX   | \
+					 SWCONFIG_LED_MODE_RX)
+#define SWCONFIG_LED_MODE_ALL		(SWCONFIG_LED_MODE_LINK | \
+					 SWCONFIG_LED_MODE_TX   | \
+					 SWCONFIG_LED_MODE_RX)
+
 struct switch_led_trigger {
 	struct led_trigger trig;
 	struct switch_dev *swdev;
@@ -36,7 +45,8 @@ struct switch_led_trigger {
 	struct delayed_work sw_led_work;
 	u32 port_mask;
 	u32 port_link;
-	unsigned long long port_traffic[SWCONFIG_LED_NUM_PORTS];
+	unsigned long long port_tx_traffic[SWCONFIG_LED_NUM_PORTS];
+	unsigned long long port_rx_traffic[SWCONFIG_LED_NUM_PORTS];
 	u8 link_speed[SWCONFIG_LED_NUM_PORTS];
 };
 
@@ -50,6 +60,7 @@ struct swconfig_trig_data {
 	bool prev_link;
 	unsigned long prev_traffic;
 	enum led_brightness prev_brightness;
+	u8 mode;
 	u8 speed_mask;
 };
 
@@ -186,6 +197,79 @@ static ssize_t swconfig_trig_speed_mask_store(struct device *dev,
 static DEVICE_ATTR(speed_mask, 0644, swconfig_trig_speed_mask_show,
 		   swconfig_trig_speed_mask_store);
 
+static ssize_t swconfig_trig_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
+	u8 mode;
+
+	read_lock(&trig_data->lock);
+	mode = trig_data->mode;
+	read_unlock(&trig_data->lock);
+
+	if (mode == 0) {
+		strcpy(buf, "none\n");
+	} else {
+		if (mode & SWCONFIG_LED_MODE_LINK)
+			strcat(buf, "link ");
+		if (mode & SWCONFIG_LED_MODE_TX)
+			strcat(buf, "tx ");
+		if (mode & SWCONFIG_LED_MODE_RX)
+			strcat(buf, "rx ");
+		strcat(buf, "\n");
+	}
+
+	return strlen(buf)+1;
+}
+
+static ssize_t swconfig_trig_mode_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct swconfig_trig_data *trig_data = led_cdev->trigger_data;
+	char copybuf[128];
+	int new_mode = -1;
+	char *p, *token;
+
+	/* take a copy since we don't want to trash the inbound buffer when using strsep */
+	strncpy(copybuf, buf, sizeof(copybuf));
+	copybuf[sizeof(copybuf) - 1] = 0;
+	p = copybuf;
+
+	while ((token = strsep(&p, " \t\n")) != NULL) {
+		if (!*token)
+			continue;
+
+		if (new_mode < 0)
+			new_mode = 0;
+
+		if (!strcmp(token, "none"))
+			new_mode = 0;
+		else if (!strcmp(token, "tx"))
+			new_mode |= SWCONFIG_LED_MODE_TX;
+		else if (!strcmp(token, "rx"))
+			new_mode |= SWCONFIG_LED_MODE_RX;
+		else if (!strcmp(token, "link"))
+			new_mode |= SWCONFIG_LED_MODE_LINK;
+		else
+			return -EINVAL;
+	}
+
+	if (new_mode < 0)
+		return -EINVAL;
+
+	write_lock(&trig_data->lock);
+	trig_data->mode = (u8)new_mode;
+	write_unlock(&trig_data->lock);
+
+	return size;
+}
+
+/* mode special file */
+static DEVICE_ATTR(mode, 0644, swconfig_trig_mode_show,
+		   swconfig_trig_mode_store);
+
 static void
 swconfig_trig_activate(struct led_classdev *led_cdev)
 {
@@ -206,6 +290,7 @@ swconfig_trig_activate(struct led_classdev *led_cdev)
 	trig_data->led_cdev = led_cdev;
 	trig_data->swdev = sw_trig->swdev;
 	trig_data->speed_mask = SWCONFIG_LED_PORT_SPEED_ALL;
+	trig_data->mode = SWCONFIG_LED_MODE_ALL;
 	led_cdev->trigger_data = trig_data;
 
 	err = device_create_file(led_cdev->dev, &dev_attr_port_mask);
@@ -216,8 +301,15 @@ swconfig_trig_activate(struct led_classdev *led_cdev)
 	if (err)
 		goto err_dev_free;
 
+	err = device_create_file(led_cdev->dev, &dev_attr_mode);
+	if (err)
+		goto err_mode_free;
+
 	return;
 
+err_mode_free:
+	device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
+
 err_dev_free:
 	device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 
@@ -237,10 +329,17 @@ swconfig_trig_deactivate(struct led_classdev *led_cdev)
 	if (trig_data) {
 		device_remove_file(led_cdev->dev, &dev_attr_port_mask);
 		device_remove_file(led_cdev->dev, &dev_attr_speed_mask);
+		device_remove_file(led_cdev->dev, &dev_attr_mode);
 		kfree(trig_data);
 	}
 }
 
+/*
+ * link off -> led off (can't be any other reason to turn it on)
+ * link on:
+ *	mode link: led on by default only if speed matches, else off
+ *	mode txrx: blink only if speed matches, else off
+ */
 static void
 swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 			struct led_classdev *led_cdev)
@@ -248,7 +347,8 @@ swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 	struct swconfig_trig_data *trig_data;
 	u32 port_mask;
 	bool link;
-	u8 speed_mask;
+	u8 speed_mask, mode;
+	enum led_brightness led_base, led_blink;
 
 	trig_data = led_cdev->trigger_data;
 	if (!trig_data)
@@ -257,35 +357,48 @@ swconfig_trig_led_event(struct switch_led_trigger *sw_trig,
 	read_lock(&trig_data->lock);
 	port_mask = trig_data->port_mask;
 	speed_mask = trig_data->speed_mask;
+	mode = trig_data->mode;
 	read_unlock(&trig_data->lock);
 
 	link = !!(sw_trig->port_link & port_mask);
 	if (!link) {
-		if (link != trig_data->prev_link)
-			swconfig_trig_set_brightness(trig_data, LED_OFF);
-	} else {
+		if (trig_data->prev_brightness != LED_OFF)
+			swconfig_trig_set_brightness(trig_data, LED_OFF); /* and stop */
+	}
+	else {
 		unsigned long traffic;
 		int speedok;	/* link speed flag */
 		int i;
 
+		led_base = LED_FULL;
+		led_blink = LED_OFF;
 		traffic = 0;
 		speedok = 0;
 		for (i = 0; i < SWCONFIG_LED_NUM_PORTS; i++) {
-			if (port_mask & (1 << i))
+			if (port_mask & (1 << i)) {
 				if (sw_trig->link_speed[i] & speed_mask) {
-					traffic += sw_trig->port_traffic[i];
+					traffic += ((mode & SWCONFIG_LED_MODE_TX) ?
+						    sw_trig->port_tx_traffic[i] : 0) +
+						((mode & SWCONFIG_LED_MODE_RX) ?
+						 sw_trig->port_rx_traffic[i] : 0);
 					speedok = 1;
 				}
+			}
 		}
 
 		if (speedok) {
 			/* At least one port speed matches speed_mask */
-			if (trig_data->prev_brightness != LED_FULL)
+			if (!(mode & SWCONFIG_LED_MODE_LINK)) {
+				led_base = LED_OFF;
+				led_blink = LED_FULL;
+			}
+
+			if (trig_data->prev_brightness != led_base)
 				swconfig_trig_set_brightness(trig_data,
-							     LED_FULL);
+							     led_base);
 			else if (traffic != trig_data->prev_traffic)
 				swconfig_trig_set_brightness(trig_data,
-							     LED_OFF);
+							     led_blink);
 		} else if (trig_data->prev_brightness != LED_OFF)
 			swconfig_trig_set_brightness(trig_data, LED_OFF);
 
@@ -371,8 +484,8 @@ swconfig_led_work_func(struct work_struct *work)
 
 			memset(&port_stats, '\0', sizeof(port_stats));
 			swdev->ops->get_port_stats(swdev, i, &port_stats);
-			sw_trig->port_traffic[i] = port_stats.tx_bytes +
-						   port_stats.rx_bytes;
+			sw_trig->port_tx_traffic[i] = port_stats.tx_bytes;
+			sw_trig->port_rx_traffic[i] = port_stats.rx_bytes;
 		}
 	}