From fc7f84970ab2ca3e62fb95fe11d35f6d2576ef85 Mon Sep 17 00:00:00 2001 From: Zhifeng Wang Date: Wed, 15 Feb 2012 11:44:45 +0800 Subject: [PATCH] hwmon: l3g4200d: Initial version of gyroscope driver BZ: 22173 This commit adds the ST L3G4200D gyroscope driver. The driver is polling based and suitable for android system. Change-Id: I50952855fc38d0ee92125a11e45a4e9ce8a1b7b5 Signed-off-by: Zhifeng Wang Reviewed-on: http://android.intel.com:8080/21906 Reviewed-by: Liu, Hong Reviewed-by: Du, Alek Reviewed-by: buildbot Tested-by: buildbot --- arch/x86/platform/intel-mid/board-blackbay.c | 18 + drivers/hwmon/Kconfig | 10 + drivers/hwmon/Makefile | 1 + drivers/hwmon/l3g4200d_poll.c | 806 +++++++++++++++++++++++++++ include/linux/input/l3g4200d_poll.h | 66 +++ 5 files changed, 901 insertions(+) create mode 100644 drivers/hwmon/l3g4200d_poll.c create mode 100644 include/linux/input/l3g4200d_poll.h diff --git a/arch/x86/platform/intel-mid/board-blackbay.c b/arch/x86/platform/intel-mid/board-blackbay.c index f4896dc..2b774d2 100644 --- a/arch/x86/platform/intel-mid/board-blackbay.c +++ b/arch/x86/platform/intel-mid/board-blackbay.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -234,6 +235,22 @@ static void __init *emc1403_platform_data(void *info) return &intr2nd_pdata; } +static void __init *l3g4200d_platform_data(void *info) +{ + static struct l3g4200d_gyr_platform_data l3g4200d_pdata; + + l3g4200d_pdata.fs_range = L3G4200D_GYR_FS_2000DPS; + l3g4200d_pdata.poll_interval = 200; + l3g4200d_pdata.negate_x = 1; + l3g4200d_pdata.negate_y = 0; + l3g4200d_pdata.negate_z = 0; + l3g4200d_pdata.axis_map_x = 0; + l3g4200d_pdata.axis_map_y = 1; + l3g4200d_pdata.axis_map_z = 2; + + return &l3g4200d_pdata; +} + static void __init *lis331dl_platform_data(void *info) { static short intr2nd_pdata; @@ -1623,6 +1640,7 @@ struct devs_id __initconst device_ids[] = { {"lsm303dl", SFI_DEV_TYPE_I2C, 0, &lsm303dlhc_accel_platform_data}, {"lsm303cmp", SFI_DEV_TYPE_I2C, 0, &no_platform_data}, + {"l3g4200d", SFI_DEV_TYPE_I2C, 0, &l3g4200d_platform_data}, {}, }; diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig index 9dc869d..35452f9 100644 --- a/drivers/hwmon/Kconfig +++ b/drivers/hwmon/Kconfig @@ -1357,6 +1357,16 @@ config SENSORS_MC13783_ADC help Support for the A/D converter on MC13783 PMIC. +config SENSORS_L3G4200D_POLL + bool "Polling based STMicroeletronics L3G4200D three-axis angular rate sensor" + depends on I2C && INPUT + default y + help + A polling based L3G4200D gyroscope sensor driver. Support retrieving + gyro data with specified polling intervals in milliseconds. Suitable + for low precision but easy use requirements such as for Android integration. + Say Yes here to enable it. + config SENSORS_LIS3DH_ACC bool "ST Accel Sensor Driver" depends on I2C diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile index 32da5f8..64f2e43 100644 --- a/drivers/hwmon/Makefile +++ b/drivers/hwmon/Makefile @@ -123,6 +123,7 @@ obj-$(CONFIG_SENSORS_LIS3DH_ACC) += lis3dh_acc.o obj-$(CONFIG_SENSORS_HMC5883) += hmc5883.o obj-$(CONFIG_SENSORS_MPU3050) += mpu3050.o obj-$(CONFIG_SENSORS_MS5607) += ms5607.o +obj-$(CONFIG_SENSORS_L3G4200D_POLL) += l3g4200d_poll.o obj-$(CONFIG_MSIC_GPADC) += intel_mid_gpadc.o obj-$(CONFIG_SENSORS_MID_CURRENT) += intel_mid_ocd.o obj-$(CONFIG_SENSORS_LSM303_MAG) += lsm303dlhc_compass.o diff --git a/drivers/hwmon/l3g4200d_poll.c b/drivers/hwmon/l3g4200d_poll.c new file mode 100644 index 0000000..8f54b7f --- /dev/null +++ b/drivers/hwmon/l3g4200d_poll.c @@ -0,0 +1,806 @@ +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** +* +* File Name : l3g4200d_poll.c +* Authors : MH - C&I BU - Application Team +* : Carmine Iascone (carmine.iascone@st.com) +* : Matteo Dameno (matteo.dameno@st.com) +* : Both authors are willing to be considered the contact +* : and update points for the driver. +* Version : V 1.1 sysfs +* Date : 2011/02/28 +* Description : L3G4200D digital output gyroscope sensor API +* +******************************************************************************** +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License version 2 as +* published by the Free Software Foundation. +* +* THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES +* OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE +* PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT. +* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, +* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE +* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING +* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +* +******************************************************************************** +* REVISON HISTORY +* +* VERSION | DATE | AUTHORS | DESCRIPTION +* 1.0 | 2010/11/19 | Carmine Iascone | First Release +* 1.1 | 2011/02/28 | Matteo Dameno | Self Test Added +*******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +/* l3g4200d gyroscope registers */ +#define WHO_AM_I 0x0F + +#define CTRL_REG1 0x20 /* CTRL REG1 */ +#define CTRL_REG2 0x21 /* CTRL REG2 */ +#define CTRL_REG3 0x22 /* CTRL_REG3 */ +#define CTRL_REG4 0x23 /* CTRL_REG4 */ +#define CTRL_REG5 0x24 /* CTRL_REG5 */ + +/* CTRL_REG1 */ +#define PM_ON 0x08 +#define ENABLE_ALL_AXES 0x07 +#define BW00 0x00 +#define BW01 0x10 +#define BW10 0x20 +#define BW11 0x30 +#define ODR100 0x00 /* ODR = 100Hz */ +#define ODR200 0x40 /* ODR = 200Hz */ +#define ODR400 0x80 /* ODR = 400Hz */ +#define ODR800 0xC0 /* ODR = 800Hz */ +#define ODR_MASK 0xF0 + +/* CTRL_REG4 bits */ +#define FS_MASK 0x30 +#define SELFTEST_MASK 0x06 +#define L3G4200D_SELFTEST_DIS 0x00 +#define L3G4200D_SELFTEST_EN_POS 0x02 +#define L3G4200D_SELFTEST_EN_NEG 0x04 + +#define AXISDATA_REG 0x28 +#define AUTO_INCREMENT 0x80 + +/* RESUME STATE INDICES */ +#define RES_CTRL_REG1 0 +#define RES_CTRL_REG2 1 +#define RES_CTRL_REG3 2 +#define RES_CTRL_REG4 3 +#define RES_CTRL_REG5 4 +#define RESUME_ENTRIES 5 + +#define MAX_POLL_DELAY 500 + +#define DEBUG 1 + +/** Registers Contents */ +#define WHOAMI_L3G4200D 0x00D3 /* Expected content for WAI register*/ + +struct output_rate { + int poll_rate_ms; + u8 mask; +}; + +static const struct output_rate odr_table[] = { + {2, ODR800|BW10}, + {3, ODR400|BW01}, + {5, ODR200|BW00}, + {10, ODR100|BW00}, +}; + +struct l3g4200d_data { + struct i2c_client *client; + struct l3g4200d_gyr_platform_data *pdata; + + struct mutex lock; + + struct input_dev *input_dev; + struct early_suspend es; + u8 resume_state[RESUME_ENTRIES]; + struct delayed_work work; + int need_resume; +}; + +static void l3g4200d_update_odr_bits(struct l3g4200d_data *gyro) +{ + int i; + + /* find the lowest ODR that could meet the poll interval requirement. + * If couldn't find one, use the highest one. + */ + for (i = ARRAY_SIZE(odr_table) - 1; i > 0; i--) { + if (odr_table[i].poll_rate_ms <= gyro->pdata->poll_interval) + break; + } + + gyro->resume_state[RES_CTRL_REG1] &= (~ODR_MASK); + gyro->resume_state[RES_CTRL_REG1] |= odr_table[i].mask; +} + +static int l3g4200d_hw_init(struct l3g4200d_data *gyro) +{ + int ret; + + /* check if the chip reports the correct id */ + ret = i2c_smbus_read_byte_data(gyro->client, WHO_AM_I); + if (ret < 0) + return ret; + + if (ret != WHOAMI_L3G4200D) { + dev_err(&gyro->client->dev, "Invalid device id, %x", ret); + return -EINVAL; + } + + /* set default settings, and put gyro in power down mode */ + gyro->resume_state[RES_CTRL_REG1] = ENABLE_ALL_AXES; + gyro->resume_state[RES_CTRL_REG2] = 0x00; + gyro->resume_state[RES_CTRL_REG3] = 0x00; + gyro->resume_state[RES_CTRL_REG4] = gyro->pdata->fs_range; + gyro->resume_state[RES_CTRL_REG5] = 0x00; + l3g4200d_update_odr_bits(gyro); + + return i2c_smbus_write_block_data(gyro->client, + AUTO_INCREMENT | CTRL_REG1, + 5, &gyro->resume_state[0]); +} + +static void l3g4200d_queue_delayed_work(struct l3g4200d_data *gyro) +{ + unsigned long delay = + msecs_to_jiffies(gyro->pdata->poll_interval); + schedule_delayed_work(&gyro->work, delay); +} + +static void l3g4200d_report_data(struct l3g4200d_data *gyro) +{ + u8 buf[6]; + s16 hw_d[3]; + s16 x, y, z; + int ret; + + ret = i2c_smbus_read_i2c_block_data(gyro->client, + AUTO_INCREMENT | AXISDATA_REG, + sizeof(buf), &buf[0]); + if (ret < 0) { + dev_err(&gyro->client->dev, "Failed to read axis data.\n"); + return; + } + + hw_d[0] = (s16) ((buf[1] << 8) | buf[0]); + hw_d[1] = (s16) ((buf[3] << 8) | buf[2]); + hw_d[2] = (s16) ((buf[5] << 8) | buf[4]); + + x = ((gyro->pdata->negate_x) ? (-hw_d[gyro->pdata->axis_map_x]) + : (hw_d[gyro->pdata->axis_map_x])); + y = ((gyro->pdata->negate_y) ? (-hw_d[gyro->pdata->axis_map_y]) + : (hw_d[gyro->pdata->axis_map_y])); + z = ((gyro->pdata->negate_z) ? (-hw_d[gyro->pdata->axis_map_z]) + : (hw_d[gyro->pdata->axis_map_z])); + +#ifdef DEBUG + { + struct timeval now; + jiffies_to_timeval(jiffies, &now); + dev_dbg(&gyro->client->dev, "%ld.%ld: X=%d, Y=%d, Z=%d", + now.tv_sec, now.tv_usec, + (int) x, + (int) y, + (int) z); + } +#endif + + input_report_rel(gyro->input_dev, REL_X, x); + input_report_rel(gyro->input_dev, REL_Y, y); + input_report_rel(gyro->input_dev, REL_Z, z); + input_sync(gyro->input_dev); +} + +static int l3g4200d_enabled(struct l3g4200d_data *gyro) +{ + return gyro->resume_state[RES_CTRL_REG1] & PM_ON; +} + +static void l3g4200d_poll_work(struct work_struct *work) +{ + + struct l3g4200d_data *gyro = + container_of(work, struct l3g4200d_data, work.work); + + mutex_lock(&gyro->lock); + + /* there is the possibility that the device is already disabled + * before this delayed work is executed. + */ + if (!l3g4200d_enabled(gyro)) + goto leave; + + l3g4200d_report_data(gyro); + l3g4200d_queue_delayed_work(gyro); +leave: + mutex_unlock(&gyro->lock); +} + +static int l3g4200d_enable(struct l3g4200d_data *gyro) +{ + int err; + + if (l3g4200d_enabled(gyro)) + return 0; + + /* Change power down mode bit of CTRL_REG1 to enable the gyro chip */ + gyro->resume_state[RES_CTRL_REG1] |= PM_ON; + err = i2c_smbus_write_byte_data(gyro->client, CTRL_REG1, + gyro->resume_state[RES_CTRL_REG1]); + if (err < 0) { + dev_err(&gyro->client->dev, "Failed to enable gyro.\n"); + return err; + } + + l3g4200d_queue_delayed_work(gyro); + return 0; +} + +static int l3g4200d_disable(struct l3g4200d_data *gyro) +{ + int err; + + if (!l3g4200d_enabled(gyro)) + return 0; + + /* Change power down mode bit of CTRL_REG1 to disable the gyro chip */ + gyro->resume_state[RES_CTRL_REG1] &= ~PM_ON; + err = i2c_smbus_write_byte_data(gyro->client, CTRL_REG1, + gyro->resume_state[RES_CTRL_REG1]); + if (err < 0) { + dev_err(&gyro->client->dev, "Failed to disable gyro.\n"); + return err; + } + + cancel_delayed_work(&gyro->work); + return 0; +} + +static ssize_t attr_polling_rate_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int val; + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + + mutex_lock(&gyro->lock); + val = gyro->pdata->poll_interval; + mutex_unlock(&gyro->lock); + + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_polling_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + unsigned long interval_ms; + int saved_interval; + int err; + + if (strict_strtoul(buf, 10, &interval_ms)) + return -EINVAL; + + if (interval_ms < (unsigned long) odr_table[0].poll_rate_ms) { + dev_info(dev, "polling interval is too small, set it to %d\n", + odr_table[0].poll_rate_ms); + interval_ms = odr_table[0].poll_rate_ms; + } + + /* set max poll interval to be 500 ms */ + if (interval_ms > MAX_POLL_DELAY) { + dev_info(dev, "polling interval is too big, set it to 500\n"); + interval_ms = MAX_POLL_DELAY; + } + + mutex_lock(&gyro->lock); + + saved_interval = gyro->pdata->poll_interval; + gyro->pdata->poll_interval = (int)interval_ms; + l3g4200d_update_odr_bits(gyro); + err = i2c_smbus_write_byte_data(gyro->client, CTRL_REG1, + gyro->resume_state[RES_CTRL_REG1]); + if (err < 0) { + dev_err(&gyro->client->dev, "Failed to change poll interval.\n"); + gyro->pdata->poll_interval = saved_interval; + l3g4200d_update_odr_bits(gyro); + mutex_unlock(&gyro->lock); + return -EIO; + } + + mutex_unlock(&gyro->lock); + + return size; +} + +static ssize_t attr_range_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + int range = 0; + u8 val; + + mutex_lock(&gyro->lock); + + val = gyro->resume_state[RES_CTRL_REG4] & FS_MASK; + switch (val) { + case L3G4200D_GYR_FS_250DPS: + range = 250; + break; + case L3G4200D_GYR_FS_500DPS: + range = 500; + break; + case L3G4200D_GYR_FS_2000DPS: + default: + range = 2000; + break; + } + + mutex_unlock(&gyro->lock); + return sprintf(buf, "%d\n", range); +} + +static ssize_t attr_range_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + unsigned long val; + u8 saved; + u8 range_bits; + int ret; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if (val != 250 && val != 500 && val != 2000) + return -EINVAL; + + mutex_lock(&gyro->lock); + switch (val) { + case 250: + range_bits = L3G4200D_GYR_FS_250DPS; + break; + case 500: + range_bits = L3G4200D_GYR_FS_500DPS; + break; + case 2000: + default: + range_bits = L3G4200D_GYR_FS_2000DPS; + break; + } + + saved = gyro->resume_state[RES_CTRL_REG4]; + gyro->resume_state[RES_CTRL_REG4] &= ~FS_MASK; + gyro->resume_state[RES_CTRL_REG4] |= range_bits; + ret = i2c_smbus_write_byte_data(gyro->client, CTRL_REG4, + gyro->resume_state[RES_CTRL_REG4]); + if (ret < 0) { + dev_err(&gyro->client->dev, "Failed to change fs range.\n"); + gyro->resume_state[RES_CTRL_REG4] = saved; + mutex_unlock(&gyro->lock); + return ret; + } + + mutex_unlock(&gyro->lock); + return size; +} + +static ssize_t attr_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + int val; + + mutex_lock(&gyro->lock); + val = (gyro->resume_state[RES_CTRL_REG1] & PM_ON) ? 1 : 0; + mutex_unlock(&gyro->lock); + + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + mutex_lock(&gyro->lock); + if (val) + l3g4200d_enable(gyro); + else + l3g4200d_disable(gyro); + mutex_unlock(&gyro->lock); + + return size; +} + +static ssize_t attr_get_selftest(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + u8 self_test_bits; + int val; + + mutex_lock(&gyro->lock); + self_test_bits = gyro->resume_state[RES_CTRL_REG4] & SELFTEST_MASK; + mutex_unlock(&gyro->lock); + + switch (self_test_bits) { + case L3G4200D_SELFTEST_EN_POS: + val = 1; + break; + case L3G4200D_SELFTEST_EN_NEG: + val = -1; + break; + default: + val = 0; + break; + } + + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_selftest(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + long val; + u8 saved; + int ret; + + if (strict_strtol(buf, 10, &val)) + return -EINVAL; + + mutex_lock(&gyro->lock); + + saved = gyro->resume_state[RES_CTRL_REG4]; + gyro->resume_state[RES_CTRL_REG4] &= ~SELFTEST_MASK; + if (val < 0) + gyro->resume_state[RES_CTRL_REG4] |= L3G4200D_SELFTEST_EN_NEG; + else if (val > 0) + gyro->resume_state[RES_CTRL_REG4] |= L3G4200D_SELFTEST_EN_POS; + else + gyro->resume_state[RES_CTRL_REG4] |= L3G4200D_SELFTEST_DIS; + + ret = i2c_smbus_write_byte_data(gyro->client, CTRL_REG4, + gyro->resume_state[RES_CTRL_REG4]); + if (ret < 0) { + dev_err(&gyro->client->dev, "Failed to change fs range.\n"); + gyro->resume_state[RES_CTRL_REG4] = saved; + mutex_unlock(&gyro->lock); + return ret; + } + + mutex_unlock(&gyro->lock); + + return size; +} + +static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr, + char *buf) +{ + ssize_t data_size = 0; + struct l3g4200d_data *gyro = dev_get_drvdata(dev); + int rc; + int i; + u8 ctl_regs[5]; + + /* read and display the 5 control registers */ + mutex_lock(&gyro->lock); + rc = i2c_smbus_read_i2c_block_data(gyro->client, + AUTO_INCREMENT | CTRL_REG1, + sizeof(ctl_regs), &ctl_regs[0]); + if (rc < 0) { + dev_err(&gyro->client->dev, + "Failed to read control register data.\n"); + mutex_unlock(&gyro->lock); + return -EINVAL; + } + mutex_unlock(&gyro->lock); + + for (i = 0; i < 5; ++i) + data_size += sprintf(&buf[data_size], "CTRL_REG%d=0x%x\n", + i, (unsigned int)ctl_regs[i]); + return data_size; +} + + +static struct device_attribute attributes[] = { + __ATTR(poll, 0666, attr_polling_rate_show, attr_polling_rate_store), + __ATTR(enable, 0666, attr_enable_show, attr_enable_store), + __ATTR(range, 0666, attr_range_show, attr_range_store), + __ATTR(enable_selftest, 0666, attr_get_selftest, attr_set_selftest), + __ATTR(registers, 0600, attr_reg_get, NULL), +}; + +static int create_sysfs_interfaces(struct device *dev) +{ + int i; + for (i = 0; i < ARRAY_SIZE(attributes); i++) + if (device_create_file(dev, attributes + i)) + goto error; + return 0; + +error: + for (i = i - 1; i >= 0; i--) + device_remove_file(dev, attributes + i); + dev_err(dev, "%s:Unable to create interface\n", __func__); + return -1; +} + +static int remove_sysfs_interfaces(struct device *dev) +{ + int i; + for (i = 0; i < ARRAY_SIZE(attributes); i++) + device_remove_file(dev, attributes + i); + return 0; +} + +#ifdef CONFIG_HAS_EARLYSUSPEND +static void l3g4200d_early_suspend(struct early_suspend *h) +{ + struct l3g4200d_data *gyro = container_of(h, struct l3g4200d_data, es); + + mutex_lock(&gyro->lock); + gyro->need_resume = l3g4200d_enabled(gyro); + l3g4200d_disable(gyro); + mutex_unlock(&gyro->lock); +} + +static void l3g4200d_late_resume(struct early_suspend *h) +{ + struct l3g4200d_data *gyro = container_of(h, struct l3g4200d_data, es); + + mutex_lock(&gyro->lock); + if (gyro->need_resume) + l3g4200d_enable(gyro); + mutex_unlock(&gyro->lock); +} +#endif /* CONFIG_HAS_EARLYSUSPEND */ + +static int l3g4200d_input_init(struct l3g4200d_data *gyro) +{ + int err; + struct device *dev = &gyro->client->dev; + + gyro->input_dev = input_allocate_device(); + if (!gyro->input_dev) { + dev_err(dev, "input device allocation failed.\n"); + return -ENOMEM; + } + + gyro->input_dev->name = L3G4200D_GYR_DEV_NAME; + gyro->input_dev->id.bustype = BUS_I2C; + gyro->input_dev->dev.parent = dev; + input_set_drvdata(gyro->input_dev, gyro); + + set_bit(EV_REL, gyro->input_dev->evbit); + set_bit(REL_X, gyro->input_dev->relbit); + set_bit(REL_Y, gyro->input_dev->relbit); + set_bit(REL_Z, gyro->input_dev->relbit); + + err = input_register_device(gyro->input_dev); + if (err) { + dev_err(dev, "unable to register input device.\n"); + input_free_device(gyro->input_dev); + return err; + } + + return 0; +} + +static int l3g4200d_validate_pdata(struct l3g4200d_data *gyro) +{ + if (gyro->pdata->axis_map_x > 2 || + gyro->pdata->axis_map_y > 2 || + gyro->pdata->axis_map_z > 2) { + dev_err(&gyro->client->dev, + "invalid axis_map value x:%u y:%u z%u\n", + gyro->pdata->axis_map_x, + gyro->pdata->axis_map_y, + gyro->pdata->axis_map_z); + return -EINVAL; + } + + /* Only allow 0 and 1 for negation boolean flag */ + if (gyro->pdata->negate_x > 1 || + gyro->pdata->negate_y > 1 || + gyro->pdata->negate_z > 1) { + dev_err(&gyro->client->dev, + "invalid negate value x:%u y:%u z:%u\n", + gyro->pdata->negate_x, + gyro->pdata->negate_y, + gyro->pdata->negate_z); + return -EINVAL; + } + + /* Enforce minimum polling interval */ + if (gyro->pdata->poll_interval < gyro->pdata->min_interval) { + dev_err(&gyro->client->dev, + "minimum poll interval violated\n"); + return -EINVAL; + } + return 0; +} + + +static int l3g4200d_probe(struct i2c_client *client, + const struct i2c_device_id *devid) +{ + int err; + struct l3g4200d_data *gyro; + + dev_info(&client->dev, "probe start.\n"); + + if (client->dev.platform_data == NULL) { + dev_err(&client->dev, "platform data is NULL. exiting.\n"); + err = -ENODEV; + goto err_out; + } + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_I2C | I2C_FUNC_SMBUS_I2C_BLOCK)) { + dev_err(&client->dev, "client not i2c capable.\n"); + err = -ENODEV; + goto err_out; + } + + gyro = kzalloc(sizeof(*gyro), GFP_KERNEL); + if (gyro == NULL) { + dev_err(&client->dev, "failed to allocate memory.\n"); + err = -ENOMEM; + goto err_out; + } + + gyro->pdata = kmalloc(sizeof(*gyro->pdata), GFP_KERNEL); + if (gyro->pdata == NULL) { + dev_err(&client->dev, "failed to allocate memory for pdata."); + err = -ENOMEM; + goto err_free_gyro; + } + memcpy(gyro->pdata, client->dev.platform_data, sizeof(*gyro->pdata)); + + err = l3g4200d_validate_pdata(gyro); + if (err < 0) { + dev_err(&client->dev, "failed to validate platform data.\n"); + goto err_free_pdata; + } + + gyro->client = client; + i2c_set_clientdata(client, gyro); + + err = l3g4200d_input_init(gyro); + if (err < 0) + goto err_free_pdata; + + err = l3g4200d_hw_init(gyro); + if (err < 0) { + dev_err(&client->dev, "failed to init l3g4200d hardware.\n"); + goto err_clean_input; + } + + err = create_sysfs_interfaces(&client->dev); + if (err < 0) + goto err_clean_input; + + mutex_init(&gyro->lock); + INIT_DELAYED_WORK(&gyro->work, l3g4200d_poll_work); + + gyro->es.level = EARLY_SUSPEND_LEVEL_DISABLE_FB + 10; + gyro->es.suspend = l3g4200d_early_suspend; + gyro->es.resume = l3g4200d_late_resume; + register_early_suspend(&gyro->es); + + dev_info(&client->dev, "probed.\n"); + return 0; + +err_clean_input: + input_unregister_device(gyro->input_dev); +err_free_pdata: + kfree(gyro->pdata); +err_free_gyro: + kfree(gyro); +err_out: + dev_err(&client->dev, "Driver Initialization failed, %d\n", err); + return err; +} + +static int l3g4200d_remove(struct i2c_client *client) +{ + struct l3g4200d_data *gyro = i2c_get_clientdata(client); + + dev_info(&client->dev, "L3G4200D driver removing\n"); + + remove_sysfs_interfaces(&client->dev); + unregister_early_suspend(&gyro->es); + + mutex_lock(&gyro->lock); + l3g4200d_disable(gyro); + mutex_unlock(&gyro->lock); + + /* there may be still a delayed work in execution, we must wait for + * its completion before we could reclaim resources. + */ + cancel_delayed_work_sync(&gyro->work); + + mutex_destroy(&gyro->lock); + input_unregister_device(gyro->input_dev); + kfree(gyro->pdata); + kfree(gyro); + return 0; +} + +static int l3g4200d_suspend(struct device *dev) +{ + return 0; +} + +static int l3g4200d_resume(struct device *dev) +{ + return 0; +} + +static const struct i2c_device_id l3g4200d_id[] = { + { L3G4200D_GYR_DEV_NAME , 0 }, + {}, +}; + +MODULE_DEVICE_TABLE(i2c, l3g4200d_id); + +static const struct dev_pm_ops l3g4200d_pm = { + .suspend = l3g4200d_suspend, + .resume = l3g4200d_resume, +}; + +static struct i2c_driver l3g4200d_driver = { + .driver = { + .owner = THIS_MODULE, + .name = L3G4200D_GYR_DEV_NAME, + .pm = &l3g4200d_pm, + }, + .probe = l3g4200d_probe, + .remove = __devexit_p(l3g4200d_remove), + .id_table = l3g4200d_id, +}; + +static int __init l3g4200d_init(void) +{ + pr_info("%s: l3g4200d polling driver init\n", L3G4200D_GYR_DEV_NAME); + return i2c_add_driver(&l3g4200d_driver); +} + +static void __exit l3g4200d_exit(void) +{ + pr_info("L3G4200D polling driver exit\n"); + i2c_del_driver(&l3g4200d_driver); + return; +} + +module_init(l3g4200d_init); +module_exit(l3g4200d_exit); + +MODULE_DESCRIPTION("l3g4200d digital gyroscope sysfs driver"); +MODULE_AUTHOR("Matteo Dameno, Carmine Iascone, STMicroelectronics"); +MODULE_LICENSE("GPL"); diff --git a/include/linux/input/l3g4200d_poll.h b/include/linux/input/l3g4200d_poll.h new file mode 100644 index 0000000..993bd20 --- /dev/null +++ b/include/linux/input/l3g4200d_poll.h @@ -0,0 +1,66 @@ +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** +* +* File Name : l3g4200d_poll.h +* Authors : MH - C&I BU - Application Team +* : Carmine Iascone (carmine.iascone@st.com) +* : Matteo Dameno (matteo.dameno@st.com) +* Version : V 1.1 sysfs +* Date : 2010/Aug/19 +* Description : L3G4200D digital output gyroscope sensor API +* +******************************************************************************** +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License version 2 as +* published by the Free Software Foundation. +* +* THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES +* OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE +* PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT. +* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, +* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE +* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING +* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. +* +******************************************************************************** +* REVISON HISTORY +* +* VERSION | DATE | AUTHORS | DESCRIPTION +* 1.0 | 2010/Aug/19 | Carmine Iascone | First Release +* 1.1 | 2011/02/28 | Matteo Dameno | Self Test Added +*******************************************************************************/ + +#ifndef __L3G4200D_H__ +#define __L3G4200D_H__ + +#define L3G4200D_GYR_DEV_NAME "l3g4200d" + +#define L3G4200D_GYR_FS_250DPS 0x00 +#define L3G4200D_GYR_FS_500DPS 0x10 +#define L3G4200D_GYR_FS_2000DPS 0x30 + +#define L3G4200D_GYR_ENABLED 1 +#define L3G4200D_GYR_DISABLED 0 + +#ifdef __KERNEL__ +struct l3g4200d_gyr_platform_data { + int (*init)(void); + void (*exit)(void); + int (*power_on)(void); + int (*power_off)(void); + int poll_interval; /* in millisecond */ + int min_interval; + + u8 fs_range; + + u8 axis_map_x; + u8 axis_map_y; + u8 axis_map_z; + + u8 negate_x; + u8 negate_y; + u8 negate_z; +}; +#endif /* __KERNEL__ */ + +#endif /* __L3G4200D_H__ */ -- 2.7.4