驱动开发：sensor3

最新推荐文章于 2024-07-23 13:48:24 发布

_Fortunate?

最新推荐文章于 2024-07-23 13:48:24 发布

阅读量128

点赞数 3

文章标签：驱动开发

本文链接：https://blog.csdn.net/qq_45696114/article/details/140631122

版权

此文补充一下上两篇sensor相关文章的一个点

后续提交时我把先进先出的队列改成标志位判断上报

以及假如在采集到相同数据对数据进行更改，避开hal层的丢帧处理

给自己做个记录，代码如下：

struct sensor_cur_data {
    int value[3];
	int last_value[3];
	bool ready_flag;
	spinlock_t	ts_lock;
};

/* Platform data for the sensor */
struct sensor_private_data {
	int type;
	struct i2c_client *client;
	struct input_dev *input_dev;
	int stop_work;
	struct delayed_work delaywork;
	struct sensor_axis axis;
	char sensor_data[40];
	atomic_t is_factory;
	wait_queue_head_t is_factory_ok;
	struct mutex data_mutex;
	struct mutex operation_mutex;
	struct mutex sensor_mutex;
	struct mutex i2c_mutex;
	int status_cur;
	int start_count;
	int devid;
	struct sensor_flag flags;
	struct i2c_device_id *i2c_id;
	struct sensor_platform_data *pdata;
	struct sensor_operate *ops;
	struct file_operations fops;
	struct miscdevice miscdev;
	struct hrtimer		input_timer_accel;
	struct hrtimer		input_timer_gyro;
	bool hirtimer_enable;
	s64	 timer_thres;
	s64	 pre_ts_sensor;
	struct sensor_cur_data *sensor_cur_data;

#ifdef CONFIG_HAS_EARLYSUSPEND
	struct early_suspend early_suspend;
#endif
};

/* drivers/input/sensors/access/qmi8658_acc.c
 *
 * Copyright (C) 2012-2015 ROCKCHIP.
 * Author: oeh<oeh@rock-chips.com>
 *
 * 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 <linux/interrupt.h>
#include <linux/i2c.h>
#include <linux/slab.h>
#include <linux/irq.h>
#include <linux/miscdevice.h>
#include <linux/gpio.h>
#include <linux/uaccess.h>
#include <asm/atomic.h>
#include <linux/delay.h>
#include <linux/input.h>
#include <linux/workqueue.h>
#include <linux/freezer.h>
#include <linux/of_gpio.h>
#ifdef CONFIG_HAS_EARLYSUSPEND
#include <linux/earlysuspend.h>
#endif
#include <linux/sensor-dev.h>
#include <linux/qmi8658.h>
#include <linux/module.h>
#include <linux/jiffies.h>
#include <linux/of_device.h>
#include <linux/hrtimer.h> 
#include <linux/time64.h>
// #include <linux/kfifo.h>

/* get time */
static s64 get_time_ns(void)
{
	struct timespec64 ts;
	ktime_get_ts64(&ts);
	return timespec64_to_ns(&ts);
}
/* sensor set rate */
static int qmi8658_set_rate(struct i2c_client *client, int rate)
{
	const short hz[] = {448, 224, 112, 56, 28};
	const int   d[] = {0x24, 0x25,0x26, 0x27, 0x28};
	int i =0, data =0, result =0;

	/* always use poll mode, no need to set rate */
	return 0;

	if ((rate < 1) || (rate > 480))
		return -1;

	while ((rate > hz[i]) && (i < 5))
		i++;
	data = d[i];

	result = sensor_write_reg(client, QMI8658_REG_CTRL2, data);
	if (result)
		return -1;

	return 0;
}

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;
	u8 databuf[2] = {0};

	databuf[0] = sensor_read_reg(client, QMI8658_REG_CTRL7);

	if (!enable) {
		if(sensor->hirtimer_enable ){
			hrtimer_cancel(&sensor->input_timer_accel);
		}
		sensor->hirtimer_enable  = false;
		databuf[0] = (databuf[0] & 0xFE) | 0x80; //accel disable;		
	} else {
		databuf[0] = (databuf[0] & 0xFE) | 0x81; //accel enable;
		qmi8658_set_rate(client, rate);
		printk("qmi8658 user set rate %dms,timer_thres = %dms\n",rate,sensor->timer_thres/1000000);		
		sensor->pre_ts_sensor = get_time_ns();
		if(sensor->hirtimer_enable  != true){
			sensor->hirtimer_enable  = true;
			hrtimer_start(&sensor->input_timer_accel, ktime_set(0, TIMER_INTERVAL), HRTIMER_MODE_ABS_PINNED);
		}
	}

	result = sensor_write_reg(client, QMI8658_REG_CTRL7, databuf[0]);
	if (result) {
		dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__);
		return -1;
	}
	dev_err(&client->dev, "zhoucs %s:activer OK, enable=%d, result=%d\n", __func__, enable, result);
	return result;
}
/* sensor init */
static int sensor_init(struct i2c_client *client)
{
	int res = 0;
	u8 read_data = 0,reg_value = 0;

	struct sensor_private_data *sensor =
	    (struct sensor_private_data *) i2c_get_clientdata(client);

	read_data = sensor_read_reg(client, sensor->ops->id_reg);

	if (read_data != sensor->ops->id_data) {
		dev_err(&client->dev, "%s:check id err,read_data:%d,ops->id_data:%d\n", 
				__func__, read_data, sensor->ops->id_data);
		return -1;
	}else{
		dev_err(&client->dev, "%s:check id success -->qmi8658\n", __func__);
	}

	res = sensor_write_reg(client, QMI8658_REG_RESET, QMI8658_SW_RESET);
	if (res) {
		dev_err(&client->dev, "set QMI8658_SW_RESET error,res: %d!\n", res);
		return res;
	}
	msleep(15);

	res = sensor_write_reg(client, QMI8658_REG_CTRL1, QMI8658_ADDR_AI_BE);
	if (res) {
		dev_err(&client->dev, "set QMI8658_REG_CTRL1 error,res: %d!\n", res);
		return res;
	}

	reg_value = QMI8658_ACC_RANGE_DEF|QMI8658_ODR_59HZ_REG_VALUE;// ±8g,odr=59hz
	res = sensor_write_reg(client, QMI8658_REG_CTRL2, reg_value);
	if (res) {
		dev_err(&client->dev, "set QMI8658_REG_CTRL2 error,res: %d!\n", res);
		return res;
	}

	reg_value = QMI8658_GYR_RANGE_2048DPS|QMI8658_ODR_59HZ_REG_VALUE;//2048dps,odr=59hz
	res = sensor_write_reg(client, QMI8658_REG_CTRL3, reg_value);
	if (res) {
		dev_err(&client->dev, "set QMI8658_REG_CTRL3 error,res: %d!\n", res);
		return res;
	}

	res = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms);
	if (res) {
		dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__);
		return res;
	}
	return res;
}

/* sensor report value */
static int gsensor_report_value(struct i2c_client *client)
{
    struct sensor_private_data *sensor =
        (struct sensor_private_data *) i2c_get_clientdata(client);
    int ret = 0;
    if (sensor->status_cur == SENSOR_ON) {
        /* Report gyroeration sensor information */
		spin_lock(&sensor->sensor_cur_data->ts_lock);
		// printk("qmi8658 input_report_abs accel x=%d,y=%d,z=%d\n",sensor->sensor_cur_data->value[0],sensor->sensor_cur_data->value[1],sensor->sensor_cur_data->value[2]);
        input_report_abs(sensor->input_dev, ABS_X, sensor->sensor_cur_data->value[0]);
        input_report_abs(sensor->input_dev, ABS_Y, sensor->sensor_cur_data->value[1]);
        input_report_abs(sensor->input_dev, ABS_Z, sensor->sensor_cur_data->value[2]);
        input_sync(sensor->input_dev);
		sensor->sensor_cur_data->ready_flag = false;
		spin_unlock(&sensor->sensor_cur_data->ts_lock);
    }
    return ret;
}

