From c8a6f5f07365d323e6c9f6763b569132c6763046 Mon Sep 17 00:00:00 2001 From: lyx Date: Thu, 1 Sep 2011 00:38:17 -0700 Subject: [PATCH] gsensor,gyroscope: add gsensor lis3dh and gyroscope k3g drivers --- drivers/input/Kconfig | 2 + drivers/input/Makefile | 1 + drivers/input/gsensor/Kconfig | 8 + drivers/input/gsensor/Makefile | 1 + drivers/input/gsensor/lis3dh_acc_misc.c | 1505 +++++++++++++++++++++++ drivers/input/gsensor/lis3dh_acc_misc.h | 137 +++ drivers/input/gyroscope/Kconfig | 22 + drivers/input/gyroscope/Makefile | 4 + drivers/input/gyroscope/k3g.c | 703 +++++++++++ 9 files changed, 2383 insertions(+) create mode 100755 drivers/input/gsensor/lis3dh_acc_misc.c create mode 100755 drivers/input/gsensor/lis3dh_acc_misc.h create mode 100755 drivers/input/gyroscope/Kconfig create mode 100755 drivers/input/gyroscope/Makefile create mode 100755 drivers/input/gyroscope/k3g.c diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig index 85b41ff032d7..feac5b7ed311 100755 --- a/drivers/input/Kconfig +++ b/drivers/input/Kconfig @@ -185,6 +185,8 @@ source "drivers/input/magnetometer/Kconfig" source "drivers/input/gsensor/Kconfig" +source "drivers/input/gyroscope/Kconfig" + source "drivers/input/jogball/Kconfig" source "drivers/input/lightsensor/Kconfig" diff --git a/drivers/input/Makefile b/drivers/input/Makefile index f9357d4c0bc7..124cd3de8fdd 100755 --- a/drivers/input/Makefile +++ b/drivers/input/Makefile @@ -22,6 +22,7 @@ obj-$(CONFIG_INPUT_TABLET) += tablet/ obj-$(CONFIG_INPUT_TOUCHSCREEN) += touchscreen/ obj-$(CONFIG_INPUT_MISC) += misc/ obj-$(CONFIG_G_SENSOR_DEVICE) += gsensor/ +obj-$(CONFIG_GYRO_SENSOR_DEVICE) += gyroscope/ obj-$(CONFIG_INPUT_JOGBALL) += jogball/ obj-$(CONFIG_INPUT_APMPOWER) += apm-power.o diff --git a/drivers/input/gsensor/Kconfig b/drivers/input/gsensor/Kconfig index c917cf3a7485..efb19a016b38 100755 --- a/drivers/input/gsensor/Kconfig +++ b/drivers/input/gsensor/Kconfig @@ -37,6 +37,14 @@ config GS_KXTF9 This driver can also be built as a module. If so, the module will be called kxtf9. +config GS_LIS3DH + bool "gs_lis3dh" + depends on G_SENSOR_DEVICE + default n + help + To have support for your specific gsesnor you will have to + select the proper drivers which depend on this option. + config GS_L3G4200D bool "gs_l3g4200d" depends on G_SENSOR_DEVICE diff --git a/drivers/input/gsensor/Makefile b/drivers/input/gsensor/Makefile index b312821ac104..2f7f9c0ad14c 100755 --- a/drivers/input/gsensor/Makefile +++ b/drivers/input/gsensor/Makefile @@ -4,3 +4,4 @@ obj-$(CONFIG_GS_MMA7660) += mma7660.o obj-$(CONFIG_GS_MMA8452) += mma8452.o obj-$(CONFIG_GS_L3G4200D) += l3g4200d.o obj-$(CONFIG_GS_KXTF9) += kxtf9.o +obj-$(CONFIG_GS_LIS3DH) += lis3dh_acc_misc.o \ No newline at end of file diff --git a/drivers/input/gsensor/lis3dh_acc_misc.c b/drivers/input/gsensor/lis3dh_acc_misc.c new file mode 100755 index 000000000000..9805ceb807eb --- /dev/null +++ b/drivers/input/gsensor/lis3dh_acc_misc.c @@ -0,0 +1,1505 @@ +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** + * + * File Name : lis3dh_acc.c + * Authors : MSH - Motion Mems BU - Application Team + * : Carmine Iascone (carmine.iascone@st.com) + * : Matteo Dameno (matteo.dameno@st.com) + * Version : V.1.0.5 + * Date : 16/08/2010 + * Description : LIS3DH accelerometer 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. + * + * THIS SOFTWARE IS SPECIFICALLY DESIGNED FOR EXCLUSIVE USE WITH ST PARTS. + * + ****************************************************************************** + Revision 1.0.0 05/11/09 + First Release + Revision 1.0.3 22/01/2010 + Linux K&R Compliant Release; + Revision 1.0.5 16/08/2010 + modified _get_acceleration_data function + modified _update_odr function + manages 2 interrupts + + ******************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +//#include +#include "lis3dh_acc_misc.h" + + +#define DEBUG 1 + +#define INTERRUPT_MANAGEMENT 1 + +#define G_MAX 16000 /** Maximum polled-device-reported g value */ + +/* +#define SHIFT_ADJ_2G 4 +#define SHIFT_ADJ_4G 3 +#define SHIFT_ADJ_8G 2 +#define SHIFT_ADJ_16G 1 +*/ + +#define SENSITIVITY_2G 1 /** mg/LSB */ +#define SENSITIVITY_4G 2 /** mg/LSB */ +#define SENSITIVITY_8G 4 /** mg/LSB */ +#define SENSITIVITY_16G 12 /** mg/LSB */ + + +#define HIGH_RESOLUTION 0x08 + +#define AXISDATA_REG 0x28 +#define WHOAMI_LIS3DH_ACC 0x33 /* Expctd content for WAI */ + +/* CONTROL REGISTERS */ +#define WHO_AM_I 0x0F /* WhoAmI register */ +#define TEMP_CFG_REG 0x1F /* temper sens control reg */ +/* ctrl 1: ODR3 ODR2 ODR ODR0 LPen Zenable Yenable Zenable */ +#define CTRL_REG1 0x20 /* control reg 1 */ +#define CTRL_REG2 0x21 /* control reg 2 */ +#define CTRL_REG3 0x22 /* control reg 3 */ +#define CTRL_REG4 0x23 /* control reg 4 */ +#define CTRL_REG5 0x24 /* control reg 5 */ +#define CTRL_REG6 0x25 /* control reg 6 */ + +#define FIFO_CTRL_REG 0x2E /* FiFo control reg */ + +#define INT_CFG1 0x30 /* interrupt 1 config */ +#define INT_SRC1 0x31 /* interrupt 1 source */ +#define INT_THS1 0x32 /* interrupt 1 threshold */ +#define INT_DUR1 0x33 /* interrupt 1 duration */ + +#define INT_CFG2 0x34 /* interrupt 2 config */ +#define INT_SRC2 0x35 /* interrupt 2 source */ +#define INT_THS2 0x36 /* interrupt 2 threshold */ +#define INT_DUR2 0x37 /* interrupt 2 duration */ + +#define TT_CFG 0x38 /* tap config */ +#define TT_SRC 0x39 /* tap source */ +#define TT_THS 0x3A /* tap threshold */ +#define TT_LIM 0x3B /* tap time limit */ +#define TT_TLAT 0x3C /* tap time latency */ +#define TT_TW 0x3D /* tap time window */ +/* end CONTROL REGISTRES */ + + +#define ENABLE_HIGH_RESOLUTION 1 + +#define LIS3DH_ACC_PM_OFF 0x00 +#define LIS3DH_ACC_ENABLE_ALL_AXES 0x07 + +#define PMODE_MASK 0x08 +#define ODR_MASK 0XF0 + +#define ODR1 0x10 /* 1Hz output data rate */ +#define ODR10 0x20 /* 10Hz output data rate */ +#define ODR25 0x30 /* 25Hz output data rate */ +#define ODR50 0x40 /* 50Hz output data rate */ +#define ODR100 0x50 /* 100Hz output data rate */ +#define ODR200 0x60 /* 200Hz output data rate */ +#define ODR400 0x70 /* 400Hz output data rate */ +#define ODR1250 0x90 /* 1250Hz output data rate */ + + + +#define IA 0x40 +#define ZH 0x20 +#define ZL 0x10 +#define YH 0x08 +#define YL 0x04 +#define XH 0x02 +#define XL 0x01 +/* */ +/* CTRL REG BITS*/ +#define CTRL_REG3_I1_AOI1 0x40 +#define CTRL_REG6_I2_TAPEN 0x80 +#define CTRL_REG6_HLACTIVE 0x02 +/* */ + +/* TAP_SOURCE_REG BIT */ +#define DTAP 0x20 +#define STAP 0x10 +#define SIGNTAP 0x08 +#define ZTAP 0x04 +#define YTAP 0x02 +#define XTAZ 0x01 + + +#define FUZZ 32 +#define FLAT 32 +#define I2C_RETRY_DELAY 5 +#define I2C_RETRIES 5 +#define I2C_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 RES_CTRL_REG6 5 + +#define RES_INT_CFG1 6 +#define RES_INT_THS1 7 +#define RES_INT_DUR1 8 +#define RES_INT_CFG2 9 +#define RES_INT_THS2 10 +#define RES_INT_DUR2 11 + +#define RES_TT_CFG 12 +#define RES_TT_THS 13 +#define RES_TT_LIM 14 +#define RES_TT_TLAT 15 +#define RES_TT_TW 16 + +#define RES_TEMP_CFG_REG 17 +#define RES_REFERENCE_REG 18 +#define RES_FIFO_CTRL_REG 19 + +#define RESUME_ENTRIES 20 +/* end RESUME STATE INDICES */ + +struct { + unsigned int cutoff_ms; + unsigned int mask; +} lis3dh_acc_odr_table[] = { + { 1, ODR1250 }, + { 3, ODR400 }, + { 5, ODR200 }, + { 10, ODR100 }, + { 20, ODR50 }, + { 40, ODR25 }, + { 100, ODR10 }, + { 1000, ODR1 }, +}; + +struct lis3dh_acc_data { + struct i2c_client *client; + struct lis3dh_acc_platform_data *pdata; + + struct mutex lock; + struct delayed_work input_work; + + struct input_dev *input_dev; + + int hw_initialized; + /* hw_working=-1 means not tested yet */ + int hw_working; + atomic_t enabled; + int on_before_suspend; + + u8 sensitivity; + + u8 resume_state[RESUME_ENTRIES]; + + int irq1; + struct work_struct irq1_work; + struct workqueue_struct *irq1_work_queue; + int irq2; + struct work_struct irq2_work; + struct workqueue_struct *irq2_work_queue; +}; + +/* + * Because misc devices can not carry a pointer from driver register to + * open, we keep this global. This limits the driver to a single instance. + */ +struct lis3dh_acc_data *lis3dh_acc_misc_data; + +static int lis3dh_acc_i2c_read(struct lis3dh_acc_data *acc, u8 * buf, int len) +{ + int err; + int tries = 0; + + struct i2c_msg msgs[] = { + { + .addr = acc->client->addr, + .flags = acc->client->flags & I2C_M_TEN, + .len = 1, + .buf = buf, }, + { + .addr = acc->client->addr, + .flags = (acc->client->flags & I2C_M_TEN) | I2C_M_RD, + .len = len, + .buf = buf, }, + }; + + do { + err = i2c_transfer(acc->client->adapter, msgs, 2); + if (err != 2) + msleep_interruptible(I2C_RETRY_DELAY); + } while ((err != 2) && (++tries < I2C_RETRIES)); + + if (err != 2) { + dev_err(&acc->client->dev, "read transfer error\n"); + err = -EIO; + } else { + err = 0; + } + + return err; +} + +static int lis3dh_acc_i2c_write(struct lis3dh_acc_data *acc, u8 * buf, int len) +{ + int err; + int tries = 0; + + struct i2c_msg msgs[] = { { .addr = acc->client->addr, + .flags = acc->client->flags & I2C_M_TEN, + .len = len + 1, .buf = buf, }, }; + do { + err = i2c_transfer(acc->client->adapter, msgs, 1); + if (err != 1) + msleep_interruptible(I2C_RETRY_DELAY); + } while ((err != 1) && (++tries < I2C_RETRIES)); + + if (err != 1) { + dev_err(&acc->client->dev, "write transfer error\n"); + err = -EIO; + } else { + err = 0; + } + + return err; +} + +static int lis3dh_acc_hw_init(struct lis3dh_acc_data *acc) +{ + int err = -1; + u8 buf[7]; + + printk(KERN_INFO "%s: hw init start\n", LIS3DH_ACC_DEV_NAME); + + buf[0] = WHO_AM_I; + err = lis3dh_acc_i2c_read(acc, buf, 1); + if (err < 0) + goto error_firstread; + else + acc->hw_working = 1; + if (buf[0] != WHOAMI_LIS3DH_ACC) { + err = -1; /* choose the right coded error */ + goto error_unknown_device; + } + + buf[0] = CTRL_REG1; + buf[1] = acc->resume_state[RES_CTRL_REG1]; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error1; + + buf[0] = TEMP_CFG_REG; + buf[1] = acc->resume_state[RES_TEMP_CFG_REG]; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error1; + + buf[0] = FIFO_CTRL_REG; + buf[1] = acc->resume_state[RES_FIFO_CTRL_REG]; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error1; + + buf[0] = (I2C_AUTO_INCREMENT | TT_THS); + buf[1] = acc->resume_state[RES_TT_THS]; + buf[2] = acc->resume_state[RES_TT_LIM]; + buf[3] = acc->resume_state[RES_TT_TLAT]; + buf[4] = acc->resume_state[RES_TT_TW]; + err = lis3dh_acc_i2c_write(acc, buf, 4); + if (err < 0) + goto error1; + buf[0] = TT_CFG; + buf[1] = acc->resume_state[RES_TT_CFG]; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error1; + + buf[0] = (I2C_AUTO_INCREMENT | INT_THS1); + buf[1] = acc->resume_state[RES_INT_THS1]; + buf[2] = acc->resume_state[RES_INT_DUR1]; + err = lis3dh_acc_i2c_write(acc, buf, 2); + if (err < 0) + goto error1; + buf[0] = INT_CFG1; + buf[1] = acc->resume_state[RES_INT_CFG1]; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error1; + + buf[0] = (I2C_AUTO_INCREMENT | INT_THS2); + buf[1] = acc->resume_state[RES_INT_THS2]; + buf[2] = acc->resume_state[RES_INT_DUR2]; + err = lis3dh_acc_i2c_write(acc, buf, 2); + if (err < 0) + goto error1; + buf[0] = INT_CFG2; + buf[1] = acc->resume_state[RES_INT_CFG2]; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error1; + + buf[0] = (I2C_AUTO_INCREMENT | CTRL_REG2); + buf[1] = acc->resume_state[RES_CTRL_REG2]; + buf[2] = acc->resume_state[RES_CTRL_REG3]; + buf[3] = acc->resume_state[RES_CTRL_REG4]; + buf[4] = acc->resume_state[RES_CTRL_REG5]; + buf[5] = acc->resume_state[RES_CTRL_REG6]; + err = lis3dh_acc_i2c_write(acc, buf, 5); + if (err < 0) + goto error1; + + acc->hw_initialized = 1; + printk(KERN_INFO "%s: hw init done\n", LIS3DH_ACC_DEV_NAME); + return 0; + +error_firstread: + acc->hw_working = 0; + dev_warn(&acc->client->dev, "Error reading WHO_AM_I: is device " + "available/working?\n"); + goto error1; +error_unknown_device: + dev_err(&acc->client->dev, + "device unknown. Expected: 0x%x," + " Replies: 0x%x\n", WHOAMI_LIS3DH_ACC, buf[0]); +error1: + acc->hw_initialized = 0; + dev_err(&acc->client->dev, "hw init error 0x%x,0x%x: %d\n", buf[0], + buf[1], err); + return err; +} + +static void lis3dh_acc_device_power_off(struct lis3dh_acc_data *acc) +{ + int err; + u8 buf[2] = { CTRL_REG1, LIS3DH_ACC_PM_OFF }; + + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + dev_err(&acc->client->dev, "soft power off failed: %d\n", err); + + if (acc->pdata->power_off) { + disable_irq_nosync(acc->irq1); + disable_irq_nosync(acc->irq2); + acc->pdata->power_off(); + acc->hw_initialized = 0; + } + if (acc->hw_initialized) { + disable_irq_nosync(acc->irq1); + disable_irq_nosync(acc->irq2); + acc->hw_initialized = 0; + } + +} + +static int lis3dh_acc_device_power_on(struct lis3dh_acc_data *acc) +{ + int err = -1; + + if (acc->pdata->power_on) { + err = acc->pdata->power_on(); + if (err < 0) { + dev_err(&acc->client->dev, + "power_on failed: %d\n", err); + return err; + } + enable_irq(acc->irq1); + enable_irq(acc->irq2); + } + + if (!acc->hw_initialized) { + err = lis3dh_acc_hw_init(acc); + if (acc->hw_working == 1 && err < 0) { + lis3dh_acc_device_power_off(acc); + return err; + } + } + + if (acc->hw_initialized) { + enable_irq(acc->irq1); + enable_irq(acc->irq2); + printk(KERN_INFO "%s: power on: irq enabled\n", + LIS3DH_ACC_DEV_NAME); + } + return 0; +} + +static irqreturn_t lis3dh_acc_isr1(int irq, void *dev) +{ + struct lis3dh_acc_data *acc = dev; + + disable_irq_nosync(irq); + queue_work(acc->irq1_work_queue, &acc->irq1_work); + printk(KERN_INFO "%s: isr1 queued\n", LIS3DH_ACC_DEV_NAME); + + return IRQ_HANDLED; +} + +static irqreturn_t lis3dh_acc_isr2(int irq, void *dev) +{ + struct lis3dh_acc_data *acc = dev; + + disable_irq_nosync(irq); + queue_work(acc->irq2_work_queue, &acc->irq2_work); + printk(KERN_INFO "%s: isr2 queued\n", LIS3DH_ACC_DEV_NAME); + + return IRQ_HANDLED; +} + + + +static void lis3dh_acc_irq1_work_func(struct work_struct *work) +{ + + struct lis3dh_acc_data *acc = + container_of(work, struct lis3dh_acc_data, irq1_work); + /* TODO add interrupt service procedure. + ie:lis3dh_acc_get_int1_source(acc); */ + ; + /* */ + printk(KERN_INFO "%s: IRQ1 triggered\n", LIS3DH_ACC_DEV_NAME); +exit: + enable_irq(acc->irq1); +} + +static void lis3dh_acc_irq2_work_func(struct work_struct *work) +{ + + struct lis3dh_acc_data *acc = + container_of(work, struct lis3dh_acc_data, irq2_work); + /* TODO add interrupt service procedure. + ie:lis3dh_acc_get_tap_source(acc); */ + ; + /* */ + + printk(KERN_INFO "%s: IRQ2 triggered\n", LIS3DH_ACC_DEV_NAME); +exit: + enable_irq(acc->irq2); +} + +int lis3dh_acc_update_g_range(struct lis3dh_acc_data *acc, u8 new_g_range) +{ + int err; + + u8 sensitivity; + u8 buf[2]; + u8 updated_val; + u8 init_val; + u8 new_val; + u8 mask = LIS3DH_ACC_FS_MASK | HIGH_RESOLUTION; + + switch (new_g_range) { + case LIS3DH_ACC_G_2G: + + sensitivity = SENSITIVITY_2G; + break; + case LIS3DH_ACC_G_4G: + + sensitivity = SENSITIVITY_4G; + break; + case LIS3DH_ACC_G_8G: + + sensitivity = SENSITIVITY_8G; + break; + case LIS3DH_ACC_G_16G: + + sensitivity = SENSITIVITY_16G; + break; + default: + dev_err(&acc->client->dev, "invalid g range requested: %u\n", + new_g_range); + return -EINVAL; + } + + if (atomic_read(&acc->enabled)) { + /* Set configuration register 4, which contains g range setting + * NOTE: this is a straight overwrite because this driver does + * not use any of the other configuration bits in this + * register. Should this become untrue, we will have to read + * out the value and only change the relevant bits --XX---- + * (marked by X) */ + buf[0] = CTRL_REG4; + err = lis3dh_acc_i2c_read(acc, buf, 1); + if (err < 0) + goto error; + init_val = buf[0]; + acc->resume_state[RES_CTRL_REG4] = init_val; + new_val = new_g_range | HIGH_RESOLUTION; + updated_val = ((mask & new_val) | ((~mask) & init_val)); + buf[1] = updated_val; + buf[0] = CTRL_REG4; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + goto error; + acc->resume_state[RES_CTRL_REG4] = updated_val; + acc->sensitivity = sensitivity; + } + + + return 0; +error: + dev_err(&acc->client->dev, "update g range failed 0x%x,0x%x: %d\n", + buf[0], buf[1], err); + + return err; +} + +int lis3dh_acc_update_odr(struct lis3dh_acc_data *acc, int poll_interval_ms) +{ + int err = -1; + int i; + u8 config[2]; + + /* Convert the poll interval into an output data rate configuration + * that is as low as possible. The ordering of these checks must be + * maintained due to the cascading cut off values - poll intervals are + * checked from shortest to longest. At each check, if the next lower + * ODR cannot support the current poll interval, we stop searching */ + for (i = ARRAY_SIZE(lis3dh_acc_odr_table) - 1; i >= 0; i--) { + if (lis3dh_acc_odr_table[i].cutoff_ms <= poll_interval_ms) + break; + } + config[1] = lis3dh_acc_odr_table[i].mask; + + config[1] |= LIS3DH_ACC_ENABLE_ALL_AXES; + + /* If device is currently enabled, we need to write new + * configuration out to it */ + if (atomic_read(&acc->enabled)) { + config[0] = CTRL_REG1; + err = lis3dh_acc_i2c_write(acc, config, 1); + if (err < 0) + goto error; + acc->resume_state[RES_CTRL_REG1] = config[1]; + } + + return 0; + +error: + dev_err(&acc->client->dev, "update odr failed 0x%x,0x%x: %d\n", + config[0], config[1], err); + + return err; +} + +/* */ + +static int lis3dh_acc_register_write(struct lis3dh_acc_data *acc, u8 *buf, + u8 reg_address, u8 new_value) +{ + int err = -1; + + if (atomic_read(&acc->enabled)) { + /* Sets configuration register at reg_address + * NOTE: this is a straight overwrite */ + buf[0] = reg_address; + buf[1] = new_value; + err = lis3dh_acc_i2c_write(acc, buf, 1); + if (err < 0) + return err; + } + return err; +} + +static int lis3dh_acc_register_read(struct lis3dh_acc_data *acc, u8 *buf, + u8 reg_address) +{ + + int err = -1; + buf[0] = (reg_address); + err = lis3dh_acc_i2c_read(acc, buf, 1); + return err; +} + +static int lis3dh_acc_register_update(struct lis3dh_acc_data *acc, u8 *buf, + u8 reg_address, u8 mask, u8 new_bit_values) +{ + int err = -1; + u8 init_val; + u8 updated_val; + err = lis3dh_acc_register_read(acc, buf, reg_address); + if (!(err < 0)) { + init_val = buf[1]; + updated_val = ((mask & new_bit_values) | ((~mask) & init_val)); + err = lis3dh_acc_register_write(acc, buf, reg_address, + updated_val); + } + return err; +} + +/* */ + +static int lis3dh_acc_get_acceleration_data(struct lis3dh_acc_data *acc, + int *xyz) +{ + int err = -1; + /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ + u8 acc_data[6]; + /* x,y,z hardware data */ + s16 hw_d[3] = { 0 }; + + acc_data[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG); + err = lis3dh_acc_i2c_read(acc, acc_data, 6); + if (err < 0) + return err; + + hw_d[0] = (((s16) ((acc_data[1] << 8) | acc_data[0])) >> 4); + hw_d[1] = (((s16) ((acc_data[3] << 8) | acc_data[2])) >> 4); + hw_d[2] = (((s16) ((acc_data[5] << 8) | acc_data[4])) >> 4); + + hw_d[0] = hw_d[0] * acc->sensitivity; + hw_d[1] = hw_d[1] * acc->sensitivity; + hw_d[2] = hw_d[2] * acc->sensitivity; + + + xyz[0] = ((acc->pdata->negate_x) ? (-hw_d[acc->pdata->axis_map_x]) + : (hw_d[acc->pdata->axis_map_x])); + xyz[1] = ((acc->pdata->negate_y) ? (-hw_d[acc->pdata->axis_map_y]) + : (hw_d[acc->pdata->axis_map_y])); + xyz[2] = ((acc->pdata->negate_z) ? (-hw_d[acc->pdata->axis_map_z]) + : (hw_d[acc->pdata->axis_map_z])); + + #ifdef DEBUG + /* + printk(KERN_INFO "%s read x=%d, y=%d, z=%d\n", + LIS3DH_ACC_DEV_NAME, xyz[0], xyz[1], xyz[2]); + */ + #endif + return err; +} + +static void lis3dh_acc_report_values(struct lis3dh_acc_data *acc, int *xyz) +{ + input_report_abs(acc->input_dev, ABS_X, xyz[0]); + input_report_abs(acc->input_dev, ABS_Y, xyz[1]); + input_report_abs(acc->input_dev, ABS_Z, xyz[2]); + input_sync(acc->input_dev); +} + +static int lis3dh_acc_enable(struct lis3dh_acc_data *acc) +{ + int err; + + if (!atomic_cmpxchg(&acc->enabled, 0, 1)) { + err = lis3dh_acc_device_power_on(acc); + if (err < 0) { + atomic_set(&acc->enabled, 0); + return err; + } + schedule_delayed_work(&acc->input_work, msecs_to_jiffies( + acc->pdata->poll_interval)); + } + + return 0; +} + +static int lis3dh_acc_disable(struct lis3dh_acc_data *acc) +{ + if (atomic_cmpxchg(&acc->enabled, 1, 0)) { + cancel_delayed_work_sync(&acc->input_work); + lis3dh_acc_device_power_off(acc); + } + + return 0; +} + +static int lis3dh_acc_misc_open(struct inode *inode, struct file *file) +{ + int err; + err = nonseekable_open(inode, file); + if (err < 0) + return err; + + file->private_data = lis3dh_acc_misc_data; + + return 0; +} + +static int lis3dh_acc_misc_ioctl(struct inode *inode, struct file *file, + unsigned int cmd, unsigned long arg) +{ + void __user *argp = (void __user *)arg; + u8 buf[4]; + u8 mask; + u8 reg_address; + u8 bit_values; + int err; + int interval; + struct lis3dh_acc_data *acc = file->private_data; + + printk(KERN_INFO "%s: %s call with cmd 0x%x and arg 0x%x\n", + LIS3DH_ACC_DEV_NAME, __func__, cmd, (unsigned int)arg); + + switch (cmd) { + case LIS3DH_ACC_IOCTL_GET_DELAY: + interval = acc->pdata->poll_interval; + if (copy_to_user(argp, &interval, sizeof(interval))) + return -EFAULT; + break; + + case LIS3DH_ACC_IOCTL_SET_DELAY: + if (copy_from_user(&interval, argp, sizeof(interval))) + return -EFAULT; + if (interval < 0 || interval > 1000) + return -EINVAL; + + acc->pdata->poll_interval = max(interval, + acc->pdata->min_interval); + err = lis3dh_acc_update_odr(acc, acc->pdata->poll_interval); + /* TODO: if update fails poll is still set */ + if (err < 0) + return err; + break; + + case LIS3DH_ACC_IOCTL_SET_ENABLE: + if (copy_from_user(&interval, argp, sizeof(interval))) + return -EFAULT; + if (interval > 1) + return -EINVAL; + if (interval) + err = lis3dh_acc_enable(acc); + else + err = lis3dh_acc_disable(acc); + return err; + break; + + case LIS3DH_ACC_IOCTL_GET_ENABLE: + interval = atomic_read(&acc->enabled); + if (copy_to_user(argp, &interval, sizeof(interval))) + return -EINVAL; + break; + + case LIS3DH_ACC_IOCTL_SET_G_RANGE: + if (copy_from_user(buf, argp, 1)) + return -EFAULT; + bit_values = buf[0]; + err = lis3dh_acc_update_g_range(acc, bit_values); + if (err < 0) + return err; + break; + +#ifdef INTERRUPT_MANAGEMENT + case LIS3DH_ACC_IOCTL_SET_CTRL_REG3: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = CTRL_REG3; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_CTRL_REG3] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_CTRL_REG3])); + break; + + case LIS3DH_ACC_IOCTL_SET_CTRL_REG6: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = CTRL_REG6; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_CTRL_REG6] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_CTRL_REG6])); + break; + + case LIS3DH_ACC_IOCTL_SET_DURATION1: + if (copy_from_user(buf, argp, 1)) + return -EFAULT; + reg_address = INT_DUR1; + mask = 0x7F; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_INT_DUR1] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_INT_DUR1])); + break; + + case LIS3DH_ACC_IOCTL_SET_THRESHOLD1: + if (copy_from_user(buf, argp, 1)) + return -EFAULT; + reg_address = INT_THS1; + mask = 0x7F; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_INT_THS1] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_INT_THS1])); + break; + + case LIS3DH_ACC_IOCTL_SET_CONFIG1: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = INT_CFG1; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_INT_CFG1] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_INT_CFG1])); + break; + + case LIS3DH_ACC_IOCTL_GET_SOURCE1: + err = lis3dh_acc_register_read(acc, buf, INT_SRC1); + if (err < 0) + return err; +#if DEBUG + printk(KERN_ALERT "INT1_SRC content: %d , 0x%x\n", + buf[0], buf[0]); +#endif + if (copy_to_user(argp, buf, 1)) + return -EINVAL; + break; + + case LIS3DH_ACC_IOCTL_SET_DURATION2: + if (copy_from_user(buf, argp, 1)) + return -EFAULT; + reg_address = INT_DUR2; + mask = 0x7F; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_INT_DUR2] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_INT_DUR2])); + break; + + case LIS3DH_ACC_IOCTL_SET_THRESHOLD2: + if (copy_from_user(buf, argp, 1)) + return -EFAULT; + reg_address = INT_THS2; + mask = 0x7F; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_INT_THS2] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_INT_THS2])); + break; + + case LIS3DH_ACC_IOCTL_SET_CONFIG2: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = INT_CFG2; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_INT_CFG2] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_INT_CFG2])); + break; + + case LIS3DH_ACC_IOCTL_GET_SOURCE2: + err = lis3dh_acc_register_read(acc, buf, INT_SRC2); + if (err < 0) + return err; +#if DEBUG + printk(KERN_ALERT "INT2_SRC content: %d , 0x%x\n", + buf[0], buf[0]); +#endif + if (copy_to_user(argp, buf, 1)) + return -EINVAL; + break; + + case LIS3DH_ACC_IOCTL_GET_TAP_SOURCE: + err = lis3dh_acc_register_read(acc, buf, TT_SRC); + if (err < 0) + return err; +#if DEBUG + printk(KERN_ALERT "TT_SRC content: %d , 0x%x\n", + buf[0], buf[0]); +#endif + if (copy_to_user(argp, buf, 1)) { + printk(KERN_ERR "%s: %s error in copy_to_user \n", + LIS3DH_ACC_DEV_NAME, __func__); + return -EINVAL; + } + break; + + case LIS3DH_ACC_IOCTL_SET_TAP_CFG: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = TT_CFG; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_TT_CFG] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_TT_CFG])); + break; + + case LIS3DH_ACC_IOCTL_SET_TAP_TLIM: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = TT_LIM; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_TT_LIM] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_TT_LIM])); + break; + + case LIS3DH_ACC_IOCTL_SET_TAP_THS: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = TT_THS; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_TT_THS] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_TT_THS])); + break; + + case LIS3DH_ACC_IOCTL_SET_TAP_TLAT: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = TT_TLAT; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_TT_TLAT] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_TT_TLAT])); + break; + + case LIS3DH_ACC_IOCTL_SET_TAP_TW: + if (copy_from_user(buf, argp, 2)) + return -EFAULT; + reg_address = TT_TW; + mask = buf[1]; + bit_values = buf[0]; + err = lis3dh_acc_register_update(acc, (u8 *) arg, reg_address, + mask, bit_values); + if (err < 0) + return err; + acc->resume_state[RES_TT_TW] = ((mask & bit_values) | + ( ~mask & acc->resume_state[RES_TT_TW])); + break; + +#endif /* INTERRUPT_MANAGEMENT */ + + default: + return -EINVAL; + } + + return 0; +} + +static const struct file_operations lis3dh_acc_misc_fops = { + .owner = THIS_MODULE, + .open = lis3dh_acc_misc_open, + .ioctl = lis3dh_acc_misc_ioctl, +}; + +static struct miscdevice lis3dh_acc_misc_device = { + .minor = MISC_DYNAMIC_MINOR, + .name = LIS3DH_ACC_DEV_NAME, + .fops = &lis3dh_acc_misc_fops, +}; + +static void lis3dh_acc_input_work_func(struct work_struct *work) +{ + struct lis3dh_acc_data *acc; + + int xyz[3] = { 0 }; + int err; + + acc = container_of((struct delayed_work *)work, + struct lis3dh_acc_data, input_work); + + mutex_lock(&acc->lock); + err = lis3dh_acc_get_acceleration_data(acc, xyz); + if (err < 0) + dev_err(&acc->client->dev, "get_acceleration_data failed\n"); + else + lis3dh_acc_report_values(acc, xyz); + + schedule_delayed_work(&acc->input_work, msecs_to_jiffies( + acc->pdata->poll_interval)); + mutex_unlock(&acc->lock); +} + +#ifdef LIS3DH_OPEN_ENABLE +int lis3dh_acc_input_open(struct input_dev *input) +{ + struct lis3dh_acc_data *acc = input_get_drvdata(input); + + return lis3dh_acc_enable(acc); +} + +void lis3dh_acc_input_close(struct input_dev *dev) +{ + struct lis3dh_acc_data *acc = input_get_drvdata(dev); + + lis3dh_acc_disable(acc); +} +#endif + +static int lis3dh_acc_validate_pdata(struct lis3dh_acc_data *acc) +{ + acc->pdata->poll_interval = max(acc->pdata->poll_interval, + acc->pdata->min_interval); + + if (acc->pdata->axis_map_x > 2 || acc->pdata->axis_map_y > 2 + || acc->pdata->axis_map_z > 2) { + dev_err(&acc->client->dev, "invalid axis_map value " + "x:%u y:%u z%u\n", acc->pdata->axis_map_x, + acc->pdata->axis_map_y, acc->pdata->axis_map_z); + return -EINVAL; + } + + /* Only allow 0 and 1 for negation boolean flag */ + if (acc->pdata->negate_x > 1 || acc->pdata->negate_y > 1 + || acc->pdata->negate_z > 1) { + dev_err(&acc->client->dev, "invalid negate value " + "x:%u y:%u z:%u\n", acc->pdata->negate_x, + acc->pdata->negate_y, acc->pdata->negate_z); + return -EINVAL; + } + + /* Enforce minimum polling interval */ + if (acc->pdata->poll_interval < acc->pdata->min_interval) { + dev_err(&acc->client->dev, "minimum poll interval violated\n"); + return -EINVAL; + } + + return 0; +} + +static int lis3dh_acc_input_init(struct lis3dh_acc_data *acc) +{ + int err; + + INIT_DELAYED_WORK(&acc->input_work, lis3dh_acc_input_work_func); + acc->input_dev = input_allocate_device(); + if (!acc->input_dev) { + err = -ENOMEM; + dev_err(&acc->client->dev, "input device allocate failed\n"); + goto err0; + } + +#ifdef LIS3DH_ACC_OPEN_ENABLE + acc->input_dev->open = lis3dh_acc_input_open; + acc->input_dev->close = lis3dh_acc_input_close; +#endif + + input_set_drvdata(acc->input_dev, acc); + + set_bit(EV_ABS, acc->input_dev->evbit); + /* next is used for interruptA sources data if the case */ + set_bit(ABS_MISC, acc->input_dev->absbit); + /* next is used for interruptB sources data if the case */ + set_bit(ABS_WHEEL, acc->input_dev->absbit); + + input_set_abs_params(acc->input_dev, ABS_X, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(acc->input_dev, ABS_Y, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(acc->input_dev, ABS_Z, -G_MAX, G_MAX, FUZZ, FLAT); + /* next is used for interruptA sources data if the case */ + input_set_abs_params(acc->input_dev, ABS_MISC, INT_MIN, INT_MAX, 0, 0); + /* next is used for interruptB sources data if the case */ + input_set_abs_params(acc->input_dev, ABS_WHEEL, INT_MIN, INT_MAX, 0, 0); + + acc->input_dev->name = "accelerometer"; + + err = input_register_device(acc->input_dev); + if (err) { + dev_err(&acc->client->dev, + "unable to register input polled device %s\n", + acc->input_dev->name); + goto err1; + } + + return 0; + +err1: + input_free_device(acc->input_dev); +err0: + return err; +} + +static void lis3dh_acc_input_cleanup(struct lis3dh_acc_data *acc) +{ + input_unregister_device(acc->input_dev); + input_free_device(acc->input_dev); +} + +static int lis3dh_acc_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + + struct lis3dh_acc_data *acc; + + int err = -1; + int tempvalue; + + pr_info("%s: probe start.\n", LIS3DH_ACC_DEV_NAME); + + if (client->dev.platform_data == NULL) { + dev_err(&client->dev, "platform data is NULL. exiting.\n"); + err = -ENODEV; + goto exit_check_functionality_failed; + } + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + dev_err(&client->dev, "client not i2c capable\n"); + err = -ENODEV; + goto exit_check_functionality_failed; + } + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_SMBUS_BYTE | + I2C_FUNC_SMBUS_BYTE_DATA | + I2C_FUNC_SMBUS_WORD_DATA)) { + dev_err(&client->dev, "client not smb-i2c capable:2\n"); + err = -EIO; + goto exit_check_functionality_failed; + } + + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_SMBUS_I2C_BLOCK)){ + dev_err(&client->dev, "client not smb-i2c capable:3\n"); + err = -EIO; + goto exit_check_functionality_failed; + } + /* + * OK. From now, we presume we have a valid client. We now create the + * client structure, even though we cannot fill it completely yet. + */ + + acc = kzalloc(sizeof(struct lis3dh_acc_data), GFP_KERNEL); + if (acc == NULL) { + err = -ENOMEM; + dev_err(&client->dev, + "failed to allocate memory for module data: " + "%d\n", err); + goto exit_alloc_data_failed; + } + + mutex_init(&acc->lock); + mutex_lock(&acc->lock); + + acc->client = client; + i2c_set_clientdata(client, acc); + + + INIT_WORK(&acc->irq1_work, lis3dh_acc_irq1_work_func); + acc->irq1_work_queue = create_singlethread_workqueue("lis3dh_acc_wq1"); + if (!acc->irq1_work_queue) { + err = -ENOMEM; + dev_err(&client->dev, "cannot create work queue1: %d\n", err); + goto err_mutexunlockfreedata; + } + + INIT_WORK(&acc->irq2_work, lis3dh_acc_irq2_work_func); + acc->irq2_work_queue = create_singlethread_workqueue("lis3dh_acc_wq2"); + if (!acc->irq2_work_queue) { + err = -ENOMEM; + dev_err(&client->dev, "cannot create work queue2: %d\n", err); + goto err_destoyworkqueue1; + } + + + + if (i2c_smbus_read_byte(client) < 0) { + printk(KERN_ERR "i2c_smbus_read_byte error!!\n"); + goto err_destoyworkqueue2; + } else { + printk(KERN_INFO "%s Device detected!\n", LIS3DH_ACC_DEV_NAME); + } + + /* read chip id */ + + tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I); + if ((tempvalue & 0x00FF) == WHOAMI_LIS3DH_ACC) { + printk(KERN_INFO "%s I2C driver registered!\n", + LIS3DH_ACC_DEV_NAME); + } else { + acc->client = NULL; + printk(KERN_INFO "I2C driver not registered!" + " Device unknown\n"); + goto err_destoyworkqueue2; + } + + acc->pdata = kmalloc(sizeof(*acc->pdata), GFP_KERNEL); + if (acc->pdata == NULL) { + err = -ENOMEM; + dev_err(&client->dev, + "failed to allocate memory for pdata: %d\n", + err); + goto err_destoyworkqueue2; + } + + memcpy(acc->pdata, client->dev.platform_data, sizeof(*acc->pdata)); + + err = lis3dh_acc_validate_pdata(acc); + if (err < 0) { + dev_err(&client->dev, "failed to validate platform data\n"); + goto exit_kfree_pdata; + } + + i2c_set_clientdata(client, acc); + + + if (acc->pdata->init) { + err = acc->pdata->init(); + if (err < 0) { + dev_err(&client->dev, "init failed: %d\n", err); + goto err2; + } + } + + memset(acc->resume_state, 0, ARRAY_SIZE(acc->resume_state)); + + acc->irq1 = gpio_to_irq(acc->pdata->gpio_int1); + printk(KERN_INFO "%s: %s has set irq1 to irq: %d mapped on gpio:%d\n", + LIS3DH_ACC_DEV_NAME, __func__, acc->irq1, + acc->pdata->gpio_int1); + acc->irq2 = gpio_to_irq(acc->pdata->gpio_int2); + printk(KERN_INFO "%s: %s has set irq2 to irq: %d mapped on gpio:%d\n", + LIS3DH_ACC_DEV_NAME, __func__, acc->irq2, + acc->pdata->gpio_int2); + + + + + acc->resume_state[RES_CTRL_REG1] = LIS3DH_ACC_ENABLE_ALL_AXES; + acc->resume_state[RES_CTRL_REG2] = 0x00; + acc->resume_state[RES_CTRL_REG3] = 0x00; + acc->resume_state[RES_CTRL_REG4] = 0x00; + acc->resume_state[RES_CTRL_REG5] = 0x00; + acc->resume_state[RES_CTRL_REG6] = 0x00; + + acc->resume_state[RES_TEMP_CFG_REG] = 0x00; + acc->resume_state[RES_FIFO_CTRL_REG] = 0x00; + acc->resume_state[RES_INT_CFG1] = 0x00; + acc->resume_state[RES_INT_THS1] = 0x00; + acc->resume_state[RES_INT_DUR1] = 0x00; + acc->resume_state[RES_INT_CFG2] = 0x00; + acc->resume_state[RES_INT_THS2] = 0x00; + acc->resume_state[RES_INT_DUR2] = 0x00; + + acc->resume_state[RES_TT_CFG] = 0x00; + acc->resume_state[RES_TT_THS] = 0x00; + acc->resume_state[RES_TT_LIM] = 0x00; + acc->resume_state[RES_TT_TLAT] = 0x00; + acc->resume_state[RES_TT_TW] = 0x00; + + err = lis3dh_acc_device_power_on(acc); + if (err < 0) { + dev_err(&client->dev, "power on failed: %d\n", err); + goto err2; + } + + atomic_set(&acc->enabled, 1); + + err = lis3dh_acc_update_g_range(acc, acc->pdata->g_range); + if (err < 0) { + dev_err(&client->dev, "update_g_range failed\n"); + goto err_power_off; + } + + err = lis3dh_acc_update_odr(acc, acc->pdata->poll_interval); + if (err < 0) { + dev_err(&client->dev, "update_odr failed\n"); + goto err_power_off; + } + + err = lis3dh_acc_input_init(acc); + if (err < 0) { + dev_err(&client->dev, "input init failed\n"); + goto err_power_off; + } + lis3dh_acc_misc_data = acc; + + err = misc_register(&lis3dh_acc_misc_device); + if (err < 0) { + dev_err(&client->dev, + "misc LIS3DH_ACC_DEV_NAME register failed\n"); + goto err_input_cleanup; + } + + lis3dh_acc_device_power_off(acc); + + /* As default, do not report information */ + atomic_set(&acc->enabled, 0); + + err = request_irq(acc->irq1, lis3dh_acc_isr1, IRQF_TRIGGER_RISING, + "lis3dh_acc_irq1", acc); + if (err < 0) { + dev_err(&client->dev, "request irq1 failed: %d\n", err); + goto err_misc_dereg; + } + disable_irq_nosync(acc->irq1); + + err = request_irq(acc->irq2, lis3dh_acc_isr2, IRQF_TRIGGER_RISING, + "lis3dh_acc_irq2", acc); + if (err < 0) { + dev_err(&client->dev, "request irq2 failed: %d\n", err); + goto err_free_irq1; + } + disable_irq_nosync(acc->irq2); + + mutex_unlock(&acc->lock); + + dev_info(&client->dev, "%s: probed\n", LIS3DH_ACC_DEV_NAME); + + return 0; + +err_free_irq1: + free_irq(acc->irq1, acc); +err_misc_dereg: + misc_deregister(&lis3dh_acc_misc_device); +err_input_cleanup: + lis3dh_acc_input_cleanup(acc); +err_power_off: + lis3dh_acc_device_power_off(acc); +err2: + if (acc->pdata->exit) + acc->pdata->exit(); +exit_kfree_pdata: + kfree(acc->pdata); +err_destoyworkqueue2: + destroy_workqueue(acc->irq2_work_queue); +err_destoyworkqueue1: + destroy_workqueue(acc->irq1_work_queue); +err_mutexunlockfreedata: + mutex_unlock(&acc->lock); + kfree(acc); +exit_alloc_data_failed: +exit_check_functionality_failed: + printk(KERN_ERR "%s: Driver Init failed\n", LIS3DH_ACC_DEV_NAME); + return err; +} + +static int __devexit lis3dh_acc_remove(struct i2c_client *client) +{ + /* TODO: revisit ordering here once _probe order is finalized */ + struct lis3dh_acc_data *acc = i2c_get_clientdata(client); + + free_irq(acc->irq1, acc); + free_irq(acc->irq2, acc); + gpio_free(acc->pdata->gpio_int1); + gpio_free(acc->pdata->gpio_int2); + destroy_workqueue(acc->irq1_work_queue); + destroy_workqueue(acc->irq2_work_queue); + + misc_deregister(&lis3dh_acc_misc_device); + lis3dh_acc_input_cleanup(acc); + lis3dh_acc_device_power_off(acc); + if (acc->pdata->exit) + acc->pdata->exit(); + kfree(acc->pdata); + kfree(acc); + + return 0; +} + +static int lis3dh_acc_resume(struct i2c_client *client) +{ + struct lis3dh_acc_data *acc = i2c_get_clientdata(client); + + if (acc->on_before_suspend) + return lis3dh_acc_enable(acc); + return 0; +} + +static int lis3dh_acc_suspend(struct i2c_client *client, pm_message_t mesg) +{ + struct lis3dh_acc_data *acc = i2c_get_clientdata(client); + + acc->on_before_suspend = atomic_read(&acc->enabled); + return lis3dh_acc_disable(acc); +} + +static const struct i2c_device_id lis3dh_acc_id[] + = { { LIS3DH_ACC_DEV_NAME, 0 }, { }, }; + +MODULE_DEVICE_TABLE(i2c, lis3dh_acc_id); + +static struct i2c_driver lis3dh_acc_driver = { + .driver = { + .name = LIS3DH_ACC_DEV_NAME, + }, + .probe = lis3dh_acc_probe, + .remove = __devexit_p(lis3dh_acc_remove), + .resume = lis3dh_acc_resume, + .suspend = lis3dh_acc_suspend, + .id_table = lis3dh_acc_id, +}; + +static int __init lis3dh_acc_init(void) +{ + printk(KERN_INFO "%s accelerometer driver: init\n", + LIS3DH_ACC_DEV_NAME); + return i2c_add_driver(&lis3dh_acc_driver); +} + +static void __exit lis3dh_acc_exit(void) +{ + #if DEBUG + printk(KERN_INFO "%s accelerometer driver exit\n", LIS3DH_ACC_DEV_NAME); + #endif + i2c_del_driver(&lis3dh_acc_driver); + return; +} + +module_init(lis3dh_acc_init); +module_exit(lis3dh_acc_exit); + +MODULE_DESCRIPTION("lis3dh accelerometer misc driver"); +MODULE_AUTHOR("STMicroelectronics"); +MODULE_LICENSE("GPL"); + diff --git a/drivers/input/gsensor/lis3dh_acc_misc.h b/drivers/input/gsensor/lis3dh_acc_misc.h new file mode 100755 index 000000000000..17b3c0fc225c --- /dev/null +++ b/drivers/input/gsensor/lis3dh_acc_misc.h @@ -0,0 +1,137 @@ + +/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** +* +* File Name : lis3dh_misc.h +* Authors : MH - C&I BU - Application Team +* : Carmine Iascone (carmine.iascone@st.com) +* : Matteo Dameno (matteo.dameno@st.com) +* Version : V 1.0.5 +* Date : 26/08/2010 +* +******************************************************************************** +* +* 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. +* +* THIS SOFTWARE IS SPECIFICALLY DESIGNED FOR EXCLUSIVE USE WITH ST PARTS. +* +*******************************************************************************/ +/******************************************************************************* +Version History. + +Revision 1-0-0 05/11/2009 +First Release +Revision 1-0-1 26/01/2010 +Linux K&R Compliant Release +Revision 1-0-5 16/08/2010 +Interrupt Management + +*******************************************************************************/ + +#ifndef __LIS3DH_H__ +#define __LIS3DH_H__ + +#include /* For IOCTL macros */ +#include + +#define SAD0L 0x00 +#define SAD0H 0x01 +#define LIS3DH_ACC_I2C_SADROOT 0x0C +#define LIS3DH_ACC_I2C_SAD_L ((LIS3DH_ACC_I2C_SADROOT<<1)|SAD0L) +#define LIS3DH_ACC_I2C_SAD_H ((LIS3DH_ACC_I2C_SADROOT<<1)|SAD0H) +#define LIS3DH_ACC_DEV_NAME "lis3dh_acc_misc" + + +#define LIS3DH_ACC_IOCTL_BASE 77 +/** The following define the IOCTL command values via the ioctl macros */ +#define LIS3DH_ACC_IOCTL_SET_DELAY _IOW(LIS3DH_ACC_IOCTL_BASE, 0, int) +#define LIS3DH_ACC_IOCTL_GET_DELAY _IOR(LIS3DH_ACC_IOCTL_BASE, 1, int) +#define LIS3DH_ACC_IOCTL_SET_ENABLE _IOW(LIS3DH_ACC_IOCTL_BASE, 2, int) +#define LIS3DH_ACC_IOCTL_GET_ENABLE _IOR(LIS3DH_ACC_IOCTL_BASE, 3, int) +#define LIS3DH_ACC_IOCTL_SET_FULLSCALE _IOW(LIS3DH_ACC_IOCTL_BASE, 4, int) +#define LIS3DH_ACC_IOCTL_SET_G_RANGE LIS3DH_ACC_IOCTL_SET_FULLSCALE + +#define LIS3DH_ACC_IOCTL_SET_CTRL_REG3 _IOW(LIS3DH_ACC_IOCTL_BASE, 6, int) +#define LIS3DH_ACC_IOCTL_SET_CTRL_REG6 _IOW(LIS3DH_ACC_IOCTL_BASE, 7, int) +#define LIS3DH_ACC_IOCTL_SET_DURATION1 _IOW(LIS3DH_ACC_IOCTL_BASE, 8, int) +#define LIS3DH_ACC_IOCTL_SET_THRESHOLD1 _IOW(LIS3DH_ACC_IOCTL_BASE, 9, int) +#define LIS3DH_ACC_IOCTL_SET_CONFIG1 _IOW(LIS3DH_ACC_IOCTL_BASE, 10, int) + +#define LIS3DH_ACC_IOCTL_SET_DURATION2 _IOW(LIS3DH_ACC_IOCTL_BASE, 11, int) +#define LIS3DH_ACC_IOCTL_SET_THRESHOLD2 _IOW(LIS3DH_ACC_IOCTL_BASE, 12, int) +#define LIS3DH_ACC_IOCTL_SET_CONFIG2 _IOW(LIS3DH_ACC_IOCTL_BASE, 13, int) + +#define LIS3DH_ACC_IOCTL_GET_SOURCE1 _IOW(LIS3DH_ACC_IOCTL_BASE, 14, int) +#define LIS3DH_ACC_IOCTL_GET_SOURCE2 _IOW(LIS3DH_ACC_IOCTL_BASE, 15, int) + +#define LIS3DH_ACC_IOCTL_GET_TAP_SOURCE _IOW(LIS3DH_ACC_IOCTL_BASE, 16, int) + +#define LIS3DH_ACC_IOCTL_SET_TAP_TW _IOW(LIS3DH_ACC_IOCTL_BASE, 17, int) +#define LIS3DH_ACC_IOCTL_SET_TAP_CFG _IOW(LIS3DH_ACC_IOCTL_BASE, 18, int) +#define LIS3DH_ACC_IOCTL_SET_TAP_TLIM _IOW(LIS3DH_ACC_IOCTL_BASE, 19, int) +#define LIS3DH_ACC_IOCTL_SET_TAP_THS _IOW(LIS3DH_ACC_IOCTL_BASE, 20, int) +#define LIS3DH_ACC_IOCTL_SET_TAP_TLAT _IOW(LIS3DH_ACC_IOCTL_BASE, 21, int) + + + + +/************************************************/ +/* Accelerometer defines section */ +/************************************************/ + +/* Accelerometer Sensor Full Scale */ +#define LIS3DH_ACC_FS_MASK 0x30 +#define LIS3DH_ACC_G_2G 0x00 +#define LIS3DH_ACC_G_4G 0x10 +#define LIS3DH_ACC_G_8G 0x20 +#define LIS3DH_ACC_G_16G 0x30 + + +/* Accelerometer Sensor Operating Mode */ +#define LIS3DH_ACC_ENABLE 0x01 +#define LIS3DH_ACC_DISABLE 0x00 + + + + + + +#ifdef __KERNEL__ +struct lis3dh_acc_platform_data { + int poll_interval; + int min_interval; + + u8 g_range; + + u8 axis_map_x; + u8 axis_map_y; + u8 axis_map_z; + + u8 negate_x; + u8 negate_y; + u8 negate_z; + + int (*init)(void); + void (*exit)(void); + int (*power_on)(void); + int (*power_off)(void); + + int gpio_int1; + int gpio_int2; + +}; +#endif /* __KERNEL__ */ + +#endif /* __LIS3DH_H__ */ + + + diff --git a/drivers/input/gyroscope/Kconfig b/drivers/input/gyroscope/Kconfig new file mode 100755 index 000000000000..596c0f3ebe76 --- /dev/null +++ b/drivers/input/gyroscope/Kconfig @@ -0,0 +1,22 @@ +# +# gyroscope drivers configuration +# + +menuconfig GYRO_SENSOR_DEVICE + bool "gyroscope device support" + default n + help + Enable this to be able to choose the drivers for controlling the + gyroscope sensor on some platforms, for example on PDAs. + +if GYRO_SENSOR_DEVICE + +config GYRO_SENSOR_K3G + bool "gyroscope k3g" + depends on GYRO_SENSOR_DEVICE + default n + help + To have support for your specific gyroscope sesnor you will have to + select the proper drivers which depend on this option. + +endif diff --git a/drivers/input/gyroscope/Makefile b/drivers/input/gyroscope/Makefile new file mode 100755 index 000000000000..002ba5fff2e2 --- /dev/null +++ b/drivers/input/gyroscope/Makefile @@ -0,0 +1,4 @@ +# gyroscope drivers + +obj-$(CONFIG_GYRO_SENSOR_K3G) += k3g.o + diff --git a/drivers/input/gyroscope/k3g.c b/drivers/input/gyroscope/k3g.c new file mode 100755 index 000000000000..1b4cd979c012 --- /dev/null +++ b/drivers/input/gyroscope/k3g.c @@ -0,0 +1,703 @@ +/* + * Copyright (C) 2010, Samsung Electronics Co. Ltd. All Rights Reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include + +/* k3g chip id */ +#define DEVICE_ID 0xD3 +/* k3g gyroscope registers */ +#define WHO_AM_I 0x0F +#define CTRL_REG1 0x20 /* power control reg */ +#define CTRL_REG2 0x21 /* power control reg */ +#define CTRL_REG3 0x22 /* power control reg */ +#define CTRL_REG4 0x23 /* interrupt control reg */ +#define CTRL_REG5 0x24 /* interrupt control reg */ +#define OUT_TEMP 0x26 /* Temperature data */ +#define STATUS_REG 0x27 +#define AXISDATA_REG 0x28 +#define OUT_Y_L 0x2A +#define FIFO_CTRL_REG 0x2E +#define FIFO_SRC_REG 0x2F +#define PM_OFF 0x00 +#define PM_NORMAL 0x08 +#define ENABLE_ALL_AXES 0x07 +#define BYPASS_MODE 0x00 +#define FIFO_MODE 0x20 + +#define FIFO_EMPTY 0x20 +#define FSS_MASK 0x1F +#define ODR_MASK 0xF0 +#define ODR105_BW12_5 0x00 /* ODR = 105Hz; BW = 12.5Hz */ +#define ODR105_BW25 0x10 /* ODR = 105Hz; BW = 25Hz */ +#define ODR210_BW12_5 0x40 /* ODR = 210Hz; BW = 12.5Hz */ +#define ODR210_BW25 0x50 /* ODR = 210Hz; BW = 25Hz */ +#define ODR210_BW50 0x60 /* ODR = 210Hz; BW = 50Hz */ +#define ODR210_BW70 0x70 /* ODR = 210Hz; BW = 70Hz */ +#define ODR420_BW20 0x80 /* ODR = 420Hz; BW = 20Hz */ +#define ODR420_BW25 0x90 /* ODR = 420Hz; BW = 25Hz */ +#define ODR420_BW50 0xA0 /* ODR = 420Hz; BW = 50Hz */ +#define ODR420_BW110 0xB0 /* ODR = 420Hz; BW = 110Hz */ +#define ODR840_BW30 0xC0 /* ODR = 840Hz; BW = 30Hz */ +#define ODR840_BW35 0xD0 /* ODR = 840Hz; BW = 35Hz */ +#define ODR840_BW50 0xE0 /* ODR = 840Hz; BW = 50Hz */ +#define ODR840_BW110 0xF0 /* ODR = 840Hz; BW = 110Hz */ + +#define MIN_ST 175 +#define MAX_ST 875 +#define AC (1 << 7) /* register auto-increment bit */ +#define MAX_ENTRY 1 +#define MAX_DELAY (MAX_ENTRY * 9523809LL) + +/* default register setting for device init */ +static const char default_ctrl_regs[] = { + 0x3F, /* 105HZ, PM-normal, xyz enable */ + 0x00, /* normal mode */ + 0x04, /* fifo wtm interrupt on */ + 0xA0, /* block data update, 2000d/s */ + 0x40, /* fifo enable */ +}; + +static const struct odr_delay { + u8 odr; /* odr reg setting */ + u32 delay_ns; /* odr in ns */ +} odr_delay_table[] = { + { ODR840_BW110, 1190476LL }, /* 840Hz */ + { ODR420_BW110, 2380952LL }, /* 420Hz */ + { ODR210_BW70, 4761904LL }, /* 210Hz */ + { ODR105_BW25, 9523809LL }, /* 105Hz */ +}; + +/* + * K3G gyroscope data + * brief structure containing gyroscope values for yaw, pitch and roll in + * signed short + */ +struct k3g_t { + s16 x; + s16 y; + s16 z; +}; + +struct k3g_data { + struct i2c_client *client; + struct input_dev *input_dev; + struct mutex lock; + struct workqueue_struct *k3g_wq; + struct work_struct work; + struct hrtimer timer; + bool enable; + bool drop_next_event; + bool interruptible; /* interrupt or polling? */ + int entries; /* number of fifo entries */ + u8 ctrl_regs[5]; /* saving register settings */ + u32 time_to_read; /* time needed to read one entry */ + ktime_t polling_delay; /* polling time for timer */ +}; + +static int k3g_read_fifo_status(struct k3g_data *k3g_data) +{ + int fifo_status; + + fifo_status = i2c_smbus_read_byte_data(k3g_data->client, FIFO_SRC_REG); + if (fifo_status < 0) { + pr_err("%s: failed to read fifo source register\n", + __func__); + return fifo_status; + } + return (fifo_status & FSS_MASK) + !(fifo_status & FIFO_EMPTY); +} + +static int k3g_restart_fifo(struct k3g_data *k3g_data) +{ + int res = 0; + + res = i2c_smbus_write_byte_data(k3g_data->client, + FIFO_CTRL_REG, BYPASS_MODE); + if (res < 0) { + pr_err("%s : failed to set bypass_mode\n", __func__); + return res; + } + + res = i2c_smbus_write_byte_data(k3g_data->client, + FIFO_CTRL_REG, FIFO_MODE | (k3g_data->entries - 1)); + + if (res < 0) + pr_err("%s : failed to set fifo_mode\n", __func__); + + return res; +} + +static void set_polling_delay(struct k3g_data *k3g_data, int res) +{ + s64 delay_ns; + + delay_ns = k3g_data->entries + 1 - res; + if (delay_ns < 0) + delay_ns = 0; + + delay_ns = delay_ns * k3g_data->time_to_read; + k3g_data->polling_delay = ns_to_ktime(delay_ns); +} + +/* gyroscope data readout */ +static int k3g_read_gyro_values(struct i2c_client *client, + struct k3g_t *data, int total_read) +{ + int err; + struct i2c_msg msg[2]; + u8 reg_buf; + u8 gyro_data[sizeof(*data) * (total_read ? (total_read - 1) : 1)]; + + msg[0].addr = client->addr; + msg[0].buf = ®_buf; + msg[0].flags = 0; + msg[0].len = 1; + + msg[1].addr = client->addr; + msg[1].flags = I2C_M_RD; + msg[1].buf = gyro_data; + + if (total_read > 1) { + reg_buf = AXISDATA_REG | AC; + msg[1].len = sizeof(gyro_data); + + err = i2c_transfer(client->adapter, msg, 2); + if (err != 2) + return (err < 0) ? err : -EIO; + } + + reg_buf = AXISDATA_REG; + msg[1].len = 1; + err = i2c_transfer(client->adapter, msg, 2); + if (err != 2) + return (err < 0) ? err : -EIO; + + reg_buf = OUT_Y_L | AC; + msg[1].len = sizeof(*data); + err = i2c_transfer(client->adapter, msg, 2); + if (err != 2) + return (err < 0) ? err : -EIO; + + data->y = (gyro_data[1] << 8) | gyro_data[0]; + data->z = (gyro_data[3] << 8) | gyro_data[2]; + data->x = (gyro_data[5] << 8) | gyro_data[4]; + + return 0; +} + +static int k3g_report_gyro_values(struct k3g_data *k3g_data) +{ + int res; + struct k3g_t data; + + res = k3g_read_gyro_values(k3g_data->client, &data, + k3g_data->entries + k3g_data->drop_next_event); + if (res < 0) + return res; + + res = k3g_read_fifo_status(k3g_data); + + k3g_data->drop_next_event = !res; + + if (res >= 31 - k3g_data->entries) { + /* reset fifo to start again - data isn't trustworthy, + * our locked read might not have worked and we + * could have done i2c read in mid register update + */ + return k3g_restart_fifo(k3g_data); + } + + input_report_rel(k3g_data->input_dev, REL_RX, data.x); + input_report_rel(k3g_data->input_dev, REL_RY, data.y); + input_report_rel(k3g_data->input_dev, REL_RZ, data.z); + input_sync(k3g_data->input_dev); + + return res; +} + +static enum hrtimer_restart k3g_timer_func(struct hrtimer *timer) +{ + struct k3g_data *k3g_data = container_of(timer, struct k3g_data, timer); + queue_work(k3g_data->k3g_wq, &k3g_data->work); + return HRTIMER_NORESTART; +} + +static void k3g_work_func(struct work_struct *work) +{ + int res; + struct k3g_data *k3g_data = container_of(work, struct k3g_data, work); + + do { + res = k3g_read_fifo_status(k3g_data); + if (res < 0) + return; + + if (res < k3g_data->entries) { + pr_warning("%s: fifo entries are less than we want\n", + __func__); + goto timer_set; + } + + res = k3g_report_gyro_values(k3g_data); + if (res < 0) + return; +timer_set: + set_polling_delay(k3g_data, res); + + } while (!ktime_to_ns(k3g_data->polling_delay)); + + hrtimer_start(&k3g_data->timer, + k3g_data->polling_delay, HRTIMER_MODE_REL); +} + +static irqreturn_t k3g_interrupt_thread(int irq, void *k3g_data_p) +{ + int res; + struct k3g_data *k3g_data = k3g_data_p; + res = k3g_report_gyro_values(k3g_data); + if (res < 0) + pr_err("%s: failed to report gyro values\n", __func__); + + return IRQ_HANDLED; +} + +static ssize_t k3g_show_enable(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct k3g_data *k3g_data = dev_get_drvdata(dev); + return sprintf(buf, "%d\n", k3g_data->enable); +} + +static ssize_t k3g_set_enable(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err = 0; + struct k3g_data *k3g_data = dev_get_drvdata(dev); + bool new_enable; + + if (sysfs_streq(buf, "1")) + new_enable = true; + else if (sysfs_streq(buf, "0")) + new_enable = false; + else { + pr_debug("%s: invalid value %d\n", __func__, *buf); + return -EINVAL; + } + + if (new_enable == k3g_data->enable) + return size; + + mutex_lock(&k3g_data->lock); + if (new_enable) { + /* turning on */ + err = i2c_smbus_write_i2c_block_data(k3g_data->client, + CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs), + k3g_data->ctrl_regs); + if (err < 0) { + err = -EIO; + goto unlock; + } + + /* reset fifo entries */ + err = k3g_restart_fifo(k3g_data); + if (err < 0) { + err = -EIO; + goto turn_off; + } + + if (k3g_data->interruptible) + enable_irq(k3g_data->client->irq); + else { + set_polling_delay(k3g_data, 0); + hrtimer_start(&k3g_data->timer, + k3g_data->polling_delay, HRTIMER_MODE_REL); + } + } else { + if (k3g_data->interruptible) + disable_irq(k3g_data->client->irq); + else { + hrtimer_cancel(&k3g_data->timer); + cancel_work_sync(&k3g_data->work); + } + /* turning off */ + err = i2c_smbus_write_byte_data(k3g_data->client, + CTRL_REG1, 0x00); + if (err < 0) + goto unlock; + } + k3g_data->enable = new_enable; + +turn_off: + if (err < 0) + i2c_smbus_write_byte_data(k3g_data->client, + CTRL_REG1, 0x00); +unlock: + mutex_unlock(&k3g_data->lock); + + return err ? err : size; +} + +static ssize_t k3g_show_delay(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct k3g_data *k3g_data = dev_get_drvdata(dev); + u64 delay; + + delay = k3g_data->time_to_read * k3g_data->entries; + delay = ktime_to_ns(ns_to_ktime(delay)); + + return sprintf(buf, "%lld\n", delay); +} + +static ssize_t k3g_set_delay(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + struct k3g_data *k3g_data = dev_get_drvdata(dev); + int odr_value = ODR105_BW25; + int res = 0; + int i; + u64 delay_ns; + u8 ctrl; + + res = strict_strtoll(buf, 10, &delay_ns); + if (res < 0) + return res; + + mutex_lock(&k3g_data->lock); + if (!k3g_data->interruptible) + hrtimer_cancel(&k3g_data->timer); + else + disable_irq(k3g_data->client->irq); + + /* round to the nearest supported ODR that is less than + * the requested value + */ + for (i = 0; i < ARRAY_SIZE(odr_delay_table); i++) + if (delay_ns <= odr_delay_table[i].delay_ns) { + odr_value = odr_delay_table[i].odr; + delay_ns = odr_delay_table[i].delay_ns; + k3g_data->time_to_read = delay_ns; + k3g_data->entries = 1; + break; + } + + if (delay_ns >= odr_delay_table[3].delay_ns) { + if (delay_ns >= MAX_DELAY) { + k3g_data->entries = MAX_ENTRY; + delay_ns = MAX_DELAY; + } else { + do_div(delay_ns, odr_delay_table[3].delay_ns); + k3g_data->entries = delay_ns; + } + k3g_data->time_to_read = odr_delay_table[3].delay_ns; + } + + if (odr_value != (k3g_data->ctrl_regs[0] & ODR_MASK)) { + ctrl = (k3g_data->ctrl_regs[0] & ~ODR_MASK); + ctrl |= odr_value; + k3g_data->ctrl_regs[0] = ctrl; + res = i2c_smbus_write_byte_data(k3g_data->client, + CTRL_REG1, ctrl); + } + + /* we see a noise in the first sample or two after we + * change rates. this delay helps eliminate that noise. + */ + msleep((u32)delay_ns * 2 / NSEC_PER_MSEC); + + /* (re)start fifo */ + k3g_restart_fifo(k3g_data); + + if (!k3g_data->interruptible) { + delay_ns = k3g_data->entries * k3g_data->time_to_read; + k3g_data->polling_delay = ns_to_ktime(delay_ns); + if (k3g_data->enable) + hrtimer_start(&k3g_data->timer, + k3g_data->polling_delay, HRTIMER_MODE_REL); + } else + enable_irq(k3g_data->client->irq); + + mutex_unlock(&k3g_data->lock); + + return size; +} + +static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, + k3g_show_enable, k3g_set_enable); +static DEVICE_ATTR(poll_delay, S_IRUGO | S_IWUSR | S_IWGRP, + k3g_show_delay, k3g_set_delay); + +static int k3g_probe(struct i2c_client *client, + const struct i2c_device_id *devid) +{ + int ret; + int err = 0; + struct k3g_data *data; + struct input_dev *input_dev; + + if (client->dev.platform_data == NULL) { + dev_err(&client->dev, "platform data is NULL. exiting.\n"); + err = -ENODEV; + goto exit; + } + + data = kzalloc(sizeof(*data), GFP_KERNEL); + if (data == NULL) { + dev_err(&client->dev, + "failed to allocate memory for module data\n"); + err = -ENOMEM; + goto exit; + } + + data->client = client; + + /* read chip id */ + ret = i2c_smbus_read_byte_data(client, WHO_AM_I); + if (ret != DEVICE_ID) { + if (ret < 0) { + pr_err("%s: i2c for reading chip id failed\n", + __func__); + err = ret; + } else { + pr_err("%s : Device identification failed\n", + __func__); + err = -ENODEV; + } + goto err_read_reg; + } + + mutex_init(&data->lock); + + /* allocate gyro input_device */ + input_dev = input_allocate_device(); + if (!input_dev) { + pr_err("%s: could not allocate input device\n", __func__); + err = -ENOMEM; + goto err_input_allocate_device; + } + + data->input_dev = input_dev; + input_set_drvdata(input_dev, data); + input_dev->name = "gyro"; + /* X */ + input_set_capability(input_dev, EV_REL, REL_RX); + input_set_abs_params(input_dev, REL_RX, -2048, 2047, 0, 0); + /* Y */ + input_set_capability(input_dev, EV_REL, REL_RY); + input_set_abs_params(input_dev, REL_RY, -2048, 2047, 0, 0); + /* Z */ + input_set_capability(input_dev, EV_REL, REL_RZ); + input_set_abs_params(input_dev, REL_RZ, -2048, 2047, 0, 0); + + err = input_register_device(input_dev); + if (err < 0) { + pr_err("%s: could not register input device\n", __func__); + input_free_device(data->input_dev); + goto err_input_register_device; + } + + memcpy(&data->ctrl_regs, &default_ctrl_regs, sizeof(default_ctrl_regs)); + if (data->client->irq >= 0) { /* interrupt */ + data->interruptible = true; + err = request_threaded_irq(data->client->irq, NULL, + k3g_interrupt_thread, IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + "k3g", data); + if (err < 0) { + pr_err("%s: can't allocate irq.\n", __func__); + goto err_request_irq; + } + disable_irq(data->client->irq); + + } else { /* polling */ + u64 delay_ns; + data->ctrl_regs[2] = 0x00; /* disable interrupt */ + /* hrtimer settings. we poll for gyro values using a timer. */ + hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + data->polling_delay = ns_to_ktime(200 * NSEC_PER_MSEC); + data->time_to_read = 10000000LL; + delay_ns = ktime_to_ns(data->polling_delay); + do_div(delay_ns, data->time_to_read); + data->entries = delay_ns; + data->timer.function = k3g_timer_func; + + /* the timer just fires off a work queue request. + We need a thread to read i2c (can be slow and blocking). */ + data->k3g_wq = create_singlethread_workqueue("k3g_wq"); + if (!data->k3g_wq) { + err = -ENOMEM; + pr_err("%s: could not create workqueue\n", __func__); + goto err_create_workqueue; + } + /* this is the thread function we run on the work queue */ + INIT_WORK(&data->work, k3g_work_func); + } + + if (device_create_file(&input_dev->dev, + &dev_attr_enable) < 0) { + pr_err("Failed to create device file(%s)!\n", + dev_attr_enable.attr.name); + goto err_device_create_file; + } + + if (device_create_file(&input_dev->dev, + &dev_attr_poll_delay) < 0) { + pr_err("Failed to create device file(%s)!\n", + dev_attr_poll_delay.attr.name); + goto err_device_create_file2; + } + + i2c_set_clientdata(client, data); + dev_set_drvdata(&input_dev->dev, data); + + return 0; + +err_device_create_file2: + device_remove_file(&input_dev->dev, &dev_attr_enable); +err_device_create_file: + if (data->interruptible) { + enable_irq(data->client->irq); + free_irq(data->client->irq, data); + } else + destroy_workqueue(data->k3g_wq); + input_unregister_device(data->input_dev); +err_create_workqueue: +err_request_irq: +err_input_register_device: +err_input_allocate_device: + mutex_destroy(&data->lock); +err_read_reg: + kfree(data); +exit: + return err; +} + +static int k3g_remove(struct i2c_client *client) +{ + int err = 0; + struct k3g_data *k3g_data = i2c_get_clientdata(client); + + device_remove_file(&k3g_data->input_dev->dev, &dev_attr_enable); + device_remove_file(&k3g_data->input_dev->dev, &dev_attr_poll_delay); + + if (k3g_data->enable) + err = i2c_smbus_write_byte_data(k3g_data->client, + CTRL_REG1, 0x00); + if (k3g_data->interruptible) { + if (!k3g_data->enable) /* no disable_irq before free_irq */ + enable_irq(k3g_data->client->irq); + free_irq(k3g_data->client->irq, k3g_data); + + } else { + hrtimer_cancel(&k3g_data->timer); + cancel_work_sync(&k3g_data->work); + destroy_workqueue(k3g_data->k3g_wq); + } + + input_unregister_device(k3g_data->input_dev); + mutex_destroy(&k3g_data->lock); + kfree(k3g_data); + + return err; +} + +static int k3g_suspend(struct device *dev) +{ + int err = 0; + struct i2c_client *client = to_i2c_client(dev); + struct k3g_data *k3g_data = i2c_get_clientdata(client); + + if (k3g_data->enable) { + mutex_lock(&k3g_data->lock); + if (!k3g_data->interruptible) { + hrtimer_cancel(&k3g_data->timer); + cancel_work_sync(&k3g_data->work); + } + err = i2c_smbus_write_byte_data(k3g_data->client, + CTRL_REG1, 0x00); + mutex_unlock(&k3g_data->lock); + } + + return err; +} + +static int k3g_resume(struct device *dev) +{ + int err = 0; + struct i2c_client *client = to_i2c_client(dev); + struct k3g_data *k3g_data = i2c_get_clientdata(client); + + if (k3g_data->enable) { + mutex_lock(&k3g_data->lock); + if (!k3g_data->interruptible) + hrtimer_start(&k3g_data->timer, + k3g_data->polling_delay, HRTIMER_MODE_REL); + err = i2c_smbus_write_i2c_block_data(client, + CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs), + k3g_data->ctrl_regs); + mutex_unlock(&k3g_data->lock); + } + + return err; +} + +static const struct dev_pm_ops k3g_pm_ops = { + .suspend = k3g_suspend, + .resume = k3g_resume +}; + +static const struct i2c_device_id k3g_id[] = { + { "k3g", 0 }, + { } +}; + +MODULE_DEVICE_TABLE(i2c, k3g_id); + +static struct i2c_driver k3g_driver = { + .probe = k3g_probe, + .remove = __devexit_p(k3g_remove), + .id_table = k3g_id, + .driver = { + .pm = &k3g_pm_ops, + .owner = THIS_MODULE, + .name = "k3g" + }, +}; + +static int __init k3g_init(void) +{ + return i2c_add_driver(&k3g_driver); +} + +static void __exit k3g_exit(void) +{ + i2c_del_driver(&k3g_driver); +} + +module_init(k3g_init); +module_exit(k3g_exit); + +MODULE_DESCRIPTION("k3g digital gyroscope driver"); +MODULE_AUTHOR("Tim SK Lee Samsung Electronics "); +MODULE_LICENSE("GPL"); -- 2.34.1