rk3588驱动开发 seneor注意点2，解决上传时间戳问题

上一篇sensor的文章（RK系列3588驱动开发：sensor注意点_rk3588接sensor-CSDN博客）中提到问题工作队列计时不准的问题，跟供应商要驱动也只有仿造传统rk平台的驱动，这个问题还是存在，那只能自己想办法
尝试过的方法有

1.增加工作队列

原因：原来是一个芯片，用杂项设备注册，框架中是一个工作队列去给3个sensor（陀螺仪，加速器，温度）驱动做数据上报，开始想的就是可能是3个同时做上报会有影响，

结果：实际做完后测试跟原来没啥差距，该唤醒的时间还是有较大波动（与平均值差2ms），最大差异与平均值比较到6ms

2。想改成中断

用sensor的中断计时来代替轮询的方式，然后综合一下条件

（1）平台没有做中断接口，可以加

（2）sensor供应商说没有10hz（odr值）的频率，最低到28hz，也就是1000/28 = 35.7ms更新寄存器并产生一个中断

（3）供应商还说sensor的误差在%5

好吧，看到第三点，前面两点直接忽略，这误差感觉还不如平台的工作队列，做了也没用

3。改工作队列为定时器

原因：因为感觉定时器会没有工作队列的延时，应该会好一点

结果：哈哈，框架里面的工作队列的这个位置我改完起不来，看log感觉加载到后面的sensor驱动出现的问题，具体啥问题没查出来，

4.在原有基础上加高精度计时器

原因：我搜索解决方法的时候看到一个驱动上报数据是用高精度定时器自己控制，又考虑到原sensor的框架，再加一个先进先出的缓冲区给原框架上报的位置缓存数据，那上报的时间就可以自己控制，不需要在根据工作队列的时间来上传，感觉可行

原：

改进：

先看测试出来上报的时间戳，周期100ms，偏差最大为1ms

不要在乎第八个那个负值，人工智障算错啦，那还是没啥偏差的100ms周期

这个前提得支持高精度计时，没有的话也用不了

/* 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=118hz
	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=118hz
	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);
    struct kfifo_data *acc_report_array;
    int ret = 0;

    acc_report_array = kmalloc(sizeof(struct kfifo_data), GFP_KERNEL);
    if (!acc_report_array) {
        printk(KERN_ERR "Failed to allocate memory for acc_report_array\n");
        return -ENOMEM;
    }

    /* check fifo array is not empty */
    if (kfifo_len(&sensor->sensor_data_fifo_accel) > 0) {
        ret = kfifo_out(&sensor->sensor_data_fifo_accel, acc_report_array, 1);
    } else {
        printk(KERN_INFO "qmi8658 fifo array empty\n");
		kfree(acc_report_array);
        return ret;
    }

    if (sensor->status_cur == SENSOR_ON) {
        /* Report acceleration sensor information */
		printk("qmi8658 input_report_abs accel x=%d,y=%d,z=%d\n",acc_report_array->value[0],acc_report_array->value[1],acc_report_array->value[2]);
        input_report_abs(sensor->input_dev, ABS_X, acc_report_array->value[0]);
        input_report_abs(sensor->input_dev, ABS_Y, acc_report_array->value[1]);
        input_report_abs(sensor->input_dev, ABS_Z, acc_report_array->value[2]);
        input_sync(sensor->input_dev);
    }
    kfree(acc_report_array);
    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;
	cur_ts = get_time_ns();
	delta_ts = cur_ts - sensor->pre_ts_sensor;

	hrtimer_forward_now(timer, ktime_set(0, TIMER_INTERVAL));
	printk("qmi8658 input_timer_accel_callback 111 \n");
	if(delta_ts > sensor->timer_thres)
	{
		/* report value*/
		printk("qmi8658 input_timer_accel_callback timer timer_thres \n");
		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++ < 10)){
		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->fifo_data) {
		dev_err(&client->dev, "%s: qmi8658 fifo_data is NULL\n", __func__);
		return -EINVAL;
	}
	sensor->fifo_data->timestamp = get_time_ns();
	sensor->fifo_data->value[0] = axis.x;
	sensor->fifo_data->value[1] = axis.y;
	sensor->fifo_data->value[2] = axis.z;
	/* get sensor value to fifo array*/
	ret = kfifo_is_full(&sensor->sensor_data_fifo_accel);
	if(ret){
		kfifo_reset(&sensor->sensor_data_fifo_accel);
		printk("qmi8658 accel fifo array full reset\n");
		kfifo_in_spinlocked(&sensor->sensor_data_fifo_accel, sensor->fifo_data, 1, &sensor->fifo_data->ts_lock);
	}else{
		kfifo_in_spinlocked(&sensor->sensor_data_fifo_accel, sensor->fifo_data, 1, &sensor->fifo_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;
	INIT_KFIFO(sensor->sensor_data_fifo_accel);
	spin_lock_init(&sensor->fifo_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");

添加啦sensor-dev.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>

#include <linux/miscdevice.h>
#ifdef CONFIG_HAS_EARLYSUSPEND
#include <linux/earlysuspend.h>
#endif

#include <dt-bindings/sensor-dev.h>
#include <linux/module.h>

#define SENSOR_ON		1
#define SENSOR_OFF		0
#define SENSOR_UNKNOW_DATA	-1

#define GPIO_HIGH 1
#define GPIO_LOW 0

#define SENSOR_FIFO_SIZE 8
#define TIMER_INTERVAL		1E6l
struct kfifo_data {
    s64 timestamp;
    int value[3];
	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 kfifo_data *fifo_data;

	DECLARE_KFIFO(sensor_data_fifo_accel, struct kfifo_data, SENSOR_FIFO_SIZE);
	DECLARE_KFIFO(sensor_data_fifo_gyro, struct kfifo_data, SENSOR_FIFO_SIZE);

#ifdef CONFIG_HAS_EARLYSUSPEND
	struct early_suspend early_suspend;
#endif
};

	fifo_data = devm_kzalloc(&client->dev, sizeof(*fifo_data), GFP_KERNEL);
	if (!fifo_data) {
		result = -ENOMEM;
		return result;
	}
	sensor->fifo_data = fifo_data;

需要再注册初始化化中开辟一下空间，这个我是放在sensor-dev.c里面做的，还有设置高精度周期等放在这个文件会比较方便

static int sensor_reset_rate(struct i2c_client *client, int rate)
{
	struct sensor_private_data *sensor = (struct sensor_private_data *) i2c_get_clientdata(client);
	int result = 0;

	if (rate < 5)
		rate = 5;
	else if (rate > 200)
		rate = 200;

	dev_info(&client->dev, "set sensor poll time to %dms\n", rate);

	/* work queue is always slow, we need more quickly to match hal rate */
	sensor->timer_thres = (s64)((rate) * 1000000);/* The timer_thres unit is ns need *1E6l*/
	if (sensor->pdata->poll_delay_ms == (rate - 8))
		return 0;

目前先写到这，基本看懂就能解决这个问题，看不懂的，后续我会写高精度的使用跟先入先出的使用出来