/*sensor hrtimer init and report fifo array data*/
static enum hrtimer_restart input_timer_accel_callback(struct hrtimer *timer)
{
	struct sensor_private_data *sensor = container_of(timer, struct sensor_private_data, input_timer_accel);
	s64 cur_ts, delta_ts;
	static int flag_report = 1;/*Prevent failure to retrieve data twice*/
	cur_ts = get_time_ns();
	delta_ts = cur_ts - sensor->pre_ts_sensor;
	hrtimer_forward_now(timer, ktime_set(0, TIMER_INTERVAL));
	if(delta_ts > sensor->timer_thres)
	{
		if(sensor->sensor_cur_data->ready_flag == false)
		{
			if(flag_report){
				spin_lock(&sensor->sensor_cur_data->ts_lock);
				sensor->sensor_cur_data->value[0] = sensor->sensor_cur_data->last_value[0]+1;
				sensor->sensor_cur_data->value[1] = sensor->sensor_cur_data->last_value[1]+1;
				sensor->sensor_cur_data->value[2] = sensor->sensor_cur_data->last_value[2]+1;
				spin_unlock(&sensor->sensor_cur_data->ts_lock);
				flag_report = 0;
			}else{
				spin_lock(&sensor->sensor_cur_data->ts_lock);
				sensor->sensor_cur_data->value[0] = sensor->sensor_cur_data->last_value[0]-1;
				sensor->sensor_cur_data->value[1] = sensor->sensor_cur_data->last_value[1]-1;
				sensor->sensor_cur_data->value[2] = sensor->sensor_cur_data->last_value[2]-1;
				spin_unlock(&sensor->sensor_cur_data->ts_lock);
				flag_report = 1;
			}
			printk("qmi8658 use last data\n");
		}else{
			spin_lock(&sensor->sensor_cur_data->ts_lock);
			sensor->sensor_cur_data->last_value[0] = sensor->sensor_cur_data->value[0];
			sensor->sensor_cur_data->last_value[1] = sensor->sensor_cur_data->value[1];
			sensor->sensor_cur_data->last_value[2] = sensor->sensor_cur_data->value[2];
			spin_unlock(&sensor->sensor_cur_data->ts_lock);
		}
		gsensor_report_value(sensor->client);
		 sensor->pre_ts_sensor = cur_ts;
		pr_debug("%s, %lld\n", __func__, cur_ts);
	}
	return HRTIMER_RESTART;
}

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;
	static int flag = 1;
	u8 buffer[6] = {0};
	// char value = 0, count = 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);

	/* Data bytes from hardware xL, xH, yL, yH, zL, zH */
	// while(((value & 0x03)!= 0x03)&&(count++ < 3)){
	// 	value = sensor_read_reg(client, QMI8658_REG_STATUSINT);
	// }
	*buffer = QMI8658_REG_GYR_XOUT_LSB;
	ret = sensor_rx_data(client, buffer, sensor->ops->read_len);
	*buffer = sensor->ops->read_reg;
	ret = sensor_rx_data(client, buffer, sensor->ops->read_len);
	if (ret < 0)
		return ret;

	x = ((buffer[1] << 8) & 0xFF00) + (buffer[0] & 0xFF);
	y = ((buffer[3] << 8) & 0xFF00) + (buffer[2] & 0xFF);
	z = ((buffer[5] << 8) & 0xFF00) + (buffer[4] & 0xFF);
	//dev_err(&client->dev, "raw_acc=%4d,%4d,%4d\n", (signed short)x, (signed short)y, (signed short)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;
	/*
	*input dev will ignore report data if data value is the same with 
	last_value,
	*sample rate will not enough by this way, so just avoid this case
	*/
	if ((sensor->axis.x == axis.x) && (sensor->axis.y == axis.y) && 
		(sensor->axis.z == axis.z)) {
		if (flag) {
			flag = 0;
			axis.x += 1;
			axis.y += 1;
			axis.z += 1;
			dev_err(&client->dev, "%s:same data1 line=%d,error\n", __func__, __LINE__);
		} else {
			flag = 1;
			axis.x -= 1;
			axis.y -= 1;
			axis.z -= 1;
			dev_err(&client->dev, "%s:same data2 line=%d,error\n", __func__, __LINE__);
		}
	}
	if (!sensor->sensor_cur_data) {
		dev_err(&client->dev, "%s: qmi8658 sensor_cur_data is NULL\n", __func__);
		return -EINVAL;
	}
	spin_lock(&sensor->sensor_cur_data->ts_lock);
	sensor->sensor_cur_data->value[0] = axis.x;
	sensor->sensor_cur_data->value[1] = axis.y;
	sensor->sensor_cur_data->value[2] = axis.z;
	sensor->sensor_cur_data->ready_flag = true;
	spin_unlock(&sensor->sensor_cur_data->ts_lock);
	return ret;
}
/* init accel fifo and timer */
static int sensor_init_accel_fifo_and_timer(struct i2c_client *client)
{
	struct sensor_private_data *sensor =
		(struct sensor_private_data *) i2c_get_clientdata(client);
	int ret = 0;
	hrtimer_init(&sensor->input_timer_accel, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED);
	sensor->input_timer_accel.function = input_timer_accel_callback;
	sensor->hirtimer_enable = false;
	sensor->sensor_cur_data->ready_flag = false;
	sensor->sensor_cur_data->last_value[0] = 0;
	sensor->sensor_cur_data->last_value[1] = 0;
	sensor->sensor_cur_data->last_value[2] = 0;
	// INIT_KFIFO(sensor->sensor_data_fifo_accel);
	spin_lock_init(&sensor->sensor_cur_data->ts_lock);
	pr_info("%s: sensor_init_accel_fifo_and_timer 1.\n", __func__);
	return ret;
}

struct sensor_operate gsensor_qmi8658_ops = {
	.name				= "qmi8658_acc",
	.type				= SENSOR_TYPE_ACCEL,
	.id_i2c				= ACCEL_ID_QMI8658,
	.read_reg			= QMI8658_REG_ACC_XOUT_LSB,
	.read_len			= 6,
	.id_reg				= QMI8658_REG_WHO_AM_I,
	.id_data 			= SENSOR_CHIP_ID_QMI8658,
	.precision			= QMI8658_PRECISION,
	.ctrl_reg 			= QMI8658_REG_CTRL7,
	.int_status_reg 	= QMI8658_REG_STATUS0,
	.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 int gsensor_qmi8658_probe(struct i2c_client *client,
				 const struct i2c_device_id *devid)
{	
	int ret = 0;
	ret = sensor_register_device(client, NULL, devid, &gsensor_qmi8658_ops);
	if(ret){
		pr_err("%s: sensor_register_device failed.\n", __func__);
		ret = -ENOMEM;
		goto register_failed;
	}
	pr_info("%s: qmi8658 1.\n", __func__);
	ret = sensor_init_accel_fifo_and_timer(client);
	if(ret){
		pr_err("%s: sensor init fifo and timer failed.\n", __func__);
		goto init_fifo_timer_failed;
	}
	return ret;

init_fifo_timer_failed:
	sensor_unregister_device(client, NULL, &gsensor_qmi8658_ops);
register_failed:
	return ret;
}

static int gsensor_qmi8658_remove(struct i2c_client *client)
{
	struct sensor_private_data *sensor =
		(struct sensor_private_data *) i2c_get_clientdata(client);
	// kfifo_reset(&sensor->sensor_data_fifo_accel);
	hrtimer_cancel(&sensor->input_timer_accel);
	return sensor_unregister_device(client, NULL, &gsensor_qmi8658_ops);
}

static const struct i2c_device_id gsensor_qmi8658_id[] = {
	{"qmi8658_acc", ACCEL_ID_QMI8658},
	{}
};

static struct i2c_driver gsensor_qmi8658_driver = {
	.probe = gsensor_qmi8658_probe,
	.remove = gsensor_qmi8658_remove,
	.shutdown = sensor_shutdown,
	.id_table = gsensor_qmi8658_id,
	.driver = {
		.name = "gsensor_qmi8658",
#ifdef CONFIG_PM
		.pm = &sensor_pm_ops,
#endif
	},
};

module_i2c_driver(gsensor_qmi8658_driver);

MODULE_AUTHOR("ouenhui <oeh@rock-chips.com");
MODULE_DESCRIPTION("qmi8658_acc 3-Axis accelerometer driver");
MODULE_LICENSE("GPL");