diff options
| -rw-r--r-- | drivers/platform/chrome/cros_ec_sensorhub.c | 80 | ||||
| -rw-r--r-- | drivers/platform/chrome/cros_ec_sensorhub_ring.c | 73 | ||||
| -rw-r--r-- | include/linux/platform_data/cros_ec_sensorhub.h | 1 |
3 files changed, 93 insertions, 61 deletions
diff --git a/drivers/platform/chrome/cros_ec_sensorhub.c b/drivers/platform/chrome/cros_ec_sensorhub.c index b7f2c00db5e1..9c4af76a9956 100644 --- 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 c48e5b38a441..24e48d96ed76 100644 --- 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 c588be843f61..0ecce6aa69d5 100644 --- 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, |