3 files changed, 163 insertions, 6 deletions

diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
index 75b54730a963..403dd3d8986e 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -26,6 +26,40 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
+struct {
+        u32 usage_id;
+        int unit; /* 0 for default others from HID sensor spec */
+        int scale_val0; /* scale, whole number */
+        int scale_val1; /* scale, fraction in micros */
+} static unit_conversion[] = {
+        {HID_USAGE_SENSOR_ACCEL_3D, 0, 9, 806650},
+        {HID_USAGE_SENSOR_ACCEL_3D,
+                HID_USAGE_SENSOR_UNITS_METERS_PER_SEC_SQRD, 1, 0},
+        {HID_USAGE_SENSOR_ACCEL_3D,
+                HID_USAGE_SENSOR_UNITS_G, 9, 806650},
+
+        {HID_USAGE_SENSOR_GYRO_3D, 0, 0, 17453},
+        {HID_USAGE_SENSOR_GYRO_3D,
+                HID_USAGE_SENSOR_UNITS_RADIANS_PER_SECOND, 1, 0},
+        {HID_USAGE_SENSOR_GYRO_3D,
+                HID_USAGE_SENSOR_UNITS_DEGREES_PER_SECOND, 0, 17453},
+
+        {HID_USAGE_SENSOR_COMPASS_3D, 0, 0, 1000},
+        {HID_USAGE_SENSOR_COMPASS_3D, HID_USAGE_SENSOR_UNITS_GAUSS, 1, 0},
+
+        {HID_USAGE_SENSOR_INCLINOMETER_3D, 0, 0, 17453},
+        {HID_USAGE_SENSOR_INCLINOMETER_3D,
+                HID_USAGE_SENSOR_UNITS_DEGREES, 0, 17453},
+        {HID_USAGE_SENSOR_INCLINOMETER_3D,
+                HID_USAGE_SENSOR_UNITS_RADIANS, 1, 0},
+
+        {HID_USAGE_SENSOR_ALS, 0, 1, 0},
+        {HID_USAGE_SENSOR_ALS, HID_USAGE_SENSOR_UNITS_LUX, 1, 0},
+
+        {HID_USAGE_SENSOR_PRESSURE, 0, 100000, 0},
+        {HID_USAGE_SENSOR_PRESSURE, HID_USAGE_SENSOR_UNITS_PASCAL, 1, 0},
+};
+
 static int pow_10(unsigned power)
 {
         int i;
@@ -113,6 +147,26 @@ static u32 convert_to_vtf_format(int size, int exp, int val1, int val2)
         return value;
 }
 
+s32 hid_sensor_read_poll_value(struct hid_sensor_common *st)
+{
+        s32 value = 0;
+        int ret;
+
+        ret = sensor_hub_get_feature(st->hsdev,
+                st->poll.report_id,
+                st->poll.index, &value);
+
+        if (ret < 0 || value < 0) {
+                return -EINVAL;
+        } else {
+                if (st->poll.units == HID_USAGE_SENSOR_UNITS_SECOND)
+                        value = value * 1000;
+        }
+
+        return value;
+}
+EXPORT_SYMBOL(hid_sensor_read_poll_value);
+
 int hid_sensor_read_samp_freq_value(struct hid_sensor_common *st,
                                 int *val1, int *val2)
 {
@@ -209,15 +263,108 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st,
 }
 EXPORT_SYMBOL(hid_sensor_write_raw_hyst_value);
 
-int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+/*
+ * This fuction applies the unit exponent to the scale.
+ * For example:
+ * 9.806650 ->exp:2-> val0[980]val1[665000]
+ * 9.000806 ->exp:2-> val0[900]val1[80600]
+ * 0.174535 ->exp:2-> val0[17]val1[453500]
+ * 1.001745 ->exp:0-> val0[1]val1[1745]
+ * 1.001745 ->exp:2-> val0[100]val1[174500]
+ * 1.001745 ->exp:4-> val0[10017]val1[450000]
+ * 9.806650 ->exp:-2-> val0[0]val1[98066]
+ */
+static void adjust_exponent_micro(int *val0, int *val1, int scale0,
+                                  int scale1, int exp)
+{
+        int i;
+        int x;
+        int res;
+        int rem;
+
+        if (exp > 0) {
+                *val0 = scale0 * pow_10(exp);
+                res = 0;
+                if (exp > 6) {
+                        *val1 = 0;
+                        return;
+                }
+                for (i = 0; i < exp; ++i) {
+                        x = scale1 / pow_10(5 - i);
+                        res += (pow_10(exp - 1 - i) * x);
+                        scale1 = scale1 % pow_10(5 - i);
+                }
+                *val0 += res;
+                        *val1 = scale1 * pow_10(exp);
+        } else if (exp < 0) {
+                exp = abs(exp);
+                if (exp > 6) {
+                        *val0 = *val1 = 0;
+                        return;
+                }
+                *val0 = scale0 / pow_10(exp);
+                rem = scale0 % pow_10(exp);
+                res = 0;
+                for (i = 0; i < (6 - exp); ++i) {
+                        x = scale1 / pow_10(5 - i);
+                        res += (pow_10(5 - exp - i) * x);
+                        scale1 = scale1 % pow_10(5 - i);
+                }
+                *val1 = rem * pow_10(6 - exp) + res;
+        } else {
+                *val0 = scale0;
+                *val1 = scale1;
+        }
+}
+
+int hid_sensor_format_scale(u32 usage_id,
+                        struct hid_sensor_hub_attribute_info *attr_info,
+                        int *val0, int *val1)
+{
+        int i;
+        int exp;
+
+        *val0 = 1;
+        *val1 = 0;
+
+        for (i = 0; i < ARRAY_SIZE(unit_conversion); ++i) {
+                if (unit_conversion[i].usage_id == usage_id &&
+                        unit_conversion[i].unit == attr_info->units) {
+                        exp  = hid_sensor_convert_exponent(
+                                                attr_info->unit_expo);
+                        adjust_exponent_micro(val0, val1,
+                                        unit_conversion[i].scale_val0,
+                                        unit_conversion[i].scale_val1, exp);
+                        break;
+                }
+        }
+
+        return IIO_VAL_INT_PLUS_MICRO;
+}
+EXPORT_SYMBOL(hid_sensor_format_scale);
+
+int hid_sensor_get_reporting_interval(struct hid_sensor_hub_device *hsdev,
                                         u32 usage_id,
                                         struct hid_sensor_common *st)
 {
-
         sensor_hub_input_get_attribute_info(hsdev,
                                         HID_FEATURE_REPORT, usage_id,
                                         HID_USAGE_SENSOR_PROP_REPORT_INTERVAL,
                                         &st->poll);
+        /* Default unit of measure is milliseconds */
+        if (st->poll.units == 0)
+                st->poll.units = HID_USAGE_SENSOR_UNITS_MILLISECOND;
+        return 0;
+
+}
+
+int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+                                        u32 usage_id,
+                                        struct hid_sensor_common *st)
+{
+
+
+        hid_sensor_get_reporting_interval(hsdev, usage_id, st);
 
         sensor_hub_input_get_attribute_info(hsdev,
                                         HID_FEATURE_REPORT, usage_id,
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index dbefbdaf7cd1..73282cee0c81 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -28,16 +28,17 @@
 #include <linux/iio/sysfs.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_common *st = iio_trigger_get_drvdata(trig);
         int state_val;
         int report_val;
 
         if (state) {
                 if (sensor_hub_device_open(st->hsdev))
                         return -EIO;
+
+                atomic_inc(&st->data_ready);
+
                 state_val = hid_sensor_get_usage_index(st->hsdev,
                         st->power_state.report_id,
                         st->power_state.index,
@@ -47,6 +48,8 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
                         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,
@@ -57,7 +60,6 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
                         st->report_state.index,
                         HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM);
         }
-        st->data_ready = state;
 
         if (state_val >= 0) {
                 state_val += st->power_state.logical_minimum;
@@ -75,6 +77,13 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 
         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 hid_sensor_common *attrb)
 {
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
index ca02f7811aa8..0f8e78c249d3 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
@@ -22,5 +22,6 @@
 int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
                                 struct hid_sensor_common *attrb);
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb);
+int hid_sensor_power_state(struct hid_sensor_common *st, bool state);
 
 #endif