From 2133e464d391e1b943ef2606a88e3788eefb5571 Mon Sep 17 00:00:00 2001 From: sathya Date: Fri, 4 Nov 2011 13:33:46 -0700 Subject: [PATCH] Added Gyro sensor driver: Fork lifted mpu3050 driver from git://jfumg-gcrmirror.jf.intel.com/a/bsp/hardware/intel/linux-2.6.git Change-Id: I82992c5870c3d9327d321d3b4781822eba5354e3 Reviewed-on: http://android.intel.com:8080/23222 Reviewed-by: Gross, Mark Tested-by: Gross, Mark --- arch/x86/platform/mfld/blackbay_pr2.c | 7 + drivers/hwmon/Kconfig | 11 + drivers/hwmon/Makefile | 1 + drivers/hwmon/mpu3050.c | 775 ++++++++++++++++++++++++++++++++++ 4 files changed, 794 insertions(+) create mode 100644 drivers/hwmon/mpu3050.c diff --git a/arch/x86/platform/mfld/blackbay_pr2.c b/arch/x86/platform/mfld/blackbay_pr2.c index 25fb9cb..a2cc34f 100644 --- a/arch/x86/platform/mfld/blackbay_pr2.c +++ b/arch/x86/platform/mfld/blackbay_pr2.c @@ -75,6 +75,8 @@ static struct lis3dh_acc_platform_data lis3dh_pdata = { .gpio_int1 = 60, .gpio_int2 = 61, }; +#define MPU_GPIO_PIN 56 + static struct i2c_board_info pr2_i2c_bus5_devs[] = { { .type = "accel", @@ -87,6 +89,11 @@ static struct i2c_board_info pr2_i2c_bus5_devs[] = { .irq = 0xff, .addr = 0x1E, }, + { + .type = "gyro", + .irq = MPU_GPIO_PIN, + .addr = 0x68, + }, }; static struct gpio_keys_button gpio_button[] = { diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig index fe4c05b..d9115cc 100644 --- a/drivers/hwmon/Kconfig +++ b/drivers/hwmon/Kconfig @@ -1376,6 +1376,17 @@ config SENSORS_HMC5883 help Say Yes here to enable the Honeywell HMC5883 Magnetometer Driver +config SENSORS_MPU3050 + bool "MPU3050 gyro sensor Driver" + depends on I2C && INPUT + default y + help + Say Yes here to enable MPU3050 gyro sensor Driver + + Gyroscope sensor MPU3050 from Invensense + please search on + http://www.invensense.com/ for product details. + config MSIC_GPADC tristate "MSIC GPADC driver for Intel Medfield platform" depends on INTEL_SCU_IPC diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile index ed36c2a..7d11ae6 100644 --- a/drivers/hwmon/Makefile +++ b/drivers/hwmon/Makefile @@ -120,6 +120,7 @@ obj-$(CONFIG_SENSORS_WM831X) += wm831x-hwmon.o obj-$(CONFIG_SENSORS_WM8350) += wm8350-hwmon.o obj-$(CONFIG_SENSORS_LIS3DH_ACC) += lis3dh_acc.o obj-$(CONFIG_SENSORS_HMC5883) += hmc5883.o +obj-$(CONFIG_SENSORS_MPU3050) += mpu3050.o obj-$(CONFIG_MSIC_GPADC) += intel_mid_gpadc.o # PMBus drivers diff --git a/drivers/hwmon/mpu3050.c b/drivers/hwmon/mpu3050.c new file mode 100644 index 0000000..60614f7 --- /dev/null +++ b/drivers/hwmon/mpu3050.c @@ -0,0 +1,775 @@ +/* + $License: + Copyright (C) 2010 InvenSense Corporation, All Rights Reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + $ + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/*==== MPU REGISTER SET ====*/ +enum mpu_register { + MPUREG_WHO_AM_I = 0, /* 00 0x00 */ + MPUREG_PRODUCT_ID, /* 01 0x01 */ + MPUREG_02_RSVD, /* 02 0x02 */ + MPUREG_03_RSVD, /* 03 0x03 */ + MPUREG_04_RSVD, /* 04 0x04 */ + MPUREG_XG_OFFS_TC, /* 05 0x05 */ + MPUREG_06_RSVD, /* 06 0x06 */ + MPUREG_07_RSVD, /* 07 0x07 */ + MPUREG_YG_OFFS_TC, /* 08 0x08 */ + MPUREG_09_RSVD, /* 09 0x09 */ + MPUREG_0A_RSVD, /* 10 0x0a */ + MPUREG_ZG_OFFS_TC, /* 11 0x0b */ + MPUREG_X_OFFS_USRH, /* 12 0x0c */ + MPUREG_X_OFFS_USRL, /* 13 0x0d */ + MPUREG_Y_OFFS_USRH, /* 14 0x0e */ + MPUREG_Y_OFFS_USRL, /* 15 0x0f */ + MPUREG_Z_OFFS_USRH, /* 16 0x10 */ + MPUREG_Z_OFFS_USRL, /* 17 0x11 */ + MPUREG_FIFO_EN1, /* 18 0x12 */ + MPUREG_FIFO_EN2, /* 19 0x13 */ + MPUREG_AUX_SLV_ADDR, /* 20 0x14 */ + MPUREG_SMPLRT_DIV, /* 21 0x15 */ + MPUREG_DLPF_FS_SYNC, /* 22 0x16 */ + MPUREG_INT_CFG, /* 23 0x17 */ + MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */ + MPUREG_19_RSVD, /* 25 0x19 */ + MPUREG_INT_STATUS, /* 26 0x1a */ + MPUREG_TEMP_OUT_H, /* 27 0x1b */ + MPUREG_TEMP_OUT_L, /* 28 0x1c */ + MPUREG_GYRO_XOUT_H, /* 29 0x1d */ + MPUREG_GYRO_XOUT_L, /* 30 0x1e */ + MPUREG_GYRO_YOUT_H, /* 31 0x1f */ + MPUREG_GYRO_YOUT_L, /* 32 0x20 */ + MPUREG_GYRO_ZOUT_H, /* 33 0x21 */ + MPUREG_GYRO_ZOUT_L, /* 34 0x22 */ + MPUREG_23_RSVD, /* 35 0x23 */ + MPUREG_24_RSVD, /* 36 0x24 */ + MPUREG_25_RSVD, /* 37 0x25 */ + MPUREG_26_RSVD, /* 38 0x26 */ + MPUREG_27_RSVD, /* 39 0x27 */ + MPUREG_28_RSVD, /* 40 0x28 */ + MPUREG_29_RSVD, /* 41 0x29 */ + MPUREG_2A_RSVD, /* 42 0x2a */ + MPUREG_2B_RSVD, /* 43 0x2b */ + MPUREG_2C_RSVD, /* 44 0x2c */ + MPUREG_2D_RSVD, /* 45 0x2d */ + MPUREG_2E_RSVD, /* 46 0x2e */ + MPUREG_2F_RSVD, /* 47 0x2f */ + MPUREG_30_RSVD, /* 48 0x30 */ + MPUREG_31_RSVD, /* 49 0x31 */ + MPUREG_32_RSVD, /* 50 0x32 */ + MPUREG_33_RSVD, /* 51 0x33 */ + MPUREG_34_RSVD, /* 52 0x34 */ + MPUREG_DMP_CFG_1, /* 53 0x35 */ + MPUREG_DMP_CFG_2, /* 54 0x36 */ + MPUREG_BANK_SEL, /* 55 0x37 */ + MPUREG_MEM_START_ADDR, /* 56 0x38 */ + MPUREG_MEM_R_W, /* 57 0x39 */ + MPUREG_FIFO_COUNTH, /* 58 0x3a */ + MPUREG_FIFO_COUNTL, /* 59 0x3b */ + MPUREG_FIFO_R_W, /* 60 0x3c */ + MPUREG_USER_CTRL, /* 61 0x3d */ + MPUREG_PWR_MGM, /* 62 0x3e */ + MPUREG_3F_RSVD, /* 63 0x3f */ + NUM_OF_MPU_REGISTERS /* 64 0x40 */ +}; + +/*==== BITS FOR MPU ====*/ + +/*---- MPU 'FIFO_EN1' register (12) ----*/ +#define BIT_TEMP_OUT 0x80 +#define BIT_GYRO_XOUT 0x40 +#define BIT_GYRO_YOUT 0x20 +#define BIT_GYRO_ZOUT 0x10 +#define BIT_ACCEL_XOUT 0x08 +#define BIT_ACCEL_YOUT 0x04 +#define BIT_ACCEL_ZOUT 0x02 +#define BIT_AUX_1OUT 0x01 +/*---- MPU 'FIFO_EN2' register (13) ----*/ +#define BIT_AUX_2OUT 0x02 +#define BIT_AUX_3OUT 0x01 +/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/ +#define BITS_EXT_SYNC_NONE 0x00 +#define BITS_EXT_SYNC_TEMP 0x20 +#define BITS_EXT_SYNC_GYROX 0x40 +#define BITS_EXT_SYNC_GYROY 0x60 +#define BITS_EXT_SYNC_GYROZ 0x80 +#define BITS_EXT_SYNC_ACCELX 0xA0 +#define BITS_EXT_SYNC_ACCELY 0xC0 +#define BITS_EXT_SYNC_ACCELZ 0xE0 +#define BITS_EXT_SYNC_MASK 0xE0 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +/*---- MPU 'INT_CFG' register (17) ----*/ +#define BIT_ACTL 0x80 +#define BIT_ACTL_LOW 0x80 +#define BIT_ACTL_HIGH 0x00 +#define BIT_OPEN 0x40 +#define BIT_OPEN_DRAIN 0x40 +#define BIT_PUSH_PULL 0x00 +#define BIT_LATCH_INT_EN 0x20 +#define BIT_INT_PULSE_WIDTH_50US 0x00 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_INT_STAT_READ_2CLEAR 0x00 +#define BIT_MPU_RDY_EN 0x04 +#define BIT_DMP_INT_EN 0x02 +#define BIT_RAW_RDY_EN 0x01 +/*---- MPU 'INT_STATUS' register (1A) ----*/ +#define BIT_INT_STATUS_FIFO_OVERLOW 0x80 +#define BIT_MPU_RDY 0x04 +#define BIT_DMP_INT 0x02 +#define BIT_RAW_RDY 0x01 +/*---- MPU 'BANK_SEL' register (37) ----*/ +#define BIT_PRFTCH_EN 0x20 +#define BIT_CFG_USER_BANK 0x10 +#define BITS_MEM_SEL 0x0f +/*---- MPU 'USER_CTRL' register (3D) ----*/ +#define BIT_DMP_EN 0x80 +#define BIT_FIFO_EN 0x40 +#define BIT_AUX_IF_EN 0x20 +#define BIT_AUX_RD_LENG 0x10 +#define BIT_AUX_IF_RST 0x08 +#define BIT_DMP_RST 0x04 +#define BIT_FIFO_RST 0x02 +#define BIT_GYRO_RST 0x01 +/*---- MPU 'PWR_MGM' register (3E) ----*/ +#define BIT_H_RESET 0x80 +#define BIT_SLEEP 0x40 +#define BIT_STBY_XG 0x20 +#define BIT_STBY_YG 0x10 +#define BIT_STBY_ZG 0x08 +#define BITS_CLKSEL 0x07 +#define BITS_PLL_X 0x01 +#define BITS_PLL_Y 0x02 +#define BITS_PLL_Z 0x03 + +/*---- MPU Silicon Revision ----*/ +#define MPU_SILICON_REV_A4 1 /* MPU A4 Device */ +#define MPU_SILICON_REV_B1 2 /* MPU B1 Device */ +#define MPU_SILICON_REV_B4 3 /* MPU B4 Device */ +#define MPU_SILICON_REV_B6 4 /* MPU B6 Device */ + +/*---- MPU Memory ----*/ +#define MPU_MEM_BANK_SIZE (256) +#define FIFO_HW_SIZE (512) + +enum MPU_MEMORY_BANKS { + MPU_MEM_RAM_BANK_0 = 0, + MPU_MEM_RAM_BANK_1, + MPU_MEM_RAM_BANK_2, + MPU_MEM_RAM_BANK_3, + MPU_MEM_NUM_RAM_BANKS, + MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS, + /* This one is always last */ + MPU_MEM_NUM_BANKS +}; + +#define MPU_NUM_AXES (3) + +/*---- structure containing control variables used by MLDL ----*/ +/*---- MPU clock source settings ----*/ +/*---- MPU filter selections ----*/ +enum mpu_filter { + MPU_FILTER_256HZ_NOLPF2 = 0, + MPU_FILTER_188HZ, + MPU_FILTER_98HZ, + MPU_FILTER_42HZ, + MPU_FILTER_20HZ, + MPU_FILTER_10HZ, + MPU_FILTER_5HZ, + MPU_FILTER_2100HZ_NOLPF, + NUM_MPU_FILTER +}; + +enum mpu_fullscale { + MPU_FS_250DPS = 0, + MPU_FS_500DPS, + MPU_FS_1000DPS, + MPU_FS_2000DPS, + NUM_MPU_FS +}; + +enum mpu_clock_sel { + MPU_CLK_SEL_INTERNAL = 0, + MPU_CLK_SEL_PLLGYROX, + MPU_CLK_SEL_PLLGYROY, + MPU_CLK_SEL_PLLGYROZ, + MPU_CLK_SEL_PLLEXT32K, + MPU_CLK_SEL_PLLEXT19M, + MPU_CLK_SEL_RESERVED, + MPU_CLK_SEL_STOP, + NUM_CLK_SEL +}; + +enum mpu_ext_sync { + MPU_EXT_SYNC_NONE = 0, + MPU_EXT_SYNC_TEMP, + MPU_EXT_SYNC_GYROX, + MPU_EXT_SYNC_GYROY, + MPU_EXT_SYNC_GYROZ, + MPU_EXT_SYNC_ACCELX, + MPU_EXT_SYNC_ACCELY, + MPU_EXT_SYNC_ACCELZ, + NUM_MPU_EXT_SYNC +}; + +#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \ + ((ext_sync << 5) | (full_scale << 3) | lpf) + +#define MPU_NAME "gyro" +#define DEFAULT_POLL_INTERVAL 200 +#define DEFAULT_FS_RANGE 3 +#define MPU_RESET_DELAY 15 +#define MPU_ENABLE_DELAY 80 + +struct mpu_data { + struct i2c_client *client; + struct mutex lock; + struct input_dev *input_dev; + struct early_suspend es; + int poll_interval; + int enabled; + int gpio; +}; + +static int mpu_reset(struct mpu_data *mpu) +{ + struct i2c_client *client = mpu->client; + int ret; + + ret = i2c_smbus_write_byte_data(client, MPUREG_PWR_MGM, BIT_H_RESET); + if (ret < 0) + return ret; + + msleep(MPU_RESET_DELAY); + return 0; +} + +static int mpu_hw_init(struct mpu_data *mpu) +{ + struct i2c_client *client = mpu->client; + int ret; + + ret = i2c_smbus_read_byte_data(client, MPUREG_PWR_MGM); + if (ret < 0) + return ret; + + ret &= ~BITS_CLKSEL; + ret |= BITS_PLL_Z; + ret = i2c_smbus_write_byte_data(client, MPUREG_PWR_MGM, ret); + if (ret < 0) + return ret; + + ret = i2c_smbus_write_byte_data(client, MPUREG_SMPLRT_DIV, + mpu->poll_interval - 1); + if (ret < 0) + return ret; + + ret = i2c_smbus_write_byte_data(client, MPUREG_DLPF_FS_SYNC, + DLPF_FS_SYNC_VALUE(BITS_EXT_SYNC_NONE, + BITS_DLPF_CFG_42HZ, + DEFAULT_FS_RANGE)); + if (ret < 0) + return ret; + + ret = i2c_smbus_write_byte_data(mpu->client, MPUREG_INT_CFG, + BIT_LATCH_INT_EN | BIT_RAW_RDY_EN | BIT_MPU_RDY_EN); + if (ret < 0) + return ret; + + return 0; +} + +static void mpu_report_values(struct mpu_data *mpu, unsigned char *xyz) +{ + short val; + + val = xyz[0]; + val = (val << 8) | xyz[1]; + input_report_rel(mpu->input_dev, REL_X, val); + + val = xyz[2]; + val = (val << 8) | (xyz[3]); + input_report_rel(mpu->input_dev, REL_Y, val); + + val = xyz[4]; + val = (val << 8) | (xyz[5]); + input_report_rel(mpu->input_dev, REL_Z, val); + + input_sync(mpu->input_dev); +} + +static irqreturn_t mpu_isr(int irq, void *dev) +{ + struct mpu_data *mpu = dev; + struct i2c_client *client = mpu->client; + int ret; + unsigned char int_stat; + unsigned char buf[6]; + + mutex_lock(&mpu->lock); + + int_stat = i2c_smbus_read_byte_data(client, MPUREG_INT_STATUS); + if (int_stat < 0) { + ret = IRQ_NONE; + goto out; + } + + dev_dbg(&client->dev, "interrupt status register = 0x%x\n", int_stat); + + if (int_stat & BIT_MPU_RDY_EN) { + /* + * Don't report data when PLL is just ready, data is + * not correct at this time. + */ + dev_dbg(&client->dev, "MPU PLL ready\n"); + goto out; + } + + if (int_stat & BIT_RAW_RDY) { + ret = i2c_smbus_read_i2c_block_data(client, MPUREG_GYRO_XOUT_H, + sizeof(buf), buf); + if (ret > 0) + mpu_report_values(mpu, buf); + } else { + dev_dbg(&client->dev, "undefined interrupt\n"); + } + + ret = IRQ_HANDLED; +out: + mutex_unlock(&mpu->lock); + return ret; +} + +static int mpu_sleep(struct mpu_data *mpu) +{ + int ret; + + ret = i2c_smbus_read_byte_data(mpu->client, MPUREG_PWR_MGM); + if (ret < 0) + return 1; + + if (ret & BIT_SLEEP) + return 1; + + return 0; +} + +static int mpu_enable(struct mpu_data *mpu) +{ + int ret; + + ret = i2c_smbus_read_byte_data(mpu->client, MPUREG_PWR_MGM); + if (ret < 0) + return ret; + + if (!(ret & BIT_SLEEP)) + return 0; + + ret &= ~BIT_SLEEP; + ret = i2c_smbus_write_byte_data(mpu->client, MPUREG_PWR_MGM, (u8)ret); + if (ret < 0) + return ret; + msleep(MPU_ENABLE_DELAY); + + return 0; +} + +static int mpu_disable(struct mpu_data *mpu) +{ + int ret; + + ret = i2c_smbus_read_byte_data(mpu->client, MPUREG_PWR_MGM); + if (ret < 0) + return ret; + + if (ret & BIT_SLEEP) + return 0; + + ret |= BIT_SLEEP; + ret = i2c_smbus_write_byte_data(mpu->client, MPUREG_PWR_MGM, (u8)ret); + if (ret < 0) + return ret; + return 0; +} + +static ssize_t attr_get_polling_rate(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct mpu_data *mpu = dev_get_drvdata(dev); + + return sprintf(buf, "%d\n", mpu->poll_interval); +} + +static ssize_t attr_set_polling_rate(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct mpu_data *mpu = dev_get_drvdata(dev); + struct i2c_client *client = mpu->client; + unsigned long interval_ms; + int ret = size; + + if (strict_strtoul(buf, 10, &interval_ms)) + return -EINVAL; + + if (interval_ms < 5) { + dev_dbg(dev, "gyro poll interval is less than 5, set it to 5.\n"); + interval_ms = 5; + } + + if (interval_ms > 255) { + dev_dbg(dev, "gyro poll interval is greater than 255, set it to 255.\n"); + interval_ms = 255; + } + + dev_dbg(dev, "gyro set poll interval %ld\n", interval_ms); + + mutex_lock(&mpu->lock); + + if (mpu_sleep(mpu)) { + ret = -ENODEV; + goto out; + } + + i2c_smbus_write_byte_data(client, MPUREG_SMPLRT_DIV, interval_ms - 1); + mpu->poll_interval = interval_ms; +out: + mutex_unlock(&mpu->lock); + return ret; +} + +static ssize_t attr_get_enable(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct mpu_data *mpu = dev_get_drvdata(dev); + + return sprintf(buf, "%d\n", mpu->enabled); +} + +static ssize_t attr_set_enable(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct mpu_data *mpu = dev_get_drvdata(dev); + unsigned long val; + + if (strict_strtoul(buf, 10, &val)) + return -EINVAL; + + dev_dbg(dev, "gyro set enable %ld\n", val); + + mutex_lock(&mpu->lock); + if (val) + mpu_enable(mpu); + else + mpu_disable(mpu); + mpu->enabled = val; + mutex_unlock(&mpu->lock); + + return size; +} + +static struct device_attribute attributes[] = { + __ATTR(poll, 0664, attr_get_polling_rate, attr_set_polling_rate), + __ATTR(enable, 0664, attr_get_enable, attr_set_enable), +}; + +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 int __devinit mpu_input_init(struct mpu_data *mpu) +{ + int ret; + struct device *dev = &mpu->client->dev; + + mpu->input_dev = input_allocate_device(); + if (!mpu->input_dev) { + dev_err(dev, "input device allocation failed\n"); + ret = -ENOMEM; + goto out; + } + + mpu->input_dev->name = MPU_NAME; + mpu->input_dev->id.bustype = BUS_I2C; + mpu->input_dev->dev.parent = &mpu->client->dev; + + input_set_drvdata(mpu->input_dev, mpu); + set_bit(EV_REL, mpu->input_dev->evbit); + set_bit(REL_X, mpu->input_dev->relbit); + set_bit(REL_Y, mpu->input_dev->relbit); + set_bit(REL_Z, mpu->input_dev->relbit); + ret = input_register_device(mpu->input_dev); + if (ret) { + dev_err(dev, "unable to register input device\n"); + input_free_device(mpu->input_dev); + goto out; + } + + ret = create_sysfs_interfaces(&mpu->client->dev); + if (ret < 0) { + dev_err(dev, "device gyro sysfs register failed\n"); + input_unregister_device(mpu->input_dev); + } + +out: + return ret; +} + +#ifdef CONFIG_HAS_EARLYSUSPEND +static void mpu_early_suspend(struct early_suspend *h) +{ + struct mpu_data *mpu = container_of(h, struct mpu_data, es); + + mutex_lock(&mpu->lock); + mpu_disable(mpu); + disable_irq(mpu->client->irq); + mutex_unlock(&mpu->lock); +} + +static void mpu_late_resume(struct early_suspend *h) +{ + struct mpu_data *mpu = container_of(h, struct mpu_data, es); + + mutex_lock(&mpu->lock); + enable_irq(mpu->client->irq); + if (mpu->enabled) + mpu_enable(mpu); + mutex_unlock(&mpu->lock); +} +#endif /* CONFIG_HAS_EARLYSUSPEND */ + +static int __devinit mpu_setup_irq(struct mpu_data *mpu) +{ + struct i2c_client *client = mpu->client; + int gpio = mpu->gpio; + int ret; + + ret = gpio_request(gpio, "gyro"); + if (ret < 0) { + dev_err(&client->dev, "Request gpio %d failed!\n", gpio); + goto out; + } + ret = gpio_direction_input(gpio); + if (ret < 0) { + dev_err(&client->dev, "failed to configure input\n"); + goto fail_gpio; + } + ret = gpio_to_irq(gpio); + if (ret < 0) { + dev_err(&client->dev, "Configure gpio to irq failed!\n"); + goto fail_gpio; + } + client->irq = ret; + ret = request_threaded_irq(client->irq, NULL, mpu_isr, + IRQF_TRIGGER_RISING, "mpuirq", mpu); + if (ret < 0) { + dev_err(&client->dev, "Can't allocate irq %d\n", client->irq); + goto fail_gpio; + } + + return 0; +fail_gpio: + gpio_free(gpio); +out: + return ret; +} + +static int __devinit mpu_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct mpu_data *mpu; + int ret; + + dev_info(&client->dev, "probe start. - new\n"); + + mpu = kzalloc(sizeof(struct mpu_data), GFP_KERNEL); + if (!mpu) { + ret = -ENOMEM; + dev_err(&client->dev, "failed to alloc memory: %d\n", ret); + goto exit_alloc_failed; + } + mutex_init(&mpu->lock); + mpu->client = client; + i2c_set_clientdata(client, mpu); + mpu->poll_interval = DEFAULT_POLL_INTERVAL; + mpu->gpio = client->irq; + + ret = mpu_input_init(mpu); + if (ret < 0) { + dev_err(&client->dev, "input init failed\n"); + goto err_input_init_fail; + } + + ret = mpu_reset(mpu); + if (ret < 0) { + dev_err(&client->dev, "reset failed\n"); + goto err_reset; + } + + ret = mpu_hw_init(mpu); + if (ret < 0) { + dev_err(&client->dev, "hw init failed\n"); + goto err_hw_init; + } + + ret = mpu_setup_irq(mpu); + if (ret < 0) { + dev_err(&client->dev, + "fail to setup irq for gpio %d\n", mpu->gpio); + goto err_setup_irq; + } + +#ifdef CONFIG_HAS_EARLYSUSPEND + mpu->es.level = EARLY_SUSPEND_LEVEL_DISABLE_FB + 10; + mpu->es.suspend = mpu_early_suspend; + mpu->es.resume = mpu_late_resume; + register_early_suspend(&mpu->es); +#endif + mpu->enabled = 0; + mpu_disable(mpu); + + dev_info(&client->dev, "probed\n"); + return 0; + +err_setup_irq: +err_hw_init: + mpu_disable(mpu); +err_reset: + input_unregister_device(mpu->input_dev); + remove_sysfs_interfaces(&client->dev); +err_input_init_fail: + kfree(mpu); +exit_alloc_failed: + dev_err(&client->dev, "Driver Init failed\n"); + return ret; +} + +static int __devexit mpu_remove(struct i2c_client *client) +{ + struct mpu_data *mpu = i2c_get_clientdata(client); + + dev_dbg(&client->dev, "enter %s\n", __func__); + if (client->irq > 0) { + free_irq(client->irq, mpu); + gpio_free(mpu->gpio); + } + + disable_irq(mpu->client->irq); + unregister_early_suspend(&mpu->es); + input_unregister_device(mpu->input_dev); + mpu_disable(mpu); + remove_sysfs_interfaces(&client->dev); + kfree(mpu); + return 0; +} + +#ifdef CONFIG_PM +static int mpu_resume(struct device *dev) +{ + return 0; +} + +static int mpu_suspend(struct device *dev) +{ + return 0; +} +#endif + +static const struct dev_pm_ops mpu_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(mpu_suspend, mpu_resume) +}; + +static const struct i2c_device_id mpu_id[] = { + { MPU_NAME, 0 }, + { } +}; +MODULE_DEVICE_TABLE(i2c, mpu_id); + +static struct i2c_driver mpu_driver = { + .driver = { + .owner = THIS_MODULE, + .name = MPU_NAME, + .pm = &mpu_pm_ops, + }, + .probe = mpu_probe, + .remove = __devexit_p(mpu_remove), + .id_table = mpu_id, +}; + +static int __init mpu_init(void) +{ + pr_info("%s driver: init\n", MPU_NAME); + return i2c_add_driver(&mpu_driver); +} + +static void __exit mpu_exit(void) +{ + pr_info("%s driver exit\n", MPU_NAME); + i2c_del_driver(&mpu_driver); +} + +module_init(mpu_init); +module_exit(mpu_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("User space character device interface for MPU3050"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS(MPU_NAME); -- 2.7.4