[RFC,v6,06/11] leds: trigger: netdev: add hardware control support

diff --git a/drivers/leds/trigger/ledtrig-netdev.c b/drivers/leds/trigger/ledtrig-netdev.c
index dd63cadb896e..ed019cb5867c 100644
--- a/drivers/leds/trigger/ledtrig-netdev.c
+++ b/drivers/leds/trigger/ledtrig-netdev.c
@@ -37,6 +37,7 @@ 
  */
 
 struct led_netdev_data {
+	enum led_blink_modes blink_mode;
 	spinlock_t lock;
 
 	struct delayed_work work;
@@ -53,11 +54,105 @@  struct led_netdev_data {
 	bool carrier_link_up;
 };
 
+struct netdev_led_attr_detail {
+	char *name;
+	bool hardware_only;
+	enum led_trigger_netdev_modes bit;
+};
+
+static struct netdev_led_attr_detail attr_details[] = {
+	{ .name = "link", .bit = TRIGGER_NETDEV_LINK},
+	{ .name = "tx", .bit = TRIGGER_NETDEV_TX},
+	{ .name = "rx", .bit = TRIGGER_NETDEV_RX},
+};
+
+static bool validate_baseline_state(struct led_netdev_data *trigger_data)
+{
+	struct led_classdev *led_cdev = trigger_data->led_cdev;
+	struct netdev_led_attr_detail *detail;
+	u32 hw_blink_mode_supported = 0;
+	bool force_sw = false;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(attr_details); i++) {
+		detail = &attr_details[i];
+
+		/* Mode not active, skip */
+		if (!test_bit(detail->bit, &trigger_data->mode))
+			continue;
+
+		/* Hardware only mode enabled on software controlled led */
+		if (led_cdev->blink_mode == SOFTWARE_CONTROLLED &&
+		    detail->hardware_only)
+			return false;
+
+		/* Check if the mode supports hardware mode */
+		if (led_cdev->blink_mode != SOFTWARE_CONTROLLED) {
+			/* With a net dev set, force software mode.
+			 * With modes are handled by hardware, led will blink
+			 * based on his own events and will ignore any event
+			 * from the provided dev.
+			 */
+			if (trigger_data->net_dev) {
+				force_sw = true;
+				continue;
+			}
+
+			/* With empty dev, check if the mode is supported */
+			if (led_trigger_blink_mode_is_supported(led_cdev, detail->bit))
+				hw_blink_mode_supported |= BIT(detail->bit);
+		}
+	}
+
+	/* We can't run modes handled by both software and hardware.
+	 * Check if we run hardware modes and check if all the modes
+	 * can be handled by hardware.
+	 */
+	if (hw_blink_mode_supported && hw_blink_mode_supported != trigger_data->mode)
+		return false;
+
+	/* Modes are valid. Decide now the running mode to later
+	 * set the baseline.
+	 * Software mode is enforced with net_dev set. With an empty
+	 * one hardware mode is selected by default (if supported).
+	 */
+	if (force_sw || led_cdev->blink_mode == SOFTWARE_CONTROLLED)
+		trigger_data->blink_mode = SOFTWARE_CONTROLLED;
+	else
+		trigger_data->blink_mode = HARDWARE_CONTROLLED;
+
+	return true;
+}
+
 static void set_baseline_state(struct led_netdev_data *trigger_data)
 {
+	int i;
 	int current_brightness;
+	struct netdev_led_attr_detail *detail;
 	struct led_classdev *led_cdev = trigger_data->led_cdev;
 
+	/* Modes already validated. Directly apply hw trigger modes */
+	if (trigger_data->blink_mode == HARDWARE_CONTROLLED) {
+		/* We are refreshing the blink modes. Reset them */
+		led_cdev->hw_control_configure(led_cdev, BIT(TRIGGER_NETDEV_LINK),
+					       BLINK_MODE_ZERO);
+
+		for (i = 0; i < ARRAY_SIZE(attr_details); i++) {
+			detail = &attr_details[i];
+
+			if (!test_bit(detail->bit, &trigger_data->mode))
+				continue;
+
+			led_cdev->hw_control_configure(led_cdev, BIT(detail->bit),
+						       BLINK_MODE_ENABLE);
+		}
+
+		led_cdev->hw_control_start(led_cdev);
+
+		return;
+	}
+
+	/* Handle trigger modes by software */
 	current_brightness = led_cdev->brightness;
 	if (current_brightness)
 		led_cdev->blink_brightness = current_brightness;
@@ -100,10 +195,15 @@  static ssize_t device_name_store(struct device *dev,
 				 size_t size)
 {
 	struct led_netdev_data *trigger_data = led_trigger_get_drvdata(dev);
+	struct net_device *old_net = trigger_data->net_dev;
+	char old_device_name[IFNAMSIZ];
 
 	if (size >= IFNAMSIZ)
 		return -EINVAL;
 
+	/* Backup old device name */
+	memcpy(old_device_name, trigger_data->device_name, IFNAMSIZ);
+
 	cancel_delayed_work_sync(&trigger_data->work);
 
 	spin_lock_bh(&trigger_data->lock);
@@ -122,6 +222,19 @@  static ssize_t device_name_store(struct device *dev,
 		trigger_data->net_dev =
 		    dev_get_by_name(&init_net, trigger_data->device_name);
 
+	if (!validate_baseline_state(trigger_data)) {
+		/* Restore old net_dev and device_name */
+		if (trigger_data->net_dev)
+			dev_put(trigger_data->net_dev);
+
+		dev_hold(old_net);
+		trigger_data->net_dev = old_net;
+		memcpy(trigger_data->device_name, old_device_name, IFNAMSIZ);
+
+		spin_unlock_bh(&trigger_data->lock);
+		return -EINVAL;
+	}
+
 	trigger_data->carrier_link_up = false;
 	if (trigger_data->net_dev != NULL)
 		trigger_data->carrier_link_up = netif_carrier_ok(trigger_data->net_dev);
@@ -159,7 +272,7 @@  static ssize_t netdev_led_attr_store(struct device *dev, const char *buf,
 				     size_t size, enum led_trigger_netdev_modes attr)
 {
 	struct led_netdev_data *trigger_data = led_trigger_get_drvdata(dev);
-	unsigned long state;
+	unsigned long state, old_mode = trigger_data->mode;
 	int ret;
 	int bit;
 
@@ -184,6 +297,12 @@  static ssize_t netdev_led_attr_store(struct device *dev, const char *buf,
 	else
 		clear_bit(bit, &trigger_data->mode);
 
+	if (!validate_baseline_state(trigger_data)) {
+		/* Restore old mode on validation fail */
+		trigger_data->mode = old_mode;
+		return -EINVAL;
+	}
+
 	set_baseline_state(trigger_data);
 
 	return size;
@@ -220,6 +339,8 @@  static ssize_t interval_store(struct device *dev,
 			      size_t size)
 {
 	struct led_netdev_data *trigger_data = led_trigger_get_drvdata(dev);
+	int old_interval = atomic_read(&trigger_data->interval);
+	u32 old_mode = trigger_data->mode;
 	unsigned long value;
 	int ret;
 
@@ -228,13 +349,22 @@  static ssize_t interval_store(struct device *dev,
 		return ret;
 
 	/* impose some basic bounds on the timer interval */
-	if (value >= 5 && value <= 10000) {
-		cancel_delayed_work_sync(&trigger_data->work);
+	if (value < 5 || value > 10000)
+		return -EINVAL;
+
+	cancel_delayed_work_sync(&trigger_data->work);
+
+	atomic_set(&trigger_data->interval, msecs_to_jiffies(value));
 
-		atomic_set(&trigger_data->interval, msecs_to_jiffies(value));
-		set_baseline_state(trigger_data);	/* resets timer */
+	if (!validate_baseline_state(trigger_data)) {
+		/* Restore old interval on validation error */
+		atomic_set(&trigger_data->interval, old_interval);
+		trigger_data->mode = old_mode;
+		return -EINVAL;
 	}
 
+	set_baseline_state(trigger_data);	/* resets timer */
+
 	return size;
 }
 
@@ -368,13 +498,25 @@  static int netdev_trig_activate(struct led_classdev *led_cdev)
 	trigger_data->mode = 0;
 	atomic_set(&trigger_data->interval, msecs_to_jiffies(50));
 	trigger_data->last_activity = 0;
+	if (led_cdev->blink_mode != SOFTWARE_CONTROLLED) {
+		/* With hw mode enabled reset any rule set by default */
+		if (led_cdev->hw_control_status(led_cdev)) {
+			rc = led_cdev->hw_control_configure(led_cdev, BIT(TRIGGER_NETDEV_LINK),
+							    BLINK_MODE_ZERO);
+			if (rc)
+				goto err;
+		}
+	}
 
 	led_set_trigger_data(led_cdev, trigger_data);
 
 	rc = register_netdevice_notifier(&trigger_data->notifier);
 	if (rc)
-		kfree(trigger_data);
+		goto err;
 
+	return 0;
+err:
+	kfree(trigger_data);
 	return rc;
 }
 
@@ -394,6 +536,7 @@  static void netdev_trig_deactivate(struct led_classdev *led_cdev)
 
 static struct led_trigger netdev_led_trigger = {
 	.name = "netdev",
+	.supported_blink_modes = SOFTWARE_HARDWARE,
 	.activate = netdev_trig_activate,
 	.deactivate = netdev_trig_deactivate,
 	.groups = netdev_trig_groups,