4 files changed, 164 insertions, 110 deletions

diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index 9021c9ce8851..5182031f7b52 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -56,9 +56,9 @@ struct sensor_hub_pending {
  * @dyn_callback_lock:  spin lock to protect callback list
  * @hid_sensor_hub_client_devs: Stores all MFD cells for a hub instance.
  * @hid_sensor_client_cnt: Number of MFD cells, (no of sensors attached).
+ * @ref_cnt:            Number of MFD clients have opened this device
  */
 struct sensor_hub_data {
-        struct hid_sensor_hub_device *hsdev;
         struct mutex mutex;
         spinlock_t lock;
         struct sensor_hub_pending pending;
@@ -67,6 +67,7 @@ struct sensor_hub_data {
         struct mfd_cell *hid_sensor_hub_client_devs;
         int hid_sensor_client_cnt;
         unsigned long quirks;
+        int ref_cnt;
 };
 
 /**
@@ -79,6 +80,7 @@ struct sensor_hub_data {
 struct hid_sensor_hub_callbacks_list {
         struct list_head list;
         u32 usage_id;
+        struct hid_sensor_hub_device *hsdev;
         struct hid_sensor_hub_callbacks *usage_callback;
         void *priv;
 };
@@ -97,20 +99,18 @@ static struct hid_report *sensor_hub_report(int id, struct hid_device *hdev,
         return NULL;
 }
 
-static int sensor_hub_get_physical_device_count(
-                                struct hid_report_enum *report_enum)
+static int sensor_hub_get_physical_device_count(struct hid_device *hdev)
 {
-        struct hid_report *report;
-        struct hid_field *field;
-        int cnt = 0;
+        int i;
+        int count = 0;
 
-        list_for_each_entry(report, &report_enum->report_list, list) {
-                field = report->field[0];
-                if (report->maxfield && field && field->physical)
-                        cnt++;
+        for (i = 0; i < hdev->maxcollection; ++i) {
+                struct hid_collection *collection = &hdev->collection[i];
+                if (collection->type == HID_COLLECTION_PHYSICAL)
+                        ++count;
         }
 
-        return cnt;
+        return count;
 }
 
 static void sensor_hub_fill_attr_info(
@@ -128,15 +128,23 @@ static void sensor_hub_fill_attr_info(
 
 static struct hid_sensor_hub_callbacks *sensor_hub_get_callback(
                                         struct hid_device *hdev,
-                                        u32 usage_id, void **priv)
+                                        u32 usage_id,
+                                        int collection_index,
+                                        struct hid_sensor_hub_device **hsdev,
+                                        void **priv)
 {
         struct hid_sensor_hub_callbacks_list *callback;
         struct sensor_hub_data *pdata = hid_get_drvdata(hdev);
 
         spin_lock(&pdata->dyn_callback_lock);
         list_for_each_entry(callback, &pdata->dyn_callback_list, list)
-                if (callback->usage_id == usage_id) {
+                if (callback->usage_id == usage_id &&
+                        (collection_index >=
+                                callback->hsdev->start_collection_index) &&
+                        (collection_index <
+                                callback->hsdev->end_collection_index)) {
                         *priv = callback->priv;
+                        *hsdev = callback->hsdev;
                         spin_unlock(&pdata->dyn_callback_lock);
                         return callback->usage_callback;
                 }
@@ -154,7 +162,8 @@ int sensor_hub_register_callback(struct hid_sensor_hub_device *hsdev,
 
         spin_lock(&pdata->dyn_callback_lock);
         list_for_each_entry(callback, &pdata->dyn_callback_list, list)
-                if (callback->usage_id == usage_id) {
+                if (callback->usage_id == usage_id &&
+                                                callback->hsdev == hsdev) {
                         spin_unlock(&pdata->dyn_callback_lock);
                         return -EINVAL;
                 }
@@ -163,6 +172,7 @@ int sensor_hub_register_callback(struct hid_sensor_hub_device *hsdev,
                 spin_unlock(&pdata->dyn_callback_lock);
                 return -ENOMEM;
         }
+        callback->hsdev = hsdev;
         callback->usage_callback = usage_callback;
         callback->usage_id = usage_id;
         callback->priv = NULL;
@@ -181,7 +191,8 @@ int sensor_hub_remove_callback(struct hid_sensor_hub_device *hsdev,
 
         spin_lock(&pdata->dyn_callback_lock);
         list_for_each_entry(callback, &pdata->dyn_callback_list, list)
-                if (callback->usage_id == usage_id) {
+                if (callback->usage_id == usage_id &&
+                                                callback->hsdev == hsdev) {
                         list_del(&callback->list);
                         kfree(callback);
                         break;
@@ -290,6 +301,28 @@ err_free:
 }
 EXPORT_SYMBOL_GPL(sensor_hub_input_attr_get_raw_value);
 
+int hid_sensor_get_usage_index(struct hid_sensor_hub_device *hsdev,
+                                u32 report_id, int field_index, u32 usage_id)
+{
+        struct hid_report *report;
+        struct hid_field *field;
+        int i;
+
+        report = sensor_hub_report(report_id, hsdev->hdev, HID_FEATURE_REPORT);
+        if (!report || (field_index >= report->maxfield))
+                goto done_proc;
+
+        field = report->field[field_index];
+        for (i = 0; i < field->maxusage; ++i) {
+                if (field->usage[i].hid == usage_id)
+                        return field->usage[i].usage_index;
+        }
+
+done_proc:
+        return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(hid_sensor_get_usage_index);
+
 int sensor_hub_input_get_attribute_info(struct hid_sensor_hub_device *hsdev,
                                 u8 type,
                                 u32 usage_id,
@@ -297,8 +330,7 @@ int sensor_hub_input_get_attribute_info(struct hid_sensor_hub_device *hsdev,
                                 struct hid_sensor_hub_attribute_info *info)
 {
         int ret = -1;
-        int i, j;
-        int collection_index = -1;
+        int i;
         struct hid_report *report;
         struct hid_field *field;
         struct hid_report_enum *report_enum;
@@ -312,44 +344,31 @@ int sensor_hub_input_get_attribute_info(struct hid_sensor_hub_device *hsdev,
         info->units = -1;
         info->unit_expo = -1;
 
-        for (i = 0; i < hdev->maxcollection; ++i) {
-                struct hid_collection *collection = &hdev->collection[i];
-                if (usage_id == collection->usage) {
-                        collection_index = i;
-                        break;
-                }
-        }
-        if (collection_index == -1)
-                goto err_ret;
-
         report_enum = &hdev->report_enum[type];
         list_for_each_entry(report, &report_enum->report_list, list) {
                 for (i = 0; i < report->maxfield; ++i) {
                         field = report->field[i];
-                        if (field->physical == usage_id &&
-                                field->logical == attr_usage_id) {
-                                sensor_hub_fill_attr_info(info, i, report->id,
-                                                          field);
-                                ret = 0;
-                        } else {
-                                for (j = 0; j < field->maxusage; ++j) {
-                                        if (field->usage[j].hid ==
-                                        attr_usage_id &&
-                                        field->usage[j].collection_index ==
-                                        collection_index) {
-                                                sensor_hub_fill_attr_info(info,
-                                                          i, report->id, field);
-                                                ret = 0;
-                                                break;
-                                        }
+                        if (field->maxusage) {
+                                if (field->physical == usage_id &&
+                                        (field->logical == attr_usage_id ||
+                                        field->usage[0].hid ==
+                                                        attr_usage_id) &&
+                                        (field->usage[0].collection_index >=
+                                        hsdev->start_collection_index) &&
+                                        (field->usage[0].collection_index <
+                                        hsdev->end_collection_index)) {
+
+                                        sensor_hub_fill_attr_info(info, i,
+                                                                report->id,
+                                                                field);
+                                        ret = 0;
+                                        break;
                                 }
                         }
-                        if (ret == 0)
-                                break;
                 }
+
         }
 
-err_ret:
         return ret;
 }
 EXPORT_SYMBOL_GPL(sensor_hub_input_get_attribute_info);
@@ -365,7 +384,7 @@ static int sensor_hub_suspend(struct hid_device *hdev, pm_message_t message)
         list_for_each_entry(callback, &pdata->dyn_callback_list, list) {
                 if (callback->usage_callback->suspend)
                         callback->usage_callback->suspend(
-                                        pdata->hsdev, callback->priv);
+                                        callback->hsdev, callback->priv);
         }
         spin_unlock(&pdata->dyn_callback_lock);
 
@@ -382,7 +401,7 @@ static int sensor_hub_resume(struct hid_device *hdev)
         list_for_each_entry(callback, &pdata->dyn_callback_list, list) {
                 if (callback->usage_callback->resume)
                         callback->usage_callback->resume(
-                                        pdata->hsdev, callback->priv);
+                                        callback->hsdev, callback->priv);
         }
         spin_unlock(&pdata->dyn_callback_lock);
 
@@ -409,6 +428,7 @@ static int sensor_hub_raw_event(struct hid_device *hdev,
         struct hid_sensor_hub_callbacks *callback = NULL;
         struct hid_collection *collection = NULL;
         void *priv = NULL;
+        struct hid_sensor_hub_device *hsdev = NULL;
 
         hid_dbg(hdev, "sensor_hub_raw_event report id:0x%x size:%d type:%d\n",
                          report->id, size, report->type);
@@ -443,23 +463,26 @@ static int sensor_hub_raw_event(struct hid_device *hdev,
                                 report->field[i]->usage->collection_index];
                 hid_dbg(hdev, "collection->usage %x\n",
                                         collection->usage);
-                callback = sensor_hub_get_callback(pdata->hsdev->hdev,
-                                                report->field[i]->physical,
-                                                        &priv);
+
+                callback = sensor_hub_get_callback(hdev,
+                                report->field[i]->physical,
+                                report->field[i]->usage[0].collection_index,
+                                &hsdev, &priv);
+
                 if (callback && callback->capture_sample) {
                         if (report->field[i]->logical)
-                                callback->capture_sample(pdata->hsdev,
+                                callback->capture_sample(hsdev,
                                         report->field[i]->logical, sz, ptr,
                                         callback->pdev);
                         else
-                                callback->capture_sample(pdata->hsdev,
+                                callback->capture_sample(hsdev,
                                         report->field[i]->usage->hid, sz, ptr,
                                         callback->pdev);
                 }
                 ptr += sz;
         }
         if (callback && collection && callback->send_event)
-                callback->send_event(pdata->hsdev, collection->usage,
+                callback->send_event(hsdev, collection->usage,
                                 callback->pdev);
         spin_unlock_irqrestore(&pdata->lock, flags);
 
@@ -472,7 +495,7 @@ int sensor_hub_device_open(struct hid_sensor_hub_device *hsdev)
         struct sensor_hub_data *data =  hid_get_drvdata(hsdev->hdev);
 
         mutex_lock(&data->mutex);
-        if (!hsdev->ref_cnt) {
+        if (!data->ref_cnt) {
                 ret = hid_hw_open(hsdev->hdev);
                 if (ret) {
                         hid_err(hsdev->hdev, "failed to open hid device\n");
@@ -480,7 +503,7 @@ int sensor_hub_device_open(struct hid_sensor_hub_device *hsdev)
                         return ret;
                 }
         }
-        hsdev->ref_cnt++;
+        data->ref_cnt++;
         mutex_unlock(&data->mutex);
 
         return ret;
@@ -492,8 +515,8 @@ void sensor_hub_device_close(struct hid_sensor_hub_device *hsdev)
         struct sensor_hub_data *data =  hid_get_drvdata(hsdev->hdev);
 
         mutex_lock(&data->mutex);
-        hsdev->ref_cnt--;
-        if (!hsdev->ref_cnt)
+        data->ref_cnt--;
+        if (!data->ref_cnt)
                 hid_hw_close(hsdev->hdev);
         mutex_unlock(&data->mutex);
 }
@@ -540,26 +563,19 @@ static int sensor_hub_probe(struct hid_device *hdev,
         struct sensor_hub_data *sd;
         int i;
         char *name;
-        struct hid_report *report;
-        struct hid_report_enum *report_enum;
-        struct hid_field *field;
         int dev_cnt;
+        struct hid_sensor_hub_device *hsdev;
+        struct hid_sensor_hub_device *last_hsdev = NULL;
 
         sd = devm_kzalloc(&hdev->dev, sizeof(*sd), GFP_KERNEL);
         if (!sd) {
                 hid_err(hdev, "cannot allocate Sensor data\n");
                 return -ENOMEM;
         }
-        sd->hsdev = devm_kzalloc(&hdev->dev, sizeof(*sd->hsdev), GFP_KERNEL);
-        if (!sd->hsdev) {
-                hid_err(hdev, "cannot allocate hid_sensor_hub_device\n");
-                return -ENOMEM;
-        }
+
         hid_set_drvdata(hdev, sd);
         sd->quirks = id->driver_data;
-        sd->hsdev->hdev = hdev;
-        sd->hsdev->vendor_id = hdev->vendor;
-        sd->hsdev->product_id = hdev->product;
+
         spin_lock_init(&sd->lock);
         spin_lock_init(&sd->dyn_callback_lock);
         mutex_init(&sd->mutex);
@@ -577,9 +593,8 @@ static int sensor_hub_probe(struct hid_device *hdev,
         }
         INIT_LIST_HEAD(&sd->dyn_callback_list);
         sd->hid_sensor_client_cnt = 0;
-        report_enum = &hdev->report_enum[HID_INPUT_REPORT];
 
-        dev_cnt = sensor_hub_get_physical_device_count(report_enum);
+        dev_cnt = sensor_hub_get_physical_device_count(hdev);
         if (dev_cnt > HID_MAX_PHY_DEVICES) {
                 hid_err(hdev, "Invalid Physical device count\n");
                 ret = -EINVAL;
@@ -593,42 +608,63 @@ static int sensor_hub_probe(struct hid_device *hdev,
                         ret = -ENOMEM;
                         goto err_stop_hw;
         }
-        list_for_each_entry(report, &report_enum->report_list, list) {
-                hid_dbg(hdev, "Report id:%x\n", report->id);
-                field = report->field[0];
-                if (report->maxfield && field &&
-                                        field->physical) {
+
+        for (i = 0; i < hdev->maxcollection; ++i) {
+                struct hid_collection *collection = &hdev->collection[i];
+
+                if (collection->type == HID_COLLECTION_PHYSICAL) {
+
+                        hsdev = kzalloc(sizeof(*hsdev), GFP_KERNEL);
+                        if (!hsdev) {
+                                hid_err(hdev, "cannot allocate hid_sensor_hub_device\n");
+                                ret = -ENOMEM;
+                                goto err_no_mem;
+                        }
+                        hsdev->hdev = hdev;
+                        hsdev->vendor_id = hdev->vendor;
+                        hsdev->product_id = hdev->product;
+                        hsdev->start_collection_index = i;
+                        if (last_hsdev)
+                                last_hsdev->end_collection_index = i;
+                        last_hsdev = hsdev;
                         name = kasprintf(GFP_KERNEL, "HID-SENSOR-%x",
-                                                field->physical);
+                                        collection->usage);
                         if (name == NULL) {
                                 hid_err(hdev, "Failed MFD device name\n");
                                         ret = -ENOMEM;
-                                        goto err_free_names;
+                                        goto err_no_mem;
                         }
                         sd->hid_sensor_hub_client_devs[
-                                sd->hid_sensor_client_cnt].id = PLATFORM_DEVID_AUTO;
+                                sd->hid_sensor_client_cnt].id =
+                                                        PLATFORM_DEVID_AUTO;
                         sd->hid_sensor_hub_client_devs[
                                 sd->hid_sensor_client_cnt].name = name;
                         sd->hid_sensor_hub_client_devs[
                                 sd->hid_sensor_client_cnt].platform_data =
-                                                sd->hsdev;
+                                                        hsdev;
                         sd->hid_sensor_hub_client_devs[
                                 sd->hid_sensor_client_cnt].pdata_size =
-                                                sizeof(*sd->hsdev);
-                        hid_dbg(hdev, "Adding %s:%p\n", name, sd);
+                                                        sizeof(*hsdev);
+                        hid_dbg(hdev, "Adding %s:%d\n", name,
+                                        hsdev->start_collection_index);
                         sd->hid_sensor_client_cnt++;
                 }
         }
+        if (last_hsdev)
+                last_hsdev->end_collection_index = i;
+
         ret = mfd_add_devices(&hdev->dev, 0, sd->hid_sensor_hub_client_devs,
                 sd->hid_sensor_client_cnt, NULL, 0, NULL);
         if (ret < 0)
-                goto err_free_names;
+                goto err_no_mem;
 
         return ret;
 
-err_free_names:
-        for (i = 0; i < sd->hid_sensor_client_cnt ; ++i)
+err_no_mem:
+        for (i = 0; i < sd->hid_sensor_client_cnt; ++i) {
                 kfree(sd->hid_sensor_hub_client_devs[i].name);
+                kfree(sd->hid_sensor_hub_client_devs[i].platform_data);
+        }
         kfree(sd->hid_sensor_hub_client_devs);
 err_stop_hw:
         hid_hw_stop(hdev);
@@ -650,8 +686,10 @@ static void sensor_hub_remove(struct hid_device *hdev)
                 complete(&data->pending.ready);
         spin_unlock_irqrestore(&data->lock, flags);
         mfd_remove_devices(&hdev->dev);
-        for (i = 0; i < data->hid_sensor_client_cnt ; ++i)
+        for (i = 0; i < data->hid_sensor_client_cnt; ++i) {
                 kfree(data->hid_sensor_hub_client_devs[i].name);
+                kfree(data->hid_sensor_hub_client_devs[i].platform_data);
+        }
         kfree(data->hid_sensor_hub_client_devs);
         hid_set_drvdata(hdev, NULL);
         mutex_destroy(&data->mutex);
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index 7dcf83998e6f..dbefbdaf7cd1 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -38,29 +38,40 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
         if (state) {
                 if (sensor_hub_device_open(st->hsdev))
                         return -EIO;
-                state_val =
-                HID_USAGE_SENSOR_PROP_POWER_STATE_D0_FULL_POWER_ENUM;
-                report_val =
-                HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM;
-
+                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 {
                 sensor_hub_device_close(st->hsdev);
-                state_val =
-                HID_USAGE_SENSOR_PROP_POWER_STATE_D4_POWER_OFF_ENUM;
-                report_val =
-                HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM;
+                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);
         }
-
         st->data_ready = state;
-        state_val += st->power_state.logical_minimum;
-        report_val += st->report_state.logical_minimum;
-        sensor_hub_set_feature(st->hsdev, st->power_state.report_id,
+
+        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)report_val);
+        }
 
         return 0;
 }
diff --git a/include/linux/hid-sensor-hub.h b/include/linux/hid-sensor-hub.h
index b914ca3f57ba..b70cfd7ff29c 100644
--- a/include/linux/hid-sensor-hub.h
+++ b/include/linux/hid-sensor-hub.h
@@ -51,13 +51,15 @@ struct hid_sensor_hub_attribute_info {
  * @hdev:               Stores the hid instance.
  * @vendor_id:          Vendor id of hub device.
  * @product_id:         Product id of hub device.
- * @ref_cnt:            Number of MFD clients have opened this device
+ * @start_collection_index: Starting index for a phy type collection
+ * @end_collection_index: Last index for a phy type collection
  */
 struct hid_sensor_hub_device {
         struct hid_device *hdev;
         u32 vendor_id;
         u32 product_id;
-        int ref_cnt;
+        int start_collection_index;
+        int end_collection_index;
 };
 
 /**
@@ -218,4 +220,7 @@ int hid_sensor_write_samp_freq_value(struct hid_sensor_common *st,
 int hid_sensor_read_samp_freq_value(struct hid_sensor_common *st,
                                         int *val1, int *val2);
 
+int hid_sensor_get_usage_index(struct hid_sensor_hub_device *hsdev,
+                                u32 report_id, int field_index, u32 usage_id);
+
 #endif
diff --git a/include/linux/hid-sensor-ids.h b/include/linux/hid-sensor-ids.h
index beaf965621c1..f8607605a31c 100644
--- a/include/linux/hid-sensor-ids.h
+++ b/include/linux/hid-sensor-ids.h
@@ -130,15 +130,15 @@
 #define HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS                0x1000
 
 /* Power state enumerations */
-#define HID_USAGE_SENSOR_PROP_POWER_STATE_UNDEFINED_ENUM                0x00
-#define HID_USAGE_SENSOR_PROP_POWER_STATE_D0_FULL_POWER_ENUM            0x01
-#define HID_USAGE_SENSOR_PROP_POWER_STATE_D1_LOW_POWER_ENUM             0x02
-#define HID_USAGE_SENSOR_PROP_POWER_STATE_D2_STANDBY_WITH_WAKE_ENUM     0x03
-#define HID_USAGE_SENSOR_PROP_POWER_STATE_D3_SLEEP_WITH_WAKE_ENUM       0x04
-#define HID_USAGE_SENSOR_PROP_POWER_STATE_D4_POWER_OFF_ENUM             0x05
+#define HID_USAGE_SENSOR_PROP_POWER_STATE_UNDEFINED_ENUM        0x200850
+#define HID_USAGE_SENSOR_PROP_POWER_STATE_D0_FULL_POWER_ENUM    0x200851
+#define HID_USAGE_SENSOR_PROP_POWER_STATE_D1_LOW_POWER_ENUM     0x200852
+#define HID_USAGE_SENSOR_PROP_POWER_STATE_D2_STANDBY_WITH_WAKE_ENUM 0x200853
+#define HID_USAGE_SENSOR_PROP_POWER_STATE_D3_SLEEP_WITH_WAKE_ENUM 0x200854
+#define HID_USAGE_SENSOR_PROP_POWER_STATE_D4_POWER_OFF_ENUM     0x200855
 
 /* Report State enumerations */
-#define HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM            0x00
-#define HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM           0x01
+#define HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM    0x200840
+#define HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM   0x200841
 
 #endif