diff options
Diffstat (limited to 'drivers/iio/gyro/st_gyro_core.c')
| -rw-r--r-- | drivers/iio/gyro/st_gyro_core.c | 60 |
1 files changed, 33 insertions, 27 deletions
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c index f9ed3488c31..ed74a906998 100644 --- a/drivers/iio/gyro/st_gyro_core.c +++ b/drivers/iio/gyro/st_gyro_core.c @@ -60,7 +60,7 @@ #define ST_GYRO_1_BDU_ADDR 0x23 #define ST_GYRO_1_BDU_MASK 0x80 #define ST_GYRO_1_DRDY_IRQ_ADDR 0x22 -#define ST_GYRO_1_DRDY_IRQ_MASK 0x08 +#define ST_GYRO_1_DRDY_IRQ_INT2_MASK 0x08 #define ST_GYRO_1_MULTIREAD_BIT true /* CUSTOM VALUES FOR SENSOR 2 */ @@ -84,7 +84,7 @@ #define ST_GYRO_2_BDU_ADDR 0x23 #define ST_GYRO_2_BDU_MASK 0x80 #define ST_GYRO_2_DRDY_IRQ_ADDR 0x22 -#define ST_GYRO_2_DRDY_IRQ_MASK 0x08 +#define ST_GYRO_2_DRDY_IRQ_INT2_MASK 0x08 #define ST_GYRO_2_MULTIREAD_BIT true static const struct iio_chan_spec st_gyro_16bit_channels[] = { @@ -158,7 +158,7 @@ static const struct st_sensors st_gyro_sensors[] = { }, .drdy_irq = { .addr = ST_GYRO_1_DRDY_IRQ_ADDR, - .mask = ST_GYRO_1_DRDY_IRQ_MASK, + .mask_int2 = ST_GYRO_1_DRDY_IRQ_INT2_MASK, }, .multi_read_bit = ST_GYRO_1_MULTIREAD_BIT, .bootime = 2, @@ -167,11 +167,10 @@ static const struct st_sensors st_gyro_sensors[] = { .wai = ST_GYRO_2_WAI_EXP, .sensors_supported = { [0] = L3GD20_GYRO_DEV_NAME, - [1] = L3GD20H_GYRO_DEV_NAME, - [2] = LSM330D_GYRO_DEV_NAME, - [3] = LSM330DLC_GYRO_DEV_NAME, - [4] = L3G4IS_GYRO_DEV_NAME, - [5] = LSM330_GYRO_DEV_NAME, + [1] = LSM330D_GYRO_DEV_NAME, + [2] = LSM330DLC_GYRO_DEV_NAME, + [3] = L3G4IS_GYRO_DEV_NAME, + [4] = LSM330_GYRO_DEV_NAME, }, .ch = (struct iio_chan_spec *)st_gyro_16bit_channels, .odr = { @@ -221,7 +220,7 @@ static const struct st_sensors st_gyro_sensors[] = { }, .drdy_irq = { .addr = ST_GYRO_2_DRDY_IRQ_ADDR, - .mask = ST_GYRO_2_DRDY_IRQ_MASK, + .mask_int2 = ST_GYRO_2_DRDY_IRQ_INT2_MASK, }, .multi_read_bit = ST_GYRO_2_MULTIREAD_BIT, .bootime = 2, @@ -302,18 +301,22 @@ static const struct iio_trigger_ops st_gyro_trigger_ops = { #define ST_GYRO_TRIGGER_OPS NULL #endif -int st_gyro_common_probe(struct iio_dev *indio_dev) +int st_gyro_common_probe(struct iio_dev *indio_dev, + struct st_sensors_platform_data *pdata) { - int err; struct st_sensor_data *gdata = iio_priv(indio_dev); + int irq = gdata->get_irq_data_ready(indio_dev); + int err; indio_dev->modes = INDIO_DIRECT_MODE; indio_dev->info = &gyro_info; + st_sensors_power_enable(indio_dev); + err = st_sensors_check_device_support(indio_dev, ARRAY_SIZE(st_gyro_sensors), st_gyro_sensors); if (err < 0) - goto st_gyro_common_probe_error; + return err; gdata->num_data_channels = ST_GYRO_NUMBER_DATA_CHANNELS; gdata->multiread_bit = gdata->sensor->multi_read_bit; @@ -324,15 +327,15 @@ int st_gyro_common_probe(struct iio_dev *indio_dev) &gdata->sensor->fs.fs_avl[0]; gdata->odr = gdata->sensor->odr.odr_avl[0].hz; - err = st_sensors_init_sensor(indio_dev); + err = st_sensors_init_sensor(indio_dev, pdata); if (err < 0) - goto st_gyro_common_probe_error; + return err; - if (gdata->get_irq_data_ready(indio_dev) > 0) { - err = st_gyro_allocate_ring(indio_dev); - if (err < 0) - goto st_gyro_common_probe_error; + err = st_gyro_allocate_ring(indio_dev); + if (err < 0) + return err; + if (irq > 0) { err = st_sensors_allocate_trigger(indio_dev, ST_GYRO_TRIGGER_OPS); if (err < 0) @@ -343,15 +346,17 @@ int st_gyro_common_probe(struct iio_dev *indio_dev) if (err) goto st_gyro_device_register_error; - return err; + dev_info(&indio_dev->dev, "registered gyroscope %s\n", + indio_dev->name); + + return 0; st_gyro_device_register_error: - if (gdata->get_irq_data_ready(indio_dev) > 0) + if (irq > 0) st_sensors_deallocate_trigger(indio_dev); st_gyro_probe_trigger_error: - if (gdata->get_irq_data_ready(indio_dev) > 0) - st_gyro_deallocate_ring(indio_dev); -st_gyro_common_probe_error: + st_gyro_deallocate_ring(indio_dev); + return err; } EXPORT_SYMBOL(st_gyro_common_probe); @@ -360,12 +365,13 @@ void st_gyro_common_remove(struct iio_dev *indio_dev) { struct st_sensor_data *gdata = iio_priv(indio_dev); + st_sensors_power_disable(indio_dev); + iio_device_unregister(indio_dev); - if (gdata->get_irq_data_ready(indio_dev) > 0) { + if (gdata->get_irq_data_ready(indio_dev) > 0) st_sensors_deallocate_trigger(indio_dev); - st_gyro_deallocate_ring(indio_dev); - } - iio_device_free(indio_dev); + + st_gyro_deallocate_ring(indio_dev); } EXPORT_SYMBOL(st_gyro_common_remove); |