1 files changed, 55 insertions, 18 deletions

diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index d60198a6ca2..a3109a6f4d8 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -26,35 +26,72 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
 #include <linux/iio/sysfs.h>
-#include "hid-sensor-attributes.h"
 #include "hid-sensor-trigger.h"
 
-static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
-						bool state)
+int hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 {
-	struct hid_sensor_iio_common *st = trig->private_data;
 	int state_val;
+	int report_val;
+
+	if (state) {
+		if (sensor_hub_device_open(st->hsdev))
+			return -EIO;
+
+		atomic_inc(&st->data_ready);
 
-	state_val = state ? 1 : 0;
-	if (IS_ENABLED(CONFIG_HID_SENSOR_ENUM_BASE_QUIRKS))
-		++state_val;
-	st->data_ready = state;
-	sensor_hub_set_feature(st->hsdev, st->power_state.report_id,
+		state_val = hid_sensor_get_usage_index(st->hsdev,
+			st->power_state.report_id,
+			st->power_state.index,
+			HID_USAGE_SENSOR_PROP_POWER_STATE_D0_FULL_POWER_ENUM);
+		report_val = hid_sensor_get_usage_index(st->hsdev,
+			st->report_state.report_id,
+			st->report_state.index,
+			HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM);
+	} else {
+		if (!atomic_dec_and_test(&st->data_ready))
+			return 0;
+		sensor_hub_device_close(st->hsdev);
+		state_val = hid_sensor_get_usage_index(st->hsdev,
+			st->power_state.report_id,
+			st->power_state.index,
+			HID_USAGE_SENSOR_PROP_POWER_STATE_D4_POWER_OFF_ENUM);
+		report_val = hid_sensor_get_usage_index(st->hsdev,
+			st->report_state.report_id,
+			st->report_state.index,
+			HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM);
+	}
+
+	if (state_val >= 0) {
+		state_val += st->power_state.logical_minimum;
+		sensor_hub_set_feature(st->hsdev, st->power_state.report_id,
 					st->power_state.index,
 					(s32)state_val);
+	}
 
-	sensor_hub_set_feature(st->hsdev, st->report_state.report_id,
+	if (report_val >= 0) {
+		report_val += st->report_state.logical_minimum;
+		sensor_hub_set_feature(st->hsdev, st->report_state.report_id,
 					st->report_state.index,
-					(s32)state_val);
+					(s32)report_val);
+	}
 
+	sensor_hub_get_feature(st->hsdev, st->power_state.report_id,
+					st->power_state.index,
+					&state_val);
 	return 0;
 }
+EXPORT_SYMBOL(hid_sensor_power_state);
+
+static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
+						bool state)
+{
+	return hid_sensor_power_state(iio_trigger_get_drvdata(trig), state);
+}
 
-void hid_sensor_remove_trigger(struct iio_dev *indio_dev)
+void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
 {
-	iio_trigger_unregister(indio_dev->trig);
-	iio_trigger_free(indio_dev->trig);
-	indio_dev->trig = NULL;
+	iio_trigger_unregister(attrb->trigger);
+	iio_trigger_free(attrb->trigger);
 }
 EXPORT_SYMBOL(hid_sensor_remove_trigger);
 
@@ -64,7 +101,7 @@ static const struct iio_trigger_ops hid_sensor_trigger_ops = {
 };
 
 int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
-				struct hid_sensor_iio_common *attrb)
+				struct hid_sensor_common *attrb)
 {
 	int ret;
 	struct iio_trigger *trig;
@@ -77,7 +114,7 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 	}
 
 	trig->dev.parent = indio_dev->dev.parent;
-	trig->private_data = attrb;
+	iio_trigger_set_drvdata(trig, attrb);
 	trig->ops = &hid_sensor_trigger_ops;
 	ret = iio_trigger_register(trig);
 
@@ -85,7 +122,7 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 		dev_err(&indio_dev->dev, "Trigger Register Failed\n");
 		goto error_free_trig;
 	}
-	indio_dev->trig = trig;
+	indio_dev->trig = attrb->trigger = trig;
 
 	return ret;