/* * 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 #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 AC (1 << 7) /* register auto-increment bit */ /* odr settings */ #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 */ /* full scale selection */ #define DPS250 250 #define DPS500 500 #define DPS2000 2000 #define FS_MASK 0x30 #define FS_250DPS 0x00 #define FS_500DPS 0x10 #define FS_2000DPS 0x20 #define DEFAULT_DPS DPS500 #define FS_DEFULAT_DPS FS_500DPS /* self tset settings */ #define MIN_ST 175 #define MAX_ST 875 #define FIFO_TEST_WTM 0x1F #define MIN_ZERO_RATE -1714 #define MAX_ZERO_RATE 1714 /* max and min entry */ #define MAX_ENTRY 20 #define MAX_DELAY (MAX_ENTRY * 9523809LL) #define K3G_MAJOR 102 #define K3G_MINOR 4 /* 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 */ 0x90, /* block data update, 500d/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; struct class *k3g_gyro_dev_class; struct device *dev; struct completion data_ready; bool enable; bool drop_next_event; bool fifo_test; /* is self_test or not? */ bool interruptible; /* interrupt or polling? */ int entries; /* number of fifo entries */ int dps; /* scale selection */ u8 fifo_data[sizeof(struct k3g_t) * 32]; /* fifo data 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; int len = sizeof(*data) * (total_read ? (total_read - 1) : 1); struct k3g_data *k3g_data = i2c_get_clientdata(client); struct i2c_msg msg[2]; u8 reg_buf; 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 = k3g_data->fifo_data; if (total_read > 1) { reg_buf = AXISDATA_REG | AC; msg[1].len = len; 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 = (k3g_data->fifo_data[1] << 8) | k3g_data->fifo_data[0]; data->z = (k3g_data->fifo_data[3] << 8) | k3g_data->fifo_data[2]; data->x = (k3g_data->fifo_data[5] << 8) | k3g_data->fifo_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); } #if defined(CONFIG_MACH_U1_NA_SPR) \ || defined(CONFIG_MACH_U1_NA_USCC) input_report_rel(k3g_data->input_dev, REL_RX, -data.x); input_report_rel(k3g_data->input_dev, REL_RY, -data.y); #else input_report_rel(k3g_data->input_dev, REL_RX, data.x); input_report_rel(k3g_data->input_dev, REL_RY, data.y); #endif 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_warn("%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; if (unlikely(k3g_data->fifo_test)) { disable_irq_nosync(irq); complete(&k3g_data->data_ready); return IRQ_HANDLED; } 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_selftest_dps_show(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->dps); } static ssize_t k3g_force_sleep(struct device *dev, struct device_attribute *attr, char *buf) { int err = 5; struct i2c_client *client = to_i2c_client(dev); struct k3g_data *k3g_data = i2c_get_clientdata(client); err = i2c_smbus_write_byte_data(k3g_data->client, CTRL_REG1, 0x00); if (err == 0) err = 1; else err = 0; return sprintf(buf, "%d\n", err); } static ssize_t k3g_selftest_dps_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct k3g_data *k3g_data = dev_get_drvdata(dev); int new_dps = 0; int err; u8 ctrl; sscanf(buf, "%d", &new_dps); /* check out dps validity */ if (new_dps != DPS250 && new_dps != DPS500 && new_dps != DPS2000) { pr_err("%s: wrong dps(%d)\n", __func__, new_dps); return -1; } ctrl = (k3g_data->ctrl_regs[3] & ~FS_MASK); if (new_dps == DPS250) ctrl |= FS_250DPS; else if (new_dps == DPS500) ctrl |= FS_500DPS; else if (new_dps == DPS2000) ctrl |= FS_2000DPS; else ctrl |= FS_DEFULAT_DPS; /* apply new dps */ mutex_lock(&k3g_data->lock); k3g_data->ctrl_regs[3] = ctrl; err = i2c_smbus_write_byte_data(k3g_data->client, CTRL_REG4, ctrl); if (err < 0) { pr_err("%s: updating dps failed\n", __func__); mutex_unlock(&k3g_data->lock); return err; } mutex_unlock(&k3g_data->lock); k3g_data->dps = new_dps; pr_err("%s: %d dps stored\n", __func__, k3g_data->dps); return count; } 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) { 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[1].delay_ns) { if (delay_ns >= MAX_DELAY) { k3g_data->entries = MAX_ENTRY; delay_ns = MAX_DELAY; } else { do_div(delay_ns, odr_delay_table[1].delay_ns); k3g_data->entries = delay_ns; } k3g_data->time_to_read = odr_delay_table[1].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); } /* (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); /*************************************************************************/ /* K3G Sysfs */ /*************************************************************************/ /* Device Initialization */ static int device_init(struct k3g_data *data) { int err; u8 buf[5]; buf[0] = 0x6f; buf[1] = 0x00; buf[2] = 0x00; buf[3] = 0x00; buf[4] = 0x00; err = i2c_smbus_write_i2c_block_data(data->client, CTRL_REG1 | AC, sizeof(buf), buf); if (err < 0) pr_err("%s: CTRL_REGs i2c writing failed\n", __func__); return err; } static ssize_t k3g_power_on(struct device *dev, struct device_attribute *attr, char *buf) { struct k3g_data *data = dev_get_drvdata(dev); int err; err = device_init(data); if (err < 0) { pr_err("%s: device_init() failed\n", __func__); return 0; } printk(KERN_INFO "[%s] result of device init = %d\n", __func__, err); return sprintf(buf, "%d\n", (err < 0 ? 0 : 1)); } static ssize_t k3g_get_temp(struct device *dev, struct device_attribute *attr, char *buf) { struct k3g_data *data = dev_get_drvdata(dev); char temp; temp = i2c_smbus_read_byte_data(data->client, OUT_TEMP); if (temp < 0) { pr_err("%s: STATUS_REGS i2c reading failed\n", __func__); return 0; } printk(KERN_INFO "[%s] read temperature : %d\n", __func__, temp); return sprintf(buf, "%d\n", temp); } static int k3g_fifo_self_test(struct k3g_data *k3g_data) { struct k3g_t raw_data; int err; int i, j; s16 raw[3] = { 0, }; u8 reg[5]; u8 fifo_pass = 2; u8 status_reg; k3g_data->fifo_test = true; /* fifo mode, enable interrupt, 500dps */ reg[0] = 0x6F; reg[1] = 0x00; reg[2] = 0x04; reg[3] = 0x90; reg[4] = 0x40; for (i = 0; i < 10; i++) { err = i2c_smbus_write_i2c_block_data(k3g_data->client, CTRL_REG1 | AC, sizeof(reg), reg); if (err >= 0) break; } if (err < 0) { pr_err("%s: CTRL_REGs i2c writing failed\n", __func__); goto exit; } /* Power up, wait for 800ms for stable output */ msleep(800); for (i = 0; i < 10; i++) { err = i2c_smbus_write_byte_data(k3g_data->client, FIFO_CTRL_REG, BYPASS_MODE); if (err >= 0) break; } if (err < 0) { pr_err("%s : failed to set bypass_mode\n", __func__); goto exit; } for (i = 0; i < 10; i++) { err = i2c_smbus_write_byte_data(k3g_data->client, FIFO_CTRL_REG, FIFO_MODE | FIFO_TEST_WTM); if (err >= 0) break; } if (err < 0) { pr_err("%s: failed to set fifo_mode\n", __func__); goto exit; } /* if interrupt mode */ if (!k3g_data->enable && k3g_data->interruptible) { enable_irq(k3g_data->client->irq); err = wait_for_completion_timeout(&k3g_data->data_ready, 5*HZ); if (err <= 0) { disable_irq(k3g_data->client->irq); if (!err) pr_err("%s: wait timed out\n", __func__); goto exit; } } /* wait for 32 fifo entries */ msleep(200); /* check out watermark status */ status_reg = i2c_smbus_read_byte_data(k3g_data->client, FIFO_SRC_REG); if (!(status_reg & 0x80)) { pr_err("%s: Watermark level is not enough\n", __func__); goto exit; } /* read fifo entries */ err = k3g_read_gyro_values(k3g_data->client, &raw_data, FIFO_TEST_WTM + 2); if (err < 0) { pr_err("%s: k3g_read_gyro_values() failed\n", __func__); goto exit; } /* print out fifo data */ printk(KERN_INFO "[gyro_self_test] fifo data\n"); for (i = 0; i < sizeof(raw_data) * (FIFO_TEST_WTM + 1); i += sizeof(raw_data)) { raw[0] = (k3g_data->fifo_data[i+1] << 8) | k3g_data->fifo_data[i]; raw[1] = (k3g_data->fifo_data[i+3] << 8) | k3g_data->fifo_data[i+2]; raw[2] = (k3g_data->fifo_data[i+5] << 8) | k3g_data->fifo_data[i+4]; pr_err("%2dth: %8d %8d %8d\n", i/6, raw[0], raw[1], raw[2]); for (j = 0; j < 3; j++) { if (raw[j] < MIN_ZERO_RATE || raw[j] > MAX_ZERO_RATE) { pr_err("%s: %dth data(%d) is out of zero-rate", __func__, i/6, raw[j]); pr_err("%s: fifo test failed\n", __func__); fifo_pass = 0; goto exit; } } } fifo_pass = 1; exit: k3g_data->fifo_test = false; /* make sure clearing interrupt */ enable_irq(k3g_data->client->irq); disable_irq(k3g_data->client->irq); /* 1: success, 0: fail, 2: retry */ return fifo_pass; } static int k3g_bypass_self_test(struct k3g_data *k3g_data, int NOST[3], int ST[3]) { int differ_x = 0, differ_y = 0, differ_z = 0; int err; int i, j; s16 raw[3] = { 0, }; s32 temp; u8 gyro_data[6] = { 0, }; u8 reg[5]; u8 bZYXDA = 0; u8 bypass_pass = 2; /* Initialize Sensor, turn on sensor, enable P/R/Y */ /* Set BDU=1, Set ODR=200Hz, Cut-Off Frequency=50Hz, FS=2000dps */ reg[0] = 0x6f; reg[1] = 0x00; reg[2] = 0x00; reg[3] = 0xA0; reg[4] = 0x02; for (i = 0; i < 10; i++) { err = i2c_smbus_write_i2c_block_data(k3g_data->client, CTRL_REG1 | AC, sizeof(reg), reg); if (err >= 0) break; } if (err < 0) { pr_err("%s: CTRL_REGs i2c writing failed\n", __func__); goto exit; } for (i = 0; i < 10; i++) { err = i2c_smbus_write_byte_data(k3g_data->client, FIFO_CTRL_REG, BYPASS_MODE); if (err >= 0) break; } if (err < 0) { pr_err("%s : failed to set bypass_mode\n", __func__); goto exit; } /* Power up, wait for 800ms for stable output */ msleep(800); /* Read 5 samples output before self-test on */ for (i = 0; i < 5; i++) { /* check ZYXDA ready bit */ for (j = 0; j < 10; j++) { temp = i2c_smbus_read_byte_data(k3g_data->client, STATUS_REG); if (temp >= 0) { bZYXDA = temp & 0x08; if (!bZYXDA) { usleep_range(10000, 10000); pr_err("%s: %d,%d: no_data_ready", __func__, i, j); continue; } else break; } } if (temp < 0) { pr_err("%s: STATUS_REGS i2c reading failed\n", __func__); goto exit; } for (j = 0; j < 10; j++) { err = i2c_smbus_read_i2c_block_data(k3g_data->client, AXISDATA_REG | AC, sizeof(gyro_data), gyro_data); if (err >= 0) break; } if (err < 0) { pr_err("%s: CTRL_REGs i2c reading failed\n", __func__); goto exit; } raw[0] = (gyro_data[1] << 8) | gyro_data[0]; raw[1] = (gyro_data[3] << 8) | gyro_data[2]; raw[2] = (gyro_data[5] << 8) | gyro_data[4]; NOST[0] += raw[0]; NOST[1] += raw[1]; NOST[2] += raw[2]; printk(KERN_INFO "[gyro_self_test] raw[0] = %d\n", raw[0]); printk(KERN_INFO "[gyro_self_test] raw[1] = %d\n", raw[1]); printk(KERN_INFO "[gyro_self_test] raw[2] = %d\n\n", raw[2]); } for (i = 0; i < 3; i++) printk(KERN_INFO "[gyro_self_test] " "SUM of NOST[%d] = %d\n", i, NOST[i]); /* calculate average of NOST and covert from ADC to DPS */ for (i = 0; i < 3; i++) { NOST[i] = (NOST[i] / 5) * 70 / 1000; printk(KERN_INFO "[gyro_self_test] " "AVG of NOST[%d] = %d\n", i, NOST[i]); } printk(KERN_INFO "\n"); /* Enable Self Test */ reg[0] = 0xA2; for (i = 0; i < 10; i++) { err = i2c_smbus_write_byte_data(k3g_data->client, CTRL_REG4, reg[0]); if (err >= 0) break; } if (temp < 0) { pr_err("%s: CTRL_REG4 i2c writing failed\n", __func__); goto exit; } msleep(100); /* Read 5 samples output after self-test on */ for (i = 0; i < 5; i++) { /* check ZYXDA ready bit */ for (j = 0; j < 10; j++) { temp = i2c_smbus_read_byte_data(k3g_data->client, STATUS_REG); if (temp >= 0) { bZYXDA = temp & 0x08; if (!bZYXDA) { usleep_range(10000, 10000); pr_err("%s: %d,%d: no_data_ready", __func__, i, j); continue; } else break; } } if (temp < 0) { pr_err("%s: STATUS_REGS i2c reading failed\n", __func__); goto exit; } for (j = 0; j < 10; j++) { err = i2c_smbus_read_i2c_block_data(k3g_data->client, AXISDATA_REG | AC, sizeof(gyro_data), gyro_data); if (err >= 0) break; } if (err < 0) { pr_err("%s: CTRL_REGs i2c reading failed\n", __func__); goto exit; } raw[0] = (gyro_data[1] << 8) | gyro_data[0]; raw[1] = (gyro_data[3] << 8) | gyro_data[2]; raw[2] = (gyro_data[5] << 8) | gyro_data[4]; ST[0] += raw[0]; ST[1] += raw[1]; ST[2] += raw[2]; printk(KERN_INFO "[gyro_self_test] raw[0] = %d\n", raw[0]); printk(KERN_INFO "[gyro_self_test] raw[1] = %d\n", raw[1]); printk(KERN_INFO "[gyro_self_test] raw[2] = %d\n\n", raw[2]); } for (i = 0; i < 3; i++) printk(KERN_INFO "[gyro_self_test] " "SUM of ST[%d] = %d\n", i, ST[i]); /* calculate average of ST and convert from ADC to dps */ for (i = 0; i < 3; i++) { /* When FS=2000, 70 mdps/digit */ ST[i] = (ST[i] / 5) * 70 / 1000; printk(KERN_INFO "[gyro_self_test] " "AVG of ST[%d] = %d\n", i, ST[i]); } /* check whether pass or not */ if (ST[0] >= NOST[0]) /* for x */ differ_x = ST[0] - NOST[0]; else differ_x = NOST[0] - ST[0]; if (ST[1] >= NOST[1]) /* for y */ differ_y = ST[1] - NOST[1]; else differ_y = NOST[1] - ST[1]; if (ST[2] >= NOST[2]) /* for z */ differ_z = ST[2] - NOST[2]; else differ_z = NOST[2] - ST[2]; printk(KERN_INFO "[gyro_self_test] differ x:%d, y:%d, z:%d\n", differ_x, differ_y, differ_z); if ((MIN_ST <= differ_x && differ_x <= MAX_ST) && (MIN_ST <= differ_y && differ_y <= MAX_ST) && (MIN_ST <= differ_z && differ_z <= MAX_ST)) bypass_pass = 1; else bypass_pass = 0; exit: return bypass_pass; } static ssize_t k3g_self_test(struct device *dev, struct device_attribute *attr, char *buf) { struct k3g_data *data = dev_get_drvdata(dev); int NOST[3] = { 0, }, ST[3] = { 0, }; int err; int i; u8 backup_regs[5]; u8 fifo_pass = 2; u8 bypass_pass = 2; /* before starting self-test, backup register */ for (i = 0; i < 10; i++) { err = i2c_smbus_read_i2c_block_data(data->client, CTRL_REG1 | AC, sizeof(backup_regs), backup_regs); if (err >= 0) break; } if (err < 0) { pr_err("%s: CTRL_REGs i2c reading failed\n", __func__); goto exit; } for (i = 0; i < 5; i++) printk(KERN_INFO "[gyro_self_test] " "backup reg[%d] = %2x\n", i, backup_regs[i]); /* fifo self test */ printk(KERN_INFO "\n[gyro_self_test] fifo self-test\n"); fifo_pass = k3g_fifo_self_test(data); if (fifo_pass) printk(KERN_INFO "[gyro_self_test] fifo self-test success\n"); else if (!fifo_pass) printk(KERN_INFO "[gyro_self_test] fifo self-test fail\n"); else printk(KERN_INFO "[gyro_self_test] fifo self-test restry\n"); /* bypass self test */ printk(KERN_INFO "\n[gyro_self_test] bypass self-test\n"); bypass_pass = k3g_bypass_self_test(data, NOST, ST); if (bypass_pass) printk(KERN_INFO "[gyro_self_test] bypass self-test success\n\n"); else if (!fifo_pass) printk(KERN_INFO "[gyro_self_test] bypass self-test fail\n\n"); else printk(KERN_INFO "[gyro_self_test] bypass self-test restry\n\n"); /* restore backup register */ for (i = 0; i < 10; i++) { err = i2c_smbus_write_i2c_block_data(data->client, CTRL_REG1 | AC, sizeof(backup_regs), backup_regs); if (err >= 0) break; } if (err < 0) pr_err("%s: CTRL_REGs i2c writing failed\n", __func__); exit: if (!data->enable) { /* If k3g is not enabled, make it go to the power down mode. */ err = i2c_smbus_write_byte_data(data->client, CTRL_REG1, 0x00); if (err < 0) pr_err("%s: CTRL_REGs i2c writing failed\n", __func__); } if (fifo_pass == 2 && bypass_pass == 2) printk(KERN_INFO "[gyro_self_test] self-test result : retry\n"); else printk(KERN_INFO "[gyro_self_test] self-test result : %s\n", fifo_pass & bypass_pass ? "pass" : "fail"); return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d\n", NOST[0], NOST[1], NOST[2], ST[0], ST[1], ST[2], fifo_pass & bypass_pass, fifo_pass); } static DEVICE_ATTR(gyro_power_on, 0664, k3g_power_on, NULL); static DEVICE_ATTR(gyro_get_temp, 0664, k3g_get_temp, NULL); static DEVICE_ATTR(gyro_selftest, 0664, k3g_self_test, NULL); static DEVICE_ATTR(gyro_selftest_dps, 0664, k3g_selftest_dps_show, k3g_selftest_dps_store); static DEVICE_ATTR(gyro_force_sleep, 0664, k3g_force_sleep, NULL); static const struct file_operations k3g_fops = { .owner = THIS_MODULE, }; /*************************************************************************/ /* End of K3G Sysfs */ /*************************************************************************/ 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; 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); init_completion(&data->data_ready); /* 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_sensor"; /* 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)); i2c_set_clientdata(client, data); dev_set_drvdata(&input_dev->dev, data); 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("%s: Failed to create device file(%s)!\n", __func__, dev_attr_enable.attr.name); goto err_device_create_file; } if (device_create_file(&input_dev->dev, &dev_attr_poll_delay) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_poll_delay.attr.name); goto err_device_create_file2; } /* register a char dev */ err = register_chrdev(K3G_MAJOR, "k3g", &k3g_fops); if (err < 0) { pr_err("%s: Failed to register chrdev(k3g)\n", __func__); goto err_register_chrdev; } /* create k3g-dev device class */ data->k3g_gyro_dev_class = class_create(THIS_MODULE, "K3G_GYRO-dev"); if (IS_ERR(data->k3g_gyro_dev_class)) { pr_err("%s: Failed to create class(K3G_GYRO-dev\n", __func__); err = PTR_ERR(data->k3g_gyro_dev_class); goto err_class_create; } /* create device node for k3g digital gyroscope */ data->dev = device_create(data->k3g_gyro_dev_class, NULL, MKDEV(K3G_MAJOR, 0), NULL, "k3g"); if (IS_ERR(data->dev)) { pr_err("%s: Failed to create device(k3g)\n", __func__); err = PTR_ERR(data->dev); goto err_device_create; } if (device_create_file(data->dev, &dev_attr_gyro_power_on) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_gyro_power_on.attr.name); goto device_create_file3; } if (device_create_file(data->dev, &dev_attr_gyro_get_temp) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_gyro_get_temp.attr.name); goto device_create_file4; } if (device_create_file(data->dev, &dev_attr_gyro_selftest) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_gyro_selftest.attr.name); goto device_create_file5; } if (device_create_file(data->dev, &dev_attr_gyro_selftest_dps) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_gyro_selftest_dps.attr.name); goto device_create_file6; } if (device_create_file(data->dev, &dev_attr_gyro_force_sleep) < 0) { pr_err("%s: Failed to create device file(%s)!\n", __func__, dev_attr_gyro_force_sleep.attr.name); goto device_create_file7; } dev_set_drvdata(data->dev, data); return 0; device_create_file7: device_remove_file(data->dev, &dev_attr_gyro_force_sleep); device_create_file6: device_remove_file(data->dev, &dev_attr_gyro_selftest); device_create_file5: device_remove_file(data->dev, &dev_attr_gyro_get_temp); device_create_file4: device_remove_file(data->dev, &dev_attr_gyro_power_on); device_create_file3: device_destroy(data->k3g_gyro_dev_class, MKDEV(K3G_MAJOR, 0)); err_device_create: class_destroy(data->k3g_gyro_dev_class); err_class_create: unregister_chrdev(K3G_MAJOR, "k3g"); err_register_chrdev: device_remove_file(&input_dev->dev, &dev_attr_poll_delay); 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->dev, &dev_attr_gyro_selftest_dps); device_remove_file(k3g_data->dev, &dev_attr_gyro_force_sleep); device_remove_file(k3g_data->dev, &dev_attr_gyro_selftest); device_remove_file(k3g_data->dev, &dev_attr_gyro_get_temp); device_remove_file(k3g_data->dev, &dev_attr_gyro_power_on); device_destroy(k3g_data->k3g_gyro_dev_class, MKDEV(K3G_MAJOR, 0)); class_destroy(k3g_data->k3g_gyro_dev_class); unregister_chrdev(K3G_MAJOR, "k3g"); 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.com"); MODULE_LICENSE("GPL");