--- /dev/null
+/*
+ $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 <http://www.gnu.org/licenses/>.
+ $
+ */
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/earlysuspend.h>
+
+/*==== 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);
projects / kernel / kernel-mfld-blackbay.git / commitdiff