From dadd0a8a2cf1fecbbb0df7713f3da81ce7c11b00 Mon Sep 17 00:00:00 2001 From: Andrey Gusakov Date: Wed, 16 Sep 2015 14:28:22 +0300 Subject: [PATCH 49/50] Add lsm9ds0 (acc gyro mag) driver Signed-off-by: Andrey Gusakov --- drivers/input/misc/Kconfig | 10 + drivers/input/misc/Makefile | 1 + drivers/input/misc/lsm9ds0_acc_mag.c | 3435 ++++++++++++++++++++++++++++++++++ drivers/input/misc/lsm9ds0_gyr.c | 1726 +++++++++++++++++ include/linux/input/lsm9ds0.h | 201 ++ 5 files changed, 5373 insertions(+) create mode 100644 drivers/input/misc/lsm9ds0_acc_mag.c create mode 100644 drivers/input/misc/lsm9ds0_gyr.c create mode 100644 include/linux/input/lsm9ds0.h diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig index bb698e1..f1eb2c8 100644 --- a/drivers/input/misc/Kconfig +++ b/drivers/input/misc/Kconfig @@ -197,6 +197,16 @@ config INPUT_MPU3050 To compile this driver as a module, choose M here: the module will be called mpu3050. +config INPUT_LSM9DS0 + tristate "LSM9DS0 iNEMO sensor" + depends on I2C + help + Say Y here if you want to support 3D accelerometer, + 3D gyroscope, 3D magnetometer LSM9DS0 + + To compile this driver as a module, choose M here: the + module will be called lsm9ds0. + config INPUT_APANEL tristate "Fujitsu Lifebook Application Panel buttons" depends on X86 && I2C && LEDS_CLASS diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile index d7fc17f..b726294 100644 --- a/drivers/input/misc/Makefile +++ b/drivers/input/misc/Makefile @@ -39,6 +39,7 @@ obj-$(CONFIG_INPUT_MAX8997_HAPTIC) += max8997_haptic.o obj-$(CONFIG_INPUT_MC13783_PWRBUTTON) += mc13783-pwrbutton.o obj-$(CONFIG_INPUT_MMA8450) += mma8450.o obj-$(CONFIG_INPUT_MPU3050) += mpu3050.o +obj-$(CONFIG_INPUT_LSM9DS0) += lsm9ds0_acc_mag.o lsm9ds0_gyr.o obj-$(CONFIG_INPUT_PCAP) += pcap_keys.o obj-$(CONFIG_INPUT_PCF50633_PMU) += pcf50633-input.o obj-$(CONFIG_INPUT_PCF8574) += pcf8574_keypad.o diff --git a/drivers/input/misc/lsm9ds0_acc_mag.c b/drivers/input/misc/lsm9ds0_acc_mag.c new file mode 100644 index 0000000..3aad517 --- /dev/null +++ b/drivers/input/misc/lsm9ds0_acc_mag.c @@ -0,0 +1,3435 @@ +/******************** (C) COPYRIGHT 2013 STMicroelectronics ******************* +* +* File Name : lsm9ds0_acc_mag.c +* Authors : AMS - Motion Sensors Div - Application Team +* : Matteo Dameno (matteo.dameno@st.com) +* : Denis Ciocca (denis.ciocca@st.com) +* : Both authors are willing to be considered the contact +* : and update points for the driver. +* Version : V.1.0.5 +* Date : 2013/Oct/23 +* Description : LSM9DS0 accelerometer & magnetometer driver +* +******************************************************************************* +* +* 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. +* +******************************************************************************/ +/****************************************************************************** +Version History. + +Revision 1-0-0 2012/05/04 + first revision +Revision 1-0-1 2012/05/07 + New sysfs architecture + Support antialiasing filter +Revision 1-0-2 2012/10/15 + I2C address bugfix +Revision 1-0-3 2013/01/21 + Move CTLREG7 resume write from acc_power_on to magn_power_on +Revision 1-0-4 2013/05/09 + Added rotation matrix +Revision 1-0-5 2013/10/23 + Corrects Mag Enable bug, Corrects missing BDU enable +******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +/* include "lsm9ds0.h" */ + +#define I2C_AUTO_INCREMENT (0x80) +#define MS_TO_NS(x) (x*1000000L) + +#define ACC_G_MAX_POS 1495040 /** max positive value acc [ug] */ +#define ACC_G_MAX_NEG 1495770 /** max negative value acc [ug] */ +#define MAG_G_MAX_POS 983520 /** max positive value mag [ugauss] */ +#define MAG_G_MAX_NEG 983040 /** max negative value mag [ugauss] */ + +#define FUZZ 0 +#define FLAT 0 + +/* Address registers */ +#define REG_WHOAMI_ADDR (0x0F) /** Who am i address register */ +#define REG_CNTRL0_ADDR (0x1F) /** CNTRL0 address register */ +#define REG_CNTRL1_ADDR (0x20) /** CNTRL1 address register */ +#define REG_CNTRL2_ADDR (0x21) /** CNTRL2 address register */ +#define REG_CNTRL3_ADDR (0x22) /** CNTRL3 address register */ +#define REG_CNTRL4_ADDR (0x23) /** CNTRL4 address register */ +#define REG_CNTRL5_ADDR (0x24) /** CNTRL5 address register */ +#define REG_CNTRL6_ADDR (0x25) /** CNTRL6 address register */ +#define REG_CNTRL7_ADDR (0x26) /** CNTRL7 address register */ + +#define REG_ACC_DATA_ADDR (0x28) /** Acc. data low address register */ +#define REG_MAG_DATA_ADDR (0x08) /** Mag. data low address register */ +#define REG_TEMP_DATA_ADDR (0x05) /** Temp. data low address register */ + +#define REG_GEN_MAG_ADDR (0x12) /** INT_CTRL_REG_M address register */ +#define INT_SRC_REG_M_ADDR (0x13) /** INT_SRC_REG_M address register */ +#define REG_GEN_MAG_THR_ADDR (0x14) /** INT_THS_L_M address register */ +#define MIG_THRESHOLD_ADDR_H (0x15) /** INT_THS_H_M address register */ +#define REG_GEN1_AXIS_ADDR (0x30) /** INT_GEN1_REG address register */ +#define INT_GEN1_SRC_ADDR (0x31) /** INT_GEN1_SRC address register */ +#define REG_GEN1_THR_ADDR (0x32) /** INT_GEN1_THS address register */ +#define REG_GEN1_DUR_ADDR (0x33) /** INT_GEN1_DUR address register */ +#define REG_GEN2_AXIS_ADDR (0x34) /** INT_GEN2_REG address register */ +#define INT_GEN2_SRC_ADDR (0x35) /** INT_GEN2_SRC address register */ +#define REG_GEN2_THR_ADDR (0x36) /** INT_GEN2_THS address register */ +#define REG_GEN2_DUR_ADDR (0x37) /** INT_GEN2_DUR address register */ + +/* Sensitivity */ +#define SENSITIVITY_ACC_2G 60 /** ug/LSB */ +#define SENSITIVITY_ACC_4G 120 /** ug/LSB */ +#define SENSITIVITY_ACC_8G 240 /** ug/LSB */ +#define SENSITIVITY_ACC_16G 730 /** ug/LSB */ + +#define SENSITIVITY_MAG_2G 80 /** ugauss/LSB */ +#define SENSITIVITY_MAG_4G 160 /** ugauss/LSB */ +#define SENSITIVITY_MAG_8G 320 /** ugauss/LSB */ +#define SENSITIVITY_MAG_12G 480 /** ugauss/LSB */ + +/* ODR */ +#define ODR_ACC_MASK (0XF0) /* Mask for odr change on acc */ +#define LSM9DS0_ACC_ODR_OFF (0x00) /* Power down */ +#define LSM9DS0_ACC_ODR3_125 (0x10) /* 3.25Hz output data rate */ +#define LSM9DS0_ACC_ODR6_25 (0x20) /* 6.25Hz output data rate */ +#define LSM9DS0_ACC_ODR12_5 (0x30) /* 12.5Hz output data rate */ +#define LSM9DS0_ACC_ODR25 (0x40) /* 25Hz output data rate */ +#define LSM9DS0_ACC_ODR50 (0x50) /* 50Hz output data rate */ +#define LSM9DS0_ACC_ODR100 (0x60) /* 100Hz output data rate */ +#define LSM9DS0_ACC_ODR200 (0x70) /* 200Hz output data rate */ +#define LSM9DS0_ACC_ODR400 (0x80) /* 400Hz output data rate */ +#define LSM9DS0_ACC_ODR800 (0x90) /* 800Hz output data rate */ +#define LSM9DS0_ACC_ODR1600 (0xA0) /* 1600Hz output data rate */ + +#define ODR_MAG_MASK (0X1C) /* Mask for odr change on mag */ +#define LSM9DS0_MAG_ODR3_125 (0x00) /* 3.25Hz output data rate */ +#define LSM9DS0_MAG_ODR6_25 (0x04) /* 6.25Hz output data rate */ +#define LSM9DS0_MAG_ODR12_5 (0x08) /* 12.5Hz output data rate */ +#define LSM9DS0_MAG_ODR25 (0x0C) /* 25Hz output data rate */ +#define LSM9DS0_MAG_ODR50 (0x10) /* 50Hz output data rate */ +#define LSM9DS0_MAG_ODR100 (0x14) /* 100Hz output data rate */ + +/* Magnetic sensor mode */ +#define MSMS_MASK (0x03) /* Mask magnetic sensor mode */ +#define POWEROFF_MAG (0x02) /* Power Down */ +#define CONTINUOS_CONVERSION (0x00) /* Continuos Conversion */ + +/* Default values loaded in probe function */ +#define WHOIAM_VALUE (0x49) /** Who Am I default value */ +#define REG_DEF_CNTRL0 (0x00) /** CNTRL0 default value */ +#define REG_DEF_CNTRL1 (0x0F) /** CNTRL1 default value */ +#define REG_DEF_CNTRL2 (0x00) /** CNTRL2 default value */ +#define REG_DEF_CNTRL3 (0x00) /** CNTRL3 default value */ +#define REG_DEF_CNTRL4 (0x00) /** CNTRL4 default value */ +#define REG_DEF_CNTRL5 (0x18) /** CNTRL5 default value */ +#define REG_DEF_CNTRL6 (0x20) /** CNTRL6 default value */ +#define REG_DEF_CNTRL7 (0x02) /** CNTRL7 default value */ + +#define REG_DEF_INT_CNTRL_MAG (0x00) /** INT_CTRL_REG_M default value */ +#define REG_DEF_INT_GEN1 (0x00) /** INT_GEN1_REG default value */ +#define REG_DEF_INT_GEN2 (0x00) /** INT_GEN2_REG default value */ +#define REG_DEF_IIG1_DURATION (0x00) /** INT_GEN1_DUR default value */ +#define REG_DEF_IIG2_DURATION (0x00) /** INT_GEN2_DUR default value */ +#define REG_DEF_IIG1_THRESHOLD (0x00) /** INT_GEN1_THS default value */ +#define REG_DEF_IIG2_THRESHOLD (0x00) /** INT_GEN2_THS default value */ +#define REG_DEF_MIG_THRESHOLD_L (0x00) /** INT_THS_L_M default value */ +#define REG_DEF_MIG_THRESHOLD_H (0x00) /** INT_THS_H_M default value */ + +#define REG_DEF_ALL_ZEROS (0x00) + +/* Accelerometer Filter */ +#define LSM9DS0_ACC_FILTER_MASK (0xC0) /* Mask for filter band change on acc */ +#define FILTER_773 773 /* Anti-Aliasing 773 Hz */ +#define FILTER_362 362 /* Anti-Aliasing 362 Hz */ +#define FILTER_194 194 /* Anti-Aliasing 194 Hz */ +#define FILTER_50 50 /* Anti-Aliasing 50 Hz */ + +/* Temperature */ +#define TEMP_MASK (0x80) /* Mask for temperature change */ +#define TEMP_ON (0x80) /* Enable temperature */ +#define TEMP_OFF (0x00) /* Disable temperature */ +#define TEMP_SENSITIVITY 8 /* Sensitivity temperature */ +#define OFFSET_TEMP 25 /* Offset temperature */ +#define NDTEMP 1000 /* Not Available temperature */ + +/* Interrupt */ +#define GEN1_PIN1_MASK (0x20) +#define GEN1_PIN2_MASK (0x40) +#define GEN2_PIN1_MASK (0x10) +#define GEN2_PIN2_MASK (0x20) +#define GEN_MAG_PIN1_MASK (0x08) +#define GEN_MAG_PIN2_MASK (0x10) +#define GEN_MAG_EN_MASK (0x01) +#define MAX_DUR_TH 127 +#define MAX_TH_MAG 131071 +#define GEN_X_HIGH_MASK (0x02) +#define GEN_X_LOW_MASK (0x01) +#define GEN_Y_HIGH_MASK (0x08) +#define GEN_Y_LOW_MASK (0x04) +#define GEN_Z_HIGH_MASK (0x20) +#define GEN_Z_LOW_MASK (0x10) +#define GEN_X_MAG_MASK (0x80) +#define GEN_Y_MAG_MASK (0x40) +#define GEN_Z_MAG_MASK (0x20) + +#define GEN1_AND_OR_MASK (0x80) +#define GEN2_AND_OR_MASK (0x83) + +#define INT_PIN_CONF_MASK (0x10) +#define INT_POLARITY_MASK (0x80) + +#define to_dev(obj) container_of(obj, struct device, kobj) +#define to_dev_attr(_attr) container_of(_attr, struct device_attribute, attr) + +static struct kobject *acc_kobj; +static struct kobject *mag_kobj; + +struct workqueue_struct *lsm9ds0_workqueue = 0; + +struct { + unsigned int cutoff_us; + u8 value; +} lsm9ds0_acc_odr_table[] = { + { 1, LSM9DS0_ACC_ODR800 }, + { 2, LSM9DS0_ACC_ODR400 }, + { 5, LSM9DS0_ACC_ODR200 }, + { 10, LSM9DS0_ACC_ODR100 }, + { 20, LSM9DS0_ACC_ODR50 }, + { 40, LSM9DS0_ACC_ODR25 }, + { 80, LSM9DS0_ACC_ODR12_5 }, + { 160, LSM9DS0_ACC_ODR6_25 }, + { 320, LSM9DS0_ACC_ODR3_125}, +}; + +struct { + unsigned int cutoff_us; + u8 value; +} lsm9ds0_mag_odr_table[] = { + { 10, LSM9DS0_MAG_ODR100 }, + { 20, LSM9DS0_MAG_ODR50 }, + { 40, LSM9DS0_MAG_ODR25 }, + { 80, LSM9DS0_MAG_ODR12_5 }, + { 160, LSM9DS0_MAG_ODR6_25 }, + { 320, LSM9DS0_MAG_ODR3_125}, +}; + +struct interrupt_enable { + atomic_t enable; + u8 address; + u8 mask; +}; + +struct interrupt_value { + int value; + u8 address; +}; + +struct lsm9ds0_interrupt { + struct interrupt_enable gen1_pin1; + struct interrupt_enable gen1_pin2; + struct interrupt_enable gen2_pin1; + struct interrupt_enable gen2_pin2; + struct interrupt_value gen1_threshold; + struct interrupt_value gen2_threshold; + struct interrupt_value gen1_duration; + struct interrupt_value gen2_duration; + struct interrupt_enable gen_mag_pin1; + struct interrupt_enable gen_mag_pin2; + struct interrupt_enable gen_mag; + struct interrupt_value gen_mag_threshold; + struct interrupt_enable gen1_axis[6]; + struct interrupt_enable gen2_axis[6]; + struct interrupt_enable gen_mag_axis[3]; + struct interrupt_enable gen1_and_or; + struct interrupt_enable gen2_and_or; + struct interrupt_enable interrupt_pin_conf; + struct interrupt_enable interrupt_polarity; +}; + +struct lsm9ds0_status { + struct i2c_client *client; + struct lsm9ds0_acc_platform_data *pdata_acc; + struct lsm9ds0_mag_platform_data *pdata_mag; + + struct mutex lock; + struct work_struct input_work_acc; + struct work_struct input_work_mag; + + struct hrtimer hr_timer_acc; + ktime_t ktime_acc; + struct hrtimer hr_timer_mag; + ktime_t ktime_mag; + + struct input_dev *input_dev_acc; + struct input_dev *input_dev_mag; + struct input_dev *input_dev_temp; + + struct lsm9ds0_interrupt *interrupt; + + int hw_initialized; + /* hw_working=-1 means not tested yet */ + int hw_working; + + atomic_t enabled_acc; + atomic_t enabled_mag; + atomic_t enabled_temp; + + int temp_value_dec; + unsigned int temp_value_flo; + + int on_before_suspend; + int use_smbus; + + u16 sensitivity_acc; + u16 sensitivity_mag; + + 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; +}; + +static const struct lsm9ds0_acc_platform_data default_lsm9ds0_acc_pdata = { + .fs_range = LSM9DS0_ACC_FS_2G, + .rot_matrix = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + .poll_interval = 100, + .min_interval = LSM9DS0_ACC_MIN_POLL_PERIOD_MS, + .aa_filter_bandwidth = ANTI_ALIASING_773, + .gpio_int1 = DEFAULT_INT1_GPIO, + .gpio_int2 = DEFAULT_INT2_GPIO, +}; + +static const struct lsm9ds0_mag_platform_data default_lsm9ds0_mag_pdata = { + .poll_interval = 100, + .min_interval = LSM9DS0_MAG_MIN_POLL_PERIOD_MS, + .fs_range = LSM9DS0_MAG_FS_2G, + .rot_matrix = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, +}; + +struct reg_rw { + u8 address; + u8 default_value; + u8 resume_value; +}; + +struct reg_r { + u8 address; + u8 value; +}; + +static struct status_registers { + struct reg_r who_am_i; + struct reg_rw cntrl0; + struct reg_rw cntrl1; + struct reg_rw cntrl2; + struct reg_rw cntrl3; + struct reg_rw cntrl4; + struct reg_rw cntrl5; + struct reg_rw cntrl6; + struct reg_rw cntrl7; + struct reg_rw int_ctrl_reg_m; + struct reg_rw int_mag_threshold_low; + struct reg_rw int_mag_threshold_high; + struct reg_rw int_gen1_reg; + struct reg_rw int_gen2_reg; + struct reg_rw int_gen1_duration; + struct reg_rw int_gen2_duration; + struct reg_rw int_gen1_threshold; + struct reg_rw int_gen2_threshold; + struct reg_r int_src_reg_m; + struct reg_r int_gen1_src; + struct reg_r int_gen2_src; + struct reg_r int_gen_mag_src; +} status_registers = { + .who_am_i.address=REG_WHOAMI_ADDR, .who_am_i.value=WHOIAM_VALUE, + .cntrl0.address=REG_CNTRL0_ADDR, .cntrl0.default_value=REG_DEF_CNTRL0, + .cntrl1.address=REG_CNTRL1_ADDR, .cntrl1.default_value=REG_DEF_CNTRL1, + .cntrl2.address=REG_CNTRL2_ADDR, .cntrl2.default_value=REG_DEF_CNTRL2, + .cntrl3.address=REG_CNTRL3_ADDR, .cntrl3.default_value=REG_DEF_CNTRL3, + .cntrl4.address=REG_CNTRL4_ADDR, .cntrl4.default_value=REG_DEF_CNTRL4, + .cntrl5.address=REG_CNTRL5_ADDR, .cntrl5.default_value=REG_DEF_CNTRL5, + .cntrl6.address=REG_CNTRL6_ADDR, .cntrl6.default_value=REG_DEF_CNTRL6, + .cntrl7.address=REG_CNTRL7_ADDR, .cntrl7.default_value=REG_DEF_CNTRL7, + .int_ctrl_reg_m.address=REG_GEN_MAG_ADDR, + .int_ctrl_reg_m.default_value=REG_DEF_INT_CNTRL_MAG, + .int_mag_threshold_low.address=REG_GEN_MAG_THR_ADDR, + .int_mag_threshold_low.default_value=REG_DEF_MIG_THRESHOLD_L, + .int_mag_threshold_low.address=MIG_THRESHOLD_ADDR_H, + .int_mag_threshold_low.default_value=REG_DEF_MIG_THRESHOLD_H, + .int_gen1_reg.address=REG_GEN1_AXIS_ADDR, + .int_gen1_reg.default_value=REG_DEF_INT_GEN1, + .int_gen2_reg.address=REG_GEN2_AXIS_ADDR, + .int_gen2_reg.default_value=REG_DEF_INT_GEN2, + .int_gen1_duration.address=REG_GEN1_DUR_ADDR, + .int_gen1_duration.default_value=REG_DEF_IIG1_DURATION, + .int_gen2_duration.address=REG_GEN2_DUR_ADDR, + .int_gen2_duration.default_value=REG_DEF_IIG2_DURATION, + .int_gen1_threshold.address=REG_GEN1_THR_ADDR, + .int_gen1_threshold.default_value=REG_DEF_IIG1_THRESHOLD, + .int_gen2_threshold.address=REG_GEN2_THR_ADDR, + .int_gen2_threshold.default_value=REG_DEF_IIG2_THRESHOLD, + .int_src_reg_m.address = INT_SRC_REG_M_ADDR, + .int_src_reg_m.value = REG_DEF_ALL_ZEROS, + .int_gen1_src.address = INT_GEN1_SRC_ADDR, + .int_gen1_src.value = REG_DEF_ALL_ZEROS, + .int_gen2_src.address = INT_GEN2_SRC_ADDR, + .int_gen2_src.value = REG_DEF_ALL_ZEROS, + .int_gen_mag_src.address = INT_SRC_REG_M_ADDR, + .int_gen_mag_src.value = REG_DEF_ALL_ZEROS, +}; + +static int lsm9ds0_i2c_read(struct lsm9ds0_status *stat, u8 *buf, int len) +{ + int ret; + u8 reg = buf[0]; + u8 cmd = reg; +#ifdef DEBUG + unsigned int ii; +#endif + + + if (len > 1) + cmd = (I2C_AUTO_INCREMENT | reg); + if (stat->use_smbus) { + if (len == 1) { + ret = i2c_smbus_read_byte_data(stat->client, cmd); + buf[0] = ret & 0xff; +#ifdef DEBUG + dev_warn(&stat->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(stat->client, + cmd, len, buf); +#ifdef DEBUG + dev_warn(&stat->client->dev, + "i2c_smbus_read_i2c_block_data: ret:%d len:%d, " + "command=0x%02x, ", + ret, len, cmd); + for (ii = 0; ii < len; ii++) + printk(KERN_DEBUG "buf[%d]=0x%02x,", + ii, buf[ii]); + + printk("\n"); +#endif + } else + ret = -1; + + if (ret < 0) { + dev_err(&stat->client->dev, + "read transfer error: len:%d, command=0x%02x\n", + len, cmd); + return 0; + } + return len; + } + + ret = i2c_master_send(stat->client, &cmd, sizeof(cmd)); + if (ret != sizeof(cmd)) + return ret; + + return i2c_master_recv(stat->client, buf, len); +} + +static int lsm9ds0_i2c_write(struct lsm9ds0_status *stat, u8 *buf, int len) +{ + int ret; + u8 reg, value; +#ifdef DEBUG + unsigned int ii; +#endif + + if (len > 1) + buf[0] = (I2C_AUTO_INCREMENT | buf[0]); + + reg = buf[0]; + value = buf[1]; + + if (stat->use_smbus) { + if (len == 1) { + ret = i2c_smbus_write_byte_data(stat->client, + reg, value); +#ifdef DEBUG + dev_warn(&stat->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(stat->client, + reg, len, buf + 1); +#ifdef DEBUG + dev_warn(&stat->client->dev, + "i2c_smbus_write_i2c_block_data: ret=%d, " + "len:%d, command=0x%02x, ", + ret, len, reg); + for (ii = 0; ii < (len + 1); ii++) + printk(KERN_DEBUG "value[%d]=0x%02x,", + ii, buf[ii]); + + printk("\n"); +#endif + return ret; + } + } + + ret = i2c_master_send(stat->client, buf, len+1); + return (ret == len+1) ? 0 : ret; +} + +static int lsm9ds0_hw_init(struct lsm9ds0_status *stat) +{ + int err = -1; + u8 buf[1]; + int i; + + pr_info("%s: hw init start\n", LSM9DS0_DEV_NAME); + + buf[0] = status_registers.who_am_i.address; + err = lsm9ds0_i2c_read(stat, buf, 1); + + if (err < 0) { + dev_warn(&stat->client->dev, "Error reading WHO_AM_I: is device" + " available/working?\n"); + goto err_firstread; + } else + stat->hw_working = 1; + + if (buf[0] != status_registers.who_am_i.value) { + dev_err(&stat->client->dev, + "device unknown. Expected: 0x%02x," + " Replies: 0x%02x\n", status_registers.who_am_i.value, buf[0]); + err = -1; + goto err_unknown_device; + } + + status_registers.cntrl1.resume_value = + status_registers.cntrl1.default_value; + status_registers.cntrl2.resume_value = + status_registers.cntrl2.default_value; + status_registers.cntrl3.resume_value = + status_registers.cntrl3.default_value; + status_registers.cntrl4.resume_value = + status_registers.cntrl4.default_value; + status_registers.cntrl5.resume_value = + status_registers.cntrl5.default_value; + status_registers.cntrl6.resume_value = + status_registers.cntrl6.default_value; + status_registers.cntrl7.resume_value = + status_registers.cntrl7.default_value; + + status_registers.int_ctrl_reg_m.resume_value = + status_registers.int_ctrl_reg_m.default_value; + status_registers.int_mag_threshold_low.resume_value = + status_registers.int_mag_threshold_low.default_value; + status_registers.int_mag_threshold_high.resume_value = + status_registers.int_mag_threshold_high.default_value; + status_registers.int_gen1_reg.resume_value = + status_registers.int_gen1_reg.default_value; + status_registers.int_gen2_reg.resume_value = + status_registers.int_gen2_reg.default_value; + status_registers.int_gen1_duration.resume_value = + status_registers.int_gen1_duration.default_value; + status_registers.int_gen2_duration.resume_value = + status_registers.int_gen2_duration.default_value; + status_registers.int_gen1_threshold.resume_value = + status_registers.int_gen1_threshold.default_value; + status_registers.int_gen2_threshold.resume_value = + status_registers.int_gen2_threshold.default_value; + + + stat->temp_value_dec = NDTEMP; + + if((stat->pdata_acc->gpio_int1 >= 0) || + (stat->pdata_acc->gpio_int2 >= 0)) { + + stat->interrupt = kmalloc(sizeof(*stat->interrupt), + GFP_KERNEL); + + if(stat->interrupt == NULL) + goto error_interrupt; + + stat->interrupt->gen1_pin1.address = REG_CNTRL3_ADDR; + stat->interrupt->gen1_pin2.address = REG_CNTRL4_ADDR; + stat->interrupt->gen2_pin1.address = REG_CNTRL3_ADDR; + stat->interrupt->gen2_pin2.address = REG_CNTRL4_ADDR; + stat->interrupt->gen_mag_pin1.address = REG_CNTRL3_ADDR; + stat->interrupt->gen_mag_pin2.address = REG_CNTRL4_ADDR; + stat->interrupt->gen_mag.address = REG_GEN_MAG_ADDR; + stat->interrupt->gen1_duration.address = REG_GEN1_DUR_ADDR; + stat->interrupt->gen2_duration.address = REG_GEN2_DUR_ADDR; + stat->interrupt->gen1_threshold.address = REG_GEN1_THR_ADDR; + stat->interrupt->gen2_threshold.address = REG_GEN2_THR_ADDR; + stat->interrupt->gen_mag_threshold.address = + REG_GEN_MAG_THR_ADDR; + + stat->interrupt->gen1_pin1.mask = GEN1_PIN1_MASK; + stat->interrupt->gen1_pin2.mask = GEN1_PIN2_MASK; + stat->interrupt->gen2_pin1.mask = GEN2_PIN1_MASK; + stat->interrupt->gen2_pin2.mask = GEN2_PIN2_MASK; + stat->interrupt->gen_mag_pin1.mask = GEN_MAG_PIN1_MASK; + stat->interrupt->gen_mag_pin2.mask = GEN_MAG_PIN2_MASK; + stat->interrupt->gen_mag.mask = GEN_MAG_EN_MASK; + + atomic_set(&stat->interrupt->gen1_pin1.enable, 0); + atomic_set(&stat->interrupt->gen1_pin2.enable, 0); + atomic_set(&stat->interrupt->gen2_pin1.enable, 0); + atomic_set(&stat->interrupt->gen2_pin2.enable, 0); + atomic_set(&stat->interrupt->gen_mag_pin1.enable, 0); + atomic_set(&stat->interrupt->gen_mag_pin2.enable, 0); + atomic_set(&stat->interrupt->gen_mag.enable, 0); + + stat->interrupt->gen1_threshold.value = 0; + stat->interrupt->gen2_threshold.value = 0; + stat->interrupt->gen1_duration.value = 0; + stat->interrupt->gen2_duration.value = 0; + stat->interrupt->gen_mag_threshold.value = 0; + + for(i=0; i<6; i++) { + stat->interrupt->gen1_axis[i].address = + REG_GEN1_AXIS_ADDR; + stat->interrupt->gen2_axis[i].address = + REG_GEN2_AXIS_ADDR; + + atomic_set(&stat->interrupt->gen1_axis[i].enable, 0); + atomic_set(&stat->interrupt->gen2_axis[i].enable, 0); + } + for(i=0; i<3; i++) { + stat->interrupt->gen_mag_axis[i].address = + REG_GEN_MAG_ADDR; + atomic_set(&stat->interrupt->gen_mag_axis[i].enable, 0); + } + + stat->interrupt->gen1_axis[0].mask = GEN_X_LOW_MASK; + stat->interrupt->gen1_axis[1].mask = GEN_Y_LOW_MASK; + stat->interrupt->gen1_axis[2].mask = GEN_Z_LOW_MASK; + stat->interrupt->gen1_axis[3].mask = GEN_X_HIGH_MASK; + stat->interrupt->gen1_axis[4].mask = GEN_Y_HIGH_MASK; + stat->interrupt->gen1_axis[5].mask = GEN_Z_HIGH_MASK; + + stat->interrupt->gen2_axis[0].mask = GEN_X_LOW_MASK; + stat->interrupt->gen2_axis[1].mask = GEN_Y_LOW_MASK; + stat->interrupt->gen2_axis[2].mask = GEN_Z_LOW_MASK; + stat->interrupt->gen2_axis[3].mask = GEN_X_HIGH_MASK; + stat->interrupt->gen2_axis[4].mask = GEN_Y_HIGH_MASK; + stat->interrupt->gen2_axis[5].mask = GEN_Z_HIGH_MASK; + + stat->interrupt->gen_mag_axis[0].mask = GEN_X_MAG_MASK; + stat->interrupt->gen_mag_axis[1].mask = GEN_Y_MAG_MASK; + stat->interrupt->gen_mag_axis[2].mask = GEN_Z_MAG_MASK; + + stat->interrupt->gen1_and_or.address = REG_GEN1_AXIS_ADDR; + stat->interrupt->gen1_and_or.mask = GEN1_AND_OR_MASK; + atomic_set(&stat->interrupt->gen1_and_or.enable, 0); + stat->interrupt->gen2_and_or.address = REG_GEN1_DUR_ADDR; + stat->interrupt->gen2_and_or.mask = GEN2_AND_OR_MASK; + atomic_set(&stat->interrupt->gen2_and_or.enable, 0); + + stat->interrupt->interrupt_pin_conf.address = REG_GEN_MAG_ADDR; + stat->interrupt->interrupt_pin_conf.mask = INT_PIN_CONF_MASK; + atomic_set(&stat->interrupt->interrupt_pin_conf.enable, 0); + + stat->interrupt->interrupt_polarity.address = REG_GEN_MAG_ADDR; + stat->interrupt->interrupt_polarity.mask = INT_POLARITY_MASK; + atomic_set(&stat->interrupt->interrupt_polarity.enable, 0); + } + + stat->hw_initialized = 1; + pr_info("%s: hw init done\n", LSM9DS0_DEV_NAME); + + return 0; + +error_interrupt: +err_unknown_device: +err_firstread: + stat->hw_working = 0; + stat->hw_initialized = 0; + return err; +} + +static irqreturn_t lsm9ds0_isr1(int irq, void *dev) +{ + struct lsm9ds0_status *stat = dev; + + disable_irq_nosync(irq); + queue_work(stat->irq1_work_queue, &stat->irq1_work); + pr_debug("%s: isr1 queued\n", LSM9DS0_DEV_NAME); + return IRQ_HANDLED; +} + +static irqreturn_t lsm9ds0_isr2(int irq, void *dev) +{ + struct lsm9ds0_status *stat = dev; + + disable_irq_nosync(irq); + queue_work(stat->irq2_work_queue, &stat->irq2_work); + pr_debug("%s: isr2 queued\n", LSM9DS0_DEV_NAME); + return IRQ_HANDLED; +} + +static void lsm9ds0_interrupt_catch(struct lsm9ds0_status *stat, int pin ) +{ + u8 buf[2]; + u8 val; + + if(atomic_read(&stat->interrupt->gen1_pin1.enable) == 1) { + buf[0] = status_registers.int_gen1_src.address; + val = lsm9ds0_i2c_read(stat, buf, 1); + if(val < 0) + return; + status_registers.int_gen1_src.value = buf[0]; + + if(((int)status_registers.int_gen1_src.value) > 64) + pr_info("interrupt send by accelerometer interrupt " + "generator 1\n"); + } + if(atomic_read(&stat->interrupt->gen_mag_pin1.enable) == 1) { + buf[0] = status_registers.int_gen_mag_src.address; + val = lsm9ds0_i2c_read(stat, buf, 1); + if(val < 0) + return; + status_registers.int_gen_mag_src.value = buf[0]; + + if(((int)status_registers.int_gen_mag_src.value) > 1) + pr_info("interrupt send by magnetometer interrupt " + "generator\n"); + } + +} + +static void lsm9ds0_irq1_work_func(struct work_struct *work) +{ + + struct lsm9ds0_status *stat = + container_of(work, struct lsm9ds0_status, irq1_work); + /* TODO add interrupt service procedure. + ie:lsm9ds0_get_int1_source(stat); */ + + lsm9ds0_interrupt_catch(stat,1); + pr_info("%s: IRQ1 triggered\n", LSM9DS0_DEV_NAME); +exit: + enable_irq(stat->irq1); +} + +static void lsm9ds0_irq2_work_func(struct work_struct *work) +{ + + struct lsm9ds0_status *stat = + container_of(work, struct lsm9ds0_status, irq2_work); + /* TODO add interrupt service procedure. + ie:lsm9ds0_get_int2_source(stat); */ + + lsm9ds0_interrupt_catch(stat,2); + pr_info("%s: IRQ2 triggered\n", LSM9DS0_DEV_NAME); +exit: + enable_irq(stat->irq2); +} + +static int lsm9ds0_acc_device_power_off(struct lsm9ds0_status *stat) +{ + int err; + u8 buf[2]; + + buf[0] = status_registers.cntrl1.address; + buf[1] = ((ODR_ACC_MASK & LSM9DS0_ACC_ODR_OFF) | + ((~ODR_ACC_MASK) & status_registers.cntrl1.resume_value)); + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + dev_err(&stat->client->dev, "accelerometer soft power off " + "failed: %d\n", err); + + if (stat->pdata_acc->power_off) { + stat->pdata_acc->power_off(); + } + + atomic_set(&stat->enabled_acc, 0); + + return 0; +} + +static int lsm9ds0_mag_device_power_off(struct lsm9ds0_status *stat) +{ + int err; + u8 buf[2]; + + buf[0] = status_registers.cntrl7.address; + buf[1] = ((MSMS_MASK & POWEROFF_MAG) | + ((~MSMS_MASK) & status_registers.cntrl7.resume_value)); + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + dev_err(&stat->client->dev, "magnetometer soft power off " + "failed: %d\n", err); + + if (stat->pdata_mag->power_off) { + stat->pdata_mag->power_off(); + } + + atomic_set(&stat->enabled_mag, 0); + + return 0; +} + +static int lsm9ds0_acc_device_power_on(struct lsm9ds0_status *stat) +{ + int err = -1; + u8 buf[5]; + + if (stat->pdata_acc->power_on) { + err = stat->pdata_acc->power_on(); + if (err < 0) { + dev_err(&stat->client->dev, + "accelerometer power_on failed: %d\n", err); + return err; + } + } + + buf[0] = status_registers.cntrl0.address; + buf[1] = status_registers.cntrl0.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.cntrl1.address; + buf[1] = status_registers.cntrl1.resume_value; + buf[2] = status_registers.cntrl2.resume_value; + buf[3] = status_registers.cntrl3.resume_value; + buf[4] = status_registers.cntrl4.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 4); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.int_gen1_reg.address; + buf[1] = status_registers.int_gen1_reg.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.int_gen1_threshold.address; + buf[1] = status_registers.int_gen1_threshold.resume_value; + buf[2] = status_registers.int_gen1_duration.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 2); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.int_gen2_reg.address; + buf[1] = status_registers.int_gen2_reg.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.int_gen2_threshold.address; + buf[1] = status_registers.int_gen2_threshold.resume_value; + buf[2] = status_registers.int_gen2_duration.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 2); + if (err < 0) + goto err_resume_state; + + atomic_set(&stat->enabled_acc, 1); + + return 0; + +err_resume_state: + atomic_set(&stat->enabled_acc, 0); + dev_err(&stat->client->dev, "accelerometer hw power on error " + "0x%02x,0x%02x: %d\n", buf[0], buf[1], err); + return err; +} + +static int lsm9ds0_mag_device_power_on(struct lsm9ds0_status *stat) +{ + int err = -1; + u8 buf[6]; + + if (stat->pdata_mag->power_on) { + err = stat->pdata_mag->power_on(); + if (err < 0) { + dev_err(&stat->client->dev, + "magnetometer power_on failed: %d\n", err); + return err; + } + } + + buf[0] = status_registers.cntrl0.address; + buf[1] = status_registers.cntrl0.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.cntrl3.address; + buf[1] = status_registers.cntrl3.resume_value; + buf[2] = status_registers.cntrl4.resume_value; + buf[3] = status_registers.cntrl5.resume_value; + buf[4] = status_registers.cntrl6.resume_value; + + err = lsm9ds0_i2c_write(stat, buf, 4); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.int_ctrl_reg_m.address; + buf[1] = status_registers.int_ctrl_reg_m.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.int_mag_threshold_low.address; + buf[1] = status_registers.int_mag_threshold_low.resume_value; + buf[2] = status_registers.int_mag_threshold_high.resume_value; + err = lsm9ds0_i2c_write(stat, buf, 2); + if (err < 0) + goto err_resume_state; + + buf[0] = status_registers.cntrl7.address; + buf[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) | + ((~MSMS_MASK) & status_registers.cntrl7.resume_value)); + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto err_resume_state; + + atomic_set(&stat->enabled_mag, 1); + + return 0; + +err_resume_state: + atomic_set(&stat->enabled_mag, 0); + dev_err(&stat->client->dev, "magnetometer hw power on error " + "0x%02x,0x%02x: %d\n", buf[0], buf[1], err); + return err; +} + +static int lsm9ds0_acc_update_filter(struct lsm9ds0_status *stat, + u8 new_bandwidth) +{ + int err=-1; + + u8 updated_val; + u8 buf[2]; + + switch (new_bandwidth) { + case ANTI_ALIASING_50: + break; + case ANTI_ALIASING_194: + break; + case ANTI_ALIASING_362: + break; + case ANTI_ALIASING_773: + break; + default: + dev_err(&stat->client->dev, "invalid accelerometer " + "update bandwidth requested: %u\n", new_bandwidth); + return -EINVAL; + } + + buf[0] = status_registers.cntrl2.address; + err = lsm9ds0_i2c_read(stat, buf, 1); + if (err < 0) + goto error; + + status_registers.cntrl2.resume_value = buf[0]; + updated_val = ((LSM9DS0_ACC_FILTER_MASK & new_bandwidth) | + ((~LSM9DS0_ACC_FILTER_MASK) & buf[0])); + buf[1] = updated_val; + buf[0] = status_registers.cntrl2.address; + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto error; + status_registers.cntrl2.resume_value = updated_val; + + return err; + +error: + dev_err(&stat->client->dev, "update accelerometer fs range failed " + "0x%02x,0x%02x: %d\n", buf[0], buf[1], err); + return err; +} + +static int lsm9ds0_acc_update_fs_range(struct lsm9ds0_status *stat, + u8 new_fs_range) +{ + int err=-1; + + u16 sensitivity; + u8 updated_val; + u8 buf[2]; + + switch (new_fs_range) { + case LSM9DS0_ACC_FS_2G: + sensitivity = SENSITIVITY_ACC_2G; + break; + case LSM9DS0_ACC_FS_4G: + sensitivity = SENSITIVITY_ACC_4G; + break; + case LSM9DS0_ACC_FS_8G: + sensitivity = SENSITIVITY_ACC_8G; + break; + case LSM9DS0_ACC_FS_16G: + sensitivity = SENSITIVITY_ACC_16G; + break; + default: + dev_err(&stat->client->dev, "invalid accelerometer " + "fs range requested: %u\n", new_fs_range); + return -EINVAL; + } + + buf[0] = status_registers.cntrl2.address; + err = lsm9ds0_i2c_read(stat, buf, 1); + if (err < 0) + goto error; + + status_registers.cntrl2.resume_value = buf[0]; + updated_val = ((LSM9DS0_ACC_FS_MASK & new_fs_range) | + ((~LSM9DS0_ACC_FS_MASK) & buf[0])); + buf[1] = updated_val; + buf[0] = status_registers.cntrl2.address; + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto error; + status_registers.cntrl2.resume_value = updated_val; + stat->sensitivity_acc = sensitivity; + + return err; + +error: + dev_err(&stat->client->dev, "update accelerometer fs range failed " + "0x%02x,0x%02x: %d\n", buf[0], buf[1], err); + return err; +} + +static int lsm9ds0_mag_update_fs_range(struct lsm9ds0_status *stat, + u8 new_fs_range) +{ + int err=-1; + + u16 sensitivity; + u8 updated_val; + u8 buf[2]; + + switch (new_fs_range) { + case LSM9DS0_MAG_FS_2G: + sensitivity = SENSITIVITY_MAG_2G; + break; + case LSM9DS0_MAG_FS_4G: + sensitivity = SENSITIVITY_MAG_4G; + break; + case LSM9DS0_MAG_FS_8G: + sensitivity = SENSITIVITY_MAG_8G; + break; + case LSM9DS0_MAG_FS_12G: + sensitivity = SENSITIVITY_MAG_12G; + break; + default: + dev_err(&stat->client->dev, "invalid magnetometer " + "fs range requested: %u\n", new_fs_range); + return -EINVAL; + } + + buf[0] = status_registers.cntrl6.address; + err = lsm9ds0_i2c_read(stat, buf, 1); + if (err < 0) + goto error; + + status_registers.cntrl6.resume_value = buf[0]; + updated_val = (LSM9DS0_MAG_FS_MASK & new_fs_range); + buf[1] = updated_val; + buf[0] = status_registers.cntrl6.address; + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto error; + status_registers.cntrl6.resume_value = updated_val; + stat->sensitivity_mag = sensitivity; + + return err; + +error: + dev_err(&stat->client->dev, "update magnetometer fs range failed " + "0x%02x,0x%02x: %d\n", buf[0], buf[1], err); + return err; +} + +static int lsm9ds0_acc_update_odr(struct lsm9ds0_status *stat, + unsigned int poll_interval_ms) +{ + int err = -1; + u8 config[2]; + int i; + + for (i = ARRAY_SIZE(lsm9ds0_acc_odr_table) - 1; i >= 0; i--) { + if ((lsm9ds0_acc_odr_table[i].cutoff_us <= poll_interval_ms) + || (i == 0)) + break; + } + + config[1] = ((ODR_ACC_MASK & lsm9ds0_acc_odr_table[i].value) | + ((~ODR_ACC_MASK) & status_registers.cntrl1.resume_value)); + + if (atomic_read(&stat->enabled_acc)) { + config[0] = status_registers.cntrl1.address; + err = lsm9ds0_i2c_write(stat, config, 1); + if (err < 0) + goto error; + status_registers.cntrl1.resume_value = config[1]; + stat->ktime_acc = ktime_set(0, MS_TO_NS(poll_interval_ms)); + } + + return err; + +error: + dev_err(&stat->client->dev, "update accelerometer odr failed " + "0x%02x,0x%02x: %d\n", config[0], config[1], err); + + return err; +} + +static int lsm9ds0_mag_update_odr(struct lsm9ds0_status *stat, + unsigned int poll_interval_ms) +{ + int err = -1; + u8 config[2]; + int i; + + for (i = ARRAY_SIZE(lsm9ds0_mag_odr_table) - 1; i >= 0; i--) { + if ((lsm9ds0_mag_odr_table[i].cutoff_us <= poll_interval_ms) + || (i == 0)) + break; + } + + config[1] = ((ODR_MAG_MASK & lsm9ds0_mag_odr_table[i].value) | + ((~ODR_MAG_MASK) & status_registers.cntrl5.resume_value)); + + if (atomic_read(&stat->enabled_mag)) { + config[0] = status_registers.cntrl5.address; + err = lsm9ds0_i2c_write(stat, config, 1); + if (err < 0) + goto error; + status_registers.cntrl5.resume_value = config[1]; + stat->ktime_mag = ktime_set(0, MS_TO_NS(poll_interval_ms)); + } + + return err; + +error: + dev_err(&stat->client->dev, "update magnetometer odr failed " + "0x%02x,0x%02x: %d\n", config[0], config[1], err); + + return err; +} + +static void lsm9ds0_validate_polling(unsigned int *min_interval, + unsigned int *poll_interval, + unsigned int min, + struct i2c_client *client) +{ + *min_interval = max(min, *min_interval); + *poll_interval = max(*poll_interval, *min_interval); +} + +static int lsm9ds0_acc_validate_pdata(struct lsm9ds0_status *stat) +{ + int res = -EINVAL; + + lsm9ds0_validate_polling(&stat->pdata_acc->min_interval, + &stat->pdata_acc->poll_interval, + (unsigned int)LSM9DS0_ACC_MIN_POLL_PERIOD_MS, + stat->client); + + switch (stat->pdata_acc->aa_filter_bandwidth) { + case ANTI_ALIASING_50: + res = 1; + break; + case ANTI_ALIASING_194: + res = 1; + break; + case ANTI_ALIASING_362: + res = 1; + break; + case ANTI_ALIASING_773: + res = 1; + break; + default: + dev_err(&stat->client->dev, "invalid accelerometer " + "bandwidth selected: %u\n", + stat->pdata_acc->aa_filter_bandwidth); + } + + return res; +} + +static int lsm9ds0_mag_validate_pdata(struct lsm9ds0_status *stat) +{ + lsm9ds0_validate_polling(&stat->pdata_mag->min_interval, + &stat->pdata_mag->poll_interval, + (unsigned int)LSM9DS0_MAG_MIN_POLL_PERIOD_MS, + stat->client); + + return 0; +} + +static int lsm9ds0_acc_enable(struct lsm9ds0_status *stat) +{ + int err; + + if (!atomic_cmpxchg(&stat->enabled_acc, 0, 1)) { + err = lsm9ds0_acc_device_power_on(stat); + if (err < 0) { + atomic_set(&stat->enabled_acc, 0); + return err; + } + hrtimer_start(&stat->hr_timer_acc, stat->ktime_acc, HRTIMER_MODE_REL); + if(!atomic_read(&stat->enabled_mag)) { + if(stat->pdata_acc->gpio_int1 >= 0) + enable_irq(stat->irq1); + if(stat->pdata_acc->gpio_int2 >= 0) + enable_irq(stat->irq2); + } + } + + return 0; +} + +static int lsm9ds0_acc_disable(struct lsm9ds0_status *stat) +{ + if (atomic_cmpxchg(&stat->enabled_acc, 1, 0)) { + cancel_work_sync(&stat->input_work_acc); + hrtimer_cancel(&stat->hr_timer_acc); + lsm9ds0_acc_device_power_off(stat); + + if(!atomic_read(&stat->enabled_mag)) { + if(stat->pdata_acc->gpio_int1 >= 0) + disable_irq_nosync(stat->irq1); + if(stat->pdata_acc->gpio_int2 >= 0) + disable_irq_nosync(stat->irq2); + } + } + + return 0; +} + +static int lsm9ds0_mag_enable(struct lsm9ds0_status *stat) +{ + int err; + + if (!atomic_cmpxchg(&stat->enabled_mag, 0, 1)) { + err = lsm9ds0_mag_device_power_on(stat); + if (err < 0) { + atomic_set(&stat->enabled_mag, 0); + return err; + } + if(!atomic_read(&stat->enabled_temp)) { + hrtimer_start(&stat->hr_timer_mag, stat->ktime_mag, HRTIMER_MODE_REL); + } + if(!atomic_read(&stat->enabled_acc)) { + if(stat->pdata_acc->gpio_int1 >= 0) + enable_irq(stat->irq1); + if(stat->pdata_acc->gpio_int2 >= 0) + enable_irq(stat->irq2); + } + } + + return 0; +} + +static int lsm9ds0_mag_disable(struct lsm9ds0_status *stat) +{ + if (atomic_cmpxchg(&stat->enabled_mag, 1, 0)) { + if(!atomic_read(&stat->enabled_temp)) { + cancel_work_sync(&stat->input_work_mag); + hrtimer_cancel(&stat->hr_timer_mag); + } + lsm9ds0_mag_device_power_off(stat); + if(!atomic_read(&stat->enabled_acc)) { + if(stat->pdata_acc->gpio_int1 >= 0) + disable_irq(stat->irq1); + if(stat->pdata_acc->gpio_int2 >= 0) + disable_irq(stat->irq2); + } + } + + return 0; +} + +static int lsm9ds0_temperature_enable(struct lsm9ds0_status *stat) +{ + int err; + u8 buf[2]; + u8 updated_val; + + buf[0] = status_registers.cntrl5.address; + err = lsm9ds0_i2c_read(stat, buf, 1); + if (err < 0) + goto error; + + status_registers.cntrl5.resume_value = buf[0]; + updated_val = ((TEMP_MASK & TEMP_ON) | + ((~TEMP_MASK) & buf[0])); + buf[1] = updated_val; + buf[0] = status_registers.cntrl5.address; + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto error; + status_registers.cntrl5.resume_value = updated_val; + + if(!atomic_read(&stat->enabled_mag)) { + hrtimer_start(&stat->hr_timer_mag, stat->ktime_mag, HRTIMER_MODE_REL); + } + atomic_set(&stat->enabled_temp, 1); + return 0; + +error: + return -1; +} + +static int lsm9ds0_temperature_disable(struct lsm9ds0_status *stat) +{ + int err; + u8 buf[2]; + u8 updated_val; + + buf[0] = status_registers.cntrl5.address; + err = lsm9ds0_i2c_read(stat, buf, 1); + if (err < 0) + goto error; + + status_registers.cntrl5.resume_value = buf[0]; + updated_val = ((TEMP_MASK & TEMP_OFF) | + ((~TEMP_MASK) & buf[0])); + buf[1] = updated_val; + buf[0] = status_registers.cntrl5.address; + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + goto error; + status_registers.cntrl5.resume_value = updated_val; + + if(!atomic_read(&stat->enabled_mag)) { + cancel_work_sync(&stat->input_work_mag); + hrtimer_cancel(&stat->hr_timer_mag); + } + atomic_set(&stat->enabled_temp, 0); + stat->temp_value_dec = NDTEMP; + return 0; + +error: + return -1; +} + +static void lsm9ds0_acc_input_cleanup(struct lsm9ds0_status *stat) +{ + input_unregister_device(stat->input_dev_acc); + input_free_device(stat->input_dev_acc); +} + +static void lsm9ds0_mag_input_cleanup(struct lsm9ds0_status *stat) +{ + input_unregister_device(stat->input_dev_mag); + input_free_device(stat->input_dev_mag); +} + +static ssize_t attr_get_polling_rate_acc(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + unsigned int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + mutex_lock(&stat->lock); + val = stat->pdata_acc->poll_interval; + mutex_unlock(&stat->lock); + return sprintf(buf, "%u\n", val); +} + +static ssize_t attr_get_polling_rate_mag(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + unsigned int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + mutex_lock(&stat->lock); + val = stat->pdata_mag->poll_interval; + mutex_unlock(&stat->lock); + return sprintf(buf, "%u\n", val); +} + +static ssize_t attr_set_polling_rate_acc(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long interval_ms; + + if (strict_strtoul(buf, 10, &interval_ms)) + return -EINVAL; + if (!interval_ms) + return -EINVAL; + interval_ms = (unsigned int)max((unsigned int)interval_ms, + stat->pdata_acc->min_interval); + mutex_lock(&stat->lock); + stat->pdata_acc->poll_interval = (unsigned int)interval_ms; + lsm9ds0_acc_update_odr(stat, interval_ms); + mutex_unlock(&stat->lock); + return size; +} + +static ssize_t attr_set_polling_rate_mag(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long interval_ms; + + if (strict_strtoul(buf, 10, &interval_ms)) + return -EINVAL; + if (!interval_ms) + return -EINVAL; + interval_ms = (unsigned int)max((unsigned int)interval_ms, + stat->pdata_mag->min_interval); + mutex_lock(&stat->lock); + stat->pdata_mag->poll_interval = (unsigned int)interval_ms; + lsm9ds0_mag_update_odr(stat, interval_ms); + mutex_unlock(&stat->lock); + return size; +} + +static ssize_t attr_get_enable_acc(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + int val = (int)atomic_read(&stat->enabled_acc); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_get_enable_mag(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + int val = (int)atomic_read(&stat->enabled_mag); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_enable_acc(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if (val) + lsm9ds0_acc_enable(stat); + else + lsm9ds0_acc_disable(stat); + + return size; +} + +static ssize_t attr_set_enable_mag(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if (val) + lsm9ds0_mag_enable(stat); + else + lsm9ds0_mag_disable(stat); + + return size; +} + +static ssize_t attr_get_range_acc(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + struct device *dev = to_dev(kobj->parent); + u8 val; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + int range = 2; + mutex_lock(&stat->lock); + val = stat->pdata_acc->fs_range ; + switch (val) { + case LSM9DS0_ACC_FS_2G: + range = 2; + break; + case LSM9DS0_ACC_FS_4G: + range = 4; + break; + case LSM9DS0_ACC_FS_8G: + range = 8; + break; + case LSM9DS0_ACC_FS_16G: + range = 16; + break; + } + mutex_unlock(&stat->lock); + return sprintf(buf, "%d\n", range); +} + +static ssize_t attr_get_range_mag(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + struct device *dev = to_dev(kobj->parent); + u8 val; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + int range = 2; + mutex_lock(&stat->lock); + val = stat->pdata_mag->fs_range ; + switch (val) { + case LSM9DS0_MAG_FS_2G: + range = 2; + break; + case LSM9DS0_MAG_FS_4G: + range = 4; + break; + case LSM9DS0_MAG_FS_8G: + range = 8; + break; + case LSM9DS0_MAG_FS_12G: + range = 12; + break; + } + mutex_unlock(&stat->lock); + return sprintf(buf, "%d\n", range); +} + +static ssize_t attr_set_range_acc(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long val; + u8 range; + int err; + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + switch (val) { + case 2: + range = LSM9DS0_ACC_FS_2G; + break; + case 4: + range = LSM9DS0_ACC_FS_4G; + break; + case 8: + range = LSM9DS0_ACC_FS_8G; + break; + case 16: + range = LSM9DS0_ACC_FS_16G; + break; + default: + dev_err(&stat->client->dev, "accelerometer invalid range " + "request: %lu, discarded\n", val); + return -EINVAL; + } + mutex_lock(&stat->lock); + err = lsm9ds0_acc_update_fs_range(stat, range); + if (err < 0) { + mutex_unlock(&stat->lock); + return err; + } + stat->pdata_acc->fs_range = range; + mutex_unlock(&stat->lock); + dev_info(&stat->client->dev, "accelerometer range set to:" + " %lu g\n", val); + + return size; +} + +static ssize_t attr_set_range_mag(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long val; + u8 range; + int err; + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + switch (val) { + case 2: + range = LSM9DS0_MAG_FS_2G; + break; + case 4: + range = LSM9DS0_MAG_FS_4G; + break; + case 8: + range = LSM9DS0_MAG_FS_8G; + break; + case 12: + range = LSM9DS0_MAG_FS_12G; + break; + default: + dev_err(&stat->client->dev, "magnetometer invalid range " + "request: %lu, discarded\n", val); + return -EINVAL; + } + mutex_lock(&stat->lock); + err = lsm9ds0_mag_update_fs_range(stat, range); + if (err < 0) { + mutex_unlock(&stat->lock); + return err; + } + stat->pdata_mag->fs_range = range; + mutex_unlock(&stat->lock); + dev_info(&stat->client->dev, "magnetometer range set to:" + " %lu g\n", val); + + return size; +} + +static ssize_t attr_get_aa_filter(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + struct device *dev = to_dev(kobj->parent); + u8 val; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + int frequency=FILTER_773; + mutex_lock(&stat->lock); + val = stat->pdata_acc->aa_filter_bandwidth; + switch (val) { + case ANTI_ALIASING_50: + frequency = FILTER_50; + break; + case ANTI_ALIASING_194: + frequency = FILTER_194; + break; + case ANTI_ALIASING_362: + frequency = FILTER_362; + break; + case ANTI_ALIASING_773: + frequency = FILTER_773; + break; + } + mutex_unlock(&stat->lock); + return sprintf(buf, "%d\n", frequency); +} + +static ssize_t attr_set_aa_filter(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + unsigned long val; + u8 frequency; + int err; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + switch (val) { + case FILTER_50: + frequency = ANTI_ALIASING_50; + break; + case FILTER_194: + frequency = ANTI_ALIASING_194; + break; + case FILTER_362: + frequency = ANTI_ALIASING_362; + break; + case FILTER_773: + frequency = ANTI_ALIASING_773; + break; + default: + dev_err(&stat->client->dev, "accelerometer invalid filter " + "request: %lu, discarded\n", val); + return -EINVAL; + } + mutex_lock(&stat->lock); + err = lsm9ds0_acc_update_filter(stat, frequency); + if (err < 0) { + mutex_unlock(&stat->lock); + return err; + } + stat->pdata_acc->aa_filter_bandwidth = frequency; + mutex_unlock(&stat->lock); + dev_info(&stat->client->dev, "accelerometer anti-aliasing filter " + "set to: %lu Hz\n", val); + + return size; +} + +static ssize_t attr_get_temp_enable(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + int val = (int)atomic_read(&stat->enabled_temp); + + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_temp_enable(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + unsigned long val; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + atomic_set(&stat->enabled_temp, (int)val); + + if(val>0) { + lsm9ds0_temperature_enable(stat); + } else { + lsm9ds0_temperature_disable(stat); + } + + return size; +} + +static ssize_t attr_get_temp(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int dec; + unsigned int flo; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + dec = stat->temp_value_dec; + flo = stat->temp_value_flo; + + if(dec==NDTEMP) + return sprintf(buf, "n.d.\n"); + + return sprintf(buf, "%d.%u\n", dec, flo); +} + +static struct kobj_attribute poll_attr_acc = +__ATTR(pollrate_ms, 0664, attr_get_polling_rate_acc, attr_set_polling_rate_acc); +static struct kobj_attribute enable_attr_acc = +__ATTR(enable_device, 0664, attr_get_enable_acc, attr_set_enable_acc); +static struct kobj_attribute fs_attr_acc = +__ATTR(full_scale, 0664, attr_get_range_acc, attr_set_range_acc); +static struct kobj_attribute aa_filter_attr = +__ATTR(anti_aliasing_frequency, 0664, attr_get_aa_filter, attr_set_aa_filter); +static struct kobj_attribute poll_attr_mag = +__ATTR(pollrate_ms, 0664, attr_get_polling_rate_mag, attr_set_polling_rate_mag); +static struct kobj_attribute enable_attr_mag = +__ATTR(enable_device, 0664, attr_get_enable_mag, attr_set_enable_mag); +static struct kobj_attribute fs_attr_mag = +__ATTR(full_scale, 0664, attr_get_range_mag, attr_set_range_mag); + +static int write_bit_on_register(struct lsm9ds0_status *stat, u8 address, + u8 *resume_value, u8 mask, int value) +{ + int err; + u8 updated_val; + u8 buf[2]; + u8 val = 0x00; + + buf[0] = address; + err = lsm9ds0_i2c_read(stat, buf, 1); + if (err < 0) + return -1; + + if(resume_value != NULL) + *resume_value = buf[0]; + + if(mask == 0) + updated_val = (u8)value; + else { + if(value>0) + val = 0xFF; + updated_val = (mask & val) | ((~mask) & buf[0]); + } + + buf[1] = updated_val; + buf[0] = address; + + err = lsm9ds0_i2c_write(stat, buf, 1); + if (err < 0) + return -1; + + if(resume_value != NULL) + *resume_value = updated_val; + + return err; +} + +static int write_gen_int(struct lsm9ds0_status *stat, + struct interrupt_enable *ie, int val) +{ + int err; + + if(val>0) + val = 1; + else + val = 0; + + err = write_bit_on_register(stat, ie->address, NULL, ie->mask, val); + if(err < 0) + return -1; + + atomic_set(&ie->enable, val); + return err; +} + +static int write_duration_threshold_int(struct lsm9ds0_status *stat, + struct interrupt_value *ie, int val) +{ + int err; + + if(val<0) + return -1; + + if(val>MAX_DUR_TH) + return -1; + + err = write_bit_on_register(stat, ie->address, NULL, 0, val); + if(err<0) + return -1; + + ie->value = val; + + return err; +} + +static int write_threshold_mag_int(struct lsm9ds0_status *stat, + struct interrupt_value *ie, int val) +{ + int err; + u8 high; + u8 low; + + if(val<0) + return -1; + + if(val>MAX_TH_MAG) + return -1; + + low = (u8)(0xff & val); + + err = write_bit_on_register(stat, ie->address, NULL, 0, low); + if(err<0) + return -1; + + high = (u8)(0xff & (val >> 8)); + + err = write_bit_on_register(stat, (ie->address)+1, NULL, 0, high); + if(err<0) + return -1; + + ie->value = val; + + return err; +} + +static ssize_t attr_get_gen1_status(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val = -1; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if(strcmp(attr->attr.name, "pin1_enable") == 0) { + val = atomic_read(&stat->interrupt->gen1_pin1.enable); + } + if(strcmp(attr->attr.name, "pin2_enable") == 0) { + val = atomic_read(&stat->interrupt->gen1_pin2.enable); + } + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen1_status(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if(strcmp(attr->attr.name, "pin1_enable") == 0) { + err = write_gen_int(stat, + &stat->interrupt->gen1_pin1, (int)val); + } + if(strcmp(attr->attr.name, "pin2_enable") == 0) { + err = write_gen_int(stat, + &stat->interrupt->gen1_pin2, (int)val); + } + return size; +} + +static ssize_t attr_get_gen2_status(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val = -1; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if(strcmp(attr->attr.name, "pin1_enable") == 0) { + val = atomic_read(&stat->interrupt->gen2_pin1.enable); + } + if(strcmp(attr->attr.name, "pin2_enable") == 0) { + val = atomic_read(&stat->interrupt->gen2_pin2.enable); + } + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen2_status(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if(strcmp(attr->attr.name, "pin1_enable") == 0) { + err = write_gen_int(stat, + &stat->interrupt->gen2_pin1, (int)val); + } + if(strcmp(attr->attr.name, "pin2_enable") == 0) { + err = write_gen_int(stat, + &stat->interrupt->gen2_pin2, (int)val); + } + return size; +} + +static ssize_t attr_get_gen1_duration(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = stat->interrupt->gen1_duration.value; + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen1_duration(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_duration_threshold_int(stat, + &stat->interrupt->gen1_duration, (int)val); + + return size; +} + +static ssize_t attr_get_gen2_duration(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = stat->interrupt->gen2_duration.value; + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen2_duration(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_duration_threshold_int(stat, + &stat->interrupt->gen2_duration, (int)val); + + return size; +} + +static ssize_t attr_get_gen1_threshold(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = stat->interrupt->gen1_threshold.value; + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen1_threshold(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_duration_threshold_int(stat, + &stat->interrupt->gen1_threshold, (int)val); + + return size; +} + +static ssize_t attr_get_gen2_threshold(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = stat->interrupt->gen2_threshold.value; + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen2_threshold(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_duration_threshold_int(stat, + &stat->interrupt->gen2_threshold, (int)val); + + return size; +} + +static ssize_t attr_get_gen_mag_status(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val = -1; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if(strcmp(attr->attr.name, "pin1_enable") == 0) { + val = atomic_read(&stat->interrupt->gen_mag_pin1.enable); + } + if(strcmp(attr->attr.name, "pin2_enable") == 0) { + val = atomic_read(&stat->interrupt->gen_mag_pin2.enable); + } + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen_mag_status(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if(strcmp(attr->attr.name, "pin1_enable") == 0) { + err = write_gen_int(stat, + &stat->interrupt->gen_mag_pin1, (int)val); + if(err >= 0) { + if((atomic_read(&stat->interrupt->gen_mag_pin2.enable))==0) + write_gen_int(stat, + &stat->interrupt->gen_mag, (int)val); + } + } + if(strcmp(attr->attr.name, "pin2_enable") == 0) { + err = write_gen_int(stat, + &stat->interrupt->gen_mag_pin2, (int)val); + if(err >= 0) { + if((atomic_read(&stat->interrupt->gen_mag_pin1.enable))==0) + write_gen_int(stat, + &stat->interrupt->gen_mag, (int)val); + } + } + return size; +} + +static ssize_t attr_get_gen_mag_threshold(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = stat->interrupt->gen_mag_threshold.value; + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen_mag_threshold(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err = -1; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_threshold_mag_int(stat, + &stat->interrupt->gen_mag_threshold, (int)val); + + return size; +} + +static int get_axis(struct lsm9ds0_status *stat, + int generator, const char *name) { + + int val; + int axis; + + if(strcmp(name, "x_high_enable") == 0) { + axis = 3; + } + if(strcmp(name, "x_low_enable") == 0) { + axis = 0; + } + if(strcmp(name, "y_high_enable") == 0) { + axis = 4; + } + if(strcmp(name, "y_low_enable") == 0) { + axis = 1; + } + if(strcmp(name, "z_high_enable") == 0) { + axis = 5; + } + if(strcmp(name, "z_low_enable") == 0) { + axis = 2; + } + + if(generator == 1) + val = atomic_read(&stat->interrupt->gen1_axis[axis].enable); + else + val = atomic_read(&stat->interrupt->gen2_axis[axis].enable); + + return val; +} + +static int set_axis(struct lsm9ds0_status *stat, int generator, + const char *name, unsigned long value) +{ + int err = -1; + int axis; + + if(strcmp(name, "x_high_enable") == 0) { + axis = 3; + } + if((strcmp(name, "x_low_enable") == 0) || + (strcmp(name, "x_enable") == 0)) { + axis = 0; + } + if(strcmp(name, "y_high_enable") == 0) { + axis = 4; + } + if((strcmp(name, "y_low_enable") == 0) || + (strcmp(name, "y_enable") == 0)) { + axis = 1; + } + if(strcmp(name, "z_high_enable") == 0) { + axis = 5; + } + if((strcmp(name, "z_low_enable") == 0) || + (strcmp(name, "z_enable") == 0)) { + axis = 2; + } + + if(generator == 1) + err = write_gen_int(stat, + &(stat->interrupt->gen1_axis[axis]), (int)value); + if(generator == 2) + err = write_gen_int(stat, + &(stat->interrupt->gen2_axis[axis]), (int)value); + if(generator == 3) + err = write_gen_int(stat, + &(stat->interrupt->gen_mag_axis[axis]), (int)value); + + if(err < 0) + return -1; + + return err; +} + +static ssize_t attr_get_gen1_axis(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = get_axis(stat,1,attr->attr.name); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen1_axis(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = set_axis(stat, 1, attr->attr.name, val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_get_gen2_axis(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = get_axis(stat,2,attr->attr.name); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen2_axis(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = set_axis(stat, 2, attr->attr.name, val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_get_gen_mag_axis(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = get_axis(stat, 3, attr->attr.name); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen_mag_axis(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = set_axis(stat, 3, attr->attr.name, val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_get_gen1_and_or(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = atomic_read(&stat->interrupt->gen1_and_or.enable); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen1_and_or(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_gen_int(stat, &(stat->interrupt->gen1_and_or), (int)val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_get_gen2_and_or(struct kobject *kobj, + struct kobj_attribute *attr, + char *buf) +{ + int val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = atomic_read(&stat->interrupt->gen2_and_or.enable); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_gen2_and_or(struct kobject *kobj, + struct kobj_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + struct device *dev = to_dev(kobj->parent); + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_gen_int(stat, &(stat->interrupt->gen2_and_or), (int)val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_set_pin_conf(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_gen_int(stat, + &(stat->interrupt->interrupt_pin_conf), (int)val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_get_pin_conf(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int val; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = atomic_read(&stat->interrupt->interrupt_pin_conf.enable); + return sprintf(buf, "%d\n", val); +} + +static ssize_t attr_set_interrupt_polarity(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + unsigned long val; + + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + err = write_gen_int(stat, + &(stat->interrupt->interrupt_polarity), (int)val); + if(err < 0) + return -1; + + return size; +} + +static ssize_t attr_get_interrupt_polarity(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int val; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + val = atomic_read(&stat->interrupt->interrupt_polarity.enable); + return sprintf(buf, "%d\n", val); +} + +static struct kobj_attribute gen1_interrupt_pin1_enable = +__ATTR(pin1_enable, 0664, attr_get_gen1_status, attr_set_gen1_status); +static struct kobj_attribute gen1_interrupt_pin2_enable = +__ATTR(pin2_enable, 0664, attr_get_gen1_status, attr_set_gen1_status); + +static struct kobj_attribute gen2_interrupt_pin1_enable = +__ATTR(pin1_enable, 0664, attr_get_gen2_status, attr_set_gen2_status); +static struct kobj_attribute gen2_interrupt_pin2_enable = +__ATTR(pin2_enable, 0664, attr_get_gen2_status, attr_set_gen2_status); + +static struct kobj_attribute gen1_duration = +__ATTR(duration, 0664, attr_get_gen1_duration, attr_set_gen1_duration); +static struct kobj_attribute gen2_duration = +__ATTR(duration, 0664, attr_get_gen2_duration, attr_set_gen2_duration); + +static struct kobj_attribute gen1_threshold = +__ATTR(threshold, 0664, attr_get_gen1_threshold, attr_set_gen1_threshold); +static struct kobj_attribute gen2_threshold = +__ATTR(threshold, 0664, attr_get_gen2_threshold, attr_set_gen2_threshold); + +static struct kobj_attribute mag_gen_interrupt_pin1 = +__ATTR(pin1_enable, 0664, attr_get_gen_mag_status, attr_set_gen_mag_status); +static struct kobj_attribute mag_gen_interrupt_pin2 = +__ATTR(pin2_enable, 0664, attr_get_gen_mag_status, attr_set_gen_mag_status); + +static struct kobj_attribute mag_gen_threshold = +__ATTR(threshold, 0664, attr_get_gen_mag_threshold, attr_set_gen_mag_threshold); + +static struct kobj_attribute gen1_x_high = +__ATTR(x_high_enable, 0664, attr_get_gen1_axis, attr_set_gen1_axis); +static struct kobj_attribute gen1_x_low = +__ATTR(x_low_enable, 0664, attr_get_gen1_axis, attr_set_gen1_axis); + +static struct kobj_attribute gen2_x_high = +__ATTR(x_high_enable, 0664, attr_get_gen2_axis, attr_set_gen2_axis); +static struct kobj_attribute gen2_x_low = +__ATTR(x_low_enable, 0664, attr_get_gen2_axis, attr_set_gen2_axis); + +static struct kobj_attribute gen1_y_high = +__ATTR(y_high_enable, 0664, attr_get_gen1_axis, attr_set_gen1_axis); +static struct kobj_attribute gen1_y_low = +__ATTR(y_low_enable, 0664, attr_get_gen1_axis, attr_set_gen1_axis); + +static struct kobj_attribute gen2_y_high = +__ATTR(y_high_enable, 0664, attr_get_gen2_axis, attr_set_gen2_axis); +static struct kobj_attribute gen2_y_low = +__ATTR(y_low_enable, 0664, attr_get_gen2_axis, attr_set_gen2_axis); + +static struct kobj_attribute gen1_z_high = +__ATTR(z_high_enable, 0664, attr_get_gen1_axis, attr_set_gen1_axis); +static struct kobj_attribute gen1_z_low = +__ATTR(z_low_enable, 0664, attr_get_gen1_axis, attr_set_gen1_axis); + +static struct kobj_attribute gen2_z_high = +__ATTR(z_high_enable, 0664, attr_get_gen2_axis, attr_set_gen2_axis); +static struct kobj_attribute gen2_z_low = +__ATTR(z_low_enable, 0664, attr_get_gen2_axis, attr_set_gen2_axis); + +static struct kobj_attribute gen_mag_x = +__ATTR(x_enable, 0664, attr_get_gen_mag_axis, attr_set_gen_mag_axis); +static struct kobj_attribute gen_mag_y = +__ATTR(y_enable, 0664, attr_get_gen_mag_axis, attr_set_gen_mag_axis); +static struct kobj_attribute gen_mag_z = +__ATTR(z_enable, 0664, attr_get_gen_mag_axis, attr_set_gen_mag_axis); + +static struct kobj_attribute gen1_and_or = +__ATTR(and(1)_or(0)_combination, 0664, attr_get_gen1_and_or, + attr_set_gen1_and_or); +static struct kobj_attribute gen2_and_or = +__ATTR(and(1)_or(0)_combination, 0664, attr_get_gen2_and_or, + attr_set_gen2_and_or); + + +static struct attribute *attributes_acc_interrupt1[] = { + &gen1_interrupt_pin1_enable.attr, + &gen1_interrupt_pin2_enable.attr, + &gen1_duration.attr, + &gen1_threshold.attr, + &gen1_x_high.attr, + &gen1_x_low.attr, + &gen1_y_high.attr, + &gen1_y_low.attr, + &gen1_z_high.attr, + &gen1_z_low.attr, + &gen1_and_or.attr, + NULL, +}; + +static struct attribute *attributes_acc_interrupt2[] = { + &gen2_interrupt_pin1_enable.attr, + &gen2_interrupt_pin2_enable.attr, + &gen2_duration.attr, + &gen2_threshold.attr, + &gen2_x_high.attr, + &gen2_x_low.attr, + &gen2_y_high.attr, + &gen2_y_low.attr, + &gen2_z_high.attr, + &gen2_z_low.attr, + &gen2_and_or.attr, + NULL, +}; + +static struct attribute *attributes_mag_interrupt[] = { + &mag_gen_interrupt_pin1.attr, + &mag_gen_interrupt_pin2.attr, + &mag_gen_threshold.attr, + &gen_mag_x.attr, + &gen_mag_y.attr, + &gen_mag_z.attr, + NULL, +}; + +static struct attribute *attributes_acc[] = { + &poll_attr_acc.attr, + &enable_attr_acc.attr, + &fs_attr_acc.attr, + &aa_filter_attr.attr, + NULL, +}; + +static struct attribute *attributes_mag[] = { + &poll_attr_mag.attr, + &enable_attr_mag.attr, + &fs_attr_mag.attr, + NULL, +}; + +static struct attribute_group attr_group_acc = { + .attrs = attributes_acc, +}; + +static struct attribute_group attr_group_mag = { + .attrs = attributes_mag, +}; + +static struct attribute_group attr_group_int1_acc = { + .attrs = attributes_acc_interrupt1, + .name = "interrupt_generator1", +}; + +static struct attribute_group attr_group_int2_acc = { + .attrs = attributes_acc_interrupt2, + .name = "interrupt_generator2", +}; + +static struct attribute_group attr_group_int_mag = { + .attrs = attributes_mag_interrupt, + .name = "interrupt_generator", +}; + +static struct device_attribute attributes_com[] = { + __ATTR(enable_temperature, 0664, attr_get_temp_enable, + attr_set_temp_enable), + __ATTR(read_temperature, 0444, attr_get_temp, NULL), +}; + +static struct device_attribute attributes_interrupt_com[] = { + __ATTR(interrupt_pin_configuration, 0664, attr_get_pin_conf, + attr_set_pin_conf), + __ATTR(interrupt_polarity, 0664, attr_get_interrupt_polarity, + attr_set_interrupt_polarity), +}; + +static int create_sysfs_interfaces(struct device *dev) +{ + int err; + int i,n; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + + acc_kobj = kobject_create_and_add("accelerometer", &dev->kobj); + if(!acc_kobj) + return -ENOMEM; + + mag_kobj = kobject_create_and_add("magnetometer", &dev->kobj); + if(!mag_kobj) + return -ENOMEM; + + err = sysfs_create_group(acc_kobj, &attr_group_acc); + if (err) + kobject_put(acc_kobj); + + err = sysfs_create_group(mag_kobj, &attr_group_mag); + if (err) + kobject_put(mag_kobj); + + if((stat->pdata_acc->gpio_int1 >= 0)|| + (stat->pdata_acc->gpio_int2 >= 0)) { + err = sysfs_create_group(acc_kobj, &attr_group_int1_acc); + if (err) + kobject_put(acc_kobj); + + err = sysfs_create_group(acc_kobj, &attr_group_int2_acc); + if (err) + kobject_put(acc_kobj); + + err = sysfs_create_group(mag_kobj, &attr_group_int_mag); + if (err) + kobject_put(mag_kobj); + + for (n = 0; n < ARRAY_SIZE(attributes_interrupt_com); n++) + if (device_create_file(dev, attributes_interrupt_com + n)) + goto error1; + } + + for (i = 0; i < ARRAY_SIZE(attributes_com); i++) + if (device_create_file(dev, attributes_com + i)) + goto error; + + + + return 0; + +error: + for ( ; i >= 0; i--) + device_remove_file(dev, attributes_com + i); + +error1: + for ( ; n >= 0; n--) + device_remove_file(dev, attributes_interrupt_com + n); + + dev_err(dev, "%s:Unable to create interface\n", __func__); + return -1; +} + +static void remove_sysfs_interfaces(struct device *dev) +{ + int i; + struct lsm9ds0_status *stat = dev_get_drvdata(dev); + kobject_put(acc_kobj); + kobject_put(mag_kobj); + for (i = 0; i < ARRAY_SIZE(attributes_com); i++) + device_remove_file(dev, attributes_com + i); + if((stat->pdata_acc->gpio_int1 >= 0)|| + (stat->pdata_acc->gpio_int2 >= 0)) { + for (i = 0; i < ARRAY_SIZE(attributes_interrupt_com); i++) + device_remove_file(dev, attributes_interrupt_com + i); + } +} + +int lsm9ds0_acc_input_open(struct input_dev *input) +{ + struct lsm9ds0_status *stat = input_get_drvdata(input); + + return lsm9ds0_acc_enable(stat); +} + +void lsm9ds0_acc_input_close(struct input_dev *dev) +{ + struct lsm9ds0_status *stat = input_get_drvdata(dev); + + lsm9ds0_acc_disable(stat); +} + +int lsm9ds0_mag_input_open(struct input_dev *input) +{ + struct lsm9ds0_status *stat = input_get_drvdata(input); + + return lsm9ds0_mag_enable(stat); +} + +void lsm9ds0_mag_input_close(struct input_dev *dev) +{ + struct lsm9ds0_status *stat = input_get_drvdata(dev); + + lsm9ds0_mag_disable(stat); +} + +static int lsm9ds0_acc_get_data(struct lsm9ds0_status *stat, int *xyz) +{ + int i, err = -1; + u8 acc_data[6]; + s32 hw_d[3] = { 0 }; + + acc_data[0] = (REG_ACC_DATA_ADDR); + err = lsm9ds0_i2c_read(stat, acc_data, 6); + if (err < 0) + return err; + + hw_d[0] = ((s32)( (s16)((acc_data[1] << 8) | (acc_data[0])))); + hw_d[1] = ((s32)( (s16)((acc_data[3] << 8) | (acc_data[2])))); + hw_d[2] = ((s32)( (s16)((acc_data[5] << 8) | (acc_data[4])))); + +#ifdef DEBUG + pr_debug("%s read x=%X %X(regH regL), x=%d(dec) [ug]\n", + LSM9DS0_ACC_DEV_NAME, acc_data[1], acc_data[0], hw_d[0]); + pr_debug("%s read y=%X %X(regH regL), y=%d(dec) [ug]\n", + LSM9DS0_ACC_DEV_NAME, acc_data[3], acc_data[2], hw_d[1]); + pr_debug("%s read z=%X %X(regH regL), z=%d(dec) [ug]\n", + LSM9DS0_ACC_DEV_NAME, acc_data[5], acc_data[4], hw_d[2]); +#endif + + hw_d[0] = hw_d[0] * stat->sensitivity_acc; + hw_d[1] = hw_d[1] * stat->sensitivity_acc; + hw_d[2] = hw_d[2] * stat->sensitivity_acc; + + for (i = 0; i < 3; i++) { + xyz[i] = stat->pdata_acc->rot_matrix[0][i] * hw_d[0] + + stat->pdata_acc->rot_matrix[1][i] * hw_d[1] + + stat->pdata_acc->rot_matrix[2][i] * hw_d[2]; + } + + return err; +} + +static int lsm9ds0_mag_get_data(struct lsm9ds0_status *stat, int *xyz) +{ + int i, err = -1; + u8 mag_data[6]; + s32 hw_d[3] = { 0 }; + + mag_data[0] = (REG_MAG_DATA_ADDR); + err = lsm9ds0_i2c_read(stat, mag_data, 6); + if (err < 0) + return err; + + hw_d[0] = ((s32)( (s16)((mag_data[1] << 8) | (mag_data[0])))); + hw_d[1] = ((s32)( (s16)((mag_data[3] << 8) | (mag_data[2])))); + hw_d[2] = ((s32)( (s16)((mag_data[5] << 8) | (mag_data[4])))); + +#ifdef DEBUG + pr_debug("%s read x=%X %X(regH regL), x=%d(dec) [ug]\n", + LSM9DS0_MAG_DEV_NAME, mag_data[1], mag_data[0], hw_d[0]); + pr_debug("%s read x=%X %X(regH regL), x=%d(dec) [ug]\n", + LSM9DS0_MAG_DEV_NAME, mag_data[3], mag_data[2], hw_d[1]); + pr_debug("%s read x=%X %X(regH regL), x=%d(dec) [ug]\n", + LSM9DS0_MAG_DEV_NAME, mag_data[5], mag_data[4], hw_d[2]); +#endif + + hw_d[0] = hw_d[0] * stat->sensitivity_mag; + hw_d[1] = hw_d[1] * stat->sensitivity_mag; + hw_d[2] = hw_d[2] * stat->sensitivity_mag; + + for (i = 0; i < 3; i++) { + xyz[i] = stat->pdata_acc->rot_matrix[0][i] * hw_d[0] + + stat->pdata_acc->rot_matrix[1][i] * hw_d[1] + + stat->pdata_acc->rot_matrix[2][i] * hw_d[2]; + } + + return err; +} + +static int lsm9ds0_temp_get_data(struct lsm9ds0_status *stat, + int *dec, int *flo) +{ + int err = -1; + u8 temp_data[2]; + s16 hw_d = 0; + + temp_data[0] = (REG_TEMP_DATA_ADDR); + err = lsm9ds0_i2c_read(stat, temp_data, 2); + if (err < 0) + return err; + + hw_d = (s16)((temp_data[1] << 8) | (temp_data[0])); + + +#ifdef DEBUG + pr_debug("%s read T=%X %X(regH regL), T=%d(dec) [C]\n", + LSM9DS0_DEV_NAME, temp_data[1], temp_data[0], hw_d); +#endif + + *dec = (int)(hw_d/TEMP_SENSITIVITY) + OFFSET_TEMP; + *flo = (((unsigned int)hw_d)%TEMP_SENSITIVITY); + + return err; +} + +static void lsm9ds0_acc_report_values(struct lsm9ds0_status *stat, int *xyz) +{ + input_report_abs(stat->input_dev_acc, ABS_X, xyz[0]); + input_report_abs(stat->input_dev_acc, ABS_Y, xyz[1]); + input_report_abs(stat->input_dev_acc, ABS_Z, xyz[2]); + input_sync(stat->input_dev_acc); +} + +static void lsm9ds0_mag_report_values(struct lsm9ds0_status *stat, int *xyz) +{ + input_report_abs(stat->input_dev_mag, ABS_X, xyz[0]); + input_report_abs(stat->input_dev_mag, ABS_Y, xyz[1]); + input_report_abs(stat->input_dev_mag, ABS_Z, xyz[2]); + input_sync(stat->input_dev_mag); +} + +static int lsm9ds0_acc_input_init(struct lsm9ds0_status *stat) +{ + int err; + + stat->input_dev_acc = input_allocate_device(); + if (!stat->input_dev_acc) { + err = -ENOMEM; + dev_err(&stat->client->dev, "accelerometer " + "input device allocation failed\n"); + goto err0; + } + + stat->input_dev_acc->open = lsm9ds0_acc_input_open; + stat->input_dev_acc->close = lsm9ds0_acc_input_close; + stat->input_dev_acc->name = LSM9DS0_ACC_DEV_NAME; + stat->input_dev_acc->id.bustype = BUS_I2C; + stat->input_dev_acc->dev.parent = &stat->client->dev; + + input_set_drvdata(stat->input_dev_acc, stat); + + set_bit(EV_ABS, stat->input_dev_acc->evbit); + + input_set_abs_params(stat->input_dev_acc, ABS_X, + -ACC_G_MAX_NEG, ACC_G_MAX_POS, FUZZ, FLAT); + input_set_abs_params(stat->input_dev_acc, ABS_Y, + -ACC_G_MAX_NEG, ACC_G_MAX_POS, FUZZ, FLAT); + input_set_abs_params(stat->input_dev_acc, ABS_Z, + -ACC_G_MAX_NEG, ACC_G_MAX_POS, FUZZ, FLAT); + + err = input_register_device(stat->input_dev_acc); + if (err) { + dev_err(&stat->client->dev, + "unable to register accelerometer input device %s\n", + stat->input_dev_acc->name); + goto err1; + } + + return 0; + +err1: + input_free_device(stat->input_dev_acc); +err0: + return err; +} + +static int lsm9ds0_mag_input_init(struct lsm9ds0_status *stat) +{ + int err; + + stat->input_dev_mag = input_allocate_device(); + if (!stat->input_dev_mag) { + err = -ENOMEM; + dev_err(&stat->client->dev, "magnetometer " + "input device allocation failed\n"); + goto err0; + } + + stat->input_dev_mag->open = lsm9ds0_mag_input_open; + stat->input_dev_mag->close = lsm9ds0_mag_input_close; + stat->input_dev_mag->name = LSM9DS0_MAG_DEV_NAME; + stat->input_dev_mag->id.bustype = BUS_I2C; + stat->input_dev_mag->dev.parent = &stat->client->dev; + + input_set_drvdata(stat->input_dev_mag, stat); + + set_bit(EV_ABS, stat->input_dev_mag->evbit); + + input_set_abs_params(stat->input_dev_mag, ABS_X, + -MAG_G_MAX_NEG, MAG_G_MAX_POS, FUZZ, FLAT); + input_set_abs_params(stat->input_dev_mag, ABS_Y, + -MAG_G_MAX_NEG, MAG_G_MAX_POS, FUZZ, FLAT); + input_set_abs_params(stat->input_dev_mag, ABS_Z, + -MAG_G_MAX_NEG, MAG_G_MAX_POS, FUZZ, FLAT); + + err = input_register_device(stat->input_dev_mag); + if (err) { + dev_err(&stat->client->dev, + "unable to register magnetometer input device %s\n", + stat->input_dev_mag->name); + goto err1; + } + + return 0; + +err1: + input_free_device(stat->input_dev_mag); +err0: + return err; +} + +static void lsm9ds0_input_cleanup(struct lsm9ds0_status *stat) +{ + input_unregister_device(stat->input_dev_acc); + input_free_device(stat->input_dev_acc); + + input_unregister_device(stat->input_dev_mag); + input_free_device(stat->input_dev_mag); +} + +static void poll_function_work_acc(struct work_struct *input_work_acc) +{ + struct lsm9ds0_status *stat; + int xyz[3] = { 0 }; + int err; + + stat = container_of((struct work_struct *)input_work_acc, + struct lsm9ds0_status, input_work_acc); + + mutex_lock(&stat->lock); + err = lsm9ds0_acc_get_data(stat, xyz); + if (err < 0) + dev_err(&stat->client->dev, "get_accelerometer_data failed\n"); + else + lsm9ds0_acc_report_values(stat, xyz); + + mutex_unlock(&stat->lock); + hrtimer_start(&stat->hr_timer_acc, stat->ktime_acc, HRTIMER_MODE_REL); +} + +static void poll_function_work_mag(struct work_struct *input_work_mag) +{ + struct lsm9ds0_status *stat; + int xyz[3] = { 0 }; + int err; + int dec; + int flo; + + stat = container_of((struct work_struct *)input_work_mag, + struct lsm9ds0_status, input_work_mag); + + mutex_lock(&stat->lock); + + if(atomic_read(&stat->enabled_temp)) { + err = lsm9ds0_temp_get_data(stat, &dec, &flo); + if (err < 0) + dev_err(&stat->client->dev, "get_temperature_data" + " failed\n"); + else { + stat->temp_value_dec = dec; + stat->temp_value_flo = flo; + } + } + + if(atomic_read(&stat->enabled_mag)) { + err = lsm9ds0_mag_get_data(stat, xyz); + if (err < 0) + dev_err(&stat->client->dev, "get_magnetometer_data" + " failed\n"); + else + lsm9ds0_mag_report_values(stat, xyz); + } + + mutex_unlock(&stat->lock); + hrtimer_start(&stat->hr_timer_mag, stat->ktime_mag, HRTIMER_MODE_REL); +} + +enum hrtimer_restart poll_function_read_acc(struct hrtimer *timer) +{ + struct lsm9ds0_status *stat; + + + stat = container_of((struct hrtimer *)timer, + struct lsm9ds0_status, hr_timer_acc); + + queue_work(lsm9ds0_workqueue, &stat->input_work_acc); + return HRTIMER_NORESTART; +} + +enum hrtimer_restart poll_function_read_mag(struct hrtimer *timer) +{ + struct lsm9ds0_status *stat; + + + stat = container_of((struct hrtimer *)timer, + struct lsm9ds0_status, hr_timer_mag); + + queue_work(lsm9ds0_workqueue, &stat->input_work_mag); + return HRTIMER_NORESTART; +} + +static int lsm9ds0_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct lsm9ds0_status *stat; + + u32 smbus_func = I2C_FUNC_SMBUS_BYTE_DATA | + I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_I2C_BLOCK; + + int err = -1; + dev_info(&client->dev, "probe start.\n"); + stat = kzalloc(sizeof(struct lsm9ds0_status), GFP_KERNEL); + if (stat == NULL) { + err = -ENOMEM; + dev_err(&client->dev, + "failed to allocate memory for module data: " + "%d\n", err); + goto exit_check_functionality_failed; + } + + stat->use_smbus = 0; + 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)){ + stat->use_smbus = 1; + dev_warn(&client->dev, "client using SMBUS\n"); + } else { + err = -ENODEV; + dev_err(&client->dev, "client nor SMBUS capable\n"); + goto exit_check_functionality_failed; + } + } + + if(lsm9ds0_workqueue == 0) + lsm9ds0_workqueue = create_workqueue("lsm9ds0_workqueue"); + + hrtimer_init(&stat->hr_timer_acc, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + stat->hr_timer_acc.function = &poll_function_read_acc; + hrtimer_init(&stat->hr_timer_mag, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + stat->hr_timer_mag.function = &poll_function_read_mag; + + mutex_init(&stat->lock); + mutex_lock(&stat->lock); + + stat->client = client; + i2c_set_clientdata(client, stat); + + stat->pdata_acc = kmalloc(sizeof(*stat->pdata_acc), GFP_KERNEL); + stat->pdata_mag = kmalloc(sizeof(*stat->pdata_mag), GFP_KERNEL); + if ((stat->pdata_acc == NULL)||(stat->pdata_mag == NULL)) { + err = -ENOMEM; + dev_err(&client->dev, + "failed to allocate memory for pdata: %d\n", err); + goto err_mutexunlock; + } + + if (client->dev.platform_data == NULL) { + memcpy(stat->pdata_acc, &default_lsm9ds0_acc_pdata, + sizeof(*stat->pdata_acc)); + memcpy(stat->pdata_mag, &default_lsm9ds0_mag_pdata, + sizeof(*stat->pdata_mag)); + dev_info(&client->dev, "using default plaform_data for " + "accelerometer and magnetometer\n"); + } else { + struct lsm9ds0_main_platform_data *tmp; + tmp = kzalloc(sizeof(struct lsm9ds0_main_platform_data), + GFP_KERNEL); + if(tmp == NULL) + goto exit_kfree_pdata; + memcpy(tmp, client->dev.platform_data, sizeof(*tmp)); + if(tmp->pdata_acc == NULL) { + memcpy(stat->pdata_acc, &default_lsm9ds0_acc_pdata, + sizeof(*stat->pdata_acc)); + dev_info(&client->dev, "using default plaform_data for " + "accelerometer\n"); + } else { + memcpy(stat->pdata_acc, tmp->pdata_acc, + sizeof(*stat->pdata_acc)); + } + if(tmp->pdata_mag == NULL) { + memcpy(stat->pdata_mag, &default_lsm9ds0_mag_pdata, + sizeof(*stat->pdata_mag)); + dev_info(&client->dev, "using default plaform_data for " + "magnetometer\n"); + } else { + memcpy(stat->pdata_mag, tmp->pdata_mag, + sizeof(*stat->pdata_mag)); + } + kfree(tmp); + } + + err = lsm9ds0_acc_validate_pdata(stat); + if (err < 0) { + dev_err(&client->dev, "failed to validate platform data for " + "accelerometer \n"); + goto exit_kfree_pdata; + } + + err = lsm9ds0_mag_validate_pdata(stat); + if (err < 0) { + dev_err(&client->dev, "failed to validate platform data for " + "magnetometer\n"); + goto exit_kfree_pdata; + } + + if (stat->pdata_acc->init) { + err = stat->pdata_acc->init(); + if (err < 0) { + dev_err(&client->dev, "accelerometer init failed: " + "%d\n", err); + goto err_pdata_acc_init; + } + } + if (stat->pdata_mag->init) { + err = stat->pdata_mag->init(); + if (err < 0) { + dev_err(&client->dev, "magnetometer init failed: " + "%d\n", err); + goto err_pdata_mag_init; + } + } + + if(stat->pdata_acc->gpio_int1 >= 0) { + if (!gpio_is_valid(stat->pdata_acc->gpio_int1)) { + dev_err(&client->dev, "The requested GPIO [%d] is not " + "available\n", stat->pdata_acc->gpio_int1); + err = -EINVAL; + goto err_gpio1_valid; + } + + err = gpio_request(stat->pdata_acc->gpio_int1, + "INTERRUPT_PIN1_LSM9DS0"); + if(err < 0) { + dev_err(&client->dev, "Unable to request GPIO [%d].\n", + stat->pdata_acc->gpio_int1); + err = -EINVAL; + goto err_gpio1_valid; + } + gpio_direction_input(stat->pdata_acc->gpio_int1); + stat->irq1 = gpio_to_irq(stat->pdata_acc->gpio_int1); + if(stat->irq1 < 0) { + dev_err(&client->dev, "GPIO [%d] cannot be used as " + "interrupt.\n", stat->pdata_acc->gpio_int1); + err = -EINVAL; + goto err_gpio1_irq; + } + pr_info("%s: %s has set irq1 to irq: %d, mapped on gpio:%d\n", + LSM9DS0_DEV_NAME, __func__, stat->irq1, + stat->pdata_acc->gpio_int1); + } + + if(stat->pdata_acc->gpio_int2 >= 0) { + if (!gpio_is_valid(stat->pdata_acc->gpio_int2)) { + dev_err(&client->dev, "The requested GPIO [%d] is not " + "available\n", stat->pdata_acc->gpio_int2); + err = -EINVAL; + goto err_gpio2_valid; + } + + err = gpio_request(stat->pdata_acc->gpio_int2, + "INTERRUPT_PIN2_LSM9DS0"); + if(err < 0) { + dev_err(&client->dev, "Unable to request GPIO [%d].\n", + stat->pdata_acc->gpio_int2); + err = -EINVAL; + goto err_gpio2_valid; + } + gpio_direction_input(stat->pdata_acc->gpio_int2); + stat->irq2 = gpio_to_irq(stat->pdata_acc->gpio_int2); + if(stat->irq2 < 0) { + dev_err(&client->dev, "GPIO [%d] cannot be used as " + "interrupt.\n", stat->pdata_acc->gpio_int2); + err = -EINVAL; + goto err_gpio2_irq; + } + pr_info("%s: %s has set irq2 to irq: %d, " + "mapped on gpio:%d\n", + LSM9DS0_DEV_NAME, __func__, stat->irq2, + stat->pdata_acc->gpio_int2); + } + + err = lsm9ds0_hw_init(stat); + if (err < 0) { + dev_err(&client->dev, "hw init failed: %d\n", err); + goto err_hw_init; + } + + err = lsm9ds0_acc_device_power_on(stat); + if (err < 0) { + dev_err(&client->dev, "accelerometer power on failed: " + "%d\n", err); + goto err_pdata_init; + } + err = lsm9ds0_mag_device_power_on(stat); + if (err < 0) { + dev_err(&client->dev, "magnetometer power on failed: " + "%d\n", err); + goto err_pdata_init; + } + + err = lsm9ds0_acc_update_fs_range(stat, stat->pdata_acc->fs_range); + if (err < 0) { + dev_err(&client->dev, "update_fs_range on accelerometer " + "failed\n"); + goto err_power_off_acc; + } + + err = lsm9ds0_mag_update_fs_range(stat, stat->pdata_mag->fs_range); + if (err < 0) { + dev_err(&client->dev, "update_fs_range on magnetometer " + "failed\n"); + goto err_power_off_mag; + } + + err = lsm9ds0_acc_update_odr(stat, stat->pdata_acc->poll_interval); + if (err < 0) { + dev_err(&client->dev, "update_odr on accelerometer failed\n"); + goto err_power_off; + } + + err = lsm9ds0_mag_update_odr(stat, stat->pdata_mag->poll_interval); + if (err < 0) { + dev_err(&client->dev, "update_odr on magnetometer failed\n"); + goto err_power_off; + } + + err = lsm9ds0_acc_update_filter(stat, + stat->pdata_acc->aa_filter_bandwidth); + if (err < 0) { + dev_err(&client->dev, "update_filter on accelerometer " + "failed\n"); + goto err_power_off; + } + + err = lsm9ds0_acc_input_init(stat); + if (err < 0) { + dev_err(&client->dev, "accelerometer input init failed\n"); + goto err_power_off; + } + + err = lsm9ds0_mag_input_init(stat); + if (err < 0) { + dev_err(&client->dev, "magnetometer input init failed\n"); + goto err_power_off; + } + + err = create_sysfs_interfaces(&client->dev); + if (err < 0) { + dev_err(&client->dev, + "device LSM9DS0_DEV_NAME sysfs register failed\n"); + goto err_input_cleanup; + } + + lsm9ds0_acc_device_power_off(stat); + lsm9ds0_mag_device_power_off(stat); + + if(stat->pdata_acc->gpio_int1 >= 0){ + INIT_WORK(&stat->irq1_work, lsm9ds0_irq1_work_func); + stat->irq1_work_queue = + create_singlethread_workqueue("lsm9ds0_wq1"); + if (!stat->irq1_work_queue) { + err = -ENOMEM; + dev_err(&client->dev, + "cannot create work queue1: %d\n", err); + goto err_remove_sysfs_int; + } + err = request_irq(stat->irq1, lsm9ds0_isr1, + IRQF_TRIGGER_RISING, "lsm9ds0_irq1", stat); + if (err < 0) { + dev_err(&client->dev, "request irq1 failed: %d\n", err); + goto err_destoyworkqueue1; + } + disable_irq_nosync(stat->irq1); + } + + if(stat->pdata_acc->gpio_int2 >= 0){ + INIT_WORK(&stat->irq2_work, lsm9ds0_irq2_work_func); + stat->irq2_work_queue = + create_singlethread_workqueue("lsm9ds0_wq2"); + if (!stat->irq2_work_queue) { + err = -ENOMEM; + dev_err(&client->dev, + "cannot create work queue2: %d\n", err); + goto err_free_irq1; + } + err = request_irq(stat->irq2, lsm9ds0_isr2, + IRQF_TRIGGER_RISING, "lsm9ds0_irq2", stat); + if (err < 0) { + dev_err(&client->dev, "request irq2 failed: %d\n", err); + goto err_destoyworkqueue2; + } + disable_irq_nosync(stat->irq2); + } + + INIT_WORK(&stat->input_work_acc, poll_function_work_acc); + INIT_WORK(&stat->input_work_mag, poll_function_work_mag); + + mutex_unlock(&stat->lock); + dev_info(&client->dev, "%s: probed\n", LSM9DS0_DEV_NAME); + return 0; + +err_destoyworkqueue2: + destroy_workqueue(stat->irq2_work_queue); +err_free_irq1: + free_irq(stat->irq1, stat); +err_destoyworkqueue1: + destroy_workqueue(stat->irq1_work_queue); +err_remove_sysfs_int: + remove_sysfs_interfaces(&client->dev); +err_input_cleanup: + lsm9ds0_input_cleanup(stat); +err_power_off: +err_power_off_mag: + lsm9ds0_mag_device_power_off(stat); +err_power_off_acc: + lsm9ds0_acc_device_power_off(stat); + kfree(stat->interrupt); +err_hw_init: +err_gpio2_irq: + gpio_free(stat->pdata_acc->gpio_int2); +err_gpio2_valid: +err_gpio1_irq: + gpio_free(stat->pdata_acc->gpio_int1); +err_gpio1_valid: +err_pdata_init: +err_pdata_mag_init: + if (stat->pdata_mag->exit) + stat->pdata_mag->exit(); +err_pdata_acc_init: + if (stat->pdata_acc->exit) + stat->pdata_acc->exit(); +exit_kfree_pdata: + kfree(stat->pdata_acc); + kfree(stat->pdata_mag); +err_mutexunlock: + mutex_unlock(&stat->lock); + kfree(stat); + if(!lsm9ds0_workqueue) { + flush_workqueue(lsm9ds0_workqueue); + destroy_workqueue(lsm9ds0_workqueue); + } +exit_check_functionality_failed: + pr_err("%s: Driver Init failed\n", LSM9DS0_DEV_NAME); + return err; +} + +static int lsm9ds0_remove(struct i2c_client *client) +{ + struct lsm9ds0_status *stat = i2c_get_clientdata(client); + + lsm9ds0_acc_disable(stat); + lsm9ds0_mag_disable(stat); + lsm9ds0_temperature_disable(stat); + + if(stat->pdata_acc->gpio_int1 >= 0) { + free_irq(stat->irq1, stat); + gpio_free(stat->pdata_acc->gpio_int1); + destroy_workqueue(stat->irq1_work_queue); + } + + if(stat->pdata_acc->gpio_int2 >= 0) { + free_irq(stat->irq2, stat); + gpio_free(stat->pdata_acc->gpio_int2); + destroy_workqueue(stat->irq2_work_queue); + } + + lsm9ds0_acc_input_cleanup(stat); + lsm9ds0_mag_input_cleanup(stat); + + remove_sysfs_interfaces(&client->dev); + + if (stat->pdata_acc->exit) + stat->pdata_acc->exit(); + + if (stat->pdata_mag->exit) + stat->pdata_mag->exit(); + + if((stat->pdata_acc->gpio_int1 >= 0)|| + (stat->pdata_acc->gpio_int2 >= 0)) { + kfree(stat->interrupt); + } + + if(!lsm9ds0_workqueue) { + flush_workqueue(lsm9ds0_workqueue); + destroy_workqueue(lsm9ds0_workqueue); + } + + kfree(stat->pdata_acc); + kfree(stat->pdata_mag); + kfree(stat); + return 0; +} + +static const struct i2c_device_id lsm9ds0_id[] = { + { LSM9DS0_DEV_NAME, 0 }, + { }, +}; +MODULE_DEVICE_TABLE(i2c, lsm9ds0_id); + +static const struct of_device_id lsm9ds0_of_match[] = { + { .compatible = "st,"LSM9DS0_DEV_NAME, }, + { }, +}; +MODULE_DEVICE_TABLE(of, lsm9ds0_of_match); + +static struct i2c_driver lsm9ds0_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = LSM9DS0_DEV_NAME, + .of_match_table = lsm9ds0_of_match, + }, + .probe = lsm9ds0_probe, + .remove = lsm9ds0_remove, + .id_table = lsm9ds0_id, +}; + +module_i2c_driver(lsm9ds0_i2c_driver); + + +MODULE_DESCRIPTION("lsm9ds0 accelerometer and magnetometer driver"); +MODULE_AUTHOR("Matteo Dameno, Denis Ciocca, STMicroelectronics"); +MODULE_LICENSE("GPL"); diff --git a/drivers/input/misc/lsm9ds0_gyr.c b/drivers/input/misc/lsm9ds0_gyr.c new file mode 100644 index 0000000..360f85c --- /dev/null +++ b/drivers/input/misc/lsm9ds0_gyr.c @@ -0,0 +1,1726 @@ +/******************** (C) COPYRIGHT 2012 STMicroelectronics ******************** +* +* File Name : lsm9ds0_gyr_sysfs.c +* Authors : MEMS Motion Sensors Products Div- Application Team +* : Matteo Dameno (matteo.dameno@st.com) +* : Denis Ciocca (denis.ciocca@st.com) +* : Both authors are willing to be considered the contact +* : and update points for the driver. +* Version : V 1.2 sysfs +* Date : 2012/Jul/10 +* Description : LSM9DS0 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.2 | 2011/Nov/11 | Matteo Dameno | enable gpio_int to be +* | | | passed as parameter at +* | | | module loading time; +* | | | corrects polling +* | | | bug at end of probing; +* 1.1.5.3 | 2011/Dec/20 | Matteo Dameno | corrects error in +* | | | I2C SADROOT; Modifies +* | | | resume suspend func. +* 1.1.5.4 | 2012/Jan/09 | Matteo Dameno | moved under input/misc; +* 1.1.5.5 | 2012/Mar/30 | Matteo Dameno | moved watermark, use_smbus, +* | | | fifomode @ struct foo_status +* | | | sysfs range input format +* | | | changed to decimal +* 1.2 | 2012/Jul/10 | Denis Ciocca | input_poll_dev removal +* 1.2.1 | 2012/Jul/10 | Denis Ciocca | added high resolution timers +*******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +/* #include "lsm9ds0.h" */ + +/* Maximum polled-device-reported rot speed value value in dps */ +#define FS_MAX 32768 +#define MS_TO_NS(x) (x*1000000L) + +/* lsm9ds0 gyroscope registers */ +#define WHO_AM_I (0x0F) + +#define SENSITIVITY_250 8750 /* udps/LSB */ +#define SENSITIVITY_500 17500 /* udps/LSB */ +#define SENSITIVITY_2000 70000 /* udps/LSB */ + +#define CTRL_REG1 (0x20) /* CTRL REG1 */ +#define CTRL_REG2 (0x21) /* CTRL REG2 */ +#define CTRL_REG3 (0x22) /* CTRL_REG3 */ +#define CTRL_REG4 (0x23) /* CTRL_REG4 */ +#define CTRL_REG5 (0x24) /* CTRL_REG5 */ +#define REFERENCE (0x25) /* REFERENCE REG */ +#define FIFO_CTRL_REG (0x2E) /* FIFO CONTROL REGISTER */ +#define FIFO_SRC_REG (0x2F) /* FIFO SOURCE REGISTER */ +#define OUT_X_L (0x28) /* 1st AXIS OUT REG of 6 */ + +#define AXISDATA_REG OUT_X_L + +/* CTRL_REG1 */ +#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) /* ODR = 95Hz */ +#define ODR190 (0x40) /* ODR = 190Hz */ +#define ODR380 (0x80) /* ODR = 380Hz */ +#define ODR760 (0xC0) /* ODR = 760Hz */ + +/* CTRL_REG3 bits */ +#define I2_DRDY (0x08) +#define I2_WTM (0x04) +#define I2_OVRUN (0x02) +#define I2_EMPTY (0x01) +#define I2_NONE (0x00) +#define I2_MASK (0x0F) + +/* CTRL_REG4 bits */ +#define FS_MASK (0x30) +#define BDU_ENABLE (0x80) + +/* CTRL_REG5 bits */ +#define FIFO_ENABLE (0x40) +#define HPF_ENALBE (0x11) + +/* FIFO_CTRL_REG bits */ +#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 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_FIFO_CTRL_REG 5 +#define RESUME_ENTRIES 6 + + +/* #define DEBUG 1 */ + +/** Registers Contents */ +#define WHOAMI_LSM9DS0_GYR (0xD4) /* Expected content for WAI register*/ + +static int int1_gpio = LSM9DS0_GYR_DEFAULT_INT1_GPIO; +static int int2_gpio = LSM9DS0_GYR_DEFAULT_INT2_GPIO; +/* module_param(int1_gpio, int, S_IRUGO); */ +module_param(int2_gpio, int, S_IRUGO); + +/* + * LSM9DS0 gyroscope data + * brief structure containing gyroscope values for yaw, pitch and roll in + * s32 + */ + +struct lsm9ds0_gyr_triple { + s32 x, /* x-axis angular rate data. */ + y, /* y-axis angluar rate data. */ + z; /* z-axis angular rate data. */ +}; + +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 struct lsm9ds0_gyr_platform_data default_lsm9ds0_gyr_pdata = { + .fs_range = LSM9DS0_GYR_FS_250DPS, + .axis_map_x = 0, + .axis_map_y = 1, + .axis_map_z = 2, + .negate_x = 0, + .negate_y = 0, + .negate_z = 0, + + .poll_interval = 100, + .min_interval = LSM9DS0_GYR_MIN_POLL_PERIOD_MS, /* 2ms */ + + .gpio_int1 = LSM9DS0_GYR_DEFAULT_INT1_GPIO, + .gpio_int2 = LSM9DS0_GYR_DEFAULT_INT2_GPIO, /* int for fifo */ + +}; + +struct workqueue_struct *lsm9ds0_gyr_workqueue = 0; + +struct lsm9ds0_gyr_status { + struct i2c_client *client; + struct lsm9ds0_gyr_platform_data *pdata; + + struct mutex lock; + + struct input_dev *input_dev; + + int hw_initialized; + atomic_t enabled; + int use_smbus; + + u8 reg_addr; + u8 resume_state[RESUME_ENTRIES]; + + u32 sensitivity; + + /* interrupt related */ + int irq2; + struct work_struct irq2_work; + struct workqueue_struct *irq2_work_queue; + + bool polling_enabled; + /* fifo related */ + u8 watermark; + u8 fifomode; + + struct hrtimer hr_timer; + ktime_t ktime; + struct work_struct polling_task; +}; + + +static int lsm9ds0_gyr_i2c_read(struct lsm9ds0_gyr_status *stat, u8 *buf, + int len) +{ + int ret; + u8 reg = buf[0]; + u8 cmd = reg; + +/* + if (len > sizeof(buf)) + dev_err(&stat->client->dev, + "read error insufficient buffer length: " + "len:%d, buf size=%d\n", + len, sizeof(buf)); +*/ + if (len > 1) + cmd = (I2C_AUTO_INCREMENT | reg); + if (stat->use_smbus) { + if (len == 1) { + ret = i2c_smbus_read_byte_data(stat->client, cmd); + buf[0] = ret & 0xff; +#ifdef DEBUG + dev_warn(&stat->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(stat->client, + cmd, len, buf); +#ifdef DEBUG + dev_warn(&stat->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++) + printk(KERN_DEBUG "buf[%d]=0x%02x,", + ii, buf[ii]); + + printk("\n"); +#endif + } else + ret = -1; + + if (ret < 0) { + dev_err(&stat->client->dev, + "read transfer error: len:%d, command=0x%02x\n", + len, cmd); + return 0; /* failure */ + } + return len; /* success */ + } + + ret = i2c_master_send(stat->client, &cmd, sizeof(cmd)); + if (ret != sizeof(cmd)) + return ret; + + return i2c_master_recv(stat->client, buf, len); +} + +static int lsm9ds0_gyr_i2c_write(struct lsm9ds0_gyr_status *stat, u8 *buf, + int len) +{ + int ret; + u8 reg, value; + + if (len > 1) + buf[0] = (I2C_AUTO_INCREMENT | buf[0]); + + reg = buf[0]; + value = buf[1]; + + if (stat->use_smbus) { + if (len == 1) { + ret = i2c_smbus_write_byte_data(stat->client, + reg, value); +#ifdef DEBUG + dev_warn(&stat->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(stat->client, + reg, len, buf + 1); +#ifdef DEBUG + dev_warn(&stat->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++) + printk(KERN_DEBUG "value[%d]=0x%02x,", + ii, buf[ii]); + + printk("\n"); +#endif + return ret; + } + } + + ret = i2c_master_send(stat->client, buf, len+1); + return (ret == len+1) ? 0 : ret; +} + + +static int lsm9ds0_gyr_register_write(struct lsm9ds0_gyr_status *stat, + u8 *buf, u8 reg_address, u8 new_value) +{ + int err; + + /* Sets configuration register at reg_address + * NOTE: this is a straight overwrite */ + buf[0] = reg_address; + buf[1] = new_value; + err = lsm9ds0_gyr_i2c_write(stat, buf, 1); + if (err < 0) + return err; + + return err; +} + +static int lsm9ds0_gyr_register_read(struct lsm9ds0_gyr_status *stat, + u8 *buf, u8 reg_address) +{ + + int err = -1; + buf[0] = (reg_address); + err = lsm9ds0_gyr_i2c_read(stat, buf, 1); + return err; +} + +static int lsm9ds0_gyr_register_update(struct lsm9ds0_gyr_status *stat, + u8 *buf, u8 reg_address, u8 mask, u8 new_bit_values) +{ + int err = -1; + u8 init_val; + u8 updated_val; + err = lsm9ds0_gyr_register_read(stat, buf, reg_address); + if (!(err < 0)) { + init_val = buf[0]; + updated_val = ((mask & new_bit_values) | ((~mask) & init_val)); + err = lsm9ds0_gyr_register_write(stat, buf, reg_address, + updated_val); + } + return err; +} + + +static int lsm9ds0_gyr_update_watermark(struct lsm9ds0_gyr_status *stat, + u8 watermark) +{ + int res = 0; + u8 buf[2]; + u8 new_value; + + mutex_lock(&stat->lock); + new_value = (watermark % 0x20); + res = lsm9ds0_gyr_register_update(stat, buf, FIFO_CTRL_REG, + FIFO_WATERMARK_MASK, new_value); + if (res < 0) { + dev_err(&stat->client->dev, "failed to update watermark\n"); + return res; + } + dev_dbg(&stat->client->dev, "%s new_value:0x%02x,watermark:0x%02x\n", + __func__, new_value, watermark); + + stat->resume_state[RES_FIFO_CTRL_REG] = + ((FIFO_WATERMARK_MASK & new_value) | + (~FIFO_WATERMARK_MASK & + stat->resume_state[RES_FIFO_CTRL_REG])); + stat->watermark = new_value; + mutex_unlock(&stat->lock); + return res; +} + +static int lsm9ds0_gyr_update_fifomode(struct lsm9ds0_gyr_status *stat, + u8 fifomode) +{ + int res; + u8 buf[2]; + u8 new_value; + + new_value = fifomode; + res = lsm9ds0_gyr_register_update(stat, buf, FIFO_CTRL_REG, + FIFO_MODE_MASK, new_value); + if (res < 0) { + dev_err(&stat->client->dev, "failed to update fifoMode\n"); + return res; + } + /* + dev_dbg(&stat->client->dev, "new_value:0x%02x,prev fifomode:0x%02x\n", + __func__, new_value, stat->fifomode); + */ + stat->resume_state[RES_FIFO_CTRL_REG] = + ((FIFO_MODE_MASK & new_value) | + (~FIFO_MODE_MASK & + stat->resume_state[RES_FIFO_CTRL_REG])); + stat->fifomode = new_value; + + return res; +} + +static int lsm9ds0_gyr_fifo_reset(struct lsm9ds0_gyr_status *stat) +{ + u8 oldmode; + int res; + + oldmode = stat->fifomode; + res = lsm9ds0_gyr_update_fifomode(stat, FIFO_MODE_BYPASS); + if (res < 0) + return res; + res = lsm9ds0_gyr_update_fifomode(stat, oldmode); + if (res >= 0) + dev_dbg(&stat->client->dev, "%s fifo reset to: 0x%02x\n", + __func__, oldmode); + + return res; +} + +static int lsm9ds0_gyr_fifo_hwenable(struct lsm9ds0_gyr_status *stat, + u8 enable) +{ + int res; + u8 buf[2]; + u8 set = 0x00; + if (enable) + set = FIFO_ENABLE; + + res = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG5, + FIFO_ENABLE, set); + if (res < 0) { + dev_err(&stat->client->dev, "fifo_hw switch to:0x%02x failed\n", + set); + return res; + } + stat->resume_state[RES_CTRL_REG5] = + ((FIFO_ENABLE & set) | + (~FIFO_ENABLE & stat->resume_state[RES_CTRL_REG5])); + dev_dbg(&stat->client->dev, "%s set to:0x%02x\n", __func__, set); + return res; +} + +static int lsm9ds0_gyr_manage_int2settings(struct lsm9ds0_gyr_status *stat, + u8 fifomode) +{ + int res; + u8 buf[2]; + bool enable_fifo_hw; + bool recognized_mode = false; + u8 int2bits = I2_NONE; +/* + if (stat->polling_enabled) { + fifomode = FIFO_MODE_BYPASS; + dbg_warn(&stat->client->dev, "in polling mode, fifo mode forced" + " to BYPASS mode\n"); + } +*/ + + + switch (fifomode) { + case FIFO_MODE_FIFO: + recognized_mode = true; + + if (stat->polling_enabled) { + int2bits = I2_NONE; + enable_fifo_hw = false; + } else { + int2bits = (I2_WTM | I2_OVRUN); + enable_fifo_hw = true; + } + res = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG3, + I2_MASK, int2bits); + if (res < 0) { + dev_err(&stat->client->dev, "%s : failed to update " + "CTRL_REG3:0x%02x\n", + __func__, fifomode); + goto err_mutex_unlock; + } + stat->resume_state[RES_CTRL_REG3] = + ((I2_MASK & int2bits) | + (~(I2_MASK) & stat->resume_state[RES_CTRL_REG3])); + /* enable_fifo_hw = true; */ + break; + + case FIFO_MODE_BYPASS: + recognized_mode = true; + + if (stat->polling_enabled) + int2bits = I2_NONE; + else + int2bits = I2_DRDY; + + res = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG3, + I2_MASK, int2bits); + if (res < 0) { + dev_err(&stat->client->dev, "%s : failed to update" + " to CTRL_REG3:0x%02x\n", + __func__, fifomode); + goto err_mutex_unlock; + } + stat->resume_state[RES_CTRL_REG3] = + ((I2_MASK & int2bits) | + (~I2_MASK & stat->resume_state[RES_CTRL_REG3])); + enable_fifo_hw = false; + break; + + default: + recognized_mode = false; + res = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG3, + I2_MASK, I2_NONE); + if (res < 0) { + dev_err(&stat->client->dev, "%s : failed to update " + "CTRL_REG3:0x%02x\n", + __func__, fifomode); + goto err_mutex_unlock; + } + enable_fifo_hw = false; + stat->resume_state[RES_CTRL_REG3] = + ((I2_MASK & 0x00) | + (~I2_MASK & stat->resume_state[RES_CTRL_REG3])); + break; + + } + if (recognized_mode) { + res = lsm9ds0_gyr_update_fifomode(stat, fifomode); + if (res < 0) { + dev_err(&stat->client->dev, "%s : failed to " + "set fifoMode\n", __func__); + goto err_mutex_unlock; + } + } + res = lsm9ds0_gyr_fifo_hwenable(stat, enable_fifo_hw); + +err_mutex_unlock: + + return res; +} + + +static int lsm9ds0_gyr_update_fs_range(struct lsm9ds0_gyr_status *stat, + u8 new_fs) +{ + int res ; + u8 buf[2]; + + u32 sensitivity; + + switch(new_fs) { + case LSM9DS0_GYR_FS_250DPS: + sensitivity = SENSITIVITY_250; + break; + case LSM9DS0_GYR_FS_500DPS: + sensitivity = SENSITIVITY_500; + break; + case LSM9DS0_GYR_FS_2000DPS: + sensitivity = SENSITIVITY_2000; + break; + default: + dev_err(&stat->client->dev, "invalid g range " + "requested: %u\n", new_fs); + return -EINVAL; + } + + + buf[0] = CTRL_REG4; + + res = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG4, + FS_MASK, new_fs); + + if (res < 0) { + dev_err(&stat->client->dev, "%s : failed to update fs:0x%02x\n", + __func__, new_fs); + return res; + } + stat->resume_state[RES_CTRL_REG4] = + ((FS_MASK & new_fs) | + (~FS_MASK & stat->resume_state[RES_CTRL_REG4])); + + stat->sensitivity = sensitivity; + return res; +} + + +static int lsm9ds0_gyr_update_odr(struct lsm9ds0_gyr_status *stat, + 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 device is currently enabled, we need to write new + * configuration out to it */ + if (atomic_read(&stat->enabled)) { + config[0] = CTRL_REG1; + err = lsm9ds0_gyr_i2c_write(stat, config, 1); + if (err < 0) + return err; + stat->resume_state[RES_CTRL_REG1] = config[1]; + stat->ktime = ktime_set(0, MS_TO_NS(poll_interval_ms)); + } + + return err; +} + +/* gyroscope data readout */ +static int lsm9ds0_gyr_get_data(struct lsm9ds0_gyr_status *stat, + struct lsm9ds0_gyr_triple *data) +{ + int err; + unsigned char gyro_out[6]; + /* y,p,r hardware data */ + s32 hw_d[3] = { 0 }; + + gyro_out[0] = (AXISDATA_REG); + + err = lsm9ds0_gyr_i2c_read(stat, gyro_out, 6); + + if (err < 0) + return err; + + hw_d[0] = (s32) ((s16)((gyro_out[1]) << 8) | gyro_out[0]); + hw_d[1] = (s32) ((s16)((gyro_out[3]) << 8) | gyro_out[2]); + hw_d[2] = (s32) ((s16)((gyro_out[5]) << 8) | gyro_out[4]); + + //hw_d[0] = hw_d[0] * stat->sensitivity; + //hw_d[1] = hw_d[1] * stat->sensitivity; + //hw_d[2] = hw_d[2] * stat->sensitivity; + + data->x = ((stat->pdata->negate_x) ? (-hw_d[stat->pdata->axis_map_x]) + : (hw_d[stat->pdata->axis_map_x])); + data->y = ((stat->pdata->negate_y) ? (-hw_d[stat->pdata->axis_map_y]) + : (hw_d[stat->pdata->axis_map_y])); + data->z = ((stat->pdata->negate_z) ? (-hw_d[stat->pdata->axis_map_z]) + : (hw_d[stat->pdata->axis_map_z])); + +#ifdef DEBUG + /* dev_info(&stat->client->dev, "gyro_out: x = %d, y = %d, z = %d\n", + data->x, data->y, data->z); */ +#endif + + return err; +} + +static void lsm9ds0_gyr_report_values(struct lsm9ds0_gyr_status *stat, + struct lsm9ds0_gyr_triple *data) +{ + input_report_abs(stat->input_dev, ABS_X, data->x); + input_report_abs(stat->input_dev, ABS_Y, data->y); + input_report_abs(stat->input_dev, ABS_Z, data->z); + input_sync(stat->input_dev); +} + +static int lsm9ds0_gyr_hw_init(struct lsm9ds0_gyr_status *stat) +{ + int err; + u8 buf[6]; + + dev_info(&stat->client->dev, "hw init\n"); + + buf[0] = (CTRL_REG1); + buf[1] = stat->resume_state[RES_CTRL_REG1]; + buf[2] = stat->resume_state[RES_CTRL_REG2]; + buf[3] = stat->resume_state[RES_CTRL_REG3]; + buf[4] = stat->resume_state[RES_CTRL_REG4]; + buf[5] = stat->resume_state[RES_CTRL_REG5]; + + err = lsm9ds0_gyr_i2c_write(stat, buf, 5); + if (err < 0) + return err; + + buf[0] = (FIFO_CTRL_REG); + buf[1] = stat->resume_state[RES_FIFO_CTRL_REG]; + err = lsm9ds0_gyr_i2c_write(stat, buf, 1); + if (err < 0) + return err; + + stat->hw_initialized = 1; + + return err; +} + +static void lsm9ds0_gyr_device_power_off(struct lsm9ds0_gyr_status *stat) +{ + int err; + u8 buf[2]; + + dev_info(&stat->client->dev, "power off\n"); + + buf[0] = (CTRL_REG1); + buf[1] = (PM_OFF); + err = lsm9ds0_gyr_i2c_write(stat, buf, 1); + if (err < 0) + dev_err(&stat->client->dev, "soft power off failed\n"); + + if (stat->pdata->power_off) { + /* disable_irq_nosync(acc->irq1); */ + disable_irq_nosync(stat->irq2); + stat->pdata->power_off(); + stat->hw_initialized = 0; + } + + if (stat->hw_initialized) { + /*if (stat->pdata->gpio_int1 >= 0)*/ + /* disable_irq_nosync(stat->irq1);*/ + if (stat->pdata->gpio_int2 >= 0) { + disable_irq_nosync(stat->irq2); + dev_info(&stat->client->dev, + "power off: irq2 disabled\n"); + } + stat->hw_initialized = 0; + } +} + +static int lsm9ds0_gyr_device_power_on(struct lsm9ds0_gyr_status *stat) +{ + int err; + + if (stat->pdata->power_on) { + err = stat->pdata->power_on(); + if (err < 0) + return err; + if (stat->pdata->gpio_int2 >= 0) + enable_irq(stat->irq2); + } + + + if (!stat->hw_initialized) { + err = lsm9ds0_gyr_hw_init(stat); + if (err < 0) { + lsm9ds0_gyr_device_power_off(stat); + return err; + } + } + + if (stat->hw_initialized) { + /* if (stat->pdata->gpio_int1) { + enable_irq(stat->irq1); + dev_info(&stat->client->dev, + "power on: irq1 enabled\n"); + } */ + dev_dbg(&stat->client->dev, "stat->pdata->gpio_int2 = %d\n", + stat->pdata->gpio_int2); + if (stat->pdata->gpio_int2 >= 0) { + enable_irq(stat->irq2); + dev_info(&stat->client->dev, + "power on: irq2 enabled\n"); + } + } + + return 0; +} + +static int lsm9ds0_gyr_enable(struct lsm9ds0_gyr_status *stat) +{ + int err; + + if (!atomic_cmpxchg(&stat->enabled, 0, 1)) { + + err = lsm9ds0_gyr_device_power_on(stat); + if (err < 0) { + atomic_set(&stat->enabled, 0); + return err; + } + + if (stat->polling_enabled) { + hrtimer_start(&(stat->hr_timer), stat->ktime, HRTIMER_MODE_REL); + } + + } + + return 0; +} + +static int lsm9ds0_gyr_disable(struct lsm9ds0_gyr_status *stat) +{ + dev_dbg(&stat->client->dev, "%s: stat->enabled = %d\n", __func__, + atomic_read(&stat->enabled)); + + if (atomic_cmpxchg(&stat->enabled, 1, 0)) { + + lsm9ds0_gyr_device_power_off(stat); + hrtimer_cancel(&stat->hr_timer); + dev_dbg(&stat->client->dev, "%s: cancel_hrtimer ", __func__); + } + return 0; +} + +static ssize_t attr_polling_rate_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + int val; + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + mutex_lock(&stat->lock); + val = stat->pdata->poll_interval; + mutex_unlock(&stat->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) +{ + int err; + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long interval_ms; + + if (strict_strtoul(buf, 10, &interval_ms)) + return -EINVAL; + if (!interval_ms) + return -EINVAL; + + mutex_lock(&stat->lock); + err = lsm9ds0_gyr_update_odr(stat, interval_ms); + if(err >= 0) + stat->pdata->poll_interval = interval_ms; + mutex_unlock(&stat->lock); + return size; +} + +static ssize_t attr_range_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + int range = 0; + u8 val; + mutex_lock(&stat->lock); + val = stat->pdata->fs_range; + + switch (val) { + case LSM9DS0_GYR_FS_250DPS: + range = 250; + break; + case LSM9DS0_GYR_FS_500DPS: + range = 500; + break; + case LSM9DS0_GYR_FS_2000DPS: + range = 2000; + break; + } + mutex_unlock(&stat->lock); + /* return sprintf(buf, "0x%02x\n", val); */ + 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 lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long val; + u8 range; + int err; + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + switch (val) { + case 250: + range = LSM9DS0_GYR_FS_250DPS; + break; + case 500: + range = LSM9DS0_GYR_FS_500DPS; + break; + case 2000: + range = LSM9DS0_GYR_FS_2000DPS; + break; + default: + dev_err(&stat->client->dev, "invalid range request: %lu," + " discarded\n", val); + return -EINVAL; + } + mutex_lock(&stat->lock); + err = lsm9ds0_gyr_update_fs_range(stat, range); + if (err >= 0) + stat->pdata->fs_range = range; + mutex_unlock(&stat->lock); + dev_info(&stat->client->dev, "range set to: %lu dps\n", val); + return size; +} + +static ssize_t attr_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + int val = atomic_read(&stat->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 lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + if (val) + lsm9ds0_gyr_enable(stat); + else + lsm9ds0_gyr_disable(stat); + + return size; +} + +static ssize_t attr_polling_mode_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int val = 0; + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + + mutex_lock(&stat->lock); + if (stat->polling_enabled) + val = 1; + mutex_unlock(&stat->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 lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + mutex_lock(&stat->lock); + if (val) { + stat->polling_enabled = true; + lsm9ds0_gyr_manage_int2settings(stat, stat->fifomode); + dev_info(dev, "polling mode enabled\n"); + if (atomic_read(&stat->enabled)) { + hrtimer_start(&(stat->hr_timer), stat->ktime, HRTIMER_MODE_REL); + } + } else { + if (stat->polling_enabled) { + hrtimer_cancel(&stat->hr_timer); + } + stat->polling_enabled = false; + lsm9ds0_gyr_manage_int2settings(stat, stat->fifomode); + dev_info(dev, "polling mode disabled\n"); + } + mutex_unlock(&stat->lock); + return size; +} + +static ssize_t attr_watermark_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long watermark; + int res; + + if (strict_strtoul(buf, 16, &watermark)) + return -EINVAL; + + res = lsm9ds0_gyr_update_watermark(stat, watermark); + if (res < 0) + return res; + + return size; +} + +static ssize_t attr_watermark_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + int val = stat->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 lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long fifomode; + int res; + + if (strict_strtoul(buf, 16, &fifomode)) + return -EINVAL; + /* if (!fifomode) + return -EINVAL; */ + + dev_dbg(dev, "%s, got value:0x%02x\n", __func__, (u8)fifomode); + + mutex_lock(&stat->lock); + res = lsm9ds0_gyr_manage_int2settings(stat, (u8) fifomode); + mutex_unlock(&stat->lock); + + if (res < 0) + return res; + return size; +} + +static ssize_t attr_fifomode_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + u8 val = stat->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 lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + u8 x[2]; + unsigned long val; + + if (strict_strtoul(buf, 16, &val)) + return -EINVAL; + mutex_lock(&stat->lock); + x[0] = stat->reg_addr; + mutex_unlock(&stat->lock); + x[1] = val; + rc = lsm9ds0_gyr_i2c_write(stat, x, 1); + return size; +} + +static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr, + char *buf) +{ + ssize_t ret; + struct lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + int rc; + u8 data; + + mutex_lock(&stat->lock); + data = stat->reg_addr; + mutex_unlock(&stat->lock); + rc = lsm9ds0_gyr_i2c_read(stat, &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 lsm9ds0_gyr_status *stat = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 16, &val)) + return -EINVAL; + + mutex_lock(&stat->lock); + + stat->reg_addr = val; + + mutex_unlock(&stat->lock); + + return size; +} +#endif /* DEBUG */ + +static struct device_attribute attributes[] = { + __ATTR(pollrate_ms, 0666, attr_polling_rate_show, + attr_polling_rate_store), + __ATTR(range, 0666, attr_range_show, attr_range_store), + __ATTR(enable_device, 0666, attr_enable_show, attr_enable_store), + __ATTR(enable_polling, 0666, attr_polling_mode_show, + attr_polling_mode_store), + __ATTR(fifo_samples, 0666, attr_watermark_show, attr_watermark_store), + __ATTR(fifo_mode, 0666, attr_fifomode_show, attr_fifomode_store), +#ifdef DEBUG + __ATTR(reg_value, 0600, attr_reg_get, attr_reg_set), + __ATTR(reg_addr, 0200, NULL, attr_addr_set), +#endif +}; + +static int create_sysfs_interfaces(struct device *dev) +{ + int i; + for (i = 0; i < ARRAY_SIZE(attributes); i++) + if (device_create_file(dev, attributes + i)) + goto error; + return 0; + +error: + for (; i >= 0; i--) + device_remove_file(dev, attributes + i); + dev_err(dev, "%s:Unable to create interface\n", __func__); + return -1; +} + +static int remove_sysfs_interfaces(struct device *dev) +{ + int i; + for (i = 0; i < ARRAY_SIZE(attributes); i++) + device_remove_file(dev, attributes + i); + return 0; +} + +static void lsm9ds0_gyr_report_triple(struct lsm9ds0_gyr_status *stat) +{ + int err; + struct lsm9ds0_gyr_triple data_out; + + err = lsm9ds0_gyr_get_data(stat, &data_out); + if (err < 0) + dev_err(&stat->client->dev, "get_gyroscope_data failed\n"); + else + lsm9ds0_gyr_report_values(stat, &data_out); +} + + +static void lsm9ds0_gyr_irq2_fifo(struct lsm9ds0_gyr_status *stat) +{ + int err; + u8 buf[2]; + u8 int_source; + u8 samples; + u8 workingmode; + u8 stored_samples; + + mutex_lock(&stat->lock); + + workingmode = stat->fifomode; + + + dev_dbg(&stat->client->dev, "%s : fifomode:0x%02x\n", __func__, + workingmode); + + + switch (workingmode) { + case FIFO_MODE_BYPASS: + { + dev_dbg(&stat->client->dev, "%s : fifomode:0x%02x\n", __func__, + stat->fifomode); + lsm9ds0_gyr_report_triple(stat); + break; + } + case FIFO_MODE_FIFO: + samples = (stat->watermark)+1; + dev_dbg(&stat->client->dev, + "%s : FIFO_SRC_REG init samples:%d\n", + __func__, samples); + err = lsm9ds0_gyr_register_read(stat, buf, FIFO_SRC_REG); + if (err < 0) + dev_err(&stat->client->dev, + "error reading fifo source reg\n"); + + int_source = buf[0]; + dev_dbg(&stat->client->dev, "%s :FIFO_SRC_REG content:0x%02x\n", + __func__, int_source); + + stored_samples = int_source & FIFO_STORED_DATA_MASK; + dev_dbg(&stat->client->dev, "%s : fifomode:0x%02x\n", __func__, + stat->fifomode); + + dev_dbg(&stat->client->dev, "%s : samples:%d stored:%d\n", + __func__, samples, stored_samples); + + for (; samples > 0; samples--) { +#ifdef DEBUG + input_report_abs(stat->input_dev, ABS_MISC, 1); + input_sync(stat->input_dev); +#endif + dev_dbg(&stat->client->dev, "%s : current sample:%d\n", + __func__, samples); + + lsm9ds0_gyr_report_triple(stat); + +#ifdef DEBUG + input_report_abs(stat->input_dev, ABS_MISC, 0); + input_sync(stat->input_dev); +#endif + } + lsm9ds0_gyr_fifo_reset(stat); + break; + } +#ifdef DEBUG + input_report_abs(stat->input_dev, ABS_MISC, 3); + input_sync(stat->input_dev); +#endif + + mutex_unlock(&stat->lock); +} + +static irqreturn_t lsm9ds0_gyr_isr2(int irq, void *dev) +{ + struct lsm9ds0_gyr_status *stat = dev; + + disable_irq_nosync(irq); +#ifdef DEBUG + input_report_abs(stat->input_dev, ABS_MISC, 2); + input_sync(stat->input_dev->input); +#endif + queue_work(stat->irq2_work_queue, &stat->irq2_work); + pr_debug("%s %s: isr2 queued\n", LSM9DS0_GYR_DEV_NAME, __func__); + + return IRQ_HANDLED; +} + +static void lsm9ds0_gyr_irq2_work_func(struct work_struct *work) +{ + + struct lsm9ds0_gyr_status *stat = + container_of(work, struct lsm9ds0_gyr_status, irq2_work); + /* TODO add interrupt service procedure. + ie:lsm9ds0_gyr_irq2_XXX(stat); */ + lsm9ds0_gyr_irq2_fifo(stat); + /* */ + pr_debug("%s %s: IRQ2 served\n", LSM9DS0_GYR_DEV_NAME, __func__); +/* exit: */ + enable_irq(stat->irq2); +} + +int lsm9ds0_gyr_input_open(struct input_dev *input) +{ + struct lsm9ds0_gyr_status *stat = input_get_drvdata(input); + dev_dbg(&stat->client->dev, "%s\n", __func__); + return lsm9ds0_gyr_enable(stat); +} + +void lsm9ds0_gyr_input_close(struct input_dev *dev) +{ + struct lsm9ds0_gyr_status *stat = input_get_drvdata(dev); + dev_dbg(&stat->client->dev, "%s\n", __func__); + lsm9ds0_gyr_disable(stat); +} + +static int lsm9ds0_gyr_validate_pdata(struct lsm9ds0_gyr_status *stat) +{ + /* checks for correctness of minimal polling period */ + stat->pdata->min_interval = + max((unsigned int) LSM9DS0_GYR_MIN_POLL_PERIOD_MS, + stat->pdata->min_interval); + + stat->pdata->poll_interval = max(stat->pdata->poll_interval, + stat->pdata->min_interval); + + if (stat->pdata->axis_map_x > 2 || + stat->pdata->axis_map_y > 2 || + stat->pdata->axis_map_z > 2) { + dev_err(&stat->client->dev, + "invalid axis_map value x:%u y:%u z%u\n", + stat->pdata->axis_map_x, + stat->pdata->axis_map_y, + stat->pdata->axis_map_z); + return -EINVAL; + } + + /* Only allow 0 and 1 for negation boolean flag */ + if (stat->pdata->negate_x > 1 || + stat->pdata->negate_y > 1 || + stat->pdata->negate_z > 1) { + dev_err(&stat->client->dev, + "invalid negate value x:%u y:%u z:%u\n", + stat->pdata->negate_x, + stat->pdata->negate_y, + stat->pdata->negate_z); + return -EINVAL; + } + + /* Enforce minimum polling interval */ + if (stat->pdata->poll_interval < stat->pdata->min_interval) { + dev_err(&stat->client->dev, + "minimum poll interval violated\n"); + return -EINVAL; + } + return 0; +} + +static int lsm9ds0_gyr_input_init(struct lsm9ds0_gyr_status *stat) +{ + int err = -1; + + dev_dbg(&stat->client->dev, "%s\n", __func__); + + stat->input_dev = input_allocate_device(); + if (!stat->input_dev) { + err = -ENOMEM; + dev_err(&stat->client->dev, + "input device allocation failed\n"); + goto err0; + } + + stat->input_dev->open = lsm9ds0_gyr_input_open; + stat->input_dev->close = lsm9ds0_gyr_input_close; + stat->input_dev->name = LSM9DS0_GYR_DEV_NAME; + + stat->input_dev->id.bustype = BUS_I2C; + stat->input_dev->dev.parent = &stat->client->dev; + + input_set_drvdata(stat->input_dev, stat); + + set_bit(EV_ABS, stat->input_dev->evbit); + +#ifdef DEBUG + set_bit(EV_KEY, stat->input_dev->keybit); + set_bit(KEY_LEFT, stat->input_dev->keybit); + input_set_abs_params(stat->input_dev, ABS_MISC, 0, 1, 0, 0); +#endif + + input_set_abs_params(stat->input_dev, ABS_X, -FS_MAX-1, FS_MAX, 0, 0); + input_set_abs_params(stat->input_dev, ABS_Y, -FS_MAX-1, FS_MAX, 0, 0); + input_set_abs_params(stat->input_dev, ABS_Z, -FS_MAX-1, FS_MAX, 0, 0); + + + err = input_register_device(stat->input_dev); + if (err) { + dev_err(&stat->client->dev, + "unable to register input polled device %s\n", + stat->input_dev->name); + goto err1; + } + + return 0; + +err1: + input_free_device(stat->input_dev); +err0: + return err; +} + +static void lsm9ds0_gyr_input_cleanup(struct lsm9ds0_gyr_status *stat) +{ + input_unregister_device(stat->input_dev); + input_free_device(stat->input_dev); +} + +static void poll_function_work(struct work_struct *polling_task) +{ + struct lsm9ds0_gyr_status *stat; + struct lsm9ds0_gyr_triple data_out; + int err; + + stat = container_of((struct work_struct *)polling_task, + struct lsm9ds0_gyr_status, polling_task); + + err = lsm9ds0_gyr_get_data(stat, &data_out); + if (err < 0) + dev_err(&stat->client->dev, "get_rotation_data failed.\n"); + else + lsm9ds0_gyr_report_values(stat, &data_out); + + hrtimer_start(&stat->hr_timer, stat->ktime, HRTIMER_MODE_REL); +} + +enum hrtimer_restart poll_function_read(struct hrtimer *timer) +{ + struct lsm9ds0_gyr_status *stat; + + stat = container_of((struct hrtimer *)timer, + struct lsm9ds0_gyr_status, hr_timer); + + queue_work(lsm9ds0_gyr_workqueue, &stat->polling_task); + return HRTIMER_NORESTART; +} + +static int lsm9ds0_gyr_probe(struct i2c_client *client, + const struct i2c_device_id *devid) +{ + struct lsm9ds0_gyr_status *stat; + + u32 smbus_func = I2C_FUNC_SMBUS_BYTE_DATA | + I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_I2C_BLOCK ; + + int err = -1; + + dev_info(&client->dev, "probe start.\n"); + + + stat = kzalloc(sizeof(*stat), GFP_KERNEL); + if (stat == NULL) { + dev_err(&client->dev, + "failed to allocate memory for module data\n"); + err = -ENOMEM; + goto err0; + } + + + /* Support for both I2C and SMBUS adapter interfaces. */ + 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)) { + stat->use_smbus = 1; + dev_warn(&client->dev, "client using SMBUS\n"); + } else { + err = -ENODEV; + dev_err(&client->dev, "client nor SMBUS capable\n"); + stat->use_smbus = 0; + goto err0; + } + } + + if(lsm9ds0_gyr_workqueue == 0) + lsm9ds0_gyr_workqueue = create_workqueue("lsm9ds0_gyr_workqueue"); + + hrtimer_init(&stat->hr_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + stat->hr_timer.function = &poll_function_read; + + mutex_init(&stat->lock); + mutex_lock(&stat->lock); + stat->client = client; + + stat->pdata = kmalloc(sizeof(*stat->pdata), GFP_KERNEL); + if (stat->pdata == NULL) { + dev_err(&client->dev, + "failed to allocate memory for pdata: %d\n", err); + goto err1; + } + + if (client->dev.platform_data == NULL) { + default_lsm9ds0_gyr_pdata.gpio_int1 = int1_gpio; + default_lsm9ds0_gyr_pdata.gpio_int2 = int2_gpio; + memcpy(stat->pdata, &default_lsm9ds0_gyr_pdata, + sizeof(*stat->pdata)); + dev_info(&client->dev, "using default plaform_data\n"); + } else { + memcpy(stat->pdata, client->dev.platform_data, + sizeof(*stat->pdata)); + } + + err = lsm9ds0_gyr_validate_pdata(stat); + if (err < 0) { + dev_err(&client->dev, "failed to validate platform data\n"); + goto err1_1; + } + + i2c_set_clientdata(client, stat); + + if (stat->pdata->init) { + err = stat->pdata->init(); + if (err < 0) { + dev_err(&client->dev, "init failed: %d\n", err); + goto err1_1; + } + } + + + memset(stat->resume_state, 0, ARRAY_SIZE(stat->resume_state)); + + stat->resume_state[RES_CTRL_REG1] = ALL_ZEROES | ENABLE_ALL_AXES + | PM_NORMAL; + stat->resume_state[RES_CTRL_REG2] = ALL_ZEROES; + stat->resume_state[RES_CTRL_REG3] = ALL_ZEROES; + stat->resume_state[RES_CTRL_REG4] = ALL_ZEROES | BDU_ENABLE; + stat->resume_state[RES_CTRL_REG5] = ALL_ZEROES; + stat->resume_state[RES_FIFO_CTRL_REG] = ALL_ZEROES; + + stat->polling_enabled = true; + dev_info(&client->dev, "polling mode enabled\n"); + + err = lsm9ds0_gyr_device_power_on(stat); + if (err < 0) { + dev_err(&client->dev, "power on failed: %d\n", err); + goto err2; + } + + atomic_set(&stat->enabled, 1); + + err = lsm9ds0_gyr_update_fs_range(stat, stat->pdata->fs_range); + if (err < 0) { + dev_err(&client->dev, "update_fs_range failed\n"); + goto err2; + } + + err = lsm9ds0_gyr_update_odr(stat, stat->pdata->poll_interval); + if (err < 0) { + dev_err(&client->dev, "update_odr failed\n"); + goto err2; + } + + err = lsm9ds0_gyr_input_init(stat); + if (err < 0) + goto err3; + + err = create_sysfs_interfaces(&client->dev); + if (err < 0) { + dev_err(&client->dev, + "%s device register failed\n", LSM9DS0_GYR_DEV_NAME); + goto err4; + } + + lsm9ds0_gyr_device_power_off(stat); + + /* As default, do not report information */ + atomic_set(&stat->enabled, 0); + + + if (stat->pdata->gpio_int2 >= 0) { + stat->irq2 = gpio_to_irq(stat->pdata->gpio_int2); + dev_info(&client->dev, "%s: %s has set irq2 to irq:" + " %d mapped on gpio:%d\n", + LSM9DS0_GYR_DEV_NAME, __func__, stat->irq2, + stat->pdata->gpio_int2); + + INIT_WORK(&stat->irq2_work, lsm9ds0_gyr_irq2_work_func); + stat->irq2_work_queue = + create_singlethread_workqueue("lsm9ds0_gyr_irq2_wq"); + if (!stat->irq2_work_queue) { + err = -ENOMEM; + dev_err(&client->dev, "cannot create " + "work queue2: %d\n", err); + goto err5; + } + + err = request_irq(stat->irq2, lsm9ds0_gyr_isr2, + IRQF_TRIGGER_HIGH, "lsm9ds0_gyr_irq2", stat); + + if (err < 0) { + dev_err(&client->dev, "request irq2 failed: %d\n", err); + goto err6; + } + disable_irq_nosync(stat->irq2); + } + + mutex_unlock(&stat->lock); + + INIT_WORK(&stat->polling_task, poll_function_work); + dev_info(&client->dev, "%s probed: device created successfully\n", + LSM9DS0_GYR_DEV_NAME); + + + return 0; + +/*err7: + free_irq(stat->irq2, stat); +*/ +err6: + destroy_workqueue(stat->irq2_work_queue); +err5: + lsm9ds0_gyr_device_power_off(stat); + remove_sysfs_interfaces(&client->dev); +err4: + lsm9ds0_gyr_input_cleanup(stat); +err3: + lsm9ds0_gyr_device_power_off(stat); +err2: + if (stat->pdata->exit) + stat->pdata->exit(); +err1_1: + mutex_unlock(&stat->lock); + kfree(stat->pdata); +err1: + destroy_workqueue(lsm9ds0_gyr_workqueue); + kfree(stat); +err0: + pr_err("%s: Driver Initialization failed\n", + LSM9DS0_GYR_DEV_NAME); + return err; +} + +static int lsm9ds0_gyr_remove(struct i2c_client *client) +{ + struct lsm9ds0_gyr_status *stat = i2c_get_clientdata(client); + + dev_info(&stat->client->dev, "driver removing\n"); + + cancel_work_sync(&stat->polling_task); + if(!lsm9ds0_gyr_workqueue) { + flush_workqueue(lsm9ds0_gyr_workqueue); + destroy_workqueue(lsm9ds0_gyr_workqueue); + } + /* + if (stat->pdata->gpio_int1 >= 0) + { + free_irq(stat->irq1, stat); + gpio_free(stat->pdata->gpio_int1); + destroy_workqueue(stat->irq1_work_queue); + } + */ + if (stat->pdata->gpio_int2 >= 0) { + free_irq(stat->irq2, stat); + gpio_free(stat->pdata->gpio_int2); + destroy_workqueue(stat->irq2_work_queue); + } + + lsm9ds0_gyr_disable(stat); + lsm9ds0_gyr_input_cleanup(stat); + + remove_sysfs_interfaces(&client->dev); + + kfree(stat->pdata); + kfree(stat); + return 0; +} + +static int lsm9ds0_gyr_suspend(struct device *dev) +{ + int err = 0; +#define SLEEP +#ifdef CONFIG_PM + struct i2c_client *client = to_i2c_client(dev); + struct lsm9ds0_gyr_status *stat = i2c_get_clientdata(client); + u8 buf[2]; + + dev_info(&client->dev, "suspend\n"); + + dev_dbg(&client->dev, "%s\n", __func__); + if (atomic_read(&stat->enabled)) { + mutex_lock(&stat->lock); + if (stat->polling_enabled) { + dev_info(&stat->client->dev, "polling mode disabled\n"); + hrtimer_cancel(&stat->hr_timer); + } +#ifdef SLEEP + err = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG1, + 0x0F, (ENABLE_NO_AXES | PM_NORMAL)); +#else + err = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG1, + 0x08, PM_OFF); +#endif /*SLEEP*/ + mutex_unlock(&stat->lock); + } +#endif /*CONFIG_PM*/ + return err; +} + +static int lsm9ds0_gyr_resume(struct device *dev) +{ + int err = 0; +#ifdef CONFIG_PM + struct i2c_client *client = to_i2c_client(dev); + struct lsm9ds0_gyr_status *stat = i2c_get_clientdata(client); + u8 buf[2]; + + + dev_info(&client->dev, "resume\n"); + + dev_dbg(&client->dev, "%s\n", __func__); + if (atomic_read(&stat->enabled)) { + mutex_lock(&stat->lock); + if (stat->polling_enabled) { + dev_info(&stat->client->dev, "polling mode enabled\n"); + hrtimer_init(&stat->hr_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + } +#ifdef SLEEP + err = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG1, + 0x0F, (ENABLE_ALL_AXES | PM_NORMAL)); +#else + err = lsm9ds0_gyr_register_update(stat, buf, CTRL_REG1, + 0x08, PM_NORMAL); +#endif + mutex_unlock(&stat->lock); + + } +#endif /*CONFIG_PM*/ + return err; +} + + +static const struct i2c_device_id lsm9ds0_gyr_id[] = { + { LSM9DS0_GYR_DEV_NAME , 0 }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, lsm9ds0_gyr_id); + +static const struct of_device_id lsm9ds0_gyr_of_match[] = { + { .compatible = "st,"LSM9DS0_GYR_DEV_NAME, }, + { }, +}; +MODULE_DEVICE_TABLE(of, lsm9ds0_gyr_of_match); + +static const struct dev_pm_ops lsm9ds0_gyr_pm = { + .suspend = lsm9ds0_gyr_suspend, + .resume = lsm9ds0_gyr_resume, +}; + +static struct i2c_driver lsm9ds0_i2c_gyr_driver = { + .driver = { + .owner = THIS_MODULE, + .name = LSM9DS0_GYR_DEV_NAME, + .pm = &lsm9ds0_gyr_pm, + .of_match_table = lsm9ds0_gyr_of_match, + }, + .probe = lsm9ds0_gyr_probe, + .remove = lsm9ds0_gyr_remove, + .id_table = lsm9ds0_gyr_id, + +}; + +module_i2c_driver(lsm9ds0_i2c_gyr_driver); + +MODULE_DESCRIPTION("lsm9ds0 gyroscope driver"); +MODULE_AUTHOR("Matteo Dameno, Denis Ciocca, STMicroelectronics"); +MODULE_LICENSE("GPL"); diff --git a/include/linux/input/lsm9ds0.h b/include/linux/input/lsm9ds0.h new file mode 100644 index 0000000..c115eb7 --- /dev/null +++ b/include/linux/input/lsm9ds0.h @@ -0,0 +1,201 @@ +/******************** (C) COPYRIGHT 2012 STMicroelectronics ******************** +* +* File Name : lsm9ds0.h +* Authors : MSH - C&I BU - Application Team +* : Matteo Dameno (matteo.dameno@st.com) +* : Denis Ciocca (denis.ciocca@st.com) +* Version : V.1.0.2 +* Date : 2013/Oct/23 +* +******************************************************************************** +* +* 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 +* 1.0.1 | 2012/Aug/30 | Denis Ciocca | corrects gyr_get_data func +* 1.0.2 | 2013/Oct/23 | Matteo Dameno | introduced acc_mag 1.0.5 +*******************************************************************************/ + +#ifndef __LSM9DS0_H__ +#define __LSM9DS0_H__ + +#define LSM9DS0_DEV_NAME "lsm9ds0_acc_mag" /* i2c system name */ +#define LSM9DS0_ACC_DEV_NAME "lsm9ds0_acc" /* Input file name */ +#define LSM9DS0_MAG_DEV_NAME "lsm9ds0_mag" /* Input file name */ +#define LSM9DS0_GYR_DEV_NAME "lsm9ds0_gyr" /* Input file name */ + +#define LSM9DS0_SAD0L_ACC_MAG (0x02) +#define LSM9DS0_SAD0H_ACC_MAG (0x01) +#define LSM9DS0_SAD0L_GYR (0x00) +#define LSM9DS0_SAD0H_GYR (0x01) + +/************************************************/ +/* Output data */ +/************************************************* +accelerometer: ug +magnetometer: ugauss +gyroscope: udps +*************************************************/ + +/************************************************/ +/* sysfs data */ +/************************************************* +accelerometer: + - pollrate->ms + - fullscale->g +magnetometer: + - pollrate->ms + - fullscale->gauss +gyroscope: + - pollrate->ms + - fullscale->dps +*************************************************/ + +#define LSM9DS0_ACC_MAG_I2C_SADROOT (0x07) + +/* I2C address if gyr SA0 pin to GND */ +#define LSM9DS0_ACC_MAG_I2C_SAD_L ((LSM9DS0_ACC_MAG_I2C_SADROOT<<2)| \ + LSM9DS0_SAD0L_ACC_MAG) +/* I2C address if gyr SA0 pin to Vdd */ +#define LSM9DS0_ACC_MAG_I2C_SAD_H ((LSM9DS0_ACC_MAG_I2C_SADROOT<<2)| \ + LSM9DS0_SAD0H_ACC_MAG) + +/************************************************/ +/* Accelerometer section defines */ +/************************************************/ + +/* Accelerometer Sensor Full Scale */ +#define LSM9DS0_ACC_FS_MASK (0x18) +#define LSM9DS0_ACC_FS_2G (0x00) /* Full scale 2g */ +#define LSM9DS0_ACC_FS_4G (0x08) /* Full scale 4g */ +#define LSM9DS0_ACC_FS_8G (0x10) /* Full scale 8g */ +#define LSM9DS0_ACC_FS_16G (0x18) /* Full scale 16g */ + +/* Accelerometer Anti-Aliasing Filter */ +#define ANTI_ALIASING_773 (0X00) +#define ANTI_ALIASING_362 (0X40) +#define ANTI_ALIASING_194 (0X80) +#define ANTI_ALIASING_50 (0XC0) + +/************************************************/ +/* Magnetometer section defines */ +/************************************************/ + +/* Magnetometer Sensor Full Scale */ +#define LSM9DS0_MAG_FS_MASK (0x60) +#define LSM9DS0_MAG_FS_2G (0x00) /* Full scale 2 gauss */ +#define LSM9DS0_MAG_FS_4G (0x20) /* Full scale 4 gauss */ +#define LSM9DS0_MAG_FS_8G (0x40) /* Full scale 8 gauss */ +#define LSM9DS0_MAG_FS_12G (0x60) /* Full scale 12 gauss */ + +/************************************************/ +/* Gyroscope section defines */ +/************************************************/ + +#define LSM9DS0_GYR_I2C_SADROOT (0x35) + +/* I2C address if gyr SA0 pin to GND */ +#define LSM9DS0_GYR_I2C_SAD_L ((LSM9DS0_GYR_I2C_SADROOT<<1)| \ + LSM9DS0_SAD0L_GYR) +/* I2C address if gyr SA0 pin to Vdd */ +#define LSM9DS0_GYR_I2C_SAD_H ((LSM9DS0_GYR_I2C_SADROOT<<1)| \ + LSM9DS0_SAD0H_GYR) + +#ifdef __KERNEL__ + +/* to set gpios numb connected to gyro interrupt pins, + * the unused ones havew to be set to -EINVAL + */ +#define DEFAULT_INT1_GPIO (-EINVAL) +#define DEFAULT_INT2_GPIO (-EINVAL) + +#define LSM9DS0_ACC_MIN_POLL_PERIOD_MS 1 +#define LSM9DS0_MAG_MIN_POLL_PERIOD_MS 5 + +#define LSM9DS0_GYR_DEFAULT_INT1_GPIO (-EINVAL) +#define LSM9DS0_GYR_DEFAULT_INT2_GPIO (-EINVAL) + +#define LSM9DS0_GYR_MIN_POLL_PERIOD_MS 2 + +#define LSM9DS0_GYR_FS_250DPS (0x00) +#define LSM9DS0_GYR_FS_500DPS (0x10) +#define LSM9DS0_GYR_FS_2000DPS (0x30) + +struct lsm9ds0_acc_platform_data { + + unsigned int poll_interval; + unsigned int min_interval; + + u8 fs_range; + + short rot_matrix[3][3]; + + u8 aa_filter_bandwidth; + + int (*init)(void); + void (*exit)(void); + int (*power_on)(void); + int (*power_off)(void); + + int gpio_int1; + int gpio_int2; +}; + +struct lsm9ds0_mag_platform_data { + + unsigned int poll_interval; + unsigned int min_interval; + + u8 fs_range; + + short rot_matrix[3][3]; + + int (*init)(void); + void (*exit)(void); + int (*power_on)(void); + int (*power_off)(void); +}; + +struct lsm9ds0_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; + + /* gpio ports for interrupt pads */ + int gpio_int1; + int gpio_int2; /* int for fifo */ + + u8 axis_map_x; + u8 axis_map_y; + u8 axis_map_z; + + u8 negate_x; + u8 negate_y; + u8 negate_z; +}; + +struct lsm9ds0_main_platform_data { + + struct lsm9ds0_acc_platform_data *pdata_acc; + struct lsm9ds0_mag_platform_data *pdata_mag; +}; + +#endif /* __KERNEL__ */ +#endif /* __LSM9DS0_H__ */ -- 1.7.10.4