platform/chrome: cros_ec_sensorhub: Allocate sensorhub resource before claiming sensors

Allocate callbacks array before enumerating the sensors: The probe routine
for these sensors (for instance cros_ec_sensors_probe) can be called
within the sensorhub probe routine (cros_ec_sensors_probe())

Fixes: 145d59baff594 ("platform/chrome: cros_ec_sensorhub: Add FIFO support")
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reported-by: Douglas Anderson <dianders@chromium.org>
Tested-by: Douglas Anderson <dianders@chromium.org>
Signed-off-by: Enric Balletbo i Serra <enric.balletbo@collabora.com>

diff --git a/drivers/platform/chrome/cros_ec_sensorhub.c b/drivers/platform/chrome/cros_ec_sensorhub.c
index b7f2c00db5e1e423f7ed4d3d7927da0685c29f18..9c4af76a9956ed567752f6920ec8db3671b2ab35 100644 (file)
--- a/drivers/platform/chrome/cros_ec_sensorhub.c
+++ b/drivers/platform/chrome/cros_ec_sensorhub.c
@@ -52,28 +52,15 @@ static int cros_ec_sensorhub_register(struct device *dev,
        int sensor_type[MOTIONSENSE_TYPE_MAX] = { 0 };
        struct cros_ec_command *msg = sensorhub->msg;
        struct cros_ec_dev *ec = sensorhub->ec;
-       int ret, i, sensor_num;
+       int ret, i;
        char *name;
 
-       sensor_num = cros_ec_get_sensor_count(ec);
-       if (sensor_num < 0) {
-               dev_err(dev,
-                       "Unable to retrieve sensor information (err:%d)\n",
-                       sensor_num);
-               return sensor_num;
-       }
-
-       sensorhub->sensor_num = sensor_num;
-       if (sensor_num == 0) {
-               dev_err(dev, "Zero sensors reported.\n");
-               return -EINVAL;
-       }
 
        msg->version = 1;
        msg->insize = sizeof(struct ec_response_motion_sense);
        msg->outsize = sizeof(struct ec_params_motion_sense);
 
-       for (i = 0; i < sensor_num; i++) {
+       for (i = 0; i < sensorhub->sensor_num; i++) {
                sensorhub->params->cmd = MOTIONSENSE_CMD_INFO;
                sensorhub->params->info.sensor_num = i;
 
@@ -140,8 +127,7 @@ static int cros_ec_sensorhub_probe(struct platform_device *pdev)
        struct cros_ec_dev *ec = dev_get_drvdata(dev->parent);
        struct cros_ec_sensorhub *data;
        struct cros_ec_command *msg;
-       int ret;
-       int i;
+       int ret, i, sensor_num;
 
        msg = devm_kzalloc(dev, sizeof(struct cros_ec_command) +
                           max((u16)sizeof(struct ec_params_motion_sense),
@@ -166,10 +152,52 @@ static int cros_ec_sensorhub_probe(struct platform_device *pdev)
        dev_set_drvdata(dev, data);
 
        /* Check whether this EC is a sensor hub. */
-       if (cros_ec_check_features(data->ec, EC_FEATURE_MOTION_SENSE)) {
+       if (cros_ec_check_features(ec, EC_FEATURE_MOTION_SENSE)) {
+               sensor_num = cros_ec_get_sensor_count(ec);
+               if (sensor_num < 0) {
+                       dev_err(dev,
+                               "Unable to retrieve sensor information (err:%d)\n",
+                               sensor_num);
+                       return sensor_num;
+               }
+               if (sensor_num == 0) {
+                       dev_err(dev, "Zero sensors reported.\n");
+                       return -EINVAL;
+               }
+               data->sensor_num = sensor_num;
+
+               /*
+                * Prepare the ring handler before enumering the
+                * sensors.
+                */
+               if (cros_ec_check_features(ec, EC_FEATURE_MOTION_SENSE_FIFO)) {
+                       ret = cros_ec_sensorhub_ring_allocate(data);
+                       if (ret)
+                               return ret;
+               }
+
+               /* Enumerate the sensors.*/
                ret = cros_ec_sensorhub_register(dev, data);
                if (ret)
                        return ret;
+
+               /*
+                * When the EC does not have a FIFO, the sensors will query
+                * their data themselves via sysfs or a software trigger.
+                */
+               if (cros_ec_check_features(ec, EC_FEATURE_MOTION_SENSE_FIFO)) {
+                       ret = cros_ec_sensorhub_ring_add(data);
+                       if (ret)
+                               return ret;
+                       /*
+                        * The msg and its data is not under the control of the
+                        * ring handler.
+                        */
+                       return devm_add_action_or_reset(dev,
+                                       cros_ec_sensorhub_ring_remove,
+                                       data);
+               }
+
        } else {
                /*
                 * If the device has sensors but does not claim to
@@ -184,22 +212,6 @@ static int cros_ec_sensorhub_probe(struct platform_device *pdev)
                }
        }
 
-       /*
-        * If the EC does not have a FIFO, the sensors will query their data
-        * themselves via sysfs or a software trigger.
-        */
-       if (cros_ec_check_features(ec, EC_FEATURE_MOTION_SENSE_FIFO)) {
-               ret = cros_ec_sensorhub_ring_add(data);
-               if (ret)
-                       return ret;
-               /*
-                * The msg and its data is not under the control of the ring
-                * handler.
-                */
-               return devm_add_action_or_reset(dev,
-                                               cros_ec_sensorhub_ring_remove,
-                                               data);
-       }
 
        return 0;
 }

diff --git a/drivers/platform/chrome/cros_ec_sensorhub_ring.c b/drivers/platform/chrome/cros_ec_sensorhub_ring.c
index c48e5b38a4417d27daee2efd6c42a71fd17b80f9..24e48d96ed7663f3f8fda74b7ace2b71c93181f1 100644 (file)
--- a/drivers/platform/chrome/cros_ec_sensorhub_ring.c
+++ b/drivers/platform/chrome/cros_ec_sensorhub_ring.c
@@ -957,17 +957,15 @@ static int cros_ec_sensorhub_event(struct notifier_block *nb,
 }
 
 /**
- * cros_ec_sensorhub_ring_add() - Add the FIFO functionality if the EC
- *                               supports it.
+ * cros_ec_sensorhub_ring_allocate() - Prepare the FIFO functionality if the EC
+ *                                    supports it.
  *
  * @sensorhub : Sensor Hub object.
  *
  * Return: 0 on success.
  */
-int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
+int cros_ec_sensorhub_ring_allocate(struct cros_ec_sensorhub *sensorhub)
 {
-       struct cros_ec_dev *ec = sensorhub->ec;
-       int ret;
        int fifo_info_length =
                sizeof(struct ec_response_motion_sense_fifo_info) +
                sizeof(u16) * sensorhub->sensor_num;
@@ -978,6 +976,49 @@ int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
        if (!sensorhub->fifo_info)
                return -ENOMEM;
 
+       /*
+        * Allocate the callback area based on the number of sensors.
+        * Add one for the sensor ring.
+        */
+       sensorhub->push_data = devm_kcalloc(sensorhub->dev,
+                       sensorhub->sensor_num,
+                       sizeof(*sensorhub->push_data),
+                       GFP_KERNEL);
+       if (!sensorhub->push_data)
+               return -ENOMEM;
+
+       sensorhub->tight_timestamps = cros_ec_check_features(
+                       sensorhub->ec,
+                       EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS);
+
+       if (sensorhub->tight_timestamps) {
+               sensorhub->batch_state = devm_kcalloc(sensorhub->dev,
+                               sensorhub->sensor_num,
+                               sizeof(*sensorhub->batch_state),
+                               GFP_KERNEL);
+               if (!sensorhub->batch_state)
+                       return -ENOMEM;
+       }
+
+       return 0;
+}
+
+/**
+ * cros_ec_sensorhub_ring_add() - Add the FIFO functionality if the EC
+ *                               supports it.
+ *
+ * @sensorhub : Sensor Hub object.
+ *
+ * Return: 0 on success.
+ */
+int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
+{
+       struct cros_ec_dev *ec = sensorhub->ec;
+       int ret;
+       int fifo_info_length =
+               sizeof(struct ec_response_motion_sense_fifo_info) +
+               sizeof(u16) * sensorhub->sensor_num;
+
        /* Retrieve FIFO information */
        sensorhub->msg->version = 2;
        sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
@@ -998,31 +1039,9 @@ int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
        if (!sensorhub->ring)
                return -ENOMEM;
 
-       /*
-        * Allocate the callback area based on the number of sensors.
-        */
-       sensorhub->push_data = devm_kcalloc(
-                       sensorhub->dev, sensorhub->sensor_num,
-                       sizeof(*sensorhub->push_data),
-                       GFP_KERNEL);
-       if (!sensorhub->push_data)
-               return -ENOMEM;
-
        sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] =
                cros_ec_get_time_ns();
 
-       sensorhub->tight_timestamps = cros_ec_check_features(
-                       ec, EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS);
-
-       if (sensorhub->tight_timestamps) {
-               sensorhub->batch_state = devm_kcalloc(sensorhub->dev,
-                               sensorhub->sensor_num,
-                               sizeof(*sensorhub->batch_state),
-                               GFP_KERNEL);
-               if (!sensorhub->batch_state)
-                       return -ENOMEM;
-       }
-
        /* Register the notifier that will act as a top half interrupt. */
        sensorhub->notifier.notifier_call = cros_ec_sensorhub_event;
        ret = blocking_notifier_chain_register(&ec->ec_dev->event_notifier,

diff --git a/include/linux/platform_data/cros_ec_sensorhub.h b/include/linux/platform_data/cros_ec_sensorhub.h
index c588be843f61ba2c37a763253a7b4bba5077b729..0ecce6aa69d5e3a1ade4ef14b12b1141b5e4be69 100644 (file)
--- a/include/linux/platform_data/cros_ec_sensorhub.h
+++ b/include/linux/platform_data/cros_ec_sensorhub.h
@@ -185,6 +185,7 @@ int cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub *sensorhub,
 void cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub *sensorhub,
                                            u8 sensor_num);
 
+int cros_ec_sensorhub_ring_allocate(struct cros_ec_sensorhub *sensorhub);
 int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub);
 void cros_ec_sensorhub_ring_remove(void *arg);
 int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub,