diff options
Diffstat (limited to 'meta-rcar-gen2/recipes-kernel/linux/linux-renesas/0049-Add-lsm9ds0-acc-gyro-mag-driver.patch')
| -rw-r--r-- | meta-rcar-gen2/recipes-kernel/linux/linux-renesas/0049-Add-lsm9ds0-acc-gyro-mag-driver.patch | 5433 |
1 files changed, 5433 insertions, 0 deletions
diff --git a/meta-rcar-gen2/recipes-kernel/linux/linux-renesas/0049-Add-lsm9ds0-acc-gyro-mag-driver.patch b/meta-rcar-gen2/recipes-kernel/linux/linux-renesas/0049-Add-lsm9ds0-acc-gyro-mag-driver.patch new file mode 100644 index 0000000..d461b3d --- /dev/null +++ b/meta-rcar-gen2/recipes-kernel/linux/linux-renesas/0049-Add-lsm9ds0-acc-gyro-mag-driver.patch @@ -0,0 +1,5433 @@ +From dadd0a8a2cf1fecbbb0df7713f3da81ce7c11b00 Mon Sep 17 00:00:00 2001 +From: Andrey Gusakov <andrey.gusakov@cogentembedded.com> +Date: Wed, 16 Sep 2015 14:28:22 +0300 +Subject: [PATCH 49/50] Add lsm9ds0 (acc gyro mag) driver + + +Signed-off-by: Andrey Gusakov <andrey.gusakov@cogentembedded.com> +--- + 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 <linux/module.h> ++#include <linux/init.h> ++#include <linux/slab.h> ++#include <linux/i2c.h> ++#include <linux/workqueue.h> ++#include <linux/input.h> ++#include <linux/kernel.h> ++#include <linux/kobject.h> ++#include <linux/gpio.h> ++#include <linux/interrupt.h> ++#include <linux/irq.h> ++#include <linux/hrtimer.h> ++#include <linux/ktime.h> ++ ++#include <linux/input/lsm9ds0.h> ++/* 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 <linux/init.h> ++#include <linux/module.h> ++#include <linux/i2c.h> ++#include <linux/mutex.h> ++#include <linux/input.h> ++#include <linux/interrupt.h> ++#include <linux/gpio.h> ++#include <linux/slab.h> ++#include <linux/moduleparam.h> ++#include <linux/kernel.h> ++#include <linux/stat.h> ++ ++ ++#include <linux/input/lsm9ds0.h> ++/* #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 |