From 7f1ae55f96671c5db8191e87a00358edcdb3e013 Mon Sep 17 00:00:00 2001 From: Zorro Liu Date: Tue, 14 Nov 2017 11:57:07 +0800 Subject: [PATCH] driver: input: sensor: gyro: add new and to pass vts 1.add mpu6500 gyro driver 2.mpu6500 and mpu6880 to pass vts Change-Id: I7a18578847e92c2cacd8d9b545455840b3a7b318 Signed-off-by: Zorro Liu --- drivers/input/sensors/gyro/Kconfig | 6 +- drivers/input/sensors/gyro/Makefile | 3 +- drivers/input/sensors/gyro/mpu6500_gyro.c | 195 ++++++++++++++++++++++ drivers/input/sensors/gyro/mpu6880_gyro.c | 146 ++++++++-------- drivers/input/sensors/sensor-dev.c | 11 +- include/linux/l3g4200d.h | 1 + include/linux/sensor-dev.h | 1 + 7 files changed, 293 insertions(+), 70 deletions(-) create mode 100644 drivers/input/sensors/gyro/mpu6500_gyro.c mode change 100755 => 100644 include/linux/l3g4200d.h diff --git a/drivers/input/sensors/gyro/Kconfig b/drivers/input/sensors/gyro/Kconfig index 88034e778444..481561061559 100755 --- a/drivers/input/sensors/gyro/Kconfig +++ b/drivers/input/sensors/gyro/Kconfig @@ -23,7 +23,11 @@ config GYRO_L3G20D config GYRO_EWTSA bool "gyroscope ewtsa" default y - + +config GYRO_MPU6500 + bool "gyroscope mpu6500_gyro" + default n + config GYRO_MPU6880 bool "gyroscope mpu6880_gyro" default y diff --git a/drivers/input/sensors/gyro/Makefile b/drivers/input/sensors/gyro/Makefile index bee9fbb3a215..09de32cac69f 100755 --- a/drivers/input/sensors/gyro/Makefile +++ b/drivers/input/sensors/gyro/Makefile @@ -4,5 +4,6 @@ obj-$(CONFIG_GYRO_SENSOR_K3G) += k3g.o obj-$(CONFIG_GYRO_L3G4200D) += l3g4200d.o obj-$(CONFIG_GYRO_L3G20D) += l3g20d.o obj-$(CONFIG_GYRO_EWTSA) += ewtsa.o +obj-$(CONFIG_GYRO_MPU6500) += mpu6500_gyro.o obj-$(CONFIG_GYRO_MPU6880) += mpu6880_gyro.o -obj-$(CONFIG_GYRO_LSM330) += lsm330_gyro.o \ No newline at end of file +obj-$(CONFIG_GYRO_LSM330) += lsm330_gyro.o diff --git a/drivers/input/sensors/gyro/mpu6500_gyro.c b/drivers/input/sensors/gyro/mpu6500_gyro.c new file mode 100644 index 000000000000..4464c20ffebb --- /dev/null +++ b/drivers/input/sensors/gyro/mpu6500_gyro.c @@ -0,0 +1,195 @@ +/* drivers/input/sensors/access/mpu6880_gyro.c + * + * Copyright (C) 2012-2015 ROCKCHIP. + * Author: ouenhui + * + * 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 +#ifdef CONFIG_HAS_EARLYSUSPEND +#include +#endif +#include +#include + +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; + u8 pwrm1 = 0; + + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + pwrm1 = sensor_read_reg(client, MPU6500_PWR_MGMT_1); + + if (!enable) { + status = BIT_GYRO_STBY; + sensor->ops->ctrl_data |= status; + if ((sensor->ops->ctrl_data & BIT_ACCEL_STBY) == BIT_ACCEL_STBY) { + pwrm1 |= MPU6500_PWRM1_SLEEP; + } + } else { + status = ~BIT_GYRO_STBY; + sensor->ops->ctrl_data &= status; + pwrm1 &= ~MPU6500_PWRM1_SLEEP; + } + + result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); + if (result) { + dev_err(&client->dev, "%s:fail to active sensor\n", __func__); + return -1; + } + msleep(20); + + result = sensor_write_reg(client, MPU6500_PWR_MGMT_1, pwrm1); + if (result) { + dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__); + return -1; + } + msleep(50); + + return result; +} + +static int sensor_init(struct i2c_client *client) +{ + int ret; + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + + /* init on mpu6500_acc.c */ + ret = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms); + if (ret) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); + return ret; + } + + return ret; +} + +static int gyro_report_value(struct i2c_client *client, struct sensor_axis *axis) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *) i2c_get_clientdata(client); + + if (sensor->status_cur == SENSOR_ON) { + /* Report gyro sensor information */ + input_report_rel(sensor->input_dev, ABS_RX, axis->x); + input_report_rel(sensor->input_dev, ABS_RY, axis->y); + input_report_rel(sensor->input_dev, ABS_RZ, axis->z); + input_sync(sensor->input_dev); + } + + return 0; +} + +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; + short x, y, z; + struct sensor_axis axis; + u8 buffer[6] = {0}; + char value = 0; + + if (sensor->ops->read_len < 6) { + dev_err(&client->dev, "%s:lenth is error,len=%d\n", __func__, sensor->ops->read_len); + return -1; + } + + memset(buffer, 0, 6); + + do { + *buffer = sensor->ops->read_reg; + ret = sensor_rx_data(client, buffer, sensor->ops->read_len); + if (ret < 0) + return ret; + } while (0); + + x = ((buffer[0] << 8) & 0xFF00) + (buffer[1] & 0xFF); + y = ((buffer[2] << 8) & 0xFF00) + (buffer[3] & 0xFF); + z = ((buffer[4] << 8) & 0xFF00) + (buffer[5] & 0xFF); + + 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; + + gyro_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)) + value = sensor_read_reg(client, sensor->ops->int_status_reg); + + return ret; +} + +struct sensor_operate gyro_mpu6500_ops = { + .name = "mpu6500_gyro", + .type = SENSOR_TYPE_GYROSCOPE, + .id_i2c = GYRO_ID_MPU6500, + .read_reg = MPU6500_GYRO_XOUT_H, + .read_len = 6, + .id_reg = SENSOR_UNKNOW_DATA, + .id_data = SENSOR_UNKNOW_DATA, + .precision = MPU6500_PRECISION, + .ctrl_reg = MPU6500_PWR_MGMT_2, + .int_status_reg = MPU6500_INT_STATUS, + .range = {-32768, 32768}, + .trig = IRQF_TRIGGER_HIGH |IRQF_ONESHOT, + .active = sensor_active, + .init = sensor_init, + .report = sensor_report_value, +}; + +/****************operate according to sensor chip:end************/ + +static struct sensor_operate *gyro_get_ops(void) +{ + return &gyro_mpu6500_ops; +} + +static int __init gyro_mpu6500_init(void) +{ + struct sensor_operate *ops = gyro_get_ops(); + int type = ops->type; + + return sensor_register_slave(type, NULL, NULL, gyro_get_ops); +} + +static void __exit gyro_mpu6500_exit(void) +{ + struct sensor_operate *ops = gyro_get_ops(); + int type = ops->type; + + sensor_unregister_slave(type, NULL, NULL, gyro_get_ops); +} + +/* must register after mpu6500_acc */ +device_initcall_sync(gyro_mpu6500_init); +module_exit(gyro_mpu6500_exit); diff --git a/drivers/input/sensors/gyro/mpu6880_gyro.c b/drivers/input/sensors/gyro/mpu6880_gyro.c index 4a94b4c0953e..883bc24b7d2e 100644 --- a/drivers/input/sensors/gyro/mpu6880_gyro.c +++ b/drivers/input/sensors/gyro/mpu6880_gyro.c @@ -35,56 +35,71 @@ 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); + (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); + u8 pwrm1 = 0; - if(!enable) - { - status = BIT_GYRO_STBY; - sensor->ops->ctrl_data |= status; - } - else - { - status = ~BIT_GYRO_STBY; + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + pwrm1 = sensor_read_reg(client, MPU6880_PWR_MGMT_1); + + if (!enable) { + status = BIT_GYRO_STBY; + sensor->ops->ctrl_data |= status; + if ((sensor->ops->ctrl_data & BIT_ACCEL_STBY) == BIT_ACCEL_STBY) { + pwrm1 |= MPU6880_PWRM1_SLEEP; + } + } else { + status = ~BIT_GYRO_STBY; sensor->ops->ctrl_data &= status; + pwrm1 &= ~MPU6880_PWRM1_SLEEP; } 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; + if (result) { + dev_err(&client->dev, "%s:fail to active sensor\n", __func__); + return -1; + } + msleep(20); + result = sensor_write_reg(client, MPU6880_PWR_MGMT_1, pwrm1); + if (result) { + dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__); + return -1; + } + msleep(50); + + return result; } static int sensor_init(struct i2c_client *client) { int ret; struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); - //已经在mpu6880_acc中初始化寄存器 - ret = sensor->ops->active(client,0,0); - if(ret) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + (struct sensor_private_data *) i2c_get_clientdata(client); + + /* init on mpu6500_acc.c */ + ret = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms); + if (ret) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return ret; - } + } + return ret; } static int gyro_report_value(struct i2c_client *client, struct sensor_axis *axis) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); - /* Report acceleration sensor information */ - input_report_rel(sensor->input_dev, ABS_RX, axis->x); - input_report_rel(sensor->input_dev, ABS_RY, axis->y); - input_report_rel(sensor->input_dev, ABS_RZ, axis->z); - input_sync(sensor->input_dev); - DBG("Gsensor x==%d y==%d z==%d\n",axis->x,axis->y,axis->z); + if (sensor->status_cur == SENSOR_ON) { + /* Report gyro sensor information */ + input_report_rel(sensor->input_dev, ABS_RX, axis->x); + input_report_rel(sensor->input_dev, ABS_RY, axis->y); + input_report_rel(sensor->input_dev, ABS_RZ, axis->z); + input_sync(sensor->input_dev); + } return 0; } @@ -92,71 +107,68 @@ static int gyro_report_value(struct i2c_client *client, struct sensor_axis *axis static int sensor_report_value(struct i2c_client *client) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); struct sensor_platform_data *pdata = sensor->pdata; int ret = 0; - short x,y,z; + short x, y, z; struct sensor_axis axis; - u8 buffer[6] = {0}; + u8 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); + + if (sensor->ops->read_len < 6) { + dev_err(&client->dev, "%s:lenth is error,len=%d\n", __func__, sensor->ops->read_len); return -1; } - + memset(buffer, 0, 6); - + do { *buffer = sensor->ops->read_reg; ret = sensor_rx_data(client, buffer, sensor->ops->read_len); if (ret < 0) - return ret; + return ret; } while (0); x = ((buffer[0] << 8) & 0xFF00) + (buffer[1] & 0xFF); y = ((buffer[2] << 8) & 0xFF00) + (buffer[3] & 0xFF); z = ((buffer[4] << 8) & 0xFF00) + (buffer[5] & 0xFF); - //printk("mpu6880_gyro: x:%d,y:%d,z:%d\n",x,y,z); - 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; + + 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; gyro_report_value(client, &axis); - if((sensor->pdata->irq_enable)&& (sensor->ops->int_status_reg >= 0)) //read sensor intterupt status register - { - + mutex_lock(&(sensor->data_mutex)); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex)); + + if ((sensor->pdata->irq_enable) && (sensor->ops->int_status_reg >= 0)) 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 gyro_mpu6880_ops = { - .name = "mpu6880_gyro", - .type = SENSOR_TYPE_GYROSCOPE, //sensor type and it should be correct - .id_i2c = GYRO_ID_MPU6880, //i2c id number - .read_reg = MPU6880_GYRO_XOUT_H, //read data - .read_len = 6, //data length - .id_reg = SENSOR_UNKNOW_DATA, //read device id from this register - .id_data = SENSOR_UNKNOW_DATA, //device id - .precision = MPU6880_PRECISION, //16 bit - .ctrl_reg = MPU6880_PWR_MGMT_2, //enable or disable - .int_status_reg = MPU6880_INT_STATUS, //intterupt status register - .range = {-MPU6880_RANGE,MPU6880_RANGE}, //range - .trig = IRQF_TRIGGER_HIGH |IRQF_ONESHOT, - .active = sensor_active, + .name = "mpu6880_gyro", + .type = SENSOR_TYPE_GYROSCOPE, + .id_i2c = GYRO_ID_MPU6880, + .read_reg = MPU6880_GYRO_XOUT_H, + .read_len = 6, + .id_reg = SENSOR_UNKNOW_DATA, + .id_data = SENSOR_UNKNOW_DATA, + .precision = MPU6880_PRECISION, + .ctrl_reg = MPU6880_PWR_MGMT_2, + .int_status_reg = MPU6880_INT_STATUS, + .range = {-32768, 32768}, + .trig = IRQF_TRIGGER_HIGH |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 *gyro_get_ops(void) { return &gyro_mpu6880_ops; @@ -166,20 +178,20 @@ static struct sensor_operate *gyro_get_ops(void) static int __init gyro_mpu6880_init(void) { struct sensor_operate *ops = gyro_get_ops(); - int result = 0; int type = ops->type; - result = sensor_register_slave(type, NULL, NULL, gyro_get_ops); - return result; + + return sensor_register_slave(type, NULL, NULL, gyro_get_ops); } static void __exit gyro_mpu6880_exit(void) { struct sensor_operate *ops = gyro_get_ops(); int type = ops->type; + sensor_unregister_slave(type, NULL, NULL, gyro_get_ops); } -//后于mpu6880_acc注册 +/* must register after mpu6880_acc */ device_initcall_sync(gyro_mpu6880_init); module_exit(gyro_mpu6880_exit); diff --git a/drivers/input/sensors/sensor-dev.c b/drivers/input/sensors/sensor-dev.c index 7bf4c3f99b18..65bdad2f4347 100644 --- a/drivers/input/sensors/sensor-dev.c +++ b/drivers/input/sensors/sensor-dev.c @@ -916,6 +916,8 @@ static long gyro_dev_ioctl(struct file *file, int result = 0; int rate; + wait_event_interruptible(sensor->is_factory_ok, (atomic_read(&sensor->is_factory) == 0)); + switch (cmd) { case L3G4200D_IOCTL_GET_ENABLE: result = !sensor->status_cur; @@ -959,7 +961,13 @@ static long gyro_dev_ioctl(struct file *file, } mutex_unlock(&sensor->operation_mutex); break; - + case L3G4200D_IOCTL_GET_CALIBRATION: + if (copy_to_user(argp, sensor->pdata->gyro_offset, sizeof(sensor->pdata->gyro_offset))) { + dev_err(&client->dev, "failed to copy gyro offset data to user\n"); + result = -EFAULT; + goto error; + } + break; default: return -ENOTTY; } @@ -1871,6 +1879,7 @@ static const struct i2c_device_id sensor_id[] = { {"l3g20d_gyro", GYRO_ID_L3G20D}, {"ewtsa_gyro", GYRO_ID_EWTSA}, {"k3g", GYRO_ID_K3G}, + {"mpu6500_gyro", GYRO_ID_MPU6500}, {"mpu6880_gyro", GYRO_ID_MPU6880}, {"lsm330_gyro", GYRO_ID_LSM330}, /*light sensor*/ diff --git a/include/linux/l3g4200d.h b/include/linux/l3g4200d.h old mode 100755 new mode 100644 index c040b5121fe4..fa82a58f44c6 --- a/include/linux/l3g4200d.h +++ b/include/linux/l3g4200d.h @@ -49,6 +49,7 @@ #define L3G4200D_IOCTL_GET_DELAY _IOR(L3G4200D_IOCTL_BASE, 1, int) #define L3G4200D_IOCTL_SET_ENABLE _IOW(L3G4200D_IOCTL_BASE, 2, int) #define L3G4200D_IOCTL_GET_ENABLE _IOR(L3G4200D_IOCTL_BASE, 3, int) +#define L3G4200D_IOCTL_GET_CALIBRATION _IOR(L3G4200D_IOCTL_BASE, 4, int[3]) #define L3G4200D_FS_250DPS 0x00 #define L3G4200D_FS_500DPS 0x10 diff --git a/include/linux/sensor-dev.h b/include/linux/sensor-dev.h index a2b809479170..3ce3577b22ea 100644 --- a/include/linux/sensor-dev.h +++ b/include/linux/sensor-dev.h @@ -80,6 +80,7 @@ enum sensor_id { GYRO_ID_L3G20D, GYRO_ID_EWTSA, GYRO_ID_K3G, + GYRO_ID_MPU6500, GYRO_ID_MPU6880, GYRO_ID_LSM330, LIGHT_ID_ALL,