diff --git a/drivers/input/gyroscope/Kconfig b/drivers/input/gyroscope/Kconfig deleted file mode 100644 index becaaef2a273..000000000000 --- a/drivers/input/gyroscope/Kconfig +++ /dev/null @@ -1,31 +0,0 @@ -# SPDX-License-Identifier: GPL-2.0 -# -# gyroscope drivers configuration -# - -menuconfig GYRO_SENSOR_DEVICE - bool "gyroscope device support" - default n - help - Enable this to be able to choose the drivers for controlling the - gyroscope sensor on some platforms, for example on PDAs. - -if GYRO_SENSOR_DEVICE - -config GYRO_SENSOR_K3G - bool "gyroscope k3g" - depends on GYRO_SENSOR_DEVICE - default n - help - To have support for your specific gyroscope sesnor you will have to - select the proper drivers which depend on this option. - -config GYRO_L3G4200D - bool "gyroscope l3g4200d" - depends on GYRO_SENSOR_DEVICE - default n - help - To have support for your specific gyroscope sesnor you will have to - select the proper drivers which depend on this option. - -endif diff --git a/drivers/input/gyroscope/Makefile b/drivers/input/gyroscope/Makefile deleted file mode 100644 index 430caef915d7..000000000000 --- a/drivers/input/gyroscope/Makefile +++ /dev/null @@ -1,7 +0,0 @@ -# SPDX-License-Identifier: GPL-2.0 -# gyroscope drivers - -obj-$(CONFIG_GYRO_SENSOR_K3G) += k3g.o -obj-$(CONFIG_GYRO_L3G4200D) += l3g4200d.o - - diff --git a/drivers/input/gyroscope/k3g.c b/drivers/input/gyroscope/k3g.c deleted file mode 100755 index 1b4cd979c012..000000000000 --- a/drivers/input/gyroscope/k3g.c +++ /dev/null @@ -1,703 +0,0 @@ -/* - * Copyright (C) 2010, Samsung Electronics Co. Ltd. All Rights Reserved. - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -//#include -#include - -/* k3g chip id */ -#define DEVICE_ID 0xD3 -/* k3g gyroscope registers */ -#define WHO_AM_I 0x0F -#define CTRL_REG1 0x20 /* power control reg */ -#define CTRL_REG2 0x21 /* power control reg */ -#define CTRL_REG3 0x22 /* power control reg */ -#define CTRL_REG4 0x23 /* interrupt control reg */ -#define CTRL_REG5 0x24 /* interrupt control reg */ -#define OUT_TEMP 0x26 /* Temperature data */ -#define STATUS_REG 0x27 -#define AXISDATA_REG 0x28 -#define OUT_Y_L 0x2A -#define FIFO_CTRL_REG 0x2E -#define FIFO_SRC_REG 0x2F -#define PM_OFF 0x00 -#define PM_NORMAL 0x08 -#define ENABLE_ALL_AXES 0x07 -#define BYPASS_MODE 0x00 -#define FIFO_MODE 0x20 - -#define FIFO_EMPTY 0x20 -#define FSS_MASK 0x1F -#define ODR_MASK 0xF0 -#define ODR105_BW12_5 0x00 /* ODR = 105Hz; BW = 12.5Hz */ -#define ODR105_BW25 0x10 /* ODR = 105Hz; BW = 25Hz */ -#define ODR210_BW12_5 0x40 /* ODR = 210Hz; BW = 12.5Hz */ -#define ODR210_BW25 0x50 /* ODR = 210Hz; BW = 25Hz */ -#define ODR210_BW50 0x60 /* ODR = 210Hz; BW = 50Hz */ -#define ODR210_BW70 0x70 /* ODR = 210Hz; BW = 70Hz */ -#define ODR420_BW20 0x80 /* ODR = 420Hz; BW = 20Hz */ -#define ODR420_BW25 0x90 /* ODR = 420Hz; BW = 25Hz */ -#define ODR420_BW50 0xA0 /* ODR = 420Hz; BW = 50Hz */ -#define ODR420_BW110 0xB0 /* ODR = 420Hz; BW = 110Hz */ -#define ODR840_BW30 0xC0 /* ODR = 840Hz; BW = 30Hz */ -#define ODR840_BW35 0xD0 /* ODR = 840Hz; BW = 35Hz */ -#define ODR840_BW50 0xE0 /* ODR = 840Hz; BW = 50Hz */ -#define ODR840_BW110 0xF0 /* ODR = 840Hz; BW = 110Hz */ - -#define MIN_ST 175 -#define MAX_ST 875 -#define AC (1 << 7) /* register auto-increment bit */ -#define MAX_ENTRY 1 -#define MAX_DELAY (MAX_ENTRY * 9523809LL) - -/* default register setting for device init */ -static const char default_ctrl_regs[] = { - 0x3F, /* 105HZ, PM-normal, xyz enable */ - 0x00, /* normal mode */ - 0x04, /* fifo wtm interrupt on */ - 0xA0, /* block data update, 2000d/s */ - 0x40, /* fifo enable */ -}; - -static const struct odr_delay { - u8 odr; /* odr reg setting */ - u32 delay_ns; /* odr in ns */ -} odr_delay_table[] = { - { ODR840_BW110, 1190476LL }, /* 840Hz */ - { ODR420_BW110, 2380952LL }, /* 420Hz */ - { ODR210_BW70, 4761904LL }, /* 210Hz */ - { ODR105_BW25, 9523809LL }, /* 105Hz */ -}; - -/* - * K3G gyroscope data - * brief structure containing gyroscope values for yaw, pitch and roll in - * signed short - */ -struct k3g_t { - s16 x; - s16 y; - s16 z; -}; - -struct k3g_data { - struct i2c_client *client; - struct input_dev *input_dev; - struct mutex lock; - struct workqueue_struct *k3g_wq; - struct work_struct work; - struct hrtimer timer; - bool enable; - bool drop_next_event; - bool interruptible; /* interrupt or polling? */ - int entries; /* number of fifo entries */ - u8 ctrl_regs[5]; /* saving register settings */ - u32 time_to_read; /* time needed to read one entry */ - ktime_t polling_delay; /* polling time for timer */ -}; - -static int k3g_read_fifo_status(struct k3g_data *k3g_data) -{ - int fifo_status; - - fifo_status = i2c_smbus_read_byte_data(k3g_data->client, FIFO_SRC_REG); - if (fifo_status < 0) { - pr_err("%s: failed to read fifo source register\n", - __func__); - return fifo_status; - } - return (fifo_status & FSS_MASK) + !(fifo_status & FIFO_EMPTY); -} - -static int k3g_restart_fifo(struct k3g_data *k3g_data) -{ - int res = 0; - - res = i2c_smbus_write_byte_data(k3g_data->client, - FIFO_CTRL_REG, BYPASS_MODE); - if (res < 0) { - pr_err("%s : failed to set bypass_mode\n", __func__); - return res; - } - - res = i2c_smbus_write_byte_data(k3g_data->client, - FIFO_CTRL_REG, FIFO_MODE | (k3g_data->entries - 1)); - - if (res < 0) - pr_err("%s : failed to set fifo_mode\n", __func__); - - return res; -} - -static void set_polling_delay(struct k3g_data *k3g_data, int res) -{ - s64 delay_ns; - - delay_ns = k3g_data->entries + 1 - res; - if (delay_ns < 0) - delay_ns = 0; - - delay_ns = delay_ns * k3g_data->time_to_read; - k3g_data->polling_delay = ns_to_ktime(delay_ns); -} - -/* gyroscope data readout */ -static int k3g_read_gyro_values(struct i2c_client *client, - struct k3g_t *data, int total_read) -{ - int err; - struct i2c_msg msg[2]; - u8 reg_buf; - u8 gyro_data[sizeof(*data) * (total_read ? (total_read - 1) : 1)]; - - msg[0].addr = client->addr; - msg[0].buf = ®_buf; - msg[0].flags = 0; - msg[0].len = 1; - - msg[1].addr = client->addr; - msg[1].flags = I2C_M_RD; - msg[1].buf = gyro_data; - - if (total_read > 1) { - reg_buf = AXISDATA_REG | AC; - msg[1].len = sizeof(gyro_data); - - err = i2c_transfer(client->adapter, msg, 2); - if (err != 2) - return (err < 0) ? err : -EIO; - } - - reg_buf = AXISDATA_REG; - msg[1].len = 1; - err = i2c_transfer(client->adapter, msg, 2); - if (err != 2) - return (err < 0) ? err : -EIO; - - reg_buf = OUT_Y_L | AC; - msg[1].len = sizeof(*data); - err = i2c_transfer(client->adapter, msg, 2); - if (err != 2) - return (err < 0) ? err : -EIO; - - data->y = (gyro_data[1] << 8) | gyro_data[0]; - data->z = (gyro_data[3] << 8) | gyro_data[2]; - data->x = (gyro_data[5] << 8) | gyro_data[4]; - - return 0; -} - -static int k3g_report_gyro_values(struct k3g_data *k3g_data) -{ - int res; - struct k3g_t data; - - res = k3g_read_gyro_values(k3g_data->client, &data, - k3g_data->entries + k3g_data->drop_next_event); - if (res < 0) - return res; - - res = k3g_read_fifo_status(k3g_data); - - k3g_data->drop_next_event = !res; - - if (res >= 31 - k3g_data->entries) { - /* reset fifo to start again - data isn't trustworthy, - * our locked read might not have worked and we - * could have done i2c read in mid register update - */ - return k3g_restart_fifo(k3g_data); - } - - input_report_rel(k3g_data->input_dev, REL_RX, data.x); - input_report_rel(k3g_data->input_dev, REL_RY, data.y); - input_report_rel(k3g_data->input_dev, REL_RZ, data.z); - input_sync(k3g_data->input_dev); - - return res; -} - -static enum hrtimer_restart k3g_timer_func(struct hrtimer *timer) -{ - struct k3g_data *k3g_data = container_of(timer, struct k3g_data, timer); - queue_work(k3g_data->k3g_wq, &k3g_data->work); - return HRTIMER_NORESTART; -} - -static void k3g_work_func(struct work_struct *work) -{ - int res; - struct k3g_data *k3g_data = container_of(work, struct k3g_data, work); - - do { - res = k3g_read_fifo_status(k3g_data); - if (res < 0) - return; - - if (res < k3g_data->entries) { - pr_warning("%s: fifo entries are less than we want\n", - __func__); - goto timer_set; - } - - res = k3g_report_gyro_values(k3g_data); - if (res < 0) - return; -timer_set: - set_polling_delay(k3g_data, res); - - } while (!ktime_to_ns(k3g_data->polling_delay)); - - hrtimer_start(&k3g_data->timer, - k3g_data->polling_delay, HRTIMER_MODE_REL); -} - -static irqreturn_t k3g_interrupt_thread(int irq, void *k3g_data_p) -{ - int res; - struct k3g_data *k3g_data = k3g_data_p; - res = k3g_report_gyro_values(k3g_data); - if (res < 0) - pr_err("%s: failed to report gyro values\n", __func__); - - return IRQ_HANDLED; -} - -static ssize_t k3g_show_enable(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct k3g_data *k3g_data = dev_get_drvdata(dev); - return sprintf(buf, "%d\n", k3g_data->enable); -} - -static ssize_t k3g_set_enable(struct device *dev, - struct device_attribute *attr, const char *buf, size_t size) -{ - int err = 0; - struct k3g_data *k3g_data = dev_get_drvdata(dev); - bool new_enable; - - if (sysfs_streq(buf, "1")) - new_enable = true; - else if (sysfs_streq(buf, "0")) - new_enable = false; - else { - pr_debug("%s: invalid value %d\n", __func__, *buf); - return -EINVAL; - } - - if (new_enable == k3g_data->enable) - return size; - - mutex_lock(&k3g_data->lock); - if (new_enable) { - /* turning on */ - err = i2c_smbus_write_i2c_block_data(k3g_data->client, - CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs), - k3g_data->ctrl_regs); - if (err < 0) { - err = -EIO; - goto unlock; - } - - /* reset fifo entries */ - err = k3g_restart_fifo(k3g_data); - if (err < 0) { - err = -EIO; - goto turn_off; - } - - if (k3g_data->interruptible) - enable_irq(k3g_data->client->irq); - else { - set_polling_delay(k3g_data, 0); - hrtimer_start(&k3g_data->timer, - k3g_data->polling_delay, HRTIMER_MODE_REL); - } - } else { - if (k3g_data->interruptible) - disable_irq(k3g_data->client->irq); - else { - hrtimer_cancel(&k3g_data->timer); - cancel_work_sync(&k3g_data->work); - } - /* turning off */ - err = i2c_smbus_write_byte_data(k3g_data->client, - CTRL_REG1, 0x00); - if (err < 0) - goto unlock; - } - k3g_data->enable = new_enable; - -turn_off: - if (err < 0) - i2c_smbus_write_byte_data(k3g_data->client, - CTRL_REG1, 0x00); -unlock: - mutex_unlock(&k3g_data->lock); - - return err ? err : size; -} - -static ssize_t k3g_show_delay(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct k3g_data *k3g_data = dev_get_drvdata(dev); - u64 delay; - - delay = k3g_data->time_to_read * k3g_data->entries; - delay = ktime_to_ns(ns_to_ktime(delay)); - - return sprintf(buf, "%lld\n", delay); -} - -static ssize_t k3g_set_delay(struct device *dev, - struct device_attribute *attr, const char *buf, size_t size) -{ - struct k3g_data *k3g_data = dev_get_drvdata(dev); - int odr_value = ODR105_BW25; - int res = 0; - int i; - u64 delay_ns; - u8 ctrl; - - res = strict_strtoll(buf, 10, &delay_ns); - if (res < 0) - return res; - - mutex_lock(&k3g_data->lock); - if (!k3g_data->interruptible) - hrtimer_cancel(&k3g_data->timer); - else - disable_irq(k3g_data->client->irq); - - /* round to the nearest supported ODR that is less than - * the requested value - */ - for (i = 0; i < ARRAY_SIZE(odr_delay_table); i++) - if (delay_ns <= odr_delay_table[i].delay_ns) { - odr_value = odr_delay_table[i].odr; - delay_ns = odr_delay_table[i].delay_ns; - k3g_data->time_to_read = delay_ns; - k3g_data->entries = 1; - break; - } - - if (delay_ns >= odr_delay_table[3].delay_ns) { - if (delay_ns >= MAX_DELAY) { - k3g_data->entries = MAX_ENTRY; - delay_ns = MAX_DELAY; - } else { - do_div(delay_ns, odr_delay_table[3].delay_ns); - k3g_data->entries = delay_ns; - } - k3g_data->time_to_read = odr_delay_table[3].delay_ns; - } - - if (odr_value != (k3g_data->ctrl_regs[0] & ODR_MASK)) { - ctrl = (k3g_data->ctrl_regs[0] & ~ODR_MASK); - ctrl |= odr_value; - k3g_data->ctrl_regs[0] = ctrl; - res = i2c_smbus_write_byte_data(k3g_data->client, - CTRL_REG1, ctrl); - } - - /* we see a noise in the first sample or two after we - * change rates. this delay helps eliminate that noise. - */ - msleep((u32)delay_ns * 2 / NSEC_PER_MSEC); - - /* (re)start fifo */ - k3g_restart_fifo(k3g_data); - - if (!k3g_data->interruptible) { - delay_ns = k3g_data->entries * k3g_data->time_to_read; - k3g_data->polling_delay = ns_to_ktime(delay_ns); - if (k3g_data->enable) - hrtimer_start(&k3g_data->timer, - k3g_data->polling_delay, HRTIMER_MODE_REL); - } else - enable_irq(k3g_data->client->irq); - - mutex_unlock(&k3g_data->lock); - - return size; -} - -static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, - k3g_show_enable, k3g_set_enable); -static DEVICE_ATTR(poll_delay, S_IRUGO | S_IWUSR | S_IWGRP, - k3g_show_delay, k3g_set_delay); - -static int k3g_probe(struct i2c_client *client, - const struct i2c_device_id *devid) -{ - int ret; - int err = 0; - struct k3g_data *data; - struct input_dev *input_dev; - - if (client->dev.platform_data == NULL) { - dev_err(&client->dev, "platform data is NULL. exiting.\n"); - err = -ENODEV; - goto exit; - } - - data = kzalloc(sizeof(*data), GFP_KERNEL); - if (data == NULL) { - dev_err(&client->dev, - "failed to allocate memory for module data\n"); - err = -ENOMEM; - goto exit; - } - - data->client = client; - - /* read chip id */ - ret = i2c_smbus_read_byte_data(client, WHO_AM_I); - if (ret != DEVICE_ID) { - if (ret < 0) { - pr_err("%s: i2c for reading chip id failed\n", - __func__); - err = ret; - } else { - pr_err("%s : Device identification failed\n", - __func__); - err = -ENODEV; - } - goto err_read_reg; - } - - mutex_init(&data->lock); - - /* allocate gyro input_device */ - input_dev = input_allocate_device(); - if (!input_dev) { - pr_err("%s: could not allocate input device\n", __func__); - err = -ENOMEM; - goto err_input_allocate_device; - } - - data->input_dev = input_dev; - input_set_drvdata(input_dev, data); - input_dev->name = "gyro"; - /* X */ - input_set_capability(input_dev, EV_REL, REL_RX); - input_set_abs_params(input_dev, REL_RX, -2048, 2047, 0, 0); - /* Y */ - input_set_capability(input_dev, EV_REL, REL_RY); - input_set_abs_params(input_dev, REL_RY, -2048, 2047, 0, 0); - /* Z */ - input_set_capability(input_dev, EV_REL, REL_RZ); - input_set_abs_params(input_dev, REL_RZ, -2048, 2047, 0, 0); - - err = input_register_device(input_dev); - if (err < 0) { - pr_err("%s: could not register input device\n", __func__); - input_free_device(data->input_dev); - goto err_input_register_device; - } - - memcpy(&data->ctrl_regs, &default_ctrl_regs, sizeof(default_ctrl_regs)); - if (data->client->irq >= 0) { /* interrupt */ - data->interruptible = true; - err = request_threaded_irq(data->client->irq, NULL, - k3g_interrupt_thread, IRQF_TRIGGER_HIGH | IRQF_ONESHOT, - "k3g", data); - if (err < 0) { - pr_err("%s: can't allocate irq.\n", __func__); - goto err_request_irq; - } - disable_irq(data->client->irq); - - } else { /* polling */ - u64 delay_ns; - data->ctrl_regs[2] = 0x00; /* disable interrupt */ - /* hrtimer settings. we poll for gyro values using a timer. */ - hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); - data->polling_delay = ns_to_ktime(200 * NSEC_PER_MSEC); - data->time_to_read = 10000000LL; - delay_ns = ktime_to_ns(data->polling_delay); - do_div(delay_ns, data->time_to_read); - data->entries = delay_ns; - data->timer.function = k3g_timer_func; - - /* the timer just fires off a work queue request. - We need a thread to read i2c (can be slow and blocking). */ - data->k3g_wq = create_singlethread_workqueue("k3g_wq"); - if (!data->k3g_wq) { - err = -ENOMEM; - pr_err("%s: could not create workqueue\n", __func__); - goto err_create_workqueue; - } - /* this is the thread function we run on the work queue */ - INIT_WORK(&data->work, k3g_work_func); - } - - if (device_create_file(&input_dev->dev, - &dev_attr_enable) < 0) { - pr_err("Failed to create device file(%s)!\n", - dev_attr_enable.attr.name); - goto err_device_create_file; - } - - if (device_create_file(&input_dev->dev, - &dev_attr_poll_delay) < 0) { - pr_err("Failed to create device file(%s)!\n", - dev_attr_poll_delay.attr.name); - goto err_device_create_file2; - } - - i2c_set_clientdata(client, data); - dev_set_drvdata(&input_dev->dev, data); - - return 0; - -err_device_create_file2: - device_remove_file(&input_dev->dev, &dev_attr_enable); -err_device_create_file: - if (data->interruptible) { - enable_irq(data->client->irq); - free_irq(data->client->irq, data); - } else - destroy_workqueue(data->k3g_wq); - input_unregister_device(data->input_dev); -err_create_workqueue: -err_request_irq: -err_input_register_device: -err_input_allocate_device: - mutex_destroy(&data->lock); -err_read_reg: - kfree(data); -exit: - return err; -} - -static int k3g_remove(struct i2c_client *client) -{ - int err = 0; - struct k3g_data *k3g_data = i2c_get_clientdata(client); - - device_remove_file(&k3g_data->input_dev->dev, &dev_attr_enable); - device_remove_file(&k3g_data->input_dev->dev, &dev_attr_poll_delay); - - if (k3g_data->enable) - err = i2c_smbus_write_byte_data(k3g_data->client, - CTRL_REG1, 0x00); - if (k3g_data->interruptible) { - if (!k3g_data->enable) /* no disable_irq before free_irq */ - enable_irq(k3g_data->client->irq); - free_irq(k3g_data->client->irq, k3g_data); - - } else { - hrtimer_cancel(&k3g_data->timer); - cancel_work_sync(&k3g_data->work); - destroy_workqueue(k3g_data->k3g_wq); - } - - input_unregister_device(k3g_data->input_dev); - mutex_destroy(&k3g_data->lock); - kfree(k3g_data); - - return err; -} - -static int k3g_suspend(struct device *dev) -{ - int err = 0; - struct i2c_client *client = to_i2c_client(dev); - struct k3g_data *k3g_data = i2c_get_clientdata(client); - - if (k3g_data->enable) { - mutex_lock(&k3g_data->lock); - if (!k3g_data->interruptible) { - hrtimer_cancel(&k3g_data->timer); - cancel_work_sync(&k3g_data->work); - } - err = i2c_smbus_write_byte_data(k3g_data->client, - CTRL_REG1, 0x00); - mutex_unlock(&k3g_data->lock); - } - - return err; -} - -static int k3g_resume(struct device *dev) -{ - int err = 0; - struct i2c_client *client = to_i2c_client(dev); - struct k3g_data *k3g_data = i2c_get_clientdata(client); - - if (k3g_data->enable) { - mutex_lock(&k3g_data->lock); - if (!k3g_data->interruptible) - hrtimer_start(&k3g_data->timer, - k3g_data->polling_delay, HRTIMER_MODE_REL); - err = i2c_smbus_write_i2c_block_data(client, - CTRL_REG1 | AC, sizeof(k3g_data->ctrl_regs), - k3g_data->ctrl_regs); - mutex_unlock(&k3g_data->lock); - } - - return err; -} - -static const struct dev_pm_ops k3g_pm_ops = { - .suspend = k3g_suspend, - .resume = k3g_resume -}; - -static const struct i2c_device_id k3g_id[] = { - { "k3g", 0 }, - { } -}; - -MODULE_DEVICE_TABLE(i2c, k3g_id); - -static struct i2c_driver k3g_driver = { - .probe = k3g_probe, - .remove = __devexit_p(k3g_remove), - .id_table = k3g_id, - .driver = { - .pm = &k3g_pm_ops, - .owner = THIS_MODULE, - .name = "k3g" - }, -}; - -static int __init k3g_init(void) -{ - return i2c_add_driver(&k3g_driver); -} - -static void __exit k3g_exit(void) -{ - i2c_del_driver(&k3g_driver); -} - -module_init(k3g_init); -module_exit(k3g_exit); - -MODULE_DESCRIPTION("k3g digital gyroscope driver"); -MODULE_AUTHOR("Tim SK Lee Samsung Electronics "); -MODULE_LICENSE("GPL"); diff --git a/drivers/input/gyroscope/l3g4200d.c b/drivers/input/gyroscope/l3g4200d.c deleted file mode 100755 index 5d057f5b0b01..000000000000 --- a/drivers/input/gyroscope/l3g4200d.c +++ /dev/null @@ -1,906 +0,0 @@ -/* drivers/i2c/chips/l3g4200d.c - l3g4200d compass driver - * - * Copyright (C) 2007-2008 HTC Corporation. - * Author: Hou-Kun Chen - * - * This software is licensed under the terms of the GNU General Public - * License version 2, as published by the Free Software Foundation, and - * may be copied, distributed, and modified under those terms. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef CONFIG_HAS_EARLYSUSPEND -#include -#endif - -#if 0 -#define DBG(x...) printk(x) -#else -#define DBG(x...) -#endif -static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id *id); - -#define L3G4200D_SPEED 200 * 1000 -#define L3G4200D_DEVID 0xD3 - -#define L3G4200D_MAJOR 102 -#define L3G4200D_MINOR 4 - -/* l3g4200d gyroscope registers */ -#define WHO_AM_I 0x0F - -#define CTRL_REG1 0x20 /* power control reg */ -#define CTRL_REG2 0x21 /* power control reg */ -#define CTRL_REG3 0x22 /* power control reg */ -#define CTRL_REG4 0x23 /* interrupt control reg */ -#define CTRL_REG5 0x24 /* interrupt control reg */ -#define AXISDATA_REG 0x28 - - -/* Addresses to scan -- protected by sense_data_mutex */ -//static char sense_data[RBUFF_SIZE + 1]; -static struct i2c_client *this_client; -static struct l3g4200d_data *this_data; -static struct miscdevice l3g4200d_device; - -static DECLARE_WAIT_QUEUE_HEAD(data_ready_wq); - -#ifdef CONFIG_HAS_EARLYSUSPEND -static struct early_suspend l3g4200d_early_suspend; -#endif -static int revision = -1; -/* AKM HW info */ -static ssize_t gsensor_vendor_show(struct device *dev, - struct device_attribute *attr, char *buf) -{ - ssize_t ret = 0; - - sprintf(buf, "%#x\n", revision); - ret = strlen(buf) + 1; - - return ret; -} - -static DEVICE_ATTR(vendor, 0444, gsensor_vendor_show, NULL); - -static struct kobject *android_gsensor_kobj; - -static int gsensor_sysfs_init(void) -{ - int ret ; - - android_gsensor_kobj = kobject_create_and_add("android_gyrosensor", NULL); - if (android_gsensor_kobj == NULL) { - printk(KERN_ERR - "L3G4200D gsensor_sysfs_init:"\ - "subsystem_register failed\n"); - ret = -ENOMEM; - goto err; - } - - ret = sysfs_create_file(android_gsensor_kobj, &dev_attr_vendor.attr); - if (ret) { - printk(KERN_ERR - "L3G4200D gsensor_sysfs_init:"\ - "sysfs_create_group failed\n"); - goto err4; - } - - return 0 ; -err4: - kobject_del(android_gsensor_kobj); -err: - return ret ; -} - -#if 0 -static int l3g4200d_rx_data(struct i2c_client *client, char *rxData, int length) -{ - int ret = 0; - char reg = rxData[0]; - ret = i2c_master_reg8_recv(client, reg, rxData, length, L3G4200D_SPEED); - return (ret > 0)? 0 : ret; -} -#else -static int l3g4200d_rx_data(struct i2c_client *client, char *rxData, int length) -{ - int ret = 0; - char reg = rxData[0]; - int i = 0; - for(i=0; i<3; i++) - { - ret = i2c_master_reg8_recv(client, reg, rxData, length, L3G4200D_SPEED); - if(ret < 0) - mdelay(1); - else - break; - } - return (ret > 0)? 0 : ret; -} -#endif -#if 0 -static int l3g4200d_tx_data(struct i2c_client *client, char *txData, int length) -{ - int ret = 0; - char reg = txData[0]; - ret = i2c_master_reg8_send(client, reg, &txData[1], length-1, L3G4200D_SPEED); - return (ret > 0)? 0 : ret; -} -#else -static int l3g4200d_tx_data(struct i2c_client *client, char *txData, int length) -{ - int ret = 0; - char reg = txData[0]; - int i = 0; - for(i=0; i<3; i++) - { - ret = i2c_master_reg8_send(client, reg, &txData[1], length-1, L3G4200D_SPEED); - if(ret < 0) - mdelay(1); - else - break; - } - return (ret > 0)? 0 : ret; -} -#endif - -#if 0 -static int gyro_rx_data(struct i2c_client *client, char *rxData, int length) -{ - int ret = 0; - int i = 0; - char reg = rxData[0]; - for(i=0; i<3; i++) - { - ret = i2c_master_reg8_recv(client, reg, rxData, length, L3G4200D_SPEED); - if(ret < 0) - mdelay(1); - else - break; - } - return (ret > 0)? 0 : ret; -} -#endif - -static int gyro_tx_data(struct i2c_client *client, char *txData, int length) -{ - int ret = 0; - char reg = txData[0]; - int i = 0; - - for(i=0; i<3; i++) - { - ret = i2c_master_reg8_send(client, reg, &txData[1], length-1, L3G4200D_SPEED); - if(ret < 0) - mdelay(1); - else - break; - } - return (ret > 0)? 0 : ret; -} - -#if 0 -/* i2c read routine for l3g4200d digital gyroscope */ -static char l3g4200d_i2c_read(unsigned char reg_addr, - unsigned char *data, - unsigned char len) -{ - char tmp; - int ret = 0; - if (this_client == NULL) /* No global client pointer? */ - return -1; - - data[0]=reg_addr; - ret = gyro_rx_data(this_client, data, len); - return tmp; -} -#endif -/* i2c write routine for l3g4200d digital gyroscope */ -static char l3g4200d_i2c_write(unsigned char reg_addr, - unsigned char *data, - unsigned char len) -{ - - char buffer[3]; - int ret = 0; - int i = 0; - if (this_client == NULL) /* No global client pointer? */ - return -1; - for (i = 0; i < len; i++) - { - buffer[0] = reg_addr+i; - buffer[1] = data[i]; - ret = gyro_tx_data(this_client, &buffer[0], 2); - } - return ret; - -} - -static char l3g4200d_read_reg(struct i2c_client *client,int addr) -{ - char tmp; - int ret = 0; - - tmp = addr; -// ret = l3g4200d_tx_data(client, &tmp, 1); - ret = l3g4200d_rx_data(client, &tmp, 1); - return tmp; -} - -static int l3g4200d_write_reg(struct i2c_client *client,int addr,int value) -{ - char buffer[3]; - int ret = 0; - - buffer[0] = addr; - buffer[1] = value; - ret = l3g4200d_tx_data(client, &buffer[0], 2); - return ret; -} - - -static char l3g4200d_get_devid(struct i2c_client *client) -{ - unsigned int devid = 0; - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - - devid = l3g4200d_read_reg(client, WHO_AM_I)&0xff; - if (devid == GYRO_DEVID_L3G4200D) { - l3g4200d->devid = devid; - printk(KERN_INFO "gyro is L3G4200D and devid=0x%x\n",devid); - return 0; - } else if (devid == GYRO_DEVID_L3G20D) - { - l3g4200d->devid = devid; - printk(KERN_INFO "gyro is L3G20D and devid=0x%x\n",devid); - return 0; - } - else - { - printk(KERN_ERR "%s:gyro device id is error,devid=%d\n",__func__,devid); - return -1; - } -} - -static int l3g4200d_active(struct i2c_client *client,int enable) -{ - int tmp; - int ret = 0; - - tmp = l3g4200d_read_reg(client,CTRL_REG1); - if(enable) - tmp |=ACTIVE_MASK; - else - tmp &=~ACTIVE_MASK; - DBG("l3g4200d_active %s (0x%x)\n",enable?"active":"standby",tmp); - ret = l3g4200d_write_reg(client,CTRL_REG1,tmp); - return ret; -} - - - -static int device_init(void) -{ - int res; - unsigned char buf[5]; - buf[0] = 0x07; //27 - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x20; //0x00 - buf[4] = 0x00; - res = l3g4200d_i2c_write(CTRL_REG1, &buf[0], 5); - return res; -} - -int l3g4200d_set_bandwidth(char bw) -{ - int res = 0; - unsigned char data; - - res = l3g4200d_read_reg(this_client, CTRL_REG1); - if (res >= 0) - data = res & 0x000F; - - data = data + bw; - res = l3g4200d_i2c_write(CTRL_REG1, &data, 1); - return res; -} - -static int l3g4200d_start_dev(struct i2c_client *client, char rate) -{ - int ret = 0; - //int tmp; - //struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - - /* standby */ - l3g4200d_active(client,0); - device_init(); - l3g4200d_set_bandwidth(rate); - l3g4200d_active(client,1); - - enable_irq(client->irq); - return ret; - -} - -static int l3g4200d_start(struct i2c_client *client, char rate) -{ - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - - DBG("%s::enter\n",__FUNCTION__); - if (l3g4200d->status == L3G4200D_OPEN) { - return 0; - } - l3g4200d->status = L3G4200D_OPEN; - return l3g4200d_start_dev(client, rate); -} - -static int l3g4200d_close_dev(struct i2c_client *client) -{ - DBG("%s :enter\n",__FUNCTION__); - disable_irq_nosync(client->irq); - return l3g4200d_active(client,0); -} - -static int l3g4200d_close(struct i2c_client *client) -{ - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - DBG("%s::enter\n",__FUNCTION__); - l3g4200d->status = L3G4200D_CLOSE; - - return l3g4200d_close_dev(client); -} - -static int l3g4200d_reset_rate(struct i2c_client *client, char rate) -{ - int ret = 0; - - DBG("%s\n",__func__); - - // ret = l3g4200d_close_dev(client); - ret = l3g4200d_start_dev(client, rate); - - return ret ; -} - -static inline int l3g4200d_convert_to_int(char value) -{ - int result; - - if (value < 1) { - result = value * 1; - } else { - result = ~(((~value & 0x7f) + 1)* 1) + 1; - } - - return result; -} - -static void l3g4200d_report_value(struct i2c_client *client, struct l3g4200d_axis *axis) -{ - struct l3g4200d_data *l3g4200d = i2c_get_clientdata(client); - //struct l3g4200d_axis *axis = (struct l3g4200d_axis *)rbuf; - - /* Report acceleration sensor information */ - input_report_rel(l3g4200d->input_dev, ABS_RX, axis->x); - input_report_rel(l3g4200d->input_dev, ABS_RY, axis->y); - input_report_rel(l3g4200d->input_dev, ABS_RZ, axis->z); - input_sync(l3g4200d->input_dev); - DBG("%s:x==%d y==%d z==%d\n",__func__,axis->x,axis->y,axis->z); -} - - -static int l3g4200d_get_data(struct i2c_client *client) -{ - //char buffer[6]; - int ret,i; - struct l3g4200d_axis axis; - struct l3g4200d_platform_data *pdata = client->dev.platform_data; - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - //int res; - unsigned char gyro_data[6]; - /* x,y,z hardware data */ - int x = 0, y = 0, z = 0; - - for(i=0;i<6;i++) - { - gyro_data[i] = AXISDATA_REG+i; - //ret = l3g4200d_tx_data(client, &buffer[0], 1); - ret = l3g4200d_rx_data(client, &gyro_data[i], 1); - } - - x = (short) (((gyro_data[1]) << 8) | gyro_data[0]); - y = (short) (((gyro_data[3]) << 8) | gyro_data[2]); - z = (short) (((gyro_data[5]) << 8) | gyro_data[4]); - - DBG("%s: x=%d y=%d z=%d \n",__func__, x,y,z); - if(pdata && pdata->orientation) - { - axis.x = (pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z; - axis.y = (pdata->orientation[3])*x + (pdata->orientation[4])*y + (pdata->orientation[5])*z; - axis.z = (pdata->orientation[6])*x + (pdata->orientation[7])*y + (pdata->orientation[8])*z; - } - else - { - axis.x = x; - axis.y = y; - axis.z = z; - } - - //filter gyro data - if((abs(l3g4200d->axis.x - axis.x) > pdata->x_min)||(abs(l3g4200d->axis.y - axis.y) > pdata->y_min)||(abs(l3g4200d->axis.z - axis.z) > pdata->z_min)) - { - l3g4200d->axis.x = axis.x; - l3g4200d->axis.y = axis.y; - l3g4200d->axis.z = axis.z; - if(abs(l3g4200d->axis.x) <= pdata->x_min) l3g4200d->axis.x = 0; - if(abs(l3g4200d->axis.y) <= pdata->y_min) l3g4200d->axis.y = 0; - if(abs(l3g4200d->axis.z) <= pdata->z_min) l3g4200d->axis.z = 0; - - l3g4200d_report_value(client, &l3g4200d->axis); - } - - return 0; -} - -/* -static int l3g4200d_trans_buff(char *rbuf, int size) -{ - //wait_event_interruptible_timeout(data_ready_wq, - // atomic_read(&data_ready), 1000); - wait_event_interruptible(data_ready_wq, - atomic_read(&data_ready)); - - atomic_set(&data_ready, 0); - memcpy(rbuf, &sense_data[0], size); - - return 0; -} -*/ - -static int l3g4200d_open(struct inode *inode, struct file *file) -{ - DBG("%s :enter\n",__FUNCTION__); - return 0;//nonseekable_open(inode, file); -} - -static int l3g4200d_release(struct inode *inode, struct file *file) -{ - DBG("%s :enter\n",__FUNCTION__); - return 0; -} -#define RBUFF_SIZE 12 /* Rx buffer size */ - -static long l3g4200d_ioctl( struct file *file, unsigned int cmd,unsigned long arg) -{ - - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(this_client); - void __user *argp = (void __user *)arg; - //char msg[RBUFF_SIZE + 1]; - int ret = -1; - char rate; - struct i2c_client *client = container_of(l3g4200d_device.parent, struct i2c_client, dev); - - switch (cmd) { - case L3G4200D_IOCTL_GET_ENABLE: - DBG("%s :L3G4200D_IOCTL_GET_ENABLE\n",__FUNCTION__); - ret=!l3g4200d->status; - if (copy_to_user(argp, &ret, sizeof(ret))) - { - printk("%s:failed to copy status to user space.\n",__FUNCTION__); - return -EFAULT; - } - break; - case L3G4200D_IOCTL_SET_ENABLE: - DBG("%s :L3G4200D_IOCTL_SET_ENABLE,flag=%d\n",__FUNCTION__,*(unsigned int *)argp); - if(*(unsigned int *)argp) - { - ret = l3g4200d_start(client, ODR100_BW12_5); - if (ret < 0) - return ret; - } - else - { - ret = l3g4200d_close(client); - if (ret < 0) - return ret; - } - ret=l3g4200d->status; - if (copy_to_user(argp, &ret, sizeof(ret))) - { - printk("%s:failed to copy sense data to user space.\n",__FUNCTION__); - return -EFAULT; - } - break; - case L3G4200D_IOCTL_SET_DELAY: - DBG("%s :L3G4200D_IOCTL_SET_DELAY,rate=%d\n",__FUNCTION__,rate); - if (copy_from_user(&rate, argp, sizeof(rate))) - return -EFAULT; - ret = l3g4200d_reset_rate(client, 0x00);//rate<<4);//0x20 - if (ret < 0) - return ret; - break; -/* - case ECS_IOCTL_GETDATA: - ret = l3g4200d_trans_buff(msg, RBUFF_SIZE); - if (ret < 0) - return ret; - if (copy_to_user(argp, &msg, sizeof(msg))) - return -EFAULT; - break; -*/ - default: - printk("%s:error,cmd=0x%x\n",__func__,cmd); - return -ENOTTY; - } - - DBG("%s:line=%d,cmd=0x%x\n",__func__,__LINE__,cmd); - return 0; -} - -static void l3g4200d_work_func(struct work_struct *work) -{ - struct l3g4200d_data *l3g4200d = container_of(work, struct l3g4200d_data, work); - struct i2c_client *client = l3g4200d->client; - - if (l3g4200d_get_data(client) < 0) - DBG(KERN_ERR "L3G4200D mma_work_func: Get data failed\n"); - - enable_irq(client->irq); -} - -static void l3g4200d_delaywork_func(struct work_struct *work) -{ - struct delayed_work *delaywork = container_of(work, struct delayed_work, work); - struct l3g4200d_data *l3g4200d = container_of(delaywork, struct l3g4200d_data, delaywork); - struct i2c_client *client = l3g4200d->client; - - if (l3g4200d_get_data(client) < 0) - DBG(KERN_ERR "L3G4200D mma_work_func: Get data failed\n"); - enable_irq(client->irq); -} - -static irqreturn_t l3g4200d_interrupt(int irq, void *dev_id) -{ - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)dev_id; - - disable_irq_nosync(irq); - schedule_delayed_work(&l3g4200d->delaywork, msecs_to_jiffies(10)); - DBG("%s :enter\n",__FUNCTION__); - return IRQ_HANDLED; -} - -static struct file_operations l3g4200d_fops = { - .owner = THIS_MODULE, - .open = l3g4200d_open, - .release = l3g4200d_release, - .unlocked_ioctl = l3g4200d_ioctl, -}; - -static struct miscdevice l3g4200d_device = { - .minor = MISC_DYNAMIC_MINOR, - .name = "gyrosensor",//"l3g4200d_daemon", - .fops = &l3g4200d_fops, -}; - -static int l3g4200d_remove(struct i2c_client *client) -{ - struct l3g4200d_data *l3g4200d = i2c_get_clientdata(client); - - misc_deregister(&l3g4200d_device); - input_unregister_device(l3g4200d->input_dev); - input_free_device(l3g4200d->input_dev); - free_irq(client->irq, l3g4200d); - kfree(l3g4200d); -#ifdef CONFIG_HAS_EARLYSUSPEND - unregister_early_suspend(&l3g4200d_early_suspend); -#endif - this_client = NULL; - return 0; -} - -#ifdef CONFIG_HAS_EARLYSUSPEND -static void l3g4200d_suspend(struct early_suspend *h) -{ - struct i2c_client *client = container_of(l3g4200d_device.parent, struct i2c_client, dev); - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - if(l3g4200d->status == L3G4200D_OPEN) - { - l3g4200d_close_dev(client); - } - - DBG("%s:%d\n",__func__,l3g4200d->status); -} - -static void l3g4200d_resume(struct early_suspend *h) -{ - struct i2c_client *client = container_of(l3g4200d_device.parent, struct i2c_client, dev); - struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - if (l3g4200d->status == L3G4200D_OPEN) - l3g4200d_start_dev(client,l3g4200d->curr_tate); - - DBG("%s:%d\n",__func__,l3g4200d->status); -} -#else -static int l3g4200d_suspend(struct i2c_client *client, pm_message_t mesg) -{ - int ret=0; - //DBG("Gsensor mma7760 enter 2 level suspend l3g4200d->status %d\n",l3g4200d->status); - //struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - //if(l3g4200d->status == L3G4200D_OPEN) - //{ - //l3g4200d->status = L3G4200D_SUSPEND; - //ret = l3g4200d_close_dev(client); - //} - return ret; -} -static int l3g4200d_resume(struct i2c_client *client) -{ - int ret=0; - //struct l3g4200d_data *l3g4200d = (struct l3g4200d_data *)i2c_get_clientdata(client); - //DBG("Gsensor mma7760 2 level resume!! l3g4200d->status %d\n",l3g4200d->status); - //if((l3g4200d->status == L3G4200D_SUSPEND) && (l3g4200d->status != L3G4200D_OPEN)) - //if (l3g4200d->status == L3G4200D_OPEN) - //ret = l3g4200d_start_dev(client, l3g4200d->curr_tate); - return ret; -} -#endif - -static const struct i2c_device_id l3g4200d_id[] = { - {"l3g4200d_gryo", 0}, - { } -}; - -static struct i2c_driver l3g4200d_driver = { - .driver = { - .name = "l3g4200d_gryo", - }, - .id_table = l3g4200d_id, - .probe = l3g4200d_probe, - .remove = __devexit_p(l3g4200d_remove), -#ifndef CONFIG_HAS_EARLYSUSPEND - .suspend = &l3g4200d_suspend, - .resume = &l3g4200d_resume, -#endif -}; - - -static int l3g4200d_init_irq(struct i2c_client *client) -{ - struct l3g4200d_data *l3g4200d; - int ret; - l3g4200d = i2c_get_clientdata(client); - DBG("gpio_to_irq(%d) is %d\n",client->irq,gpio_to_irq(client->irq)); - if ( !gpio_is_valid(client->irq)) { - DBG("+++++++++++gpio_is_invalid\n"); - return -EINVAL; - } - ret = gpio_request(client->irq, "l3g4200d_int"); - if (ret) { - DBG( "failed to request mma7990_trig GPIO%d\n",gpio_to_irq(client->irq)); - return ret; - } - ret = gpio_direction_input(client->irq); - if (ret) { - DBG("failed to set mma7990_trig GPIO gpio input\n"); - return ret; - } - gpio_pull_updown(client->irq, GPIOPullUp); - client->irq = gpio_to_irq(client->irq); - ret = request_irq(client->irq, l3g4200d_interrupt, IRQF_TRIGGER_LOW, client->dev.driver->name, l3g4200d); - DBG("request irq is %d,ret is 0x%x\n",client->irq,ret); - if (ret ) { - DBG(KERN_ERR "l3g4200d_init_irq: request irq failed,ret is %d\n",ret); - return ret; - } - disable_irq(client->irq); - init_waitqueue_head(&data_ready_wq); - - return 0; -} - - -static int l3g4200d_validate_pdata(struct l3g4200d_data *gyro) -{ - if (gyro->pdata->axis_map_x > 2 || - gyro->pdata->axis_map_y > 2 || - gyro->pdata->axis_map_z > 2) { - dev_err(&gyro->client->dev, - "invalid axis_map value x:%u y:%u z%u\n", - gyro->pdata->axis_map_x, gyro->pdata->axis_map_y, - gyro->pdata->axis_map_z); - return -EINVAL; - } - - /* Only allow 0 and 1 for negation boolean flag */ - if (gyro->pdata->negate_x > 1 || - gyro->pdata->negate_y > 1 || - gyro->pdata->negate_z > 1) { - dev_err(&gyro->client->dev, - "invalid negate value x:%u y:%u z:%u\n", - gyro->pdata->negate_x, gyro->pdata->negate_y, - gyro->pdata->negate_z); - return -EINVAL; - } - - return 0; -} - - - - -static int l3g4200d_probe(struct i2c_client *client, const struct i2c_device_id *id) -{ - struct l3g4200d_data *l3g4200d; - struct l3g4200d_platform_data *pdata = pdata = client->dev.platform_data; - int i,err; - - if(pdata && pdata->init) - pdata->init(); - - l3g4200d = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL); - if (!l3g4200d) { - DBG("[l3g4200d]:alloc data failed.\n"); - err = -ENOMEM; - goto exit_alloc_data_failed; - } - - INIT_WORK(&l3g4200d->work, l3g4200d_work_func); - INIT_DELAYED_WORK(&l3g4200d->delaywork, l3g4200d_delaywork_func); - - l3g4200d->client = client; - i2c_set_clientdata(client, l3g4200d); - - this_client = client; - - l3g4200d->pdata = kmalloc(sizeof(*l3g4200d->pdata), GFP_KERNEL); - - if (l3g4200d->pdata == NULL) - goto exit_kfree; - - memcpy(l3g4200d->pdata, client->dev.platform_data, sizeof(*l3g4200d->pdata)); - - err = l3g4200d_validate_pdata(l3g4200d); - if (err < 0) { - dev_err(&client->dev, "failed to validate platform data\n"); - goto exit_kfree_pdata; - } - this_data=l3g4200d; - - //try three times - for(i=0; i<3; i++) - { - err = l3g4200d_get_devid(client); - if (!err) - break; - } - if(err) - { - printk("%s:fail\n",__func__); - goto exit_kfree_pdata; - } - - - err = l3g4200d_init_irq(client); - if (err < 0) { - DBG(KERN_ERR - "l3g4200d_probe: l3g4200d_init_irq failed\n"); - goto exit_request_gpio_irq_failed; - } - - l3g4200d->input_dev = input_allocate_device(); - if (!l3g4200d->input_dev) { - err = -ENOMEM; - DBG(KERN_ERR - "l3g4200d_probe: Failed to allocate input device\n"); - goto exit_input_allocate_device_failed; - } - - //set_bit(EV_ABS, l3g4200d->input_dev->evbit); - - /* x-axis acceleration */ - input_set_capability(l3g4200d->input_dev, EV_REL, REL_RX); - input_set_abs_params(l3g4200d->input_dev, ABS_RX, -32768, 32768, 0, 0); //2g full scale range - /* y-axis acceleration */ - input_set_capability(l3g4200d->input_dev, EV_REL, REL_RY); - input_set_abs_params(l3g4200d->input_dev, ABS_RY, -32768, 32768, 0, 0); //2g full scale range - /* z-axis acceleration */ - input_set_capability(l3g4200d->input_dev, EV_REL, REL_RZ); - input_set_abs_params(l3g4200d->input_dev, ABS_RZ, -32768, 32768, 0, 0); //2g full scale range - - l3g4200d->input_dev->name = "gyro"; - l3g4200d->input_dev->dev.parent = &client->dev; - l3g4200d->axis.x = 0; - l3g4200d->axis.y = 0; - l3g4200d->axis.z = 0; - - err = input_register_device(l3g4200d->input_dev); - if (err < 0) { - DBG(KERN_ERR - "l3g4200d_probe: Unable to register input device: %s\n", - l3g4200d->input_dev->name); - goto exit_input_register_device_failed; - } - - l3g4200d_device.parent = &client->dev; - err = misc_register(&l3g4200d_device); - if (err < 0) { - DBG(KERN_ERR - "l3g4200d_probe: mmad_device register failed\n"); - goto exit_misc_device_register_l3g4200d_device_failed; - } - - err = gsensor_sysfs_init(); - if (err < 0) { - DBG(KERN_ERR - "l3g4200d_probe: gsensor sysfs init failed\n"); - goto exit_gsensor_sysfs_init_failed; - } - -#ifdef CONFIG_HAS_EARLYSUSPEND - l3g4200d_early_suspend.suspend = l3g4200d_suspend; - l3g4200d_early_suspend.resume = l3g4200d_resume; - l3g4200d_early_suspend.level = 0x2; - register_early_suspend(&l3g4200d_early_suspend); -#endif - - l3g4200d->status = L3G4200D_CLOSE; -#if 0 -// l3g4200d_start_test(this_client); - l3g4200d_start(client, L3G4200D_RATE_12P5); -#endif - - return 0; - -exit_gsensor_sysfs_init_failed: - misc_deregister(&l3g4200d_device); -exit_misc_device_register_l3g4200d_device_failed: - input_unregister_device(l3g4200d->input_dev); -exit_input_register_device_failed: - input_free_device(l3g4200d->input_dev); -exit_input_allocate_device_failed: - free_irq(client->irq, l3g4200d); -exit_kfree_pdata: -exit_request_gpio_irq_failed: - kfree(l3g4200d->pdata); -exit_kfree: - kfree(l3g4200d); -exit_alloc_data_failed: - DBG("%s error\n",__FUNCTION__); - return err; -} - - -static int __init l3g4200d_i2c_init(void) -{ - return i2c_add_driver(&l3g4200d_driver); -} - -static void __exit l3g4200d_i2c_exit(void) -{ - i2c_del_driver(&l3g4200d_driver); -} - -module_init(l3g4200d_i2c_init); -module_exit(l3g4200d_i2c_exit); - -