From: luowei Date: Tue, 3 Sep 2013 11:34:30 +0000 (+0800) Subject: add angle caculation support between two gsensor X-Git-Tag: firefly_0821_release~6665^2~1 X-Git-Url: http://demsky.eecs.uci.edu/git/?a=commitdiff_plain;h=f7d7863e3ffa3ae792749c2cbd8c9a2296874b70;p=firefly-linux-kernel-4.4.55.git add angle caculation support between two gsensor --- diff --git a/drivers/input/sensors/Kconfig b/drivers/input/sensors/Kconfig index 813330bfb316..15f514567e01 100755 --- a/drivers/input/sensors/Kconfig +++ b/drivers/input/sensors/Kconfig @@ -5,10 +5,10 @@ comment "handle all sensors" menuconfig SENSOR_DEVICE - tristate "handle gsensor,compass,gyroscope,lsensor psensor etc" + tristate "handle angle,accel,compass,gyroscope,lsensor psensor etc" if SENSOR_DEVICE - +source "drivers/input/sensors/angle/Kconfig" source "drivers/input/sensors/accel/Kconfig" source "drivers/input/sensors/compass/Kconfig" source "drivers/input/sensors/gyro/Kconfig" diff --git a/drivers/input/sensors/Makefile b/drivers/input/sensors/Makefile index 1b23c47bea21..809b50b40d39 100755 --- a/drivers/input/sensors/Makefile +++ b/drivers/input/sensors/Makefile @@ -1,4 +1,5 @@ # sensor drivers +obj-$(CONFIG_ANGLE_DEVICE) += angle/ obj-$(CONFIG_GSENSOR_DEVICE) += accel/ obj-$(CONFIG_COMPASS_DEVICE) += compass/ obj-$(CONFIG_GYROSCOPE_DEVICE) += gyro/ diff --git a/drivers/input/sensors/angle/Kconfig b/drivers/input/sensors/angle/Kconfig new file mode 100755 index 000000000000..9c09a48b7649 --- /dev/null +++ b/drivers/input/sensors/angle/Kconfig @@ -0,0 +1,26 @@ +# +# gsensor drivers configuration +# + +menuconfig ANGLE_DEVICE + bool "angle device support" + help + Enable this to be able to choose the drivers for controlling the + g_sensor on some platforms, for example on PDAs. + +if ANGLE_DEVICE + +config ANGLE_KXTIK + bool "angle kxtik" + help + To have support for your specific gsesnor you will have to + select the proper drivers which depend on this option. + +config ANGLE_LIS3DH + bool "angle lis3dh" + help + To have support for your specific gsesnor you will have to + select the proper drivers which depend on this option. + +endif + diff --git a/drivers/input/sensors/angle/Makefile b/drivers/input/sensors/angle/Makefile new file mode 100755 index 000000000000..104642da8afd --- /dev/null +++ b/drivers/input/sensors/angle/Makefile @@ -0,0 +1,2 @@ +obj-$(CONFIG_ANGLE_KXTIK) += angle_kxtik.o +obj-$(CONFIG_ANGLE_LIS3DH) += angle_lis3dh.o diff --git a/drivers/input/sensors/angle/angle_kxtik.c b/drivers/input/sensors/angle/angle_kxtik.c new file mode 100755 index 000000000000..f9aef1b285f2 --- /dev/null +++ b/drivers/input/sensors/angle/angle_kxtik.c @@ -0,0 +1,394 @@ +/* drivers/input/sensors/access/angle_kxtik.c + * + * Copyright (C) 2012-2015 ROCKCHIP. + * Author: luowei + * + * 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 +#include + +#define KXTIK_DEVID_1004 0x05 //chip id +#define KXTIK_DEVID_J9_1005 0x07 //chip id +#define KXTIK_DEVID_J2_1009 0x09 //chip id +#define KXTIK_DEVID_1013 0x11 //chip id +#define KXTIK_RANGE 2000000 + +#define KXTIK_XOUT_HPF_L (0x00) /* 0000 0000 */ +#define KXTIK_XOUT_HPF_H (0x01) /* 0000 0001 */ +#define KXTIK_YOUT_HPF_L (0x02) /* 0000 0010 */ +#define KXTIK_YOUT_HPF_H (0x03) /* 0000 0011 */ +#define KXTIK_ZOUT_HPF_L (0x04) /* 0001 0100 */ +#define KXTIK_ZOUT_HPF_H (0x05) /* 0001 0101 */ +#define KXTIK_XOUT_L (0x06) /* 0000 0110 */ +#define KXTIK_XOUT_H (0x07) /* 0000 0111 */ +#define KXTIK_YOUT_L (0x08) /* 0000 1000 */ +#define KXTIK_YOUT_H (0x09) /* 0000 1001 */ +#define KXTIK_ZOUT_L (0x0A) /* 0001 1010 */ +#define KXTIK_ZOUT_H (0x0B) /* 0001 1011 */ +#define KXTIK_ST_RESP (0x0C) /* 0000 1100 */ +#define KXTIK_WHO_AM_I (0x0F) /* 0000 1111 */ +#define KXTIK_TILT_POS_CUR (0x10) /* 0001 0000 */ +#define KXTIK_TILT_POS_PRE (0x11) /* 0001 0001 */ +#define KXTIK_INT_SRC_REG1 (0x15) /* 0001 0101 */ +#define KXTIK_INT_SRC_REG2 (0x16) /* 0001 0110 */ +#define KXTIK_STATUS_REG (0x18) /* 0001 1000 */ +#define KXTIK_INT_REL (0x1A) /* 0001 1010 */ +#define KXTIK_CTRL_REG1 (0x1B) /* 0001 1011 */ +#define KXTIK_CTRL_REG2 (0x1C) /* 0001 1100 */ +#define KXTIK_CTRL_REG3 (0x1D) /* 0001 1101 */ +#define KXTIK_INT_CTRL_REG1 (0x1E) /* 0001 1110 */ +#define KXTIK_INT_CTRL_REG2 (0x1F) /* 0001 1111 */ +#define KXTIK_INT_CTRL_REG3 (0x20) /* 0010 0000 */ +#define KXTIK_DATA_CTRL_REG (0x21) /* 0010 0001 */ +#define KXTIK_TILT_TIMER (0x28) /* 0010 1000 */ +#define KXTIK_WUF_TIMER (0x29) /* 0010 1001 */ +#define KXTIK_TDT_TIMER (0x2B) /* 0010 1011 */ +#define KXTIK_TDT_H_THRESH (0x2C) /* 0010 1100 */ +#define KXTIK_TDT_L_THRESH (0x2D) /* 0010 1101 */ +#define KXTIK_TDT_TAP_TIMER (0x2E) /* 0010 1110 */ +#define KXTIK_TDT_TOTAL_TIMER (0x2F) /* 0010 1111 */ +#define KXTIK_TDT_LATENCY_TIMER (0x30) /* 0011 0000 */ +#define KXTIK_TDT_WINDOW_TIMER (0x31) /* 0011 0001 */ +#define KXTIK_WUF_THRESH (0x5A) /* 0101 1010 */ +#define KXTIK_TILT_ANGLE (0x5C) /* 0101 1100 */ +#define KXTIK_HYST_SET (0x5F) /* 0101 1111 */ + +/* CONTROL REGISTER 1 BITS */ +#define KXTIK_DISABLE 0x7F +#define KXTIK_ENABLE (1 << 7) +#define KXTIK_DRDYE (1 << 5) +/* INPUT_ABS CONSTANTS */ +#define FUZZ 3 +#define FLAT 3 +/* RESUME STATE INDICES */ +#define RES_DATA_CTRL 0 +#define RES_CTRL_REG1 1 +#define RES_INT_CTRL1 2 +#define RESUME_ENTRIES 3 + +/* CTRL_REG1: set resolution, g-range, data ready enable */ +/* Output resolution: 8-bit valid or 12-bit valid */ +#define KXTIK_RES_8BIT 0 +#define KXTIK_RES_12BIT (1 << 6) +/* Output g-range: +/-2g, 4g, or 8g */ +#define KXTIK_G_2G 0 +#define KXTIK_G_4G (1 << 3) +#define KXTIK_G_8G (1 << 4) + +/* DATA_CTRL_REG: controls the output data rate of the part */ +#define KXTIK_ODR12_5F 0 +#define KXTIK_ODR25F 1 +#define KXTIK_ODR50F 2 +#define KXTIK_ODR100F 3 +#define KXTIK_ODR200F 4 +#define KXTIK_ODR400F 5 +#define KXTIK_ODR800F 6 + +/* kxtik */ +#define KXTIK_PRECISION 12 +#define KXTIK_BOUNDARY (0x1 << (KXTIK_PRECISION - 1)) +#define KXTIK_GRAVITY_STEP KXTIK_RANGE / KXTIK_BOUNDARY + + +/****************operate according to sensor chip:start************/ + +static int sensor_active(struct i2c_client *client, int enable, int rate) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + int result = 0; + int status = 0; + + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + + //register setting according to chip datasheet + if(enable) + { + status = KXTIK_ENABLE; //kxtik + sensor->ops->ctrl_data |= status; + } + else + { + status = ~KXTIK_ENABLE; //kxtik + sensor->ops->ctrl_data &= status; + } + + DBG("%s:reg=0x%x,reg_ctrl=0x%x,enable=%d\n",__func__,sensor->ops->ctrl_reg, sensor->ops->ctrl_data, enable); + result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); + if(result) + printk("%s:fail to active sensor\n",__func__); + + return result; + +} + +static int sensor_init(struct i2c_client *client) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + int result = 0; + int i = 0; + unsigned char id_reg = KXTIK_WHO_AM_I; + unsigned char id_data = 0; + unsigned char ctrl_data_save = 0; + + result = sensor->ops->active(client,0,0); + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + + sensor->status_cur = SENSOR_OFF; + + for(i=0; i<3; i++) + { + result = sensor_rx_data(client, &id_reg, 1); + id_data = id_reg; + if(!result) + break; + } + + if(result) + { + printk("%s:fail to read id,result=%d\n",__func__, result); + return result; + } + + sensor->devid = id_data; + + result = sensor_write_reg(client, KXTIK_DATA_CTRL_REG, KXTIK_ODR400F); + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + + if(sensor->pdata->irq_enable) //open interrupt + { + +/* +BIT 4 IEA sets the polarity of the physical interrupt pin (7) +IEA = 0 ¿ polarity of the physical interrupt pin (7) is active low +IEA = 1 ¿ polarity of the physical interrupt pin (7) is active high +BIT 3 IEL sets the response of the physical interrupt pin (7) +IEL = 0 ¿ the physical interrupt pin (7) latches until it is cleared by reading INT_REL +IEL = 1 ¿ the physical interrupt pin (7) will transmit one pulse with a period of 0.03 - 0.05ms +*/ + if (id_data == KXTIK_DEVID_1004) + result = sensor_write_reg(client, KXTIK_INT_CTRL_REG1, 0x34);//enable int,active high,need read INT_REL + else + result = sensor_write_reg(client, KXTIK_INT_CTRL_REG1, 0x28);//enable int,active high,need read INT_REL + + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + } + + ctrl_data_save = sensor_read_reg(client, sensor->ops->ctrl_reg); + DBG("%s: ctrl_data_save = 0x%x\n", __func__, ctrl_data_save); + sensor->ops->ctrl_data = (KXTIK_RES_12BIT | KXTIK_G_2G); + if(sensor->pdata->irq_enable) + { + sensor->ops->ctrl_data &= ~KXTIK_ENABLE; + sensor->ops->ctrl_data |= KXTIK_DRDYE; + } + DBG("%s: first write sensor->ops->ctrl_data = 0x%x\n", __func__, sensor->ops->ctrl_data); + result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + if(ctrl_data_save & 0x80) + sensor->ops->ctrl_data |= KXTIK_ENABLE; + else + sensor->ops->ctrl_data &= ~KXTIK_ENABLE; + DBG("%s: second write sensor->ops->ctrl_data = 0x%x\n", __func__, sensor->ops->ctrl_data); + result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + + DBG("%s:%s id=0x%x\n",__func__,sensor->ops->name, id_data); + return result; +} + +static int sensor_convert_data(struct i2c_client *client, char high_byte, char low_byte) +{ + s64 result; + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + //int precision = sensor->ops->precision; + switch (sensor->devid) { + case KXTIK_DEVID_1004: + case KXTIK_DEVID_1013: + case KXTIK_DEVID_J9_1005: + case KXTIK_DEVID_J2_1009: + result = (((int)high_byte << 8) | ((int)low_byte ))>>4; + if (result < KXTIK_BOUNDARY) + result = result* KXTIK_GRAVITY_STEP; + else + result = ~( ((~result & (0x7fff>>(16-KXTIK_PRECISION)) ) + 1) + * KXTIK_GRAVITY_STEP) + 1; + break; + + default: + printk(KERN_ERR "%s: devid wasn't set correctly\n",__func__); + return -EFAULT; + } + return (int)result; +} + +static int angle_report_value(struct i2c_client *client, struct sensor_axis *axis) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + + /* Report acceleration sensor information */ + input_report_abs(sensor->input_dev, ABS_X, axis->x); + input_report_abs(sensor->input_dev, ABS_Y, axis->y); + input_report_abs(sensor->input_dev, ABS_Z, axis->z); + input_sync(sensor->input_dev); + DBG("Gsensor x==%d y==%d z==%d\n",axis->x,axis->y,axis->z); + + return 0; +} + +#define GSENSOR_MIN 10 +static int sensor_report_value(struct i2c_client *client) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + struct sensor_platform_data *pdata = sensor->pdata; + int ret = 0; + int x,y,z; + struct sensor_axis axis; + char buffer[6] = {0}; + char value = 0; + + if(sensor->ops->read_len < 6) //sensor->ops->read_len = 6 + { + printk("%s:lenth is error,len=%d\n",__func__,sensor->ops->read_len); + return -1; + } + + memset(buffer, 0, 6); + + /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ + do { + *buffer = sensor->ops->read_reg; + ret = sensor_rx_data(client, buffer, sensor->ops->read_len); + if (ret < 0) + return ret; + } while (0); + + //this angle need 6 bytes buffer + x = sensor_convert_data(sensor->client, buffer[1], buffer[0]); //buffer[1]:high bit + y = sensor_convert_data(sensor->client, buffer[3], buffer[2]); + z = sensor_convert_data(sensor->client, buffer[5], buffer[4]); + + 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; + + DBG( "%s: axis = %d %d %d \n", __func__, axis.x, axis.y, axis.z); + + //Report event only while value is changed to save some power + if((abs(sensor->axis.x - axis.x) > GSENSOR_MIN) || (abs(sensor->axis.y - axis.y) > GSENSOR_MIN) || (abs(sensor->axis.z - axis.z) > GSENSOR_MIN)) + { + angle_report_value(client, &axis); + + /* »¥³âµØ»º´æÊý¾Ý. */ + mutex_lock(&(sensor->data_mutex) ); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex) ); + } + + if((sensor->pdata->irq_enable)&& (sensor->ops->int_status_reg >= 0)) //read sensor intterupt status register + { + + value = sensor_read_reg(client, sensor->ops->int_status_reg); + DBG("%s:sensor int status :0x%x\n",__func__,value); + } + + return ret; +} + +struct sensor_operate angle_kxtik_ops = { + .name = "angle_kxtik", + .type = SENSOR_TYPE_ANGLE, //sensor type and it should be correct + .id_i2c = ANGLE_ID_KXTIK, //i2c id number + .read_reg = KXTIK_XOUT_L, //read data + .read_len = 6, //data length + .id_reg = SENSOR_UNKNOW_DATA, //read device id from this register + .id_data = SENSOR_UNKNOW_DATA, + .precision = KXTIK_PRECISION, //12 bits + .ctrl_reg = KXTIK_CTRL_REG1, //enable or disable + .int_status_reg = KXTIK_INT_REL, //intterupt status register + .range = {-KXTIK_RANGE,KXTIK_RANGE}, //range + .trig = IRQF_TRIGGER_LOW|IRQF_ONESHOT, + .active = sensor_active, + .init = sensor_init, + .report = sensor_report_value, +}; + +/****************operate according to sensor chip:end************/ + +//function name should not be changed +static struct sensor_operate *angle_get_ops(void) +{ + return &angle_kxtik_ops; +} + + +static int __init angle_kxtik_init(void) +{ + struct sensor_operate *ops = angle_get_ops(); + int result = 0; + int type = ops->type; + result = sensor_register_slave(type, NULL, NULL, angle_get_ops); + printk("%s\n",__func__); + return result; +} + +static void __exit angle_kxtik_exit(void) +{ + struct sensor_operate *ops = angle_get_ops(); + int type = ops->type; + sensor_unregister_slave(type, NULL, NULL, angle_get_ops); +} + + +module_init(angle_kxtik_init); +module_exit(angle_kxtik_exit); + diff --git a/drivers/input/sensors/angle/angle_lis3dh.c b/drivers/input/sensors/angle/angle_lis3dh.c new file mode 100755 index 000000000000..f37e54ef7e3a --- /dev/null +++ b/drivers/input/sensors/angle/angle_lis3dh.c @@ -0,0 +1,336 @@ +/* drivers/input/sensors/access/kxtik.c + * + * Copyright (C) 2012-2015 ROCKCHIP. + * Author: luowei + * + * 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 +#include + + +#define LIS3DH_INT_COUNT (0x0E) +#define LIS3DH_WHO_AM_I (0x0F) + +/* full scale setting - register & mask */ +#define LIS3DH_TEMP_CFG_REG (0x1F) +#define LIS3DH_CTRL_REG1 (0x20) +#define LIS3DH_CTRL_REG2 (0x21) +#define LIS3DH_CTRL_REG3 (0x22) +#define LIS3DH_CTRL_REG4 (0x23) +#define LIS3DH_CTRL_REG5 (0x24) +#define LIS3DH_CTRL_REG6 (0x25) +#define LIS3DH_REFERENCE (0x26) +#define LIS3DH_STATUS_REG (0x27) +#define LIS3DH_OUT_X_L (0x28) +#define LIS3DH_OUT_X_H (0x29) +#define LIS3DH_OUT_Y_L (0x2a) +#define LIS3DH_OUT_Y_H (0x2b) +#define LIS3DH_OUT_Z_L (0x2c) +#define LIS3DH_OUT_Z_H (0x2d) +#define LIS3DH_FIFO_CTRL_REG (0x2E) + +#define LIS3DH_INT1_CFG (0x30) +#define LIS3DH_INT1_SRC (0x31) +#define LIS3DH_INT1_THS (0x32) +#define LIS3DH_INT1_DURATION (0x33) + +#define LIS3DH_DEVID (0x33) //chip id +#define LIS3DH_ACC_DISABLE (0x08) + +#define LIS3DH_RANGE 2000000 + +/* LIS3DH */ +#define LIS3DH_PRECISION 16 +#define LIS3DH_BOUNDARY (0x1 << (LIS3DH_PRECISION - 1)) +#define LIS3DH_GRAVITY_STEP (LIS3DH_RANGE / LIS3DH_BOUNDARY) + +#define ODR1 0x10 /* 1Hz output data rate */ +#define ODR10 0x20 /* 10Hz output data rate */ +#define ODR25 0x30 /* 25Hz output data rate */ +#define ODR50 0x40 /* 50Hz output data rate */ +#define ODR100 0x50 /* 100Hz output data rate */ +#define ODR200 0x60 /* 200Hz output data rate */ +#define ODR400 0x70 /* 400Hz output data rate */ +#define ODR1250 0x90 /* 1250Hz output data rate */ + + + +struct sensor_reg_data { + char reg; + char data; +}; + +/****************operate according to sensor chip:start************/ + +static int sensor_active(struct i2c_client *client, int enable, int rate) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + int result = 0; + int status = 0; + + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + + sensor->ops->ctrl_data |= ODR100; //100HZ,if 0 then power down + + //register setting according to chip datasheet + if(!enable) + { + status = LIS3DH_ACC_DISABLE; //lis3dh + sensor->ops->ctrl_data |= status; + } + else + { + status = ~LIS3DH_ACC_DISABLE; //lis3dh + sensor->ops->ctrl_data &= status; + } + + DBG("%s:reg=0x%x,reg_ctrl=0x%x,enable=%d\n",__func__,sensor->ops->ctrl_reg, sensor->ops->ctrl_data, enable); + result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); + if(result) + printk("%s:fail to active sensor\n",__func__); + + return result; + +} + +static int sensor_init(struct i2c_client *client) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + int result = 0; + int i; + struct sensor_reg_data reg_data[] = + { + {LIS3DH_CTRL_REG2,0X00}, + {LIS3DH_CTRL_REG4,0x08}, //High resolution output mode: 1, Normal mode + {LIS3DH_CTRL_REG6,0x40}, + {LIS3DH_TEMP_CFG_REG,0x00}, // + {LIS3DH_FIFO_CTRL_REG,0x00}, // + {LIS3DH_INT1_CFG,0xFF}, //6 direction position recognition + {LIS3DH_INT1_THS,0x7F}, //Interrupt 1 threshold + {LIS3DH_INT1_DURATION,0x7F}, //Duration value 0x00->ox7f + }; + + result = sensor->ops->active(client,0,0); + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + + sensor->status_cur = SENSOR_OFF; + + for(i=0;i<(sizeof(reg_data)/sizeof(struct sensor_reg_data));i++) + { + result = sensor_write_reg(client, reg_data[i].reg, reg_data[i].data); + if(result) + { + printk("%s:line=%d,i=%d,error\n",__func__,__LINE__,i); + return result; + } + } + + + if(sensor->pdata->irq_enable) + { + + result = sensor_write_reg(client, LIS3DH_CTRL_REG3, 0x40);//I1_AOI1 =1 if motion + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + + result = sensor_write_reg(client, LIS3DH_CTRL_REG5, 0x08); + if(result) + { + printk("%s:line=%d,error\n",__func__,__LINE__); + return result; + } + + } + + return result; +} + +static int sensor_convert_data(struct i2c_client *client, char high_byte, char low_byte) +{ + s64 result; + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + //int precision = sensor->ops->precision; + switch (sensor->devid) { + case LIS3DH_DEVID: + result = ((int)high_byte << 8) | (int)low_byte; + if (result < LIS3DH_BOUNDARY) + result = result* LIS3DH_GRAVITY_STEP; + else + result = ~( ((~result & (0x7fff>>(16-LIS3DH_PRECISION)) ) + 1) + * LIS3DH_GRAVITY_STEP) + 1; + break; + + default: + printk(KERN_ERR "%s: devid wasn't set correctly\n",__func__); + return -EFAULT; + } + + return (int)result; +} + +static int angle_report_value(struct i2c_client *client, struct sensor_axis *axis) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + + /* Report acceleration sensor information */ + input_report_abs(sensor->input_dev, ABS_X, axis->x); + input_report_abs(sensor->input_dev, ABS_Y, axis->y); + input_report_abs(sensor->input_dev, ABS_Z, axis->z); + input_sync(sensor->input_dev); + DBG("Gsensor x==%d y==%d z==%d\n",axis->x,axis->y,axis->z); + + return 0; +} + +#define GSENSOR_MIN 10 +static int sensor_report_value(struct i2c_client *client) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + struct sensor_platform_data *pdata = sensor->pdata; + int ret = 0; + int x,y,z; + struct sensor_axis axis; + char buffer[6] = {0}; + char value = 0; + + if(sensor->ops->read_len < 6) //sensor->ops->read_len = 6 + { + printk("%s:lenth is error,len=%d\n",__func__,sensor->ops->read_len); + return -1; + } + + memset(buffer, 0, 6); + + value = sensor_read_reg(client, LIS3DH_STATUS_REG); + if((value & 0x0f) == 0) + { + printk("%s:line=%d,value=0x%x,data is not ready\n",__func__,__LINE__,value); + return -1; + } + + + /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ + do { + *buffer = sensor->ops->read_reg; + ret = sensor_rx_data(client, buffer, sensor->ops->read_len); + if (ret < 0) + return ret; + } while (0); + + //this angle need 6 bytes buffer + x = sensor_convert_data(sensor->client, buffer[1], buffer[0]); //buffer[1]:high bit + y = sensor_convert_data(sensor->client, buffer[3], buffer[2]); + z = sensor_convert_data(sensor->client, buffer[5], buffer[4]); + + 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; + + DBG( "%s: axis = %d %d %d \n", __func__, axis.x, axis.y, axis.z); + + //Report event only while value is changed to save some power + if((abs(sensor->axis.x - axis.x) > GSENSOR_MIN) || (abs(sensor->axis.y - axis.y) > GSENSOR_MIN) || (abs(sensor->axis.z - axis.z) > GSENSOR_MIN)) + { + angle_report_value(client, &axis); + + /* »¥³âµØ»º´æÊý¾Ý. */ + mutex_lock(&(sensor->data_mutex) ); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex) ); + } + + if((sensor->pdata->irq_enable)&& (sensor->ops->int_status_reg >= 0)) //read sensor intterupt status register + { + + value = sensor_read_reg(client, sensor->ops->int_status_reg); + DBG("%s:sensor int status :0x%x\n",__func__,value); + } + + return ret; +} + +struct sensor_operate angle_lis3dh_ops = { + .name = "angle_lis3dh", + .type = SENSOR_TYPE_ANGLE, //sensor type and it should be correct + .id_i2c = ANGLE_ID_LIS3DH, //i2c id number + .read_reg = (LIS3DH_OUT_X_L | 0x80), //read data + .read_len = 6, //data length + .id_reg = LIS3DH_WHO_AM_I, //read device id from this register + .id_data = LIS3DH_DEVID, //device id + .precision = LIS3DH_PRECISION, //12 bits + .ctrl_reg = LIS3DH_CTRL_REG1, //enable or disable + .int_status_reg = LIS3DH_INT1_SRC, //intterupt status register + .range = {-LIS3DH_RANGE,LIS3DH_RANGE}, //range + .trig = (IRQF_TRIGGER_LOW|IRQF_ONESHOT), + .active = sensor_active, + .init = sensor_init, + .report = sensor_report_value, +}; + +/****************operate according to sensor chip:end************/ + +//function name should not be changed +static struct sensor_operate *angle_get_ops(void) +{ + return &angle_lis3dh_ops; +} + + +static int __init angle_lis3dh_init(void) +{ + struct sensor_operate *ops = angle_get_ops(); + int result = 0; + int type = ops->type; + result = sensor_register_slave(type, NULL, NULL, angle_get_ops); + return result; +} + +static void __exit angle_lis3dh_exit(void) +{ + struct sensor_operate *ops = angle_get_ops(); + int type = ops->type; + sensor_unregister_slave(type, NULL, NULL, angle_get_ops); +} + + +module_init(angle_lis3dh_init); +module_exit(angle_lis3dh_exit); + + diff --git a/drivers/input/sensors/sensor-dev.c b/drivers/input/sensors/sensor-dev.c index a681199c1c21..037f9e9ee5e0 100755 --- a/drivers/input/sensors/sensor-dev.c +++ b/drivers/input/sensors/sensor-dev.c @@ -67,9 +67,9 @@ static ssize_t sensor_proc_write(struct file *file, const char __user *buffer, num = c - '0'; - printk("%s command list:close:%d, accel:%d, compass:%d, gyro:%d, light:%d, psensor:%d, temp:%d, pressure:%d,total:%d,num=%d\n",__func__, + printk("%s command list:close:%d,angle:%d accel:%d, compass:%d, gyro:%d, light:%d, psensor:%d, temp:%d, pressure:%d,total:%d,num=%d\n",__func__, - SENSOR_TYPE_NULL, SENSOR_TYPE_ACCEL,SENSOR_TYPE_COMPASS,SENSOR_TYPE_GYROSCOPE,SENSOR_TYPE_LIGHT,SENSOR_TYPE_PROXIMITY, + SENSOR_TYPE_NULL, SENSOR_TYPE_ANGLE, SENSOR_TYPE_ACCEL,SENSOR_TYPE_COMPASS,SENSOR_TYPE_GYROSCOPE,SENSOR_TYPE_LIGHT,SENSOR_TYPE_PROXIMITY, SENSOR_TYPE_TEMPERATURE,SENSOR_TYPE_PRESSURE,SENSOR_NUM_TYPES,num); @@ -347,7 +347,7 @@ static int sensor_irq_init(struct i2c_client *client) goto error; } client->irq = irq; - if((sensor->pdata->type == SENSOR_TYPE_GYROSCOPE) || (sensor->pdata->type == SENSOR_TYPE_ACCEL)) + if((sensor->pdata->type == SENSOR_TYPE_GYROSCOPE) || (sensor->pdata->type == SENSOR_TYPE_ACCEL) || (sensor->pdata->type == SENSOR_TYPE_ANGLE)) disable_irq_nosync(client->irq);//disable irq if(((sensor->pdata->type == SENSOR_TYPE_LIGHT) || (sensor->pdata->type == SENSOR_TYPE_PROXIMITY))&& (!(sensor->ops->trig & IRQF_SHARED))) disable_irq_nosync(client->irq);//disable irq @@ -389,6 +389,154 @@ static void sensor_resume(struct early_suspend *h) } #endif +static int angle_dev_open(struct inode *inode, struct file *file) +{ + //struct sensor_private_data *sensor = g_sensor[SENSOR_TYPE_ACCEL]; + //struct i2c_client *client = sensor->client; + + int result = 0; + + + return result; +} + + +static int angle_dev_release(struct inode *inode, struct file *file) +{ + //struct sensor_private_data *sensor = g_sensor[SENSOR_TYPE_ANGLE]; + //struct i2c_client *client = sensor->client; + + int result = 0; + + + return result; +} + +/* ioctl - I/O control */ +static long angle_dev_ioctl(struct file *file, + unsigned int cmd, unsigned long arg) +{ + struct sensor_private_data *sensor = g_sensor[SENSOR_TYPE_ANGLE]; + struct i2c_client *client = sensor->client; + void __user *argp = (void __user *)arg; + struct sensor_axis axis = {0}; + char rate; + int result = 0; + + switch (cmd) { + case GSENSOR_IOCTL_APP_SET_RATE: + if (copy_from_user(&rate, argp, sizeof(rate))) + { + result = -EFAULT; + goto error; + } + break; + default: + break; + } + + switch (cmd) { + case GSENSOR_IOCTL_START: + DBG("%s:GSENSOR_IOCTL_START start,status=%d\n", __func__,sensor->status_cur); + mutex_lock(&sensor->operation_mutex); + if(++sensor->start_count == 1) + { + if(sensor->status_cur == SENSOR_OFF) + { + atomic_set(&(sensor->data_ready), 0); + if ( (result = sensor->ops->active(client, 1, 0) ) < 0 ) { + mutex_unlock(&sensor->operation_mutex); + printk("%s:fail to active sensor,ret=%d\n",__func__,result); + goto error; + } + if(sensor->pdata->irq_enable) + { + DBG("%s:enable irq,irq=%d\n",__func__,client->irq); + enable_irq(client->irq); //enable irq + } + else + { + PREPARE_DELAYED_WORK(&sensor->delaywork, sensor_delaywork_func); + schedule_delayed_work(&sensor->delaywork, msecs_to_jiffies(sensor->pdata->poll_delay_ms)); + } + sensor->status_cur = SENSOR_ON; + } + } + mutex_unlock(&sensor->operation_mutex); + DBG("%s:GSENSOR_IOCTL_START OK\n", __func__); + break; + + case GSENSOR_IOCTL_CLOSE: + DBG("%s:GSENSOR_IOCTL_CLOSE start,status=%d\n", __func__,sensor->status_cur); + mutex_lock(&sensor->operation_mutex); + if(--sensor->start_count == 0) + { + if(sensor->status_cur == SENSOR_ON) + { + atomic_set(&(sensor->data_ready), 0); + if ( (result = sensor->ops->active(client, 0, 0) ) < 0 ) { + mutex_unlock(&sensor->operation_mutex); + goto error; + } + + if(sensor->pdata->irq_enable) + { + DBG("%s:disable irq,irq=%d\n",__func__,client->irq); + disable_irq_nosync(client->irq);//disable irq + } + else + cancel_delayed_work_sync(&sensor->delaywork); + sensor->status_cur = SENSOR_OFF; + } + + DBG("%s:GSENSOR_IOCTL_CLOSE OK\n", __func__); + } + + mutex_unlock(&sensor->operation_mutex); + break; + + case GSENSOR_IOCTL_APP_SET_RATE: + DBG("%s:GSENSOR_IOCTL_APP_SET_RATE start\n", __func__); + mutex_lock(&sensor->operation_mutex); + result = sensor_reset_rate(client, rate); + if (result < 0){ + mutex_unlock(&sensor->operation_mutex); + goto error; + } + + sensor->status_cur = SENSOR_ON; + mutex_unlock(&sensor->operation_mutex); + DBG("%s:GSENSOR_IOCTL_APP_SET_RATE OK\n", __func__); + break; + + case GSENSOR_IOCTL_GETDATA: + mutex_lock(&sensor->data_mutex); + memcpy(&axis, &sensor->axis, sizeof(sensor->axis)); //get data from buffer + mutex_unlock(&sensor->data_mutex); + break; + default: + result = -ENOTTY; + goto error; + } + + switch (cmd) { + case GSENSOR_IOCTL_GETDATA: + if ( copy_to_user(argp, &axis, sizeof(axis) ) ) { + printk("failed to copy sense data to user space."); + result = -EFAULT; + goto error; + } + DBG("%s:GSENSOR_IOCTL_GETDATA OK\n", __func__); + break; + default: + break; + } + +error: + return result; +} + + static int gsensor_dev_open(struct inode *inode, struct file *file) { //struct sensor_private_data *sensor = g_sensor[SENSOR_TYPE_ACCEL]; @@ -1279,6 +1427,26 @@ static int sensor_misc_device_register(struct sensor_private_data *sensor, int t switch(type) { + case SENSOR_TYPE_ANGLE: + if(!sensor->ops->misc_dev) + { + sensor->fops.owner = THIS_MODULE; + sensor->fops.unlocked_ioctl = angle_dev_ioctl; + sensor->fops.open = angle_dev_open; + sensor->fops.release = angle_dev_release; + + sensor->miscdev.minor = MISC_DYNAMIC_MINOR; + sensor->miscdev.name = "angle"; + sensor->miscdev.fops = &sensor->fops; + } + else + { + memcpy(&sensor->miscdev, sensor->ops->misc_dev, sizeof(*sensor->ops->misc_dev)); + + } + + break; + case SENSOR_TYPE_ACCEL: if(!sensor->ops->misc_dev) { @@ -1572,6 +1740,17 @@ int sensor_probe(struct i2c_client *client, const struct i2c_device_id *devid) switch(type) { + case SENSOR_TYPE_ANGLE: + sensor->input_dev->name = "angle"; + set_bit(EV_ABS, sensor->input_dev->evbit); + /* x-axis acceleration */ + input_set_abs_params(sensor->input_dev, ABS_X, sensor->ops->range[0], sensor->ops->range[1], 0, 0); //2g full scale range + /* y-axis acceleration */ + input_set_abs_params(sensor->input_dev, ABS_Y, sensor->ops->range[0], sensor->ops->range[1], 0, 0); //2g full scale range + /* z-axis acceleration */ + input_set_abs_params(sensor->input_dev, ABS_Z, sensor->ops->range[0], sensor->ops->range[1], 0, 0); //2g full scale range + break; + case SENSOR_TYPE_ACCEL: sensor->input_dev->name = "gsensor"; set_bit(EV_ABS, sensor->input_dev->evbit); @@ -1740,6 +1919,9 @@ static int sensor_remove(struct i2c_client *client) } static const struct i2c_device_id sensor_id[] = { + /*angle*/ + {"angle_kxtik", ANGLE_ID_KXTIK}, + {"angle_lis3dh", ANGLE_ID_LIS3DH}, /*gsensor*/ {"gsensor", ACCEL_ID_ALL}, {"gs_mma8452", ACCEL_ID_MMA845X}, diff --git a/include/linux/sensor-dev.h b/include/linux/sensor-dev.h index 863bd538b10e..582994c49834 100755 --- a/include/linux/sensor-dev.h +++ b/include/linux/sensor-dev.h @@ -25,7 +25,8 @@ #define SENSOR_UNKNOW_DATA -1 enum sensor_type { - SENSOR_TYPE_NULL, + SENSOR_TYPE_NULL, + SENSOR_TYPE_ANGLE, SENSOR_TYPE_ACCEL, SENSOR_TYPE_COMPASS, SENSOR_TYPE_GYROSCOPE, @@ -38,7 +39,11 @@ enum sensor_type { enum sensor_id { ID_INVALID = 0, - + + ANGLE_ID_ALL, + ANGLE_ID_KXTIK, + ANGLE_ID_LIS3DH, + ACCEL_ID_ALL, ACCEL_ID_LIS331, ACCEL_ID_LSM303DLX,