input: R3GD20: Add R3GD20 gyroscope for HTC
HTC kernel version: evitaul-jb-crc-3.4.10-ec474a3
Change-Id: I94ff18fb64b2d106ce4a80e19ef898e9d1f9b265
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 3a37f33..888c6ad 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -705,3 +705,15 @@
help
Say Y here to enable the CM3629 short distance proximity
sensor with ambient light sensor.
+
+config SENSORS_R3GD20
+ tristate "ST R3GD20 3-axis gyroscope"
+ depends on I2C
+ help
+ This driver provides support for the R3GD20 chip which is a 3-axis
+ gyroscope.
+
+ This driver can also be built as a module. If so the module for the
+ gyroscope will be called r3gd20.
+
+ Say Y here if you have a device containing the r3gd20 chip.
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 00b376c..7061165 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -68,3 +68,4 @@
obj-$(CONFIG_SENSORS_PANASONIC_GYRO) += ewtzmu2.o
obj-$(CONFIG_SENSORS_BMA250) += bma250.o
obj-$(CONFIG_INPUT_CAPELLA_CM3629) += cm3629.o
+obj-$(CONFIG_SENSORS_R3GD20) += r3gd20.o
diff --git a/drivers/input/misc/r3gd20.c b/drivers/input/misc/r3gd20.c
new file mode 100644
index 0000000..fc638f0
--- /dev/null
+++ b/drivers/input/misc/r3gd20.c
@@ -0,0 +1,1970 @@
+/******************** (C) COPYRIGHT 2011 STMicroelectronics ********************
+*
+* File Name : r3gd20_gyr_sysfs.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.5 sysfs
+* Date : 2011/Sep/24
+* Description : R3GD20 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/May/02 | Carmine Iascone | First Release
+* 1.1.3 | 2011/Jun/24 | Matteo Dameno | Corrects ODR Bug
+* 1.1.4 | 2011/Sep/02 | Matteo Dameno | SMB Bus Mng,
+* | | | forces BDU setting
+* 1.1.5 | 2011/Sep/24 | Matteo Dameno | Introduces FIFO Feat.
+* 1.1.5.1 | 2011/Nov/6 | Morris Chen | change name from l3g to r3g
+* | change default FS to 2000DPS
+* | change default poll_rate to 50ms
+* | chage the attribute of sysfs file as 666
+*******************************************************************************/
+
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/input-polldev.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include <linux/r3gd20.h>
+#include <linux/delay.h>
+#include <linux/export.h>
+#include <linux/module.h>
+
+#define D(x...) printk(KERN_DEBUG "[GYRO][R3GD20] " x)
+#define I(x...) printk(KERN_INFO "[GYRO][R3GD20] " x)
+#define E(x...) printk(KERN_ERR "[GYRO][R3GD20 ERROR] " x)
+#define DIF(x...) \
+ if (debug_flag) \
+ printk(KERN_DEBUG "[GYRO][R3GD20 DEBUG] " x)
+
+#define FS_MAX 32768
+
+#define WHO_AM_I 0x0F
+
+#define CTRL_REG1 0x20
+#define CTRL_REG2 0x21
+#define CTRL_REG3 0x22
+#define CTRL_REG4 0x23
+#define CTRL_REG5 0x24
+#define REFERENCE 0x25
+#define FIFO_CTRL_REG 0x2E
+#define FIFO_SRC_REG 0x2F
+#define OUT_X_L 0x28
+
+#define AXISDATA_REG OUT_X_L
+
+#define ALL_ZEROES 0x00
+#define PM_OFF 0x00
+#define PM_NORMAL 0x08
+#define ENABLE_ALL_AXES 0x07
+#define ENABLE_NO_AXES 0x00
+#define BW00 0x00
+#define BW01 0x10
+#define BW10 0x20
+#define BW11 0x30
+#define ODR095 0x00
+#define ODR190 0x40
+#define ODR380 0x80
+#define ODR760 0xC0
+
+#define I2_DRDY 0x08
+#define I2_WTM 0x04
+#define I2_OVRUN 0x02
+#define I2_EMPTY 0x01
+#define I2_NONE 0x00
+#define I2_MASK 0x0F
+
+#define FS_MASK 0x30
+#define BDU_ENABLE 0x80
+
+#define FIFO_ENABLE 0x40
+#define HPF_ENALBE 0x11
+
+#define FIFO_MODE_MASK 0xE0
+#define FIFO_MODE_BYPASS 0x00
+#define FIFO_MODE_FIFO 0x20
+#define FIFO_MODE_STREAM 0x40
+#define FIFO_MODE_STR2FIFO 0x60
+#define FIFO_MODE_BYPASS2STR 0x80
+#define FIFO_WATERMARK_MASK 0x1F
+
+#define FIFO_STORED_DATA_MASK 0x1F
+
+
+#define FUZZ 0
+#define FLAT 0
+#define I2C_AUTO_INCREMENT 0x80
+
+#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 RES_FIFO_CTRL_REG 5
+#define RESUME_ENTRIES 6
+
+#define TOLERENCE 1071
+
+#define DEBUG 0
+
+#define HTC_WQ 1
+#define HTC_SUSPEND 1
+#define HTC_ATTR 1
+
+#define HW_WAKE_UP_TIME 160
+
+#define WHOAMI_R3GD20 0x00D4
+
+
+struct r3gd20_triple {
+ short x,
+ y,
+ z;
+};
+
+struct output_rate {
+ int poll_rate_ms;
+ u8 mask;
+};
+
+static const struct output_rate odr_table[] = {
+
+ { 2, ODR760|BW10},
+ { 3, ODR380|BW01},
+ { 6, ODR190|BW00},
+ { 11, ODR095|BW00},
+};
+
+static int use_smbus;
+
+static const struct r3gd20_gyr_platform_data default_r3gd20_gyr_pdata = {
+ .fs_range = R3GD20_GYR_FS_2000DPS,
+ .axis_map_x = 0,
+ .axis_map_y = 1,
+ .axis_map_z = 2,
+ .negate_x = 0,
+ .negate_y = 0,
+ .negate_z = 0,
+
+ .poll_interval = 50,
+ .min_interval = R3GD20_MIN_POLL_PERIOD_MS,
+
+
+
+
+ .watermark = 0,
+ .fifomode = 0,
+};
+
+#ifdef HTC_WQ
+static void polling_do_work(struct work_struct *w);
+static DECLARE_DELAYED_WORK(polling_work, polling_do_work);
+#endif
+
+struct r3gd20_data {
+ struct i2c_client *client;
+ struct r3gd20_gyr_platform_data *pdata;
+
+ struct mutex lock;
+
+ struct input_polled_dev *input_poll_dev;
+ int hw_initialized;
+
+ atomic_t enabled;
+
+ u8 reg_addr;
+ u8 resume_state[RESUME_ENTRIES];
+
+ int irq2;
+ struct work_struct irq2_work;
+ struct workqueue_struct *irq2_work_queue;
+
+ bool polling_enabled;
+#ifdef HTC_WQ
+ struct workqueue_struct *gyro_wq;
+ struct input_dev *gyro_input_dev;
+#endif
+
+#ifdef HTC_ATTR
+ struct class *htc_gyro_class;
+ struct device *gyro_dev;
+#endif
+ int cali_data_x;
+ int cali_data_y;
+ int cali_data_z;
+};
+
+#ifdef HTC_WQ
+struct r3gd20_data *g_gyro;
+#endif
+
+static int debug_flag;
+
+static int r3gd20_i2c_read(struct r3gd20_data *gyr,
+ u8 *buf, int len)
+{
+ int ret;
+ u8 reg = buf[0];
+ u8 cmd = reg;
+
+
+ if (use_smbus) {
+ if (len == 1) {
+ ret = i2c_smbus_read_byte_data(gyr->client, cmd);
+ buf[0] = ret & 0xff;
+#if DEBUG
+ dev_warn(&gyr->client->dev,
+ "i2c_smbus_read_byte_data: ret=0x%02x, len:%d ,"
+ "command=0x%02x, buf[0]=0x%02x\n",
+ ret, len, cmd , buf[0]);
+#endif
+ } else if (len > 1) {
+
+ ret = i2c_smbus_read_i2c_block_data(gyr->client,
+ cmd, len, buf);
+#if DEBUG
+ dev_warn(&gyr->client->dev,
+ "i2c_smbus_read_i2c_block_data: ret:%d len:%d, "
+ "command=0x%02x, ",
+ ret, len, cmd);
+
+ unsigned int ii;
+ for (ii = 0; ii < len; ii++)
+ D("buf[%d]=0x%02x,", ii, buf[ii]);
+
+ D("\n");
+#endif
+ } else
+ ret = -1;
+
+ if (ret < 0) {
+ dev_err(&gyr->client->dev,
+ "read transfer error: len:%d, command=0x%02x\n",
+ len, cmd);
+ return 0;
+ }
+ return len;
+ }
+
+
+ ret = i2c_master_send(gyr->client, &cmd, sizeof(cmd));
+ if (ret != sizeof(cmd))
+ return ret;
+
+ return i2c_master_recv(gyr->client, buf, len);
+}
+
+static int r3gd20_i2c_write(struct r3gd20_data *gyr, u8 *buf, int len)
+{
+ int ret;
+ u8 reg, value;
+
+ reg = buf[0];
+ value = buf[1];
+
+ if (use_smbus) {
+ if (len == 1) {
+ ret = i2c_smbus_write_byte_data(gyr->client, reg, value);
+#if DEBUG
+ dev_warn(&gyr->client->dev,
+ "i2c_smbus_write_byte_data: ret=%d, len:%d, "
+ "command=0x%02x, value=0x%02x\n",
+ ret, len, reg , value);
+#endif
+ return ret;
+ } else if (len > 1) {
+ ret = i2c_smbus_write_i2c_block_data(gyr->client,
+ reg, len, buf + 1);
+#if DEBUG
+ dev_warn(&gyr->client->dev,
+ "i2c_smbus_write_i2c_block_data: ret=%d, "
+ "len:%d, command=0x%02x, ",
+ ret, len, reg);
+ unsigned int ii;
+ for (ii = 0; ii < (len + 1); ii++)
+ D("value[%d]=0x%02x,", ii, buf[ii]);
+
+ D("\n");
+#endif
+ return ret;
+ }
+ }
+
+ ret = i2c_master_send(gyr->client, buf, len+1);
+ return (ret == len+1) ? 0 : ret;
+}
+
+
+static int r3gd20_register_write(struct r3gd20_data *gyro, u8 *buf,
+ u8 reg_address, u8 new_value)
+{
+ int err;
+
+ buf[0] = reg_address;
+ buf[1] = new_value;
+ err = r3gd20_i2c_write(gyro, buf, 1);
+ if (err < 0)
+ return err;
+
+ return err;
+}
+
+static int r3gd20_register_read(struct r3gd20_data *gyro, u8 *buf,
+ u8 reg_address)
+{
+
+ int err = -1;
+ buf[0] = (reg_address);
+ err = r3gd20_i2c_read(gyro, buf, 1);
+ return err;
+}
+
+static int r3gd20_register_update(struct r3gd20_data *gyro, u8 *buf,
+ u8 reg_address, u8 mask, u8 new_bit_values)
+{
+ int err = -1;
+ u8 init_val;
+ u8 updated_val;
+ err = r3gd20_register_read(gyro, buf, reg_address);
+ if (!(err < 0)) {
+ init_val = buf[0];
+ updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
+ err = r3gd20_register_write(gyro, buf, reg_address,
+ updated_val);
+ }
+ return err;
+}
+
+#if 1
+static int r3gd20_update_watermark(struct r3gd20_data *gyro,
+ u8 watermark)
+{
+ int res = 0;
+ u8 buf[2];
+ u8 new_value;
+
+ mutex_lock(&gyro->lock);
+ new_value = (watermark % 0x20);
+ res = r3gd20_register_update(gyro, buf, FIFO_CTRL_REG,
+ FIFO_WATERMARK_MASK, new_value);
+ if (res < 0) {
+ E("%s : failed to update watermark\n", __func__);
+ return res;
+ }
+ E("%s : new_value:0x%02x,watermark:0x%02x\n",
+ __func__, new_value, watermark);
+
+ gyro->resume_state[RES_FIFO_CTRL_REG] =
+ ((FIFO_WATERMARK_MASK & new_value) |
+ (~FIFO_WATERMARK_MASK &
+ gyro->resume_state[RES_FIFO_CTRL_REG]));
+ gyro->pdata->watermark = new_value;
+ mutex_unlock(&gyro->lock);
+ return res;
+}
+
+static int r3gd20_update_fifomode(struct r3gd20_data *gyro, u8 fifomode)
+{
+ int res;
+ u8 buf[2];
+ u8 new_value;
+
+ new_value = fifomode;
+ res = r3gd20_register_update(gyro, buf, FIFO_CTRL_REG,
+ FIFO_MODE_MASK, new_value);
+ if (res < 0) {
+ E("%s : failed to update fifoMode\n", __func__);
+ return res;
+ }
+ gyro->resume_state[RES_FIFO_CTRL_REG] =
+ ((FIFO_MODE_MASK & new_value) |
+ (~FIFO_MODE_MASK &
+ gyro->resume_state[RES_FIFO_CTRL_REG]));
+ gyro->pdata->fifomode = new_value;
+
+ return res;
+}
+
+static int r3gd20_fifo_reset(struct r3gd20_data *gyro)
+{
+ u8 oldmode;
+ int res;
+
+ oldmode = gyro->pdata->fifomode;
+ res = r3gd20_update_fifomode(gyro, FIFO_MODE_BYPASS);
+ if (res < 0)
+ return res;
+ res = r3gd20_update_fifomode(gyro, oldmode);
+ if (res >= 0)
+ I("%s : fifo reset to: 0x%02x\n", __func__, oldmode);
+ return res;
+}
+
+static int r3gd20_fifo_hwenable(struct r3gd20_data *gyro,
+ u8 enable)
+{
+ int res;
+ u8 buf[2];
+ u8 set = 0x00;
+ if (enable)
+ set = FIFO_ENABLE;
+ res = r3gd20_register_update(gyro, buf, CTRL_REG5,
+ FIFO_ENABLE, set);
+ if (res < 0) {
+ E("%s : fifo_hw switch to:0x%02x failed\n", __func__, set);
+ return res;
+ }
+ gyro->resume_state[RES_CTRL_REG5] =
+ ((FIFO_ENABLE & set) |
+ (~FIFO_ENABLE & gyro->resume_state[RES_CTRL_REG5]));
+ I("%s : fifo_hw_enable set to:0x%02x\n", __func__, set);
+ return res;
+}
+
+static int r3gd20_manage_int2settings(struct r3gd20_data *gyro,
+ u8 fifomode)
+{
+ int res;
+ u8 buf[2];
+ bool enable_fifo_hw;
+ bool recognized_mode = false;
+ u8 int2bits = I2_NONE;
+
+
+ switch (fifomode) {
+ case FIFO_MODE_FIFO:
+
+ recognized_mode = true;
+ int2bits = (I2_WTM | I2_OVRUN);
+ res = r3gd20_register_update(gyro, buf, CTRL_REG3,
+ I2_MASK, int2bits);
+ if (res < 0) {
+ E("%s : failed to update CTRL_REG3:0x%02x\n",
+ __func__, fifomode);
+ goto err_mutex_unlock;
+ }
+ gyro->resume_state[RES_CTRL_REG3] =
+ ((I2_MASK & int2bits) |
+ (~(I2_MASK) & gyro->resume_state[RES_CTRL_REG3]));
+ enable_fifo_hw = true;
+ break;
+
+ case FIFO_MODE_BYPASS:
+ recognized_mode = true;
+
+ if (gyro->polling_enabled)
+ int2bits = I2_NONE;
+ else
+ int2bits = I2_DRDY;
+ res = r3gd20_register_update(gyro, buf, CTRL_REG3,
+ I2_MASK, int2bits);
+ if (res < 0) {
+ E("%s : failed to update to CTRL_REG3:0x%02x\n",
+ __func__, fifomode);
+ goto err_mutex_unlock;
+ }
+ gyro->resume_state[RES_CTRL_REG3] =
+ ((I2_MASK & int2bits) |
+ (~I2_MASK & gyro->resume_state[RES_CTRL_REG3]));
+ enable_fifo_hw = false;
+ break;
+ default:
+ recognized_mode = false;
+ res = r3gd20_register_update(gyro, buf, CTRL_REG3,
+ I2_MASK, I2_NONE);
+ if (res < 0) {
+ E("%s : failed to update CTRL_REG3:0x%02x\n",
+ __func__, fifomode);
+ goto err_mutex_unlock;
+ }
+ enable_fifo_hw = false;
+ gyro->resume_state[RES_CTRL_REG3] =
+ ((I2_MASK & 0x00) |
+ (~I2_MASK & gyro->resume_state[RES_CTRL_REG3]));
+ break;
+
+ }
+ if (recognized_mode) {
+ res = r3gd20_update_fifomode(gyro, fifomode);
+ if (res < 0) {
+ E("%s : failed to set fifoMode\n", __func__);
+ goto err_mutex_unlock;
+ }
+ }
+ res = r3gd20_fifo_hwenable(gyro, enable_fifo_hw);
+
+err_mutex_unlock:
+
+ return res;
+}
+
+#endif
+static int r3gd20_update_fs_range(struct r3gd20_data *gyro,
+ u8 new_fs)
+{
+ int res ;
+ u8 buf[2];
+
+ buf[0] = CTRL_REG4;
+
+ res = r3gd20_register_update(gyro, buf, CTRL_REG4,
+ FS_MASK, new_fs);
+
+ if (res < 0) {
+ E("%s : failed to update fs:0x%02x\n",
+ __func__, new_fs);
+ return res;
+ }
+ gyro->resume_state[RES_CTRL_REG4] =
+ ((FS_MASK & new_fs) |
+ (~FS_MASK & gyro->resume_state[RES_CTRL_REG4]));
+
+ return res;
+}
+
+
+static int r3gd20_update_odr(struct r3gd20_data *gyro,
+ unsigned int poll_interval_ms)
+{
+ int err = -1;
+ int i;
+ u8 config[2];
+
+ for (i = ARRAY_SIZE(odr_table) - 1; i >= 0; i--) {
+ if ((odr_table[i].poll_rate_ms <= poll_interval_ms) || (i == 0))
+ break;
+ }
+
+ config[1] = odr_table[i].mask;
+ config[1] |= (ENABLE_ALL_AXES + PM_NORMAL);
+
+ if (atomic_read(&gyro->enabled)) {
+ config[0] = CTRL_REG1;
+ err = r3gd20_i2c_write(gyro, config, 1);
+ if (err < 0)
+ return err;
+ gyro->resume_state[RES_CTRL_REG1] = config[1];
+ }
+
+
+ return err;
+}
+
+static int r3gd20_get_data(struct r3gd20_data *gyro,
+ struct r3gd20_triple *data)
+{
+ int err;
+ unsigned char gyro_out[6];
+
+ s16 hw_d[3] = { 0 };
+
+ gyro_out[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+
+ err = r3gd20_i2c_read(gyro, gyro_out, 6);
+
+ if (err < 0)
+ return err;
+
+ hw_d[0] = (s16) (((gyro_out[1]) << 8) | gyro_out[0]);
+ hw_d[1] = (s16) (((gyro_out[3]) << 8) | gyro_out[2]);
+ hw_d[2] = (s16) (((gyro_out[5]) << 8) | gyro_out[4]);
+
+ data->x = ((gyro->pdata->negate_x) ? (-hw_d[gyro->pdata->axis_map_x])
+ : (hw_d[gyro->pdata->axis_map_x]));
+ data->y = ((gyro->pdata->negate_y) ? (-hw_d[gyro->pdata->axis_map_y])
+ : (hw_d[gyro->pdata->axis_map_y]));
+ data->z = ((gyro->pdata->negate_z) ? (-hw_d[gyro->pdata->axis_map_z])
+ : (hw_d[gyro->pdata->axis_map_z]));
+
+ DIF("gyro_out: x = %d, y = %d, z = %d\n",
+ data->x, data->y, data->z);
+
+ return err;
+}
+
+static void r3gd20_report_values(struct r3gd20_data *gyr,
+ struct r3gd20_triple *data)
+{
+ struct input_dev *input = gyr->input_poll_dev->input;
+
+#ifdef HTC_WQ
+ input = g_gyro->gyro_input_dev;
+#endif
+
+ input_report_abs(input, ABS_X, data->x);
+ input_report_abs(input, ABS_Y, data->y);
+ input_report_abs(input, ABS_Z, data->z);
+ input_sync(input);
+}
+
+#ifdef HTC_WQ
+static void polling_do_work(struct work_struct *w)
+{
+ struct r3gd20_data *gyro = g_gyro;
+ struct r3gd20_triple data_out;
+ int err;
+
+ mutex_lock(&gyro->lock);
+ err = r3gd20_get_data(gyro, &data_out);
+ if (err < 0)
+ dev_err(&gyro->client->dev, "get_gyroscope_data failed\n");
+ else
+ r3gd20_report_values(gyro, &data_out);
+
+ mutex_unlock(&gyro->lock);
+
+ DIF("interval = %d\n", gyro->input_poll_dev->
+ poll_interval);
+
+ queue_delayed_work(gyro->gyro_wq, &polling_work,
+ msecs_to_jiffies(gyro->input_poll_dev->
+ poll_interval));
+}
+#endif
+
+static int r3gd20_hw_init(struct r3gd20_data *gyro)
+{
+ int err;
+ u8 buf[6];
+
+ I("%s hw init\n", R3GD20_GYR_DEV_NAME);
+
+ buf[0] = (I2C_AUTO_INCREMENT | CTRL_REG1);
+ buf[1] = gyro->resume_state[RES_CTRL_REG1];
+ buf[2] = gyro->resume_state[RES_CTRL_REG2];
+ buf[3] = gyro->resume_state[RES_CTRL_REG3];
+ buf[4] = gyro->resume_state[RES_CTRL_REG4];
+ buf[5] = gyro->resume_state[RES_CTRL_REG5];
+
+ err = r3gd20_i2c_write(gyro, buf, 5);
+ if (err < 0)
+ return err;
+
+ buf[0] = FIFO_CTRL_REG;
+ buf[1] = gyro->resume_state[RES_FIFO_CTRL_REG];
+ err = r3gd20_i2c_write(gyro, buf, 1);
+ if (err < 0)
+ return err;
+
+ gyro->hw_initialized = 1;
+
+ return err;
+}
+
+static void r3gd20_device_power_off(struct r3gd20_data *dev_data)
+{
+ int err;
+ u8 buf[2];
+
+ I("%s:\n", __func__);
+
+ buf[0] = CTRL_REG1;
+ buf[1] = PM_OFF;
+ err = r3gd20_i2c_write(dev_data, buf, 1);
+ if (err < 0)
+ dev_err(&dev_data->client->dev, "soft power off failed\n");
+
+ if (dev_data->pdata->power_off) {
+
+ disable_irq_nosync(dev_data->irq2);
+ dev_data->pdata->power_off();
+ dev_data->hw_initialized = 0;
+ }
+
+ if (dev_data->hw_initialized) {
+
+
+ if (dev_data->pdata->gpio_int2 > 0) {
+ disable_irq_nosync(dev_data->irq2);
+ I("%s: power off: irq2 disabled\n",
+ R3GD20_GYR_DEV_NAME);
+ }
+ dev_data->hw_initialized = 0;
+ }
+}
+
+static int r3gd20_device_power_on(struct r3gd20_data *dev_data)
+{
+ int err;
+
+ if (dev_data->pdata->power_on) {
+ err = dev_data->pdata->power_on();
+ if (err < 0)
+ return err;
+ if (dev_data->pdata->gpio_int2 > 0)
+ enable_irq(dev_data->irq2);
+ }
+
+
+ if (!dev_data->hw_initialized) {
+ err = r3gd20_hw_init(dev_data);
+ if (err < 0) {
+ r3gd20_device_power_off(dev_data);
+ return err;
+ }
+ }
+
+ if (dev_data->hw_initialized) {
+ D("dev_data->pdata->gpio_int2 = %d\n", dev_data->pdata->gpio_int2);
+ if (dev_data->pdata->gpio_int2 > 0) {
+ enable_irq(dev_data->irq2);
+ I("%s: power on: irq2 enabled\n",
+ R3GD20_GYR_DEV_NAME);
+ }
+ }
+
+ return 0;
+}
+
+static int r3gd20_enable(struct r3gd20_data *dev_data)
+{
+ int err;
+
+ D("%s: enabled = %d\n", __func__, atomic_read(&dev_data->enabled));
+
+ if (!atomic_cmpxchg(&dev_data->enabled, 0, 1)) {
+ if (dev_data->pdata->power_LPM)
+ dev_data->pdata->power_LPM(0);
+
+ err = r3gd20_device_power_on(dev_data);
+ if (err < 0) {
+ atomic_set(&dev_data->enabled, 0);
+ return err;
+ }
+#ifdef HTC_WQ
+ D("Manually queue work!! HW_WAKE_UP_TIME = %d\n",
+ HW_WAKE_UP_TIME);
+ queue_delayed_work(dev_data->gyro_wq, &polling_work,
+ msecs_to_jiffies(HW_WAKE_UP_TIME));
+#else
+ D("%s: queue work: interval = %d\n",
+ __func__, dev_data->input_poll_dev->poll_interval);
+ schedule_delayed_work(&dev_data->input_poll_dev->work,
+ msecs_to_jiffies(dev_data->input_poll_dev->poll_interval));
+#endif
+ }
+
+ return 0;
+}
+
+static int r3gd20_disable(struct r3gd20_data *dev_data)
+{
+ DIF("%s: dev_data->enabled = %d\n", __func__,
+ atomic_read(&dev_data->enabled));
+
+ if (atomic_cmpxchg(&dev_data->enabled, 1, 0))
+ r3gd20_device_power_off(dev_data);
+
+ I("%s: polling disabled\n", __func__);
+ DIF("%s: dev_data->enabled = %d\n", __func__,
+ atomic_read(&dev_data->enabled));
+
+ if (dev_data->pdata->power_LPM)
+ dev_data->pdata->power_LPM(1);
+
+#ifdef HTC_WQ
+ cancel_delayed_work_sync(&polling_work);
+#else
+ cancel_delayed_work_sync(&dev_data->input_poll_dev->work);
+#endif
+
+ return 0;
+}
+
+static ssize_t attr_polling_rate_show(struct device *dev,
+ struct device_attribute *attr,
+ char *buf)
+{
+ int val;
+
+ struct r3gd20_data *gyro = g_gyro;
+
+ mutex_lock(&gyro->lock);
+ val = gyro->input_poll_dev->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 r3gd20_data *gyro = g_gyro;
+ unsigned long interval_ms;
+
+ if (strict_strtoul(buf, 10, &interval_ms))
+ return -EINVAL;
+ if (!interval_ms)
+ return -EINVAL;
+ interval_ms = max((unsigned int)interval_ms, gyro->pdata->min_interval);
+ mutex_lock(&gyro->lock);
+ gyro->input_poll_dev->poll_interval = interval_ms;
+ gyro->pdata->poll_interval = interval_ms;
+ D("r3gd20: %s: gyro->input_poll_dev->poll_interval = %d\n", __func__, gyro->input_poll_dev->poll_interval);
+ r3gd20_update_odr(gyro, interval_ms);
+ mutex_unlock(&gyro->lock);
+ return size;
+}
+
+static ssize_t attr_range_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ int range = 0;
+ u8 val;
+ mutex_lock(&gyro->lock);
+ val = gyro->pdata->fs_range;
+
+ switch (val) {
+ case R3GD20_GYR_FS_250DPS:
+ range = 250;
+ break;
+ case R3GD20_GYR_FS_500DPS:
+ range = 500;
+ break;
+ case R3GD20_GYR_FS_2000DPS:
+ 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 r3gd20_data *gyro = g_gyro;
+ unsigned long val;
+ if (strict_strtoul(buf, 10, &val))
+ return -EINVAL;
+ mutex_lock(&gyro->lock);
+ gyro->pdata->fs_range = val;
+ r3gd20_update_fs_range(gyro, val);
+ mutex_unlock(&gyro->lock);
+ return size;
+}
+
+static ssize_t attr_enable_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ int val = atomic_read(&gyro->enabled);
+
+ 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 r3gd20_data *gyro = g_gyro;
+ unsigned long val;
+
+ DIF("%s: buf = %s", __func__, buf);
+
+ if (strict_strtoul(buf, 10, &val))
+ return -EINVAL;
+
+ if (val)
+ r3gd20_enable(gyro);
+ else
+ r3gd20_disable(gyro);
+
+ return size;
+}
+
+static ssize_t attr_polling_mode_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+ int val = 0;
+
+ struct r3gd20_data *gyro = g_gyro;
+
+ mutex_lock(&gyro->lock);
+ if (gyro->polling_enabled)
+ val = 1;
+ mutex_unlock(&gyro->lock);
+ return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_polling_mode_store(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t size)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ unsigned long val;
+
+ if (strict_strtoul(buf, 10, &val))
+ return -EINVAL;
+
+ mutex_lock(&gyro->lock);
+ if (val) {
+ gyro->polling_enabled = true;
+ r3gd20_manage_int2settings(gyro, FIFO_MODE_BYPASS);
+ if (gyro->polling_enabled) {
+ D("polling enabled\n");
+#ifdef HTC_WQ
+ queue_delayed_work(gyro->gyro_wq, &polling_work,
+ msecs_to_jiffies(gyro->input_poll_dev->
+ poll_interval));
+#else
+ schedule_delayed_work(&gyro->input_poll_dev->work,
+ msecs_to_jiffies(gyro->
+ pdata->poll_interval));
+#endif
+ }
+ } else {
+ if (gyro->polling_enabled) {
+ D("polling disabled\n");
+#ifdef HTC_WQ
+ cancel_delayed_work_sync(&polling_work);
+#else
+ cancel_delayed_work_sync(&gyro->input_poll_dev->work);
+#endif
+ }
+ gyro->polling_enabled = false;
+ r3gd20_manage_int2settings(gyro, gyro->pdata->fifomode);
+ }
+ mutex_unlock(&gyro->lock);
+ return size;
+}
+
+static ssize_t attr_watermark_store(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t size)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ unsigned long watermark;
+ int res;
+
+ if (strict_strtoul(buf, 16, &watermark))
+ return -EINVAL;
+
+ res = r3gd20_update_watermark(gyro, watermark);
+ if (res < 0)
+ return res;
+
+ return size;
+}
+
+static ssize_t attr_watermark_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ int val = gyro->pdata->watermark;
+ return sprintf(buf, "0x%02x\n", val);
+}
+
+static ssize_t attr_fifomode_store(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t size)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ unsigned long fifomode;
+ int res;
+
+ if (strict_strtoul(buf, 16, &fifomode))
+ return -EINVAL;
+
+ D("%s, got value:0x%02x\n", __func__, (u8)fifomode);
+
+ mutex_lock(&gyro->lock);
+ res = r3gd20_manage_int2settings(gyro, (u8) fifomode);
+ mutex_unlock(&gyro->lock);
+
+ if (res < 0)
+ return res;
+ return size;
+}
+
+static ssize_t attr_fifomode_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ u8 val = gyro->pdata->fifomode;
+ return sprintf(buf, "0x%02x\n", val);
+}
+
+#ifdef DEBUG
+static ssize_t attr_reg_set(struct device *dev, struct device_attribute *attr,
+ const char *buf, size_t size)
+{
+ int rc;
+
+ struct r3gd20_data *gyro = g_gyro;
+ u8 x[2];
+ unsigned long val;
+
+ if (strict_strtoul(buf, 16, &val))
+ return -EINVAL;
+ mutex_lock(&gyro->lock);
+ x[0] = gyro->reg_addr;
+ mutex_unlock(&gyro->lock);
+ x[1] = val;
+ rc = r3gd20_i2c_write(gyro, x, 1);
+ return size;
+}
+
+static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr,
+ char *buf)
+{
+ ssize_t ret;
+
+ struct r3gd20_data *gyro = g_gyro;
+ int rc;
+ u8 data;
+
+ mutex_lock(&gyro->lock);
+ data = gyro->reg_addr;
+ mutex_unlock(&gyro->lock);
+ rc = r3gd20_i2c_read(gyro, &data, 1);
+ ret = sprintf(buf, "0x%02x\n", data);
+ return ret;
+}
+
+static ssize_t attr_addr_set(struct device *dev, struct device_attribute *attr,
+ const char *buf, size_t size)
+{
+
+ struct r3gd20_data *gyro = g_gyro;
+ unsigned long val;
+
+ if (strict_strtoul(buf, 16, &val))
+ return -EINVAL;
+
+ mutex_lock(&gyro->lock);
+
+ gyro->reg_addr = val;
+
+ mutex_unlock(&gyro->lock);
+
+ return size;
+}
+#endif
+
+static ssize_t attr_debug_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+ char *s = buf;
+
+ s += sprintf(s, "debug_flag = %d\n", debug_flag);
+
+ return s - buf;
+}
+
+static ssize_t attr_debug_store(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t count)
+{
+ debug_flag = -1;
+ sscanf(buf, "%d", &debug_flag);
+
+ D("%s: debug_flag = %d\n", __func__, debug_flag);
+
+ return count;
+}
+
+static ssize_t attr_cali_data_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+ char *s = buf;
+ struct r3gd20_data *gyro = g_gyro;
+
+ s += sprintf(s, "Stored calibration data (x, y, z) = (%d, %d, %d)\n",
+ gyro->cali_data_x, gyro->cali_data_y,
+ gyro->cali_data_z);
+
+ D("%s: Calibration data (x, y, z) = (%d, %d, %d)\n",
+ __func__, gyro->cali_data_x, gyro->cali_data_y,
+ gyro->cali_data_z);
+ return s - buf;
+}
+
+static int is_valid_cali(int cali_data)
+{
+ if ((cali_data < TOLERENCE) && (cali_data > -TOLERENCE))
+ return 1;
+ else
+ return 0;
+}
+
+static ssize_t attr_cali_data_store(struct device *dev,
+ struct device_attribute *attr,
+ const char *buf, size_t count)
+{
+ struct r3gd20_data *gyro = g_gyro;
+
+ D("%s: \n", __func__);
+
+ if(sscanf(buf, "%d %d %d", &(gyro->cali_data_x), &(gyro->cali_data_y),
+ &(gyro->cali_data_z)) != 3) {
+ E("%s: input format error!\n", __func__);
+ return count;
+ }
+
+ if (!is_valid_cali(gyro->cali_data_x) ||
+ !is_valid_cali(gyro->cali_data_y) ||
+ !is_valid_cali(gyro->cali_data_z)) {
+ E("%s: Invalid calibration data (x, y, z) = (%d, %d, %d)",
+ __func__, gyro->cali_data_x, gyro->cali_data_y,
+ gyro->cali_data_z);
+ return count;
+ }
+
+ D("%s: Stored calibration data (x, y, z) = (%d, %d, %d)\n",
+ __func__, gyro->cali_data_x, gyro->cali_data_y,
+ gyro->cali_data_z);
+
+ return count;
+}
+
+static struct device_attribute attributes[] = {
+ __ATTR(pollrate_ms, 0664, attr_polling_rate_show,
+ attr_polling_rate_store),
+ __ATTR(range, 0664, attr_range_show, attr_range_store),
+ __ATTR(enable_device, 0664, attr_enable_show, attr_enable_store),
+ __ATTR(enable_polling, 0664, attr_polling_mode_show, attr_polling_mode_store),
+ __ATTR(fifo_samples, 0664, attr_watermark_show, attr_watermark_store),
+ __ATTR(fifo_mode, 0664, attr_fifomode_show, attr_fifomode_store),
+ __ATTR(cali_data, 0664, attr_cali_data_show, attr_cali_data_store),
+#ifdef DEBUG
+ __ATTR(reg_value, 0664, attr_reg_get, attr_reg_set),
+ __ATTR(reg_addr, 0664, NULL, attr_addr_set),
+ __ATTR(debug, 0664, attr_debug_show, attr_debug_store),
+#endif
+};
+
+static int create_sysfs_interfaces(struct device *dev)
+{
+ int i;
+
+#ifdef HTC_ATTR
+ struct r3gd20_data *gyro = g_gyro;
+ int ret = 0;
+
+ if (gyro == NULL) {
+ E("%s: g_gyro == NULL!!\n", __func__);
+ return -2;
+ }
+
+ gyro->htc_gyro_class = class_create(THIS_MODULE, "htc_gyro");
+ if (IS_ERR(gyro->htc_gyro_class)) {
+ ret = PTR_ERR(gyro->htc_gyro_class);
+ gyro->htc_gyro_class = NULL;
+ E("%s: could not allocate gyro->htc_gyro_class\n", __func__);
+ goto err_create_class;
+ }
+
+ gyro->gyro_dev = device_create(gyro->htc_gyro_class,
+ NULL, 0, "%s", "gyro");
+ if (unlikely(IS_ERR(gyro->gyro_dev))) {
+ ret = PTR_ERR(gyro->gyro_dev);
+ gyro->gyro_dev = NULL;
+ E("%s: could not allocate gyro->gyro_dev\n", __func__);
+ goto err_create_gyro_device;
+ }
+
+ for (i = 0; i < ARRAY_SIZE(attributes); i++)
+ if (device_create_file(gyro->gyro_dev, attributes + i)) {
+ E("%s: could not allocate attribute files\n",
+ __func__);
+ goto err_create_device_file;
+ }
+ return 0;
+
+err_create_device_file:
+ device_unregister(gyro->gyro_dev);
+err_create_gyro_device:
+ class_destroy(gyro->htc_gyro_class);
+err_create_class:
+ return ret;
+
+#else
+
+ for (i = 0; i < ARRAY_SIZE(attributes); i++)
+ if (device_create_file(dev, attributes + i))
+ goto error;
+ return 0;
+
+error:
+ for ( ; i >= 0; i--)
+ device_remove_file(dev, attributes + i);
+ dev_err(dev, "%s:Unable to create interface\n", __func__);
+ return -1;
+
+#endif
+
+}
+
+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;
+}
+
+static void report_triple(struct r3gd20_data *gyro)
+{
+ int err;
+ struct r3gd20_triple data_out;
+
+ err = r3gd20_get_data(gyro, &data_out);
+ if (err < 0)
+ dev_err(&gyro->client->dev, "get_gyroscope_data failed\n");
+ else
+ r3gd20_report_values(gyro, &data_out);
+}
+
+static void r3gd20_input_poll_func(struct input_polled_dev *dev)
+{
+ struct r3gd20_data *gyro = dev->private;
+
+ struct r3gd20_triple data_out;
+
+ int err;
+
+ DIF("%s: gyro->enabled = %d\n",
+ __func__, atomic_read(&gyro->enabled));
+ if (atomic_read(&gyro->enabled) == 0)
+ return;
+
+
+
+
+ mutex_lock(&gyro->lock);
+ err = r3gd20_get_data(gyro, &data_out);
+ if (err < 0)
+ dev_err(&gyro->client->dev, "get_gyroscope_data failed\n");
+ else
+ r3gd20_report_values(gyro, &data_out);
+
+ mutex_unlock(&gyro->lock);
+
+}
+
+static void r3gd20_irq2_fifo(struct r3gd20_data *gyro)
+{
+ int err;
+ u8 buf[2];
+ u8 int_source;
+ u8 samples;
+ u8 workingmode;
+ u8 stored_samples;
+
+ mutex_lock(&gyro->lock);
+
+ workingmode = gyro->pdata->fifomode;
+
+
+
+
+
+ switch (workingmode) {
+ case FIFO_MODE_BYPASS:
+ {
+ report_triple(gyro);
+ break;
+ }
+ case FIFO_MODE_FIFO:
+ samples = (gyro->pdata->watermark)+1;
+
+ err = r3gd20_register_read(gyro, buf, FIFO_SRC_REG);
+ if (err > 0)
+ dev_err(&gyro->client->dev, "error reading fifo source reg\n");
+
+ int_source = buf[0];
+
+
+ stored_samples = int_source & FIFO_STORED_DATA_MASK;
+
+
+ for (; samples > 0; samples--) {
+#if DEBUG
+ input_report_abs(gyro->input_poll_dev->input, ABS_MISC, 1);
+ input_sync(gyro->input_poll_dev->input);
+#endif
+
+ report_triple(gyro);
+
+#if DEBUG
+ input_report_abs(gyro->input_poll_dev->input, ABS_MISC, 0);
+ input_sync(gyro->input_poll_dev->input);
+#endif
+ }
+ r3gd20_fifo_reset(gyro);
+ break;
+ }
+#if DEBUG
+ input_report_abs(gyro->input_poll_dev->input, ABS_MISC, 3);
+ input_sync(gyro->input_poll_dev->input);
+#endif
+
+ mutex_unlock(&gyro->lock);
+}
+
+static irqreturn_t r3gd20_isr2(int irq, void *dev)
+{
+ struct r3gd20_data *gyro = dev;
+
+ disable_irq_nosync(irq);
+#if DEBUG
+ input_report_abs(gyro->input_poll_dev->input, ABS_MISC, 2);
+ input_sync(gyro->input_poll_dev->input);
+#endif
+ queue_work(gyro->irq2_work_queue, &gyro->irq2_work);
+ I("%s: isr2 queued\n", R3GD20_GYR_DEV_NAME);
+
+ return IRQ_HANDLED;
+}
+
+static void r3gd20_irq2_work_func(struct work_struct *work)
+{
+
+ struct r3gd20_data *gyro =
+ container_of(work, struct r3gd20_data, irq2_work);
+ r3gd20_irq2_fifo(gyro);
+
+ I("%s: IRQ2 served\n", R3GD20_GYR_DEV_NAME);
+ enable_irq(gyro->irq2);
+}
+
+
+int r3gd20_input_open(struct input_dev *input)
+{
+ struct r3gd20_data *gyro = input_get_drvdata(input);
+
+ DIF("%s:\n", __func__);
+
+
+ return 0;
+
+ return r3gd20_enable(gyro);
+}
+
+void r3gd20_input_close(struct input_dev *dev)
+{
+ struct r3gd20_data *gyro = input_get_drvdata(dev);
+
+ r3gd20_disable(gyro);
+}
+
+static int r3gd20_validate_pdata(struct r3gd20_data *gyro)
+{
+
+ gyro->pdata->min_interval =
+ max((unsigned int) R3GD20_MIN_POLL_PERIOD_MS,
+ gyro->pdata->min_interval);
+
+ gyro->pdata->poll_interval = max(gyro->pdata->poll_interval,
+ gyro->pdata->min_interval);
+
+ 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;
+ }
+
+
+ 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;
+ }
+
+
+ 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 r3gd20_input_init(struct r3gd20_data *gyro)
+{
+ int err = -1;
+ struct input_dev *input;
+
+
+
+ gyro->input_poll_dev = input_allocate_polled_device();
+ if (!gyro->input_poll_dev) {
+ err = -ENOMEM;
+ dev_err(&gyro->client->dev,
+ "input device allocation failed\n");
+ goto err0;
+ }
+
+ gyro->input_poll_dev->private = gyro;
+ gyro->input_poll_dev->poll = r3gd20_input_poll_func;
+ gyro->input_poll_dev->poll_interval = gyro->pdata->poll_interval;
+
+#ifdef HTC_WQ
+ input = input_allocate_device();
+ if (!input) {
+ E("%s: could not allocate ls input device\n", __func__);
+ return -ENOMEM;
+ }
+ gyro->gyro_input_dev = input;
+#else
+ input = gyro->input_poll_dev->input;
+#endif
+
+ input->open = r3gd20_input_open;
+ input->close = r3gd20_input_close;
+
+ input->id.bustype = BUS_I2C;
+ input->dev.parent = &gyro->client->dev;
+
+#ifdef HTC_WQ
+ input_set_drvdata(input, gyro);
+#else
+ input_set_drvdata(gyro->input_poll_dev->input, gyro);
+#endif
+
+ set_bit(EV_ABS, input->evbit);
+
+#if DEBUG
+ set_bit(EV_KEY, input->keybit);
+ set_bit(KEY_LEFT, input->keybit);
+ input_set_abs_params(input, ABS_MISC, 0, 1, 0, 0);
+#endif
+
+ input_set_abs_params(input, ABS_X, -FS_MAX, FS_MAX, FUZZ, FLAT);
+ input_set_abs_params(input, ABS_Y, -FS_MAX, FS_MAX, FUZZ, FLAT);
+ input_set_abs_params(input, ABS_Z, -FS_MAX, FS_MAX, FUZZ, FLAT);
+
+ input->name = R3GD20_GYR_DEV_NAME;
+
+#ifdef HTC_WQ
+ err = input_register_device(input);
+#else
+ err = input_register_polled_device(gyro->input_poll_dev);
+#endif
+ if (err) {
+ dev_err(&gyro->client->dev,
+ "unable to register input polled device %s\n",
+ gyro->input_poll_dev->input->name);
+ goto err1;
+ }
+
+ return 0;
+
+err1:
+ input_free_polled_device(gyro->input_poll_dev);
+err0:
+ return err;
+}
+
+static void r3gd20_input_cleanup(struct r3gd20_data *gyro)
+{
+ input_unregister_polled_device(gyro->input_poll_dev);
+ input_free_polled_device(gyro->input_poll_dev);
+}
+
+static int to_signed_int(char *value)
+{
+ int ret_int = 0;
+
+ if (value == NULL)
+ ret_int = 0;
+ else {
+ ret_int = value[0] | (value[1] << 8) |
+ (value[2] << 16) |
+ (value[3] << 24);
+ }
+
+ return ret_int;
+}
+
+static int r3gd20_probe(struct i2c_client *client,
+ const struct i2c_device_id *devid)
+{
+ struct r3gd20_data *gyro;
+
+ u32 smbus_func = I2C_FUNC_SMBUS_BYTE_DATA |
+ I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_I2C_BLOCK ;
+
+ int err = -1;
+
+ I("%s: probe start v03.\n", R3GD20_GYR_DEV_NAME);
+
+
+ if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+ dev_warn(&client->dev, "client not i2c capable\n");
+ if (i2c_check_functionality(client->adapter, smbus_func)) {
+ use_smbus = 1;
+ dev_warn(&client->dev, "client using SMBUS\n");
+ } else {
+ err = -ENODEV;
+ dev_err(&client->dev, "client nor SMBUS capable\n");
+ goto err0;
+ }
+ }
+
+
+
+ gyro = kzalloc(sizeof(*gyro), GFP_KERNEL);
+ if (gyro == NULL) {
+ dev_err(&client->dev,
+ "failed to allocate memory for module data\n");
+ err = -ENOMEM;
+ goto err0;
+ }
+
+ g_gyro = gyro;
+
+ mutex_init(&gyro->lock);
+ mutex_lock(&gyro->lock);
+ gyro->client = client;
+
+ gyro->pdata = kmalloc(sizeof(*gyro->pdata), GFP_KERNEL);
+ if (gyro->pdata == NULL) {
+ dev_err(&client->dev,
+ "failed to allocate memory for pdata: %d\n", err);
+ goto err1;
+ }
+
+ if (client->dev.platform_data == NULL) {
+ memcpy(gyro->pdata, &default_r3gd20_gyr_pdata,
+ sizeof(*gyro->pdata));
+ } else {
+ memcpy(gyro->pdata, client->dev.platform_data,
+ sizeof(*gyro->pdata));
+ }
+
+ err = r3gd20_validate_pdata(gyro);
+ if (err < 0) {
+ dev_err(&client->dev, "failed to validate platform data\n");
+ goto err1_1;
+ }
+
+ i2c_set_clientdata(client, gyro);
+
+ if (gyro->pdata->init) {
+ err = gyro->pdata->init();
+ if (err < 0) {
+ dev_err(&client->dev, "init failed: %d\n", err);
+ goto err1_1;
+ }
+ }
+
+
+ memset(gyro->resume_state, 0, ARRAY_SIZE(gyro->resume_state));
+
+ gyro->resume_state[RES_CTRL_REG1] = ALL_ZEROES | ENABLE_ALL_AXES;
+ gyro->resume_state[RES_CTRL_REG2] = ALL_ZEROES;
+ gyro->resume_state[RES_CTRL_REG3] = ALL_ZEROES;
+ gyro->resume_state[RES_CTRL_REG4] = ALL_ZEROES | BDU_ENABLE;
+ gyro->resume_state[RES_CTRL_REG5] = ALL_ZEROES;
+ gyro->resume_state[RES_FIFO_CTRL_REG] = ALL_ZEROES;
+
+ gyro->polling_enabled = true;
+
+ err = r3gd20_device_power_on(gyro);
+ if (err < 0) {
+ dev_err(&client->dev, "power on failed: %d\n", err);
+ goto err2;
+ }
+
+ atomic_set(&gyro->enabled, 1);
+
+ err = r3gd20_update_fs_range(gyro, gyro->pdata->fs_range);
+ if (err < 0) {
+ dev_err(&client->dev, "update_fs_range failed\n");
+ goto err2;
+ }
+
+ err = r3gd20_update_odr(gyro, gyro->pdata->poll_interval);
+ if (err < 0) {
+ dev_err(&client->dev, "update_odr failed\n");
+ goto err2;
+ }
+
+ err = r3gd20_input_init(gyro);
+ if (err < 0)
+ goto err3;
+
+ err = create_sysfs_interfaces(&client->dev);
+ if (err < 0) {
+ dev_err(&client->dev,
+ "%s device register failed\n", R3GD20_GYR_DEV_NAME);
+ goto err4;
+ }
+
+ r3gd20_device_power_off(gyro);
+
+
+ atomic_set(&gyro->enabled, 0);
+
+
+ if (gyro->pdata->gpio_int2 > 0) {
+ gyro->irq2 = gpio_to_irq(gyro->pdata->gpio_int2);
+ I("%s: %s has set irq2 to irq:"
+ " %d mapped on gpio:%d\n",
+ R3GD20_GYR_DEV_NAME, __func__, gyro->irq2,
+ gyro->pdata->gpio_int2);
+
+ INIT_WORK(&gyro->irq2_work, r3gd20_irq2_work_func);
+ gyro->irq2_work_queue =
+ create_singlethread_workqueue("r3gd20_gyr_wq2");
+ if (!gyro->irq2_work_queue) {
+ err = -ENOMEM;
+ dev_err(&client->dev, "cannot create "
+ "work queue2: %d\n", err);
+ goto err5;
+ }
+
+ err = request_irq(gyro->irq2, r3gd20_isr2,
+ IRQF_TRIGGER_HIGH, "r3gd20_gyr_irq2", gyro);
+
+ if (err < 0) {
+ dev_err(&client->dev, "request irq2 failed: %d\n", err);
+ goto err6;
+ }
+ disable_irq_nosync(gyro->irq2);
+ }
+
+ mutex_unlock(&gyro->lock);
+
+ gyro->cali_data_x = to_signed_int(&gyro_gsensor_kvalue[4]);
+ gyro->cali_data_y = to_signed_int(&gyro_gsensor_kvalue[8]);
+ gyro->cali_data_z = to_signed_int(&gyro_gsensor_kvalue[12]);
+ D("%s: Calibration data (x, y, z) = (%d, %d, %d)\n",
+ __func__, gyro->cali_data_x, gyro->cali_data_y,
+ gyro->cali_data_z);
+
+#ifdef HTC_WQ
+ gyro->gyro_wq = create_singlethread_workqueue("gyro_wq");
+ if (!gyro->gyro_wq) {
+ E("%s: can't create workqueue\n", __func__);
+ err = -ENOMEM;
+ goto err_create_singlethread_workqueue;
+ }
+#endif
+ debug_flag = 0;
+
+ I("%s: %s probed: device created successfully\n",
+ __func__, R3GD20_GYR_DEV_NAME);
+
+ return 0;
+
+
+#ifdef HTC_WQ
+err_create_singlethread_workqueue:
+#endif
+err6:
+ destroy_workqueue(gyro->irq2_work_queue);
+err5:
+ r3gd20_device_power_off(gyro);
+ remove_sysfs_interfaces(&client->dev);
+err4:
+ r3gd20_input_cleanup(gyro);
+err3:
+ r3gd20_device_power_off(gyro);
+err2:
+ if (gyro->pdata->exit)
+ gyro->pdata->exit();
+err1_1:
+ mutex_unlock(&gyro->lock);
+ kfree(gyro->pdata);
+err1:
+ kfree(gyro);
+err0:
+ E("%s: Driver Initialization failed\n", R3GD20_GYR_DEV_NAME);
+ return err;
+}
+
+static int r3gd20_remove(struct i2c_client *client)
+{
+ struct r3gd20_data *gyro = i2c_get_clientdata(client);
+#if DEBUG
+ I("R3GD20 driver removing\n");
+#endif
+
+ if (gyro->pdata->gpio_int2 > 0) {
+ free_irq(gyro->irq2, gyro);
+ gpio_free(gyro->pdata->gpio_int2);
+ destroy_workqueue(gyro->irq2_work_queue);
+ }
+
+ r3gd20_input_cleanup(gyro);
+ r3gd20_device_power_off(gyro);
+ remove_sysfs_interfaces(&client->dev);
+
+ kfree(gyro->pdata);
+ kfree(gyro);
+ return 0;
+}
+
+#ifdef HTC_SUSPEND
+
+static int r3gd20_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+#ifdef CONFIG_SUSPEND
+ struct r3gd20_data *data = i2c_get_clientdata(client);
+ u8 buf[2];
+ int err = -1;
+
+ DIF("%s: ++\n", __func__);
+
+#if DEBUG
+ I("r3gd20_suspend\n");
+#endif
+
+
+ mutex_lock(&data->lock);
+ if (data->polling_enabled) {
+ D("polling disabled\n");
+#ifdef HTC_WQ
+ cancel_delayed_work_sync(&polling_work);
+#else
+ cancel_delayed_work_sync(&data->input_poll_dev->work);
+#endif
+
+ }
+
+#ifdef SLEEP
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x0F, (ENABLE_NO_AXES | PM_NORMAL));
+#else
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x08, PM_OFF);
+#endif
+ mutex_unlock(&data->lock);
+
+
+#endif
+ D("%s:--\n", __func__);
+ return err;
+}
+
+static int r3gd20_resume(struct i2c_client *client)
+{
+#ifdef CONFIG_SUSPEND
+ struct r3gd20_data *data = i2c_get_clientdata(client);
+ u8 buf[2];
+ int err = -1;
+
+ D("%s:++\n", __func__);
+#if DEBUG
+ I("r3gd20_resume\n");
+#endif
+
+ if (atomic_read(&data->enabled)) {
+ mutex_lock(&data->lock);
+
+ if (data->polling_enabled) {
+ D("polling enabled\n");
+#ifdef HTC_WQ
+ queue_delayed_work(data->gyro_wq, &polling_work,
+ msecs_to_jiffies(data->input_poll_dev->
+ poll_interval));
+#else
+ schedule_delayed_work(&data->input_poll_dev->work,
+ msecs_to_jiffies(data->
+ pdata->poll_interval));
+#endif
+ }
+#ifdef SLEEP
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x0F, (ENABLE_ALL_AXES | PM_NORMAL));
+#else
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x08, PM_NORMAL);
+#endif
+ mutex_unlock(&data->lock);
+
+ }
+
+#endif
+ D("%s:--\n", __func__);
+ return 0;
+}
+
+#else
+
+static int r3gd20_suspend(struct device *dev)
+{
+#define SLEEP
+#ifdef CONFIG_SUSPEND
+ struct i2c_client *client = to_i2c_client(dev);
+ struct r3gd20_data *data = i2c_get_clientdata(client);
+ u8 buf[2];
+ int err = -1;
+
+ D("%s:\n", __func__);
+
+#if DEBUG
+ I("r3gd20_suspend\n");
+#endif
+ I("%s\n", __func__);
+ if (atomic_read(&data->enabled)) {
+ mutex_lock(&data->lock);
+ if (data->polling_enabled) {
+ D("polling disabled\n");
+#ifdef HTC_WQ
+ cancel_delayed_work_sync(&polling_work);
+#else
+ cancel_delayed_work_sync(&data->input_poll_dev->work);
+#endif
+
+ }
+#ifdef SLEEP
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x0F, (ENABLE_NO_AXES | PM_NORMAL));
+#else
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x08, PM_OFF);
+#endif
+ mutex_unlock(&data->lock);
+ }
+
+#endif
+ return err;
+}
+
+static int r3gd20_resume(struct device *dev)
+{
+#ifdef CONFIG_SUSPEND
+ struct i2c_client *client = to_i2c_client(dev);
+ struct r3gd20_data *data = i2c_get_clientdata(client);
+ u8 buf[2];
+ int err = -1;
+
+ D("%s:\n", __func__);
+#if DEBUG
+ I("r3gd20_resume\n");
+#endif
+ I("%s\n", __func__);
+ if (atomic_read(&data->enabled)) {
+ mutex_lock(&data->lock);
+ if (data->polling_enabled) {
+ D("polling enabled\n");
+#ifdef HTC_WQ
+ queue_delayed_work(data->gyro_wq, &polling_work,
+ msecs_to_jiffies(data->input_poll_dev->
+ poll_interval));
+#else
+ schedule_delayed_work(&data->input_poll_dev->work,
+ msecs_to_jiffies(data->
+ pdata->poll_interval));
+#endif
+ }
+#ifdef SLEEP
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x0F, (ENABLE_ALL_AXES | PM_NORMAL));
+#else
+ err = r3gd20_register_update(data, buf, CTRL_REG1,
+ 0x08, PM_NORMAL);
+#endif
+ mutex_unlock(&data->lock);
+
+ }
+
+#endif
+ return 0;
+}
+
+#endif
+
+static const struct i2c_device_id r3gd20_id[] = {
+ { R3GD20_GYR_DEV_NAME , 0 },
+ {},
+};
+
+MODULE_DEVICE_TABLE(i2c, r3gd20_id);
+
+#ifndef HTC_SUSPEND
+static struct dev_pm_ops r3gd20_pm = {
+ .suspend = r3gd20_suspend,
+ .resume = r3gd20_resume,
+};
+#endif
+
+static struct i2c_driver r3gd20_driver = {
+ .driver = {
+ .owner = THIS_MODULE,
+ .name = R3GD20_GYR_DEV_NAME,
+#ifndef HTC_SUSPEND
+ .pm = &r3gd20_pm,
+#endif
+ },
+ .probe = r3gd20_probe,
+ .remove = __devexit_p(r3gd20_remove),
+ .id_table = r3gd20_id,
+#ifdef HTC_SUSPEND
+ .suspend = r3gd20_suspend,
+ .resume = r3gd20_resume,
+#endif
+
+};
+
+static int __init r3gd20_init(void)
+{
+#if DEBUG
+ I("%s: gyroscope sysfs driver init\n", R3GD20_GYR_DEV_NAME);
+#endif
+ return i2c_add_driver(&r3gd20_driver);
+}
+
+static void __exit r3gd20_exit(void)
+{
+#if DEBUG
+ I("%s exit\n", R3GD20_GYR_DEV_NAME);
+#endif
+ i2c_del_driver(&r3gd20_driver);
+ return;
+}
+
+module_init(r3gd20_init);
+module_exit(r3gd20_exit);
+
+MODULE_DESCRIPTION("r3gd20 digital gyroscope sysfs driver");
+MODULE_AUTHOR("Matteo Dameno, Carmine Iascone, STMicroelectronics");
+MODULE_LICENSE("GPL");
+
diff --git a/include/linux/r3gd20.h b/include/linux/r3gd20.h
new file mode 100644
index 0000000..24d7f8c
--- /dev/null
+++ b/include/linux/r3gd20.h
@@ -0,0 +1,97 @@
+/******************** (C) COPYRIGHT 2011 STMicroelectronics ********************
+*
+* File Name : r3gd20.h
+* 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.5 sysfs
+* Date : 2011/Sep/24
+* Description : R3GD20 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/May/02 | Carmine Iascone | First Release
+* 1.1.3 | 2011/Jun/24 | Matteo Dameno | Corrects ODR Bug
+* 1.1.4 | 2011/Sep/02 | Matteo Dameno | SMB Bus Mng,
+* | | | forces BDU setting
+* 1.1.5 | 2011/Sep/24 | Matteo Dameno | Introduces FIFO Feat.
+* 1.1.5.1 | 2011/Nov/6 | Morris Chen | change name from l3g to r3g
+* | change default FS to 2000DPS
+* | change default poll_rate to 50ms
+* | chage the attribute of sysfs file as 666
+*******************************************************************************/
+
+#ifndef __R3GD20_H__
+#define __R3GD20_H__
+
+
+#define R3GD20_MIN_POLL_PERIOD_MS 2
+
+#define SAD0L 0x00
+#define SAD0H 0x01
+#define R3GD20_GYR_I2C_SADROOT 0x6A
+#define R3GD20_GYR_I2C_SAD_L ((R3GD20_GYR_I2C_SADROOT<<1)|SAD0L)
+#define R3GD20_GYR_I2C_SAD_H ((R3GD20_GYR_I2C_SADROOT<<1)|SAD0H)
+
+#define R3GD20_GYR_DEV_NAME "r3gd20_gyr"
+
+#define R3GD20_GYR_FS_250DPS 0x00
+#define R3GD20_GYR_FS_500DPS 0x10
+#define R3GD20_GYR_FS_2000DPS 0x30
+
+#define R3GD20_GYR_ENABLED 1
+#define R3GD20_GYR_DISABLED 0
+
+extern unsigned char gyro_gsensor_kvalue[37];
+
+#ifdef __KERNEL__
+struct r3gd20_gyr_platform_data {
+ int (*init)(void);
+ void (*exit)(void);
+ int (*power_on)(void);
+ int (*power_off)(void);
+ unsigned int poll_interval;
+ unsigned int min_interval;
+
+ u8 fs_range;
+
+
+ u8 watermark;
+ u8 fifomode;
+
+
+ int gpio_int1;
+ int gpio_int2;
+
+
+ u8 axis_map_x;
+ u8 axis_map_y;
+ u8 axis_map_z;
+
+ u8 negate_x;
+ u8 negate_y;
+ u8 negate_z;
+
+ int (*power_LPM)(int on);
+};
+#endif
+
+#endif