summaryrefslogtreecommitdiff
path: root/drivers/staging/iio/gyro/l3g4200d_core.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/staging/iio/gyro/l3g4200d_core.c')
-rw-r--r--drivers/staging/iio/gyro/l3g4200d_core.c931
1 files changed, 931 insertions, 0 deletions
diff --git a/drivers/staging/iio/gyro/l3g4200d_core.c b/drivers/staging/iio/gyro/l3g4200d_core.c
new file mode 100644
index 00000000000..8455a7994e8
--- /dev/null
+++ b/drivers/staging/iio/gyro/l3g4200d_core.c
@@ -0,0 +1,931 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2012
+ * License Terms: GNU General Public License, version 2
+ *
+ * Mostly this gyroscope device is a copy of magnetometer
+ * driver lsm303dlh or viceversa, so the code is mostly based
+ * on lsm303dlh driver.
+ *
+ * Author: Naga Radhesh Y <naga.radheshy@stericsson.com>
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+#include <linux/l3g4200d.h>
+
+#include "../iio.h"
+#include "../sysfs.h"
+
+/* Idenitification register */
+#define L3G4200D_WHO_AM_I 0x0F
+/* control register1 */
+#define L3G4200D_CTRL_REG1 0x20
+/* control register2 */
+#define L3G4200D_CTRL_REG2 0x21
+/* control register3 */
+#define L3G4200D_CTRL_REG3 0x22
+/* control register4 */
+#define L3G4200D_CTRL_REG4 0x23
+/* control register5 */
+#define L3G4200D_CTRL_REG5 0x24
+/* out temperature */
+#define L3G4200D_OUT_TEMP 0x26
+/* data output X register */
+#define L3G4200D_OUT_X_L_A 0x28
+/* data output Y register */
+#define L3G4200D_OUT_Y_L_A 0x2A
+/* data output Z register */
+#define L3G4200D_OUT_Z_L_A 0x2C
+/* status register */
+#define L3G4200D_STATUS_REG_A 0x27
+
+/* control register 1, Mode selection */
+#define L3G4200D_CR1_PM_BIT 3
+#define L3G4200D_CR1_PM_MASK (0x01 << L3G4200D_CR1_PM_BIT)
+/* control register 1, Data Rate */
+#define L3G4200D_CR1_DR_BIT 4
+#define L3G4200D_CR1_DR_MASK (0x0F << L3G4200D_CR1_DR_BIT)
+/* control register 1, x,y,z enable bits */
+#define L3G4200D_CR1_EN_BIT 0
+#define L3G4200D_CR1_EN_MASK (0x7 << L3G4200D_CR1_EN_BIT)
+#define L3G4200D_CR1_AXIS_ENABLE 7
+
+/* control register 4, self test */
+#define L3G4200D_CR4_ST_BIT 1
+#define L3G4200D_CR4_ST_MASK (0x03 << L3G4200D_CR4_ST_BIT)
+/* control register 4, full scale */
+#define L3G4200D_CR4_FS_BIT 4
+#define L3G4200D_CR4_FS_MASK (0x3 << L3G4200D_CR4_FS_BIT)
+/* control register 4, endianness */
+#define L3G4200D_CR4_BLE_BIT 6
+#define L3G4200D_CR4_BLE_MASK (0x1 << L3G4200D_CR4_BLE_BIT)
+/* control register 4, Block data update */
+#define L3G4200D_CR4_BDU_BIT 7
+#define L3G4200D_CR4_BDU_MASK (0x1 << L3G4200D_CR4_BDU_BIT)
+
+/* Gyroscope operating mode */
+#define L3G4200D_MODE_OFF 0x00
+#define L3G4200D_MODE_NORMAL 0x01
+
+/* Expected content for WAI register */
+#define WHOAMI_L3G4200D 0x00D3
+/* Expected content for WAI register for L3GD20*/
+#define WHOAMI_L3GD20 0x00D4
+
+/*
+ * CTRL_REG1 register rate settings
+ *
+ * DR1 DR0 BW1 BW0 Output data rate[Hz]
+ * 0 0 0 0 100
+ * 0 0 0 1 100
+ * 0 0 1 0 100
+ * 0 0 1 1 100
+ * 0 1 0 0 200
+ * 0 1 0 1 200
+ * 0 1 1 0 200
+ * 0 1 1 1 200
+ * 1 0 0 0 400
+ * 1 0 0 1 400
+ * 1 0 1 0 400
+ * 1 0 1 1 400
+ * 1 1 0 0 800
+ * 1 1 0 1 800
+ * 1 1 1 0 800
+ * 1 1 1 1 800
+ */
+#define L3G4200D_ODR_MIN_VAL 0x00
+#define L3G4200D_ODR_MAX_VAL 0x0F
+#define L3G4200D_RATE_100 0x00
+#define L3G4200D_RATE_200 0x04
+#define L3G4200D_RATE_400 0x08
+#define L3G4200D_RATE_800 0x0C
+
+/*
+ * CTRL_REG4 register range settings
+ *
+ * FS1 FS0 FUll scale range
+ * 0 0 250
+ * 0 1 500
+ * 1 0 2000
+ * 1 1 2000
+ */
+#define L3G4200D_RANGE_250 0x00
+#define L3G4200D_RANGE_500 0x01
+#define L3G4200D_RANGE_2000 0x03
+
+/* device status defines */
+#define L3G4200D_DEVICE_OFF 0
+#define L3G4200D_DEVICE_ON 1
+#define L3G4200D_DEVICE_SUSPENDED 2
+
+/* status register */
+#define L3G4200D_SR_REG_A 0x27
+/* status register, ready */
+#define L3G4200D_XYZ_DATA_RDY 0x80
+#define L3G4200D_XYZ_DATA_RDY_BIT 3
+#define L3G4200D_XYZ_DATA_RDY_MASK (0x1 << L3G4200D_XYZ_DATA_RDY_BIT)
+
+/* Multiple byte transfer enable */
+#define MULTIPLE_I2C_TR 0x80
+
+/*
+ * struct l3g4200d_data - data structure used by l3g4200d driver
+ * @client: i2c client
+ * @lock: mutex lock for sysfs operations
+ * @regulator: regulator
+ * @early_suspend: early suspend structure
+ * @pdata: l3g4200d platform data
+ * @mode: current mode of operation
+ * @rate: current sampling rate
+ * @range: current range value of Gyroscope
+ * @device_status: device is ON, OFF or SUSPENDED
+ */
+
+struct l3g4200d_data {
+ struct i2c_client *client;
+ struct mutex lock;
+ struct regulator *regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+ struct early_suspend early_suspend;
+#endif
+ struct l3g4200d_gyr_platform_data *pdata;
+
+ u8 mode;
+ u8 rate;
+ u8 range;
+ int device_status;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void l3g4200d_early_suspend(struct early_suspend *data);
+static void l3g4200d_late_resume(struct early_suspend *data);
+#endif
+static inline int is_device_on(struct l3g4200d_data *data)
+{
+ struct i2c_client *client = data->client;
+ /* Perform read/write operation only when device is active */
+ if (data->device_status != L3G4200D_DEVICE_ON) {
+ dev_err(&client->dev,
+ "device is switched off, make it on using mode");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+/* To disable regulator and status */
+static int l3g4200d_disable(struct l3g4200d_data *data)
+{
+ data->device_status = L3G4200D_DEVICE_OFF;
+
+ regulator_disable(data->regulator);
+
+ return 0;
+}
+
+/* To enable regulator and status */
+static int l3g4200d_enable(struct l3g4200d_data *data)
+{
+ data->device_status = L3G4200D_DEVICE_ON;
+
+ regulator_enable(data->regulator);
+
+ return 0;
+}
+
+static s32 l3g4200d_set_mode(struct i2c_client *client, u8 mode)
+{
+ int reg_val;
+
+ if (mode > L3G4200D_MODE_NORMAL) {
+ dev_err(&client->dev, "given mode not supported\n");
+ return -EINVAL;
+ }
+
+ reg_val = i2c_smbus_read_byte_data(client, L3G4200D_CTRL_REG1);
+
+ reg_val |= L3G4200D_CR1_AXIS_ENABLE;
+ reg_val &= ~L3G4200D_CR1_PM_MASK;
+
+ reg_val |= ((mode << L3G4200D_CR1_PM_BIT) & L3G4200D_CR1_PM_MASK);
+
+ /* the 4th bit indicates the gyroscope sensor mode */
+ return i2c_smbus_write_byte_data(client, L3G4200D_CTRL_REG1, reg_val);
+}
+
+static s32 l3g4200d_set_rate(struct i2c_client *client, u8 rate)
+{
+ int reg_val;
+
+ if (rate > L3G4200D_ODR_MAX_VAL) {
+ dev_err(&client->dev, "given rate not supported\n");
+ return -EINVAL;
+ }
+ reg_val = i2c_smbus_read_byte_data(client, L3G4200D_CTRL_REG1);
+
+ reg_val &= ~L3G4200D_CR1_DR_MASK;
+
+ reg_val |= ((rate << L3G4200D_CR1_DR_BIT) & L3G4200D_CR1_DR_MASK);
+
+ /* upper 4 bits indicate ODR of Gyroscope */
+ return i2c_smbus_write_byte_data(client, L3G4200D_CTRL_REG1, reg_val);
+}
+
+static s32 l3g4200d_set_range(struct i2c_client *client, u8 range)
+{
+ int reg_val;
+
+ if (range > L3G4200D_RANGE_2000) {
+ dev_err(&client->dev, "given range not supported\n");
+ return -EINVAL;
+ }
+
+ reg_val = (range << L3G4200D_CR4_FS_BIT);
+ /*
+ * If BDU is enabled, output registers are not updated until MSB
+ * and LSB reading completes.Otherwise we will end up reading
+ * wrong data.
+ */
+ reg_val |= L3G4200D_CR4_BDU_MASK;
+
+ /* 5th and 6th bits indicate rate of gyroscope */
+ return i2c_smbus_write_byte_data(client, L3G4200D_CTRL_REG4, reg_val);
+}
+
+/*
+ * To read output x/y/z data register,
+ * in this case x,y and z are not
+ * mapped w.r.t board orientation.
+ * Reading just raw data from device
+ */
+static ssize_t l3g4200d_xyz_read(struct iio_dev *indio_dev,
+ int address,
+ int *buf)
+{
+
+ struct l3g4200d_data *data = iio_priv(indio_dev);
+ int lsb , msb;
+ int ret;
+ s16 val;
+
+ /* Perform read/write operation, only when device is active */
+ ret = is_device_on(data);
+ if (ret)
+ return -EINVAL;
+ mutex_lock(&data->lock);
+
+ ret = i2c_smbus_read_byte_data(data->client, L3G4200D_SR_REG_A);
+
+ /* wait till data is written to all six registers */
+ while (!(ret & L3G4200D_XYZ_DATA_RDY_MASK))
+ ret = i2c_smbus_read_byte_data(data->client, L3G4200D_SR_REG_A);
+
+ lsb = i2c_smbus_read_byte_data(data->client, address);
+ if (lsb < 0) {
+ dev_err(&data->client->dev, "reading xyz failed\n");
+ mutex_unlock(&data->lock);
+ return -EINVAL;
+ }
+ msb = i2c_smbus_read_byte_data(data->client, (address + 1));
+ if (msb < 0) {
+ dev_err(&data->client->dev, "reading xyz failed\n");
+ mutex_unlock(&data->lock);
+ return -EINVAL;
+ }
+ val = ((msb << 8) | lsb);
+
+ *buf = (s16)val;
+ mutex_unlock(&data->lock);
+
+ return IIO_VAL_INT;
+}
+
+/*
+ * To read output x,y,z data register.
+ * After reading change x,y and z values
+ * w.r.t the orientation of the device.
+ */
+static ssize_t l3g4200d_readdata(struct device *dev,
+ struct device_attribute *attr,
+ char *buf)
+{
+
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+ struct l3g4200d_gyr_platform_data *pdata = data->pdata;
+ struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+ u8 map_x = pdata->axis_map_x;
+ u8 map_y = pdata->axis_map_y;
+ u8 map_z = pdata->axis_map_z;
+ int ret;
+ unsigned char gyr_data[6];
+ s16 val[3];
+
+ /* Perform read/write operation, only when device is active */
+ ret = is_device_on(data);
+ if (ret)
+ return -EINVAL;
+
+ mutex_lock(&data->lock);
+
+ ret = i2c_smbus_read_byte_data(data->client, L3G4200D_SR_REG_A);
+ /* wait till data is written to all six registers */
+ while (!((ret & L3G4200D_XYZ_DATA_RDY_MASK)))
+ ret = i2c_smbus_read_byte_data(data->client, L3G4200D_SR_REG_A);
+
+ ret = i2c_smbus_read_i2c_block_data(data->client,
+ this_attr->address | MULTIPLE_I2C_TR, 6, gyr_data);
+
+ if (ret < 0) {
+ dev_err(&data->client->dev, "reading xyz failed\n");
+ mutex_unlock(&data->lock);
+ return -EINVAL;
+ }
+
+ /* MSB is at lower address */
+ val[0] = (s16)
+ (((gyr_data[1]) << 8) | gyr_data[0]);
+ val[1] = (s16)
+ (((gyr_data[3]) << 8) | gyr_data[2]);
+ val[2] = (s16)
+ (((gyr_data[5]) << 8) | gyr_data[4]);
+
+ /* modify the x,y and z values w.r.t orientation of device*/
+ if (pdata->negative_x)
+ val[map_x] = -val[map_x];
+ if (pdata->negative_y)
+ val[map_y] = -val[map_y];
+ if (pdata->negative_z)
+ val[map_z] = -val[map_z];
+
+ mutex_unlock(&data->lock);
+
+ return sprintf(buf, "%d:%d:%d:%lld\n", val[map_x], val[map_y],
+ val[map_z], iio_get_time_ns());
+}
+
+static ssize_t get_gyrotemp(struct iio_dev *indio_dev,
+ int address,
+ int *buf)
+{
+ struct l3g4200d_data *data = iio_priv(indio_dev);
+ int ret;
+ /* Perform read/write operation, only when device is active */
+ ret = is_device_on(data);
+ if (ret)
+ return -EINVAL;
+
+ ret = i2c_smbus_read_byte_data(data->client, address);
+ if (ret < 0) {
+ dev_err(&data->client->dev, "Error in reading Gyro temperature");
+ return ret;
+ }
+ *buf = ret;
+
+ return IIO_VAL_INT;
+}
+
+static ssize_t show_operating_mode(struct device *dev,
+ struct device_attribute *attr,
+ char *buf)
+{
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+
+ return sprintf(buf, "%d\n", data->mode);
+}
+
+static ssize_t set_operating_mode(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t count)
+{
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+ int error;
+ unsigned long mode = 0;
+
+ mutex_lock(&data->lock);
+
+ error = kstrtoul(buf, 10, &mode);
+ if (error) {
+ count = error;
+ goto exit;
+ }
+
+ /* check if the received power mode is either 0 or 1 */
+ if (mode < L3G4200D_MODE_OFF || mode > L3G4200D_MODE_NORMAL) {
+ dev_err(&data->client->dev, "trying to set invalid mode\n");
+ count = -EINVAL;
+ goto exit;
+ }
+ /*
+ * If device is drived to sleep mode in suspend, update mode
+ * and return
+ */
+ if (data->device_status == L3G4200D_DEVICE_SUSPENDED &&
+ mode == L3G4200D_MODE_OFF) {
+ data->mode = mode;
+ goto exit;
+ }
+
+ /* if same mode as existing, return */
+ if (data->mode == mode)
+ goto exit;
+
+ /* Enable the regulator if it is not turned ON earlier */
+ if (data->device_status == L3G4200D_DEVICE_OFF ||
+ data->device_status == L3G4200D_DEVICE_SUSPENDED)
+ l3g4200d_enable(data);
+
+ dev_dbg(dev, "set operating mode to %lu\n", mode);
+ error = l3g4200d_set_mode(data->client, mode);
+ if (error < 0) {
+ dev_err(&data->client->dev, "Error in setting the mode\n");
+ count = -EINVAL;
+ goto exit;
+ }
+
+ data->mode = mode;
+
+ /* If mode is OFF then disable the regulator */
+ if (data->mode == L3G4200D_MODE_OFF) {
+ /* fall back to default values */
+ data->rate = L3G4200D_RATE_100;
+ data->range = L3G4200D_ODR_MIN_VAL;
+ l3g4200d_disable(data);
+ }
+exit:
+ mutex_unlock(&data->lock);
+ return count;
+}
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("100 200 400 800");
+
+static ssize_t set_sampling_frequency(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t count)
+{
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+ unsigned long rate = 0;
+ int err;
+
+ /* Perform read/write operation, only when device is active */
+ err = is_device_on(data);
+ if (err)
+ return -EINVAL;
+
+ if (strncmp(buf, "100" , 3) == 0)
+ rate = L3G4200D_RATE_100;
+
+ else if (strncmp(buf, "200" , 3) == 0)
+ rate = L3G4200D_RATE_200;
+
+ else if (strncmp(buf, "400" , 3) == 0)
+ rate = L3G4200D_RATE_400;
+
+ else if (strncmp(buf, "800" , 3) == 0)
+ rate = L3G4200D_RATE_800;
+ else
+ return -EINVAL;
+
+ mutex_lock(&data->lock);
+
+ if (l3g4200d_set_rate(data->client, rate)) {
+ dev_err(&data->client->dev, "set rate failed\n");
+ count = -EINVAL;
+ goto exit;
+ }
+ data->rate = rate;
+
+exit:
+ mutex_unlock(&data->lock);
+ return count;
+}
+
+/* sampling frequency - output rate in Hz */
+static const char * const reg_to_rate[] = {
+ "100",
+ "100",
+ "100",
+ "100",
+ "200",
+ "200",
+ "200",
+ "200",
+ "400",
+ "400",
+ "400",
+ "400",
+ "800",
+ "800",
+ "800",
+ "800"
+};
+
+static ssize_t show_sampling_frequency(struct device *dev,
+ struct device_attribute *attr,
+ char *buf)
+{
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+
+ return sprintf(buf, "%s\n", reg_to_rate[data->rate]);
+}
+
+static IIO_CONST_ATTR(gyro_xyz_scale_available, "8750000, 17500000, 70000000");
+
+static const int xyz_to_nanoscale[] = {
+ 8750000, 17500000, 70000000
+};
+
+static const char const scale_to_range[] = {
+ L3G4200D_RANGE_250,
+ L3G4200D_RANGE_500,
+ L3G4200D_RANGE_2000,
+};
+
+static int l3g4200d_write_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int val, int val2,
+ long mask)
+{
+ struct l3g4200d_data *data = iio_priv(indio_dev);
+ int ret = -EINVAL, i;
+ bool flag = false;
+ char end;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ ret = is_device_on(data);
+ if (ret)
+ return -EINVAL;
+ mutex_lock(&data->lock);
+ end = ARRAY_SIZE(xyz_to_nanoscale);
+ for (i = 0; i < end; i++) {
+ if (val == xyz_to_nanoscale[i]) {
+ flag = true;
+ break;
+ }
+ }
+ if (flag) {
+ ret = l3g4200d_set_range(data->client, scale_to_range[i]);
+ data->range = i;
+ }
+ mutex_unlock(&data->lock);
+ default:
+ break;
+ }
+ return ret;
+}
+
+static int l3g4200d_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask)
+{
+ struct l3g4200d_data *data = iio_priv(indio_dev);
+ switch (mask) {
+ case 0:
+ switch (chan->type) {
+ case IIO_ANGL_VEL:
+ return l3g4200d_xyz_read(indio_dev,
+ chan->address, val);
+ case IIO_TEMP:
+ return get_gyrotemp(indio_dev, chan->address , val);
+ default:
+ break;
+ }
+ break;
+ case IIO_CHAN_INFO_SCALE:
+ switch (chan->type) {
+ case IIO_ANGL_VEL:
+ *val = 0;
+ /* scale for X/Y and Z are different */
+ *val2 = xyz_to_nanoscale[data->range];
+ return IIO_VAL_INT_PLUS_NANO;
+ default:
+ break;
+ }
+ break;
+ default:
+ break;
+ }
+ return -EINVAL;
+}
+
+#define L3G4200D_CHANNEL(axis, addr) \
+ { \
+ .type = IIO_ANGL_VEL, \
+ .modified = 1, \
+ .channel2 = IIO_MOD_##axis, \
+ .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+ .address = addr, \
+ }
+
+#define L3G4200D_TEMP_CHANNEL(addr) \
+ { \
+ .type = IIO_TEMP, \
+ .indexed = 1, \
+ .channel = 0, \
+ .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+ .address = addr, \
+ }
+
+static const struct iio_chan_spec l3g4200d_channels[] = {
+ L3G4200D_CHANNEL(X, L3G4200D_OUT_X_L_A),
+ L3G4200D_CHANNEL(Y, L3G4200D_OUT_Y_L_A),
+ L3G4200D_CHANNEL(Z, L3G4200D_OUT_Z_L_A),
+ L3G4200D_TEMP_CHANNEL(L3G4200D_OUT_TEMP),
+};
+
+static IIO_DEVICE_ATTR(gyro_raw,
+ S_IRUGO,
+ l3g4200d_readdata,
+ NULL,
+ L3G4200D_OUT_X_L_A);
+static IIO_DEVICE_ATTR(sampling_frequency,
+ S_IWUGO | S_IRUGO,
+ show_sampling_frequency,
+ set_sampling_frequency,
+ L3G4200D_CTRL_REG1);
+static IIO_DEVICE_ATTR(mode,
+ S_IWUGO | S_IRUGO,
+ show_operating_mode,
+ set_operating_mode,
+ L3G4200D_CTRL_REG1);
+
+static struct attribute *l3g4200d_attributes[] = {
+ &iio_dev_attr_mode.dev_attr.attr,
+ &iio_dev_attr_sampling_frequency.dev_attr.attr,
+ &iio_dev_attr_gyro_raw.dev_attr.attr,
+ &iio_const_attr_sampling_frequency_available.dev_attr.attr,
+ &iio_const_attr_gyro_xyz_scale_available.dev_attr.attr,
+ NULL
+};
+
+static const struct attribute_group l3g4200d_group = {
+ .attrs = l3g4200d_attributes,
+};
+
+static const struct iio_info l3g4200d_info = {
+ .attrs = &l3g4200d_group,
+ .read_raw = &l3g4200d_read_raw,
+ .write_raw = &l3g4200d_write_raw,
+ .driver_module = THIS_MODULE,
+};
+
+static void l3g4200d_setup(struct l3g4200d_data *data)
+{
+ /* set mode */
+ l3g4200d_set_mode(data->client, data->mode);
+ /* set rate */
+ l3g4200d_set_rate(data->client, data->rate);
+ /* set range */
+ l3g4200d_set_range(data->client, scale_to_range[data->range]);
+}
+
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+static int l3g4200d_suspend(struct device *dev)
+{
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+ int ret = 0;
+
+ if (data->mode == L3G4200D_MODE_OFF)
+ return 0;
+
+ mutex_lock(&data->lock);
+
+ /* Set the device to sleep mode */
+ l3g4200d_set_mode(data->client, L3G4200D_MODE_OFF);
+
+ /* Disable regulator */
+ l3g4200d_disable(data);
+
+ data->device_status = L3G4200D_DEVICE_SUSPENDED;
+
+ mutex_unlock(&data->lock);
+
+ return ret;
+}
+
+static int l3g4200d_resume(struct device *dev)
+{
+ struct l3g4200d_data *data = iio_priv(dev_get_drvdata(dev));
+ int ret = 0;
+
+
+ if (data->device_status == L3G4200D_DEVICE_ON ||
+ data->device_status == L3G4200D_DEVICE_OFF) {
+ return 0;
+ }
+ mutex_lock(&data->lock);
+
+ /* Enable regulator */
+ l3g4200d_enable(data);
+
+ /* Set mode,rate and range */
+ l3g4200d_setup(data);
+
+ mutex_unlock(&data->lock);
+ return ret;
+}
+
+static const struct dev_pm_ops l3g4200d_dev_pm_ops = {
+ .suspend = l3g4200d_suspend,
+ .resume = l3g4200d_resume,
+};
+#endif
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void l3g4200d_early_suspend(struct early_suspend *data)
+{
+ struct l3g4200d_data *ddata =
+ container_of(data, struct l3g4200d_data, early_suspend);
+
+ if (ddata->mode == L3G4200D_MODE_OFF)
+ return;
+
+ mutex_lock(&ddata->lock);
+
+ /* Set the device to sleep mode */
+ l3g4200d_set_mode(ddata->client, L3G4200D_MODE_OFF);
+
+ /* Disable regulator */
+ l3g4200d_disable(ddata);
+
+ ddata->device_status = L3G4200D_DEVICE_SUSPENDED;
+
+ mutex_unlock(&ddata->lock);
+
+}
+
+static void l3g4200d_late_resume(struct early_suspend *data)
+{
+ struct l3g4200d_data *ddata =
+ container_of(data, struct l3g4200d_data, early_suspend);
+
+ if (ddata->device_status == L3G4200D_DEVICE_ON ||
+ ddata->device_status == L3G4200D_DEVICE_OFF) {
+ return;
+ }
+ mutex_lock(&ddata->lock);
+
+ /* Enable regulator */
+ l3g4200d_enable(ddata);
+
+ /* Set mode,rate and range */
+ l3g4200d_setup(ddata);
+
+ mutex_unlock(&ddata->lock);
+
+}
+#endif
+
+static int l3g4200d_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+ struct l3g4200d_data *data;
+ struct iio_dev *indio_dev;
+ int err = 0;
+
+ indio_dev = iio_allocate_device(sizeof(*data));
+ if (indio_dev == NULL) {
+ dev_err(&client->dev, "memory allocation failed\n");
+ err = -ENOMEM;
+ goto exit;
+ }
+ data = iio_priv(indio_dev);
+
+ data->mode = L3G4200D_MODE_OFF;
+ data->range = L3G4200D_RANGE_250;
+ data->rate = L3G4200D_ODR_MIN_VAL;
+ data->device_status = L3G4200D_DEVICE_OFF;
+ data->client = client;
+
+ /* check for valid platform data */
+ if (!client->dev.platform_data) {
+ dev_err(&client->dev, "Invalid platform data\n");
+ err = -ENOMEM;
+ goto exit1;
+ }
+ data->pdata = client->dev.platform_data;
+
+ i2c_set_clientdata(client, indio_dev);
+
+ data->regulator = regulator_get(&client->dev, "vdd");
+ if (IS_ERR(data->regulator)) {
+ err = PTR_ERR(data->regulator);
+ dev_err(&client->dev, "failed to get regulator = %d\n", err);
+ goto exit1;
+ }
+ /* Enable regulator */
+ l3g4200d_enable(data);
+
+ mutex_init(&data->lock);
+
+ err = i2c_smbus_read_byte_data(client, L3G4200D_WHO_AM_I);
+ if (err < 0) {
+ dev_err(&client->dev, "failed to read of the chip\n");
+ goto exit2;
+ }
+ if (err == WHOAMI_L3G4200D || err == WHOAMI_L3GD20)
+ dev_info(&client->dev,
+ "3-Axis Gyroscope device identification: %d\n",
+ err);
+ else {
+ dev_info(&client->dev,
+ "Gyroscope identification did not match\n");
+ goto exit2;
+ }
+
+ l3g4200d_setup(data);
+
+ indio_dev->info = &l3g4200d_info;
+ indio_dev->name = id->name;
+ indio_dev->dev.parent = &client->dev;
+ indio_dev->channels = l3g4200d_channels;
+ indio_dev->num_channels = ARRAY_SIZE(l3g4200d_channels);
+ indio_dev->modes = INDIO_DIRECT_MODE;
+
+ err = iio_device_register(indio_dev);
+ if (err)
+ goto exit2;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+ data->early_suspend.level =
+ EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+ data->early_suspend.suspend = l3g4200d_early_suspend;
+ data->early_suspend.resume = l3g4200d_late_resume;
+ register_early_suspend(&data->early_suspend);
+#endif
+
+ /* Disable regulator */
+ l3g4200d_disable(data);
+
+ return 0;
+
+exit2:
+ iio_free_device(indio_dev);
+ mutex_destroy(&data->lock);
+ l3g4200d_disable(data);
+ regulator_put(data->regulator);
+exit1:
+ kfree(data);
+exit:
+ return err;
+}
+
+static int __devexit l3g4200d_remove(struct i2c_client *client)
+{
+ struct iio_dev *indio_dev = i2c_get_clientdata(client);
+ struct l3g4200d_data *data = iio_priv(indio_dev);
+ int ret;
+
+ /* safer to make device off */
+ if (data->mode != L3G4200D_MODE_OFF) {
+ /* set mode to off */
+ ret = l3g4200d_set_mode(client, L3G4200D_MODE_OFF);
+ if (ret < 0) {
+ dev_err(&client->dev,
+ "could not turn off the device %d",
+ ret);
+ return ret;
+ }
+ if (data->device_status == L3G4200D_DEVICE_ON) {
+ regulator_disable(data->regulator);
+ data->device_status = L3G4200D_DEVICE_OFF;
+ }
+ }
+ regulator_put(data->regulator);
+ mutex_destroy(&data->lock);
+ iio_device_unregister(indio_dev);
+ iio_free_device(indio_dev);
+ kfree(data);
+ return 0;
+}
+
+static const struct i2c_device_id l3g4200d_id[] = {
+ { "l3g4200d", 0 },
+ { },
+};
+MODULE_DEVICE_TABLE(i2c, l3g4200d_id);
+
+static struct i2c_driver l3g4200d_driver = {
+ .driver = {
+ .name = "l3g4200d",
+ #if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+ .pm = &l3g4200d_dev_pm_ops,
+ #endif
+ },
+ .id_table = l3g4200d_id,
+ .probe = l3g4200d_probe,
+ .remove = l3g4200d_remove,
+};
+
+module_i2c_driver(l3g4200d_driver);
+
+MODULE_DESCRIPTION("l3g4200d Gyroscope Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Naga Radhesh Y <naga.radheshy@stericsson.com>");
generated by cgit v1.2.3 (git 2.39.1) at 2024-05-04 04:29:53 +0000