Merge "input: sensors: add enable and poll_delay handler for mpu3050 sensor driver" (7c6e6a2b) · Commits · e / devices / android_kernel_sony_msm8994

drivers/input/misc/mpu3050.c

+90 −54

Original line number	Diff line number	Diff line
		@@ -122,6 +122,7 @@ struct mpu3050_sensor {
		struct input_dev *idev;
		struct mpu3050_gyro_platform_data *platform_data;
		struct delayed_work input_work;
		struct sensors_classdev cdev;
		u32 use_poll;
		u32 poll_interval;
		u32 dlpf_index;
		@@ -141,6 +142,10 @@ static struct sensors_classdev sensors_cdev = {
		.min_delay = 2000,
		.fifo_reserved_event_count = 0,
		.fifo_max_event_count = 0,
		.enabled = 0,
		.delay_msec = MPU3050_DEFAULT_POLL_INTERVAL,
		.sensors_enable = NULL,
		.sensors_poll_delay = NULL,
		};

		struct sensor_regulator {
		@@ -258,6 +263,40 @@ error_vdd:
		return rc;
		}

		static int mpu3050_poll_delay_set(struct sensors_classdev *sensors_cdev,
		unsigned int delay_msec)
		{
		struct mpu3050_sensor *sensor = container_of(sensors_cdev,
		struct mpu3050_sensor, cdev);
		unsigned int dlpf_index;
		u8 divider, reg;
		int ret;

		dlpf_index = interval_to_dlpf_cfg(delay_msec);
		divider = delay_msec * dlpf_table[dlpf_index].sample_rate - 1;

		if (sensor->dlpf_index != dlpf_index) {
		/* Set low pass filter and full scale */
		reg = dlpf_table[dlpf_index].cfg;
		reg \|= MPU3050_DEFAULT_FS_RANGE << 3;
		reg \|= MPU3050_EXT_SYNC_NONE << 5;
		ret = i2c_smbus_write_byte_data(sensor->client,
		MPU3050_DLPF_FS_SYNC, reg);
		if (!ret)
		sensor->dlpf_index = dlpf_index;
		}

		if (sensor->poll_interval != delay_msec) {
		/* Output frequency divider. The poll interval */
		ret = i2c_smbus_write_byte_data(sensor->client,
		MPU3050_SMPLRT_DIV, divider);
		if (!ret)
		sensor->poll_interval = delay_msec;
		}

		return 0;
		}

		/**
		* mpu3050_attr_get_polling_rate - get the sampling rate
		*/
		@@ -280,8 +319,6 @@ static ssize_t mpu3050_attr_set_polling_rate(struct device *dev,
		{
		struct mpu3050_sensor *sensor = dev_get_drvdata(dev);
		unsigned long interval_ms;
		unsigned int dlpf_index;
		u8 divider, reg;
		int ret;

		if (kstrtoul(buf, 10, &interval_ms))
		@@ -290,29 +327,39 @@ static ssize_t mpu3050_attr_set_polling_rate(struct device *dev,
		(interval_ms > MPU3050_MAX_POLL_INTERVAL))
		return -EINVAL;

		dlpf_index = interval_to_dlpf_cfg(interval_ms);
		divider = interval_ms * dlpf_table[dlpf_index].sample_rate - 1;
		ret = mpu3050_poll_delay_set(&sensor->cdev, interval_ms);

		if (sensor->dlpf_index != dlpf_index) {
		/* Set low pass filter and full scale */
		reg = dlpf_table[dlpf_index].cfg;
		reg \|= MPU3050_DEFAULT_FS_RANGE << 3;
		reg \|= MPU3050_EXT_SYNC_NONE << 5;
		ret = i2c_smbus_write_byte_data(sensor->client,
		MPU3050_DLPF_FS_SYNC, reg);
		if (ret == 0)
		sensor->dlpf_index = dlpf_index;
		return ret < 0 ? ret : size;
		}
		static int mpu3050_enable_set(struct sensors_classdev *sensors_cdev,
		unsigned int enabled)
		{
		struct mpu3050_sensor *sensor = container_of(sensors_cdev,
		struct mpu3050_sensor, cdev);

		if (sensor->poll_interval != interval_ms) {
		/* Output frequency divider. The poll interval */
		ret = i2c_smbus_write_byte_data(sensor->client,
		MPU3050_SMPLRT_DIV, divider);
		if (ret == 0)
		sensor->poll_interval = interval_ms;

		if (enabled && (!sensor->enable)) {
		sensor->enable = enabled;
		pm_runtime_get_sync(sensor->dev);
		if (sensor->use_poll)
		schedule_delayed_work(&sensor->input_work,
		msecs_to_jiffies(sensor->poll_interval));
		else
		enable_irq(sensor->client->irq);
		} else if (!enabled && sensor->enable) {
		if (sensor->use_poll)
		cancel_delayed_work_sync(&sensor->input_work);
		else
		disable_irq(sensor->client->irq);
		pm_runtime_put_sync(sensor->dev);
		sensor->enable = enabled;
		} else {
		dev_warn(&sensor->client->dev,
		"ignore enable state change from %d to %d\n",
		sensor->enable, enabled);
		}

		return size;
		return 0;
		}

		/**
		@@ -325,31 +372,13 @@ static ssize_t mpu3050_attr_set_enable(struct device *dev,
		{
		struct mpu3050_sensor *sensor = dev_get_drvdata(dev);
		unsigned long val;
		u32 enable;
		int err;

		if (kstrtoul(buf, 10, &val))
		return -EINVAL;
		enable = (u32)val == 0 ? 0 : 1;
		if (enable && (!sensor->enable)) {
		sensor->enable = enable;
		pm_runtime_get_sync(sensor->dev);
		if (sensor->use_poll)
		schedule_delayed_work(&sensor->input_work,
		msecs_to_jiffies(sensor->poll_interval));
		else
		enable_irq(sensor->client->irq);
		} else if (!enable && sensor->enable) {
		if (sensor->use_poll)
		cancel_delayed_work_sync(&sensor->input_work);
		else
		disable_irq(sensor->client->irq);
		pm_runtime_put_sync(sensor->dev);
		sensor->enable = enable;
		} else {
		dev_warn(&sensor->client->dev,
		"ignore enable state change from %d to %d\n",
		sensor->enable, enable);
		}
		err = mpu3050_enable_set(&sensor->cdev, val);
		if (err < 0)
		return err;

		return count;
		}
		@@ -679,6 +708,19 @@ static int mpu3050_probe(struct i2c_client *client,
		sensor->enable_gpio = -EINVAL;
		}

		sensor->cdev = sensors_cdev;
		sensor->cdev.min_delay = MPU3050_MIN_POLL_INTERVAL;
		sensor->cdev.delay_msec = sensor->poll_interval;
		sensor->cdev.sensors_enable = mpu3050_enable_set;
		sensor->cdev.sensors_poll_delay = mpu3050_poll_delay_set;
		ret = sensors_classdev_register(&client->dev, &sensor->cdev);

		if (ret) {
		dev_err(&client->dev, "class device create failed: %d\n", ret);
		error = -EINVAL;
		goto err_free_mem;
		}

		if (gpio_is_valid(sensor->enable_gpio)) {
		ret = gpio_request(sensor->enable_gpio, "GYRO_EN_PM");
		gpio_direction_output(sensor->enable_gpio, 1);
		@@ -690,7 +732,7 @@ static int mpu3050_probe(struct i2c_client *client,
		if (ret < 0) {
		dev_err(&client->dev, "failed to detect device\n");
		error = -ENXIO;
		goto err_free_mem;
		goto err_class_sysfs;
		}

		for (i = 0; i < ARRAY_SIZE(mpu3050_chip_ids); i++)
		@@ -700,7 +742,7 @@ static int mpu3050_probe(struct i2c_client *client,
		if (i == ARRAY_SIZE(mpu3050_chip_ids)) {
		dev_err(&client->dev, "unsupported chip id\n");
		error = -ENXIO;
		goto err_free_mem;
		goto err_class_sysfs;
		}

		idev->name = "MPU3050";
		@@ -775,16 +817,10 @@ static int mpu3050_probe(struct i2c_client *client,
		goto err_free_irq;
		}

		error = sensors_classdev_register(&client->dev, &sensors_cdev);
		if (error < 0) {
		dev_err(&client->dev, "failed to create class device\n");
		goto err_input_cleanup;
		}

		error = create_sysfs_interfaces(&idev->dev);
		if (error < 0) {
		dev_err(&client->dev, "failed to create sysfs\n");
		goto err_class_sysfs;
		goto err_input_cleanup;
		}

		pm_runtime_enable(&client->dev);
		@@ -792,8 +828,6 @@ static int mpu3050_probe(struct i2c_client *client,

		return 0;

		err_class_sysfs:
		sensors_classdev_unregister(&sensors_cdev);
		err_input_cleanup:
		input_unregister_device(idev);
		err_free_irq:
		@@ -805,6 +839,8 @@ err_free_gpio:
		gpio_free(sensor->platform_data->gpio_int);
		err_pm_set_suspended:
		pm_runtime_set_suspended(&client->dev);
		err_class_sysfs:
		sensors_classdev_unregister(&sensor->cdev);
		err_free_mem:
		input_free_device(idev);
		kfree(sensor);