Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 76cd2ee1 authored by Sathish Ambley's avatar Sathish Ambley
Browse files

HID: Read calibration data from external sensor 



Adding IOCTLs to give userspace control over to start / stop
IMU streaming and read calibration data from the external
device.

Change-Id: Iee5103e3a13cc31e4afcac13284e3594f49735bf
Signed-off-by: default avatarRohit Bandi <rohitbandi@codeaurora.org>
Signed-off-by: default avatarSathish Ambley <sathishambley@codeaurora.org>
parent 3b6eb2af

drivers/hid/hid-qvr.c

+339 −60
Original line number Diff line number Diff line
@@ -36,32 +36,26 @@ 
#include <linux/spinlock.h>

#include <linux/timekeeping.h>

#include <linux/soc/qcom/smem_state.h>

#include <linux/ioctl.h>

#include <linux/fs.h>

#include <linux/cdev.h>

#include <linux/wait.h>

#include <linux/jiffies.h>

#include <linux/uaccess.h>

#include "hid-ids.h"

#include "hid-qvr.h"

#include "hid-trace.h"



static struct dma_buf *qvr_buf;

static void *vaddr;

static size_t vsize;

static uint64_t ts_base;

static uint64_t ts_offset;

#define QVR_START_IMU		_IO('q', 1)

#define QVR_STOP_IMU		_IO('q', 2)

#define QVR_READ_CALIB_DATA_LEN	_IOR('q', 3, int32_t)

#define QVR_READ_CALIB_DATA	_IOR('q', 4, struct qvr_calib_data)



struct gpio_info {

	unsigned int smem_bit;

	struct qcom_smem_state *smem_state;

};





static struct device *qvr_device;

static struct gpio_info gpio_info_out;



static struct hid_driver qvr_external_sensor_driver;

static int fd;



const static int msg_size = 368;

const static int hid_request_report_id = 2;

const static int hid_request_report_size = 64;



struct qvr_buf_index {

	int most_recent_index;

	uint8_t padding[60];
@@ -83,9 +77,170 @@ struct qvr_sensor_t { 
	uint8_t padding[4];

};



struct qvr_calib_data {

	__u64 data_ptr;

};



struct qvr_external_sensor {

	struct hid_device *hdev;

	struct device *device;

	struct dma_buf *qvr_buf;

	struct class *class;

	struct device *dev;

	void *vaddr;

	u8 *calib_data_pkt;

	struct cdev cdev;

	struct gpio_info gpio_info_out;

	dev_t dev_no;

	uint64_t ts_base;

	uint64_t ts_offset;

	size_t vsize;

	int calib_data_len;

	int calib_data_recv;

	int ext_ack;

	int fd;

};



const static int msg_size = 368;

const static int hid_request_report_id = 2;

const static int hid_request_report_size = 64;



static DECLARE_WAIT_QUEUE_HEAD(wq);

static struct qvr_external_sensor qvr_external_sensor;



static int read_calibration_len(void)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	__u8 *hid_buf;

	int ret;



	hid_buf = kzalloc(256, GFP_KERNEL);

	if (hid_buf == NULL)

		return -ENOMEM;



	hid_buf[0] = 2;

	hid_buf[1] = 20;



	ret = hid_hw_raw_request(sensor->hdev, hid_buf[0],

		hid_buf,

		hid_request_report_size,

		HID_FEATURE_REPORT,

		HID_REQ_SET_REPORT);



	ret = wait_event_interruptible_timeout(wq,

		sensor->calib_data_len != -1, msecs_to_jiffies(1000));

	if (ret == 0) {

		kfree(hid_buf);

		return -ETIME;

	}



	kfree(hid_buf);

	return sensor->calib_data_len;

}



static uint8_t *read_calibration_data(void)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	__u8 *hid_buf;

	int ret, total_read_len;

	uint8_t read_len;

	uint8_t *complete_data = NULL;



	if (sensor->calib_data_len < 0) {

		pr_err("%s: calibration data len missing\n", __func__);

		return NULL;

	}



	hid_buf = kzalloc(256, GFP_KERNEL);

	if (hid_buf == NULL)

		return NULL;



	hid_buf[0] = 2;

	hid_buf[1] = 21;



	complete_data = kzalloc(sensor->calib_data_len, GFP_KERNEL);

	if (complete_data == NULL) {

		kfree(hid_buf);

		return NULL;

	}

	total_read_len = 0;

	while (total_read_len < sensor->calib_data_len) {

		sensor->calib_data_recv = 0;

		ret = hid_hw_raw_request(sensor->hdev, hid_buf[0],

			hid_buf,

			hid_request_report_size,

			HID_FEATURE_REPORT,

			HID_REQ_SET_REPORT);

		ret = wait_event_interruptible_timeout(wq,

			sensor->calib_data_recv == 1, msecs_to_jiffies(1000));

		if (ret == 0) {

			pr_err("%s:get calibration data timeout\n", __func__);

			kfree(hid_buf);

			kfree(complete_data);

			return NULL;

		}

		if (sensor->calib_data_pkt == NULL) {

			kfree(hid_buf);

			kfree(complete_data);

			return NULL;

		}

		read_len = sensor->calib_data_pkt[2];

		if (total_read_len > sensor->calib_data_len - read_len) {

			kfree(hid_buf);

			kfree(complete_data);

			return NULL;

		}

		memcpy(&complete_data[total_read_len],

			&sensor->calib_data_pkt[3], read_len);

		total_read_len += read_len;

	}



	kfree(hid_buf);

	return complete_data;

}



static int control_imu_stream(bool status)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	__u8 *hid_buf;

	int ret;



	sensor->ext_ack = 0;

	hid_buf = kzalloc(256, GFP_KERNEL);

	if (hid_buf == NULL)

		return -ENOMEM;



	hid_buf[0] = 2;

	hid_buf[1] = 25;

	hid_buf[2] = status;



int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid)

	ret = hid_hw_raw_request(sensor->hdev, hid_buf[0],

		hid_buf,

		hid_request_report_size,

		HID_FEATURE_REPORT,

		HID_REQ_SET_REPORT);

	ret = wait_event_interruptible_timeout(wq, sensor->ext_ack == 1,

		msecs_to_jiffies(1000));

	if (!ret && status) {

		pr_debug("qvr: falling back - start IMU stream failed\n");

		hid_buf[0] = hid_request_report_id;

		hid_buf[1] = 7;

		ret = hid_hw_raw_request(sensor->hdev, hid_buf[0], hid_buf,

				hid_request_report_size,

				HID_FEATURE_REPORT,

				HID_REQ_SET_REPORT);

	}

	kfree(hid_buf);

	if (ret > 0)

		return 0;



	return -ETIME;

}





static int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	struct qvr_sensor_t *sensor_buf;

	struct qvr_sensor_t *data;

	static int buf_index;
@@ -100,19 +255,22 @@ int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid) 
	 */

	memcpy((void *)&imuData, (void *)message + 1, msg_size);



	if (!ts_base)

		ts_base = ktime_to_ns(ktime_get_boottime());

	if (!ts_offset)

		ts_offset = imuData.gts0;

	index_buf = (struct qvr_buf_index *)

		((uintptr_t)vaddr + (vsize / 2) + (8 * sizeof(*sensor_buf)));

	sensor_buf = (struct qvr_sensor_t *)((uintptr_t)vaddr + (vsize / 2));

	if (!sensor->ts_base)

		sensor->ts_base = ktime_to_ns(ktime_get_boottime());

	if (!sensor->ts_offset)

		sensor->ts_offset = imuData.gts0;

	index_buf = (struct qvr_buf_index *)((uintptr_t)sensor->vaddr +

			(sensor->vsize / 2) + (8 * sizeof(*sensor_buf)));

	sensor_buf = (struct qvr_sensor_t *)((uintptr_t)sensor->vaddr +

			(sensor->vsize / 2));



	data = (struct qvr_sensor_t *)&(sensor_buf[buf_index]);

	if (ts_offset > imuData.gts0)

		data->ats = ts_base + ((ts_offset - imuData.gts0) * 100);

	if (sensor->ts_offset > imuData.gts0)

		data->ats = sensor->ts_base +

				((sensor->ts_offset - imuData.gts0) * 100);

	else

		data->ats = ts_base + ((imuData.gts0 - ts_offset) * 100);

		data->ats = sensor->ts_base +

				((imuData.gts0 - sensor->ts_offset) * 100);

	if (imuData.mts0 == 0)

		data->mts = 0;

	else
@@ -162,38 +320,39 @@ static int register_smp2p(struct device *dev, char *node_name, 
	}



	return 0;



}

static int kernel_map_gyro_buffer(int fd)



static int kernel_map_gyro_buffer(void)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	int ret = 0;



	qvr_buf = dma_buf_get(fd);

	if (IS_ERR_OR_NULL(qvr_buf)) {

	sensor->qvr_buf = dma_buf_get(sensor->fd);

	if (IS_ERR_OR_NULL(sensor->qvr_buf)) {

		ret = -ENOMEM;

		pr_err("dma_buf_get failed for fd: %d\n", fd);

		pr_err("dma_buf_get failed for fd: %d\n", sensor->fd);

		goto done;

	}

	ret = dma_buf_begin_cpu_access(qvr_buf, DMA_BIDIRECTIONAL);

	ret = dma_buf_begin_cpu_access(sensor->qvr_buf, DMA_BIDIRECTIONAL);

	if (ret) {

		pr_err("%s: dma_buf_begin_cpu_access failed\n", __func__);

		goto err_dma;

	}

	vsize = qvr_buf->size;

	vaddr = dma_buf_kmap(qvr_buf, 0);

	if (IS_ERR_OR_NULL(vaddr)) {

	sensor->vsize = sensor->qvr_buf->size;

	sensor->vaddr = dma_buf_kmap(sensor->qvr_buf, 0);

	if (IS_ERR_OR_NULL(sensor->vaddr)) {

		ret = -ENOMEM;

		pr_err("dma_buf_kmap failed for fd: %d\n", fd);

		pr_err("dma_buf_kmap failed for fd: %d\n", sensor->fd);

		goto err_end_access;

	}



	return 0;



err_end_access:

	dma_buf_end_cpu_access(qvr_buf, DMA_BIDIRECTIONAL);

	dma_buf_end_cpu_access(sensor->qvr_buf, DMA_BIDIRECTIONAL);

err_dma:

	dma_buf_put(qvr_buf);

	qvr_buf = NULL;

	dma_buf_put(sensor->qvr_buf);

	sensor->qvr_buf = NULL;

done:

	return ret;
@@ -202,37 +361,40 @@ static int kernel_map_gyro_buffer(int fd) 


static void kernel_unmap_gyro_buffer(void)

{

	if (IS_ERR_OR_NULL(vaddr))

	struct qvr_external_sensor *sensor = &qvr_external_sensor;



	if (IS_ERR_OR_NULL(sensor->vaddr))

		return;

	dma_buf_kunmap(qvr_buf, 0, vaddr);

	dma_buf_end_cpu_access(qvr_buf, DMA_BIDIRECTIONAL);

	vaddr = NULL;

	dma_buf_put(qvr_buf);

	qvr_buf = NULL;

	dma_buf_kunmap(sensor->qvr_buf, 0, sensor->vaddr);

	dma_buf_end_cpu_access(sensor->qvr_buf, DMA_BIDIRECTIONAL);

	sensor->vaddr = NULL;

	dma_buf_put(sensor->qvr_buf);

	sensor->qvr_buf = NULL;

}



static ssize_t fd_show(struct kobject *kobj,

	struct kobj_attribute *attr,

	char *buf)

{

	return snprintf(buf, 16, "%d\n", fd);

	return snprintf(buf, 16, "%d\n", qvr_external_sensor.fd);

}



static ssize_t fd_store(struct kobject *kobj,

	struct kobj_attribute *attr,

	const char *buf, size_t count)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	int ret;



	ret = kstrtoint(buf, 10, &fd);

	ret = kstrtoint(buf, 10, &sensor->fd);

	if (ret < 0)

		return ret;

	if (fd == -1)

	if (sensor->fd == -1)

		kernel_unmap_gyro_buffer();

	else

		kernel_map_gyro_buffer(fd);

	ts_base = 0;

	ts_offset = 0;

		kernel_map_gyro_buffer();

	sensor->ts_base = 0;

	sensor->ts_offset = 0;



	return count;

}
@@ -240,7 +402,7 @@ static ssize_t fd_store(struct kobject *kobj, 
static ssize_t ts_base_show(struct kobject *kobj,

	struct kobj_attribute *attr, char *buf)

{

	return  snprintf(buf, 16, "%lld\n", ts_base);

	return snprintf(buf, 16, "%lld\n", qvr_external_sensor.ts_base);

}



static ssize_t ts_base_store(struct kobject *kobj,
@@ -253,7 +415,7 @@ static ssize_t ts_base_store(struct kobject *kobj, 
static ssize_t ts_offset_show(struct kobject *kobj,

	struct kobj_attribute *attr, char *buf)

{

	return  snprintf(buf, 16, "%lld\n", ts_offset * 100);

	return snprintf(buf, 16, "%lld\n", qvr_external_sensor.ts_offset * 100);

}



static ssize_t ts_offset_store(struct kobject *kobj,
@@ -289,11 +451,13 @@ static struct kobject *qvr_external_sensor_kobj; 
static int qvr_external_sensor_probe(struct hid_device *hdev,

	const struct hid_device_id *id)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	int ret;

	char *node_name = "qcom,smp2p-interrupt-qvrexternal-5-out";

	__u8 *hid_buf;

	sensor->hdev = hdev;



	ret = register_smp2p(&hdev->dev, node_name, &gpio_info_out);

	ret = register_smp2p(&hdev->dev, node_name, &sensor->gpio_info_out);

	if (ret) {

		pr_err("%s: register_smp2p failed\n", __func__);

		goto err_free;
@@ -319,7 +483,7 @@ static int qvr_external_sensor_probe(struct hid_device *hdev, 
		HID_REQ_SET_REPORT);

	kfree(hid_buf);



	qvr_device = &hdev->dev;

	sensor->device = &hdev->dev;



	return 0;
@@ -328,22 +492,96 @@ static int qvr_external_sensor_probe(struct hid_device *hdev, 


}



static int qvr_external_sensor_fops_open(struct inode *inode,

	struct file *file)

{

	return 0;

}



static int qvr_external_sensor_fops_close(struct inode *inode,

	struct file *file)

{

	return 0;

}

static long qvr_external_sensor_ioctl(struct file *file, unsigned int cmd,

	unsigned long arg)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	struct qvr_calib_data data;

	uint8_t *calib_data;

	void __user *argp = (void __user *)arg;

	int ret;



	if (sensor->device == NULL) {

		pr_err("%s: device not connected\n", __func__);

		return -EINVAL;

	}



	switch (cmd) {

	case QVR_START_IMU:

		ret = control_imu_stream(1);

		return ret;

	case QVR_STOP_IMU:

		ret = control_imu_stream(0);

		return ret;

	case QVR_READ_CALIB_DATA_LEN:

		sensor->calib_data_len = -1;

		ret = read_calibration_len();

		if (ret < 0)

			return ret;

		if (copy_to_user(argp, &sensor->calib_data_len,

					sizeof(sensor->calib_data_len)))

			return -EFAULT;

		return 0;

	case QVR_READ_CALIB_DATA:

		sensor->calib_data_recv = 0;

		calib_data = read_calibration_data();

		if (calib_data == NULL)

			return -ENOMEM;

		data.data_ptr = (__u64)arg;

		if (copy_to_user(u64_to_user_ptr(data.data_ptr), calib_data,

				sensor->calib_data_len)) {

			kfree(calib_data);

			return -EFAULT;

		}

		kfree(calib_data);

		return 0;

	default:

		pr_err("%s: wrong command\n", __func__);

		return -EINVAL;



	}

	return 0;

}

static int qvr_external_sensor_raw_event(struct hid_device *hid,

	struct hid_report *report,

	u8 *data, int size)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	static int val;

	int ret = -1;



	if (vaddr != NULL && report->id == 0x1) {

	if (sensor->vaddr != NULL && report->id == 0x1) {

		ret = qvr_send_package_wrap(data/*hid_value*/, size, hid);

		if (ret == 0) {

			val = 1 ^ val;

			qcom_smem_state_update_bits(gpio_info_out.smem_state,

				BIT(gpio_info_out.smem_bit), val);

			qcom_smem_state_update_bits(

				sensor->gpio_info_out.smem_state,

				BIT(sensor->gpio_info_out.smem_bit), val);

			ret = -1;

		}

	}

	if (report->id == 0x2) {

		if (data[0] == 2 && data[1] == 0) /*calibration data len*/

			sensor->calib_data_len = (data[3] << 24)

				| (data[4] << 16) | (data[5] << 8) | data[6];

		else if (data[0] == 2 && data[1] == 1) { /*calibration data*/

			sensor->calib_data_pkt = data;

			sensor->calib_data_recv = 1;

		} else if (data[0] == 2 && data[1] == 4) /*calibration ack*/

			sensor->ext_ack = 1;



	}

	return ret;

}
@@ -359,6 +597,14 @@ static struct hid_device_id qvr_external_sensor_table[] = { 
};

MODULE_DEVICE_TABLE(hid, qvr_external_sensor_table);



static const struct file_operations qvr_external_sensor_ops = {

	.owner = THIS_MODULE,

	.open = qvr_external_sensor_fops_open,

	.unlocked_ioctl = qvr_external_sensor_ioctl,

	.compat_ioctl = qvr_external_sensor_ioctl,

	.release = qvr_external_sensor_fops_close,

};



static struct hid_driver qvr_external_sensor_driver = {

	.name = "qvr_external_sensor",

	.id_table = qvr_external_sensor_table,
@@ -371,6 +617,7 @@ module_hid_driver(qvr_external_sensor_driver); 


static int __init qvr_external_sensor_init(void)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;

	int ret = 0;



	qvr_external_sensor_kobj =
@@ -385,15 +632,47 @@ static int __init qvr_external_sensor_init(void) 
		return -ENOMEM;

	}



	ret = alloc_chrdev_region(&sensor->dev_no, 0, 1, "qvr_external_sensor");

	if (ret < 0) {

		pr_err("%s: alloc_chrdev_region failed\n");

		return ret;

	}

	cdev_init(&sensor->cdev, &qvr_external_sensor_ops);

	ret = cdev_add(&sensor->cdev, sensor->dev_no, 1);



	if (ret < 0) {

		pr_err("%s: cdev_add failed\n");

		return ret;

	}

	sensor->class = class_create(THIS_MODULE, "qvr_external_sensor");

	if (sensor->class == NULL) {

		cdev_del(&sensor->cdev);

		unregister_chrdev_region(sensor->dev_no, 1);

		return -ret;

	}

	sensor->dev = device_create(sensor->class, NULL,

				MKDEV(MAJOR(sensor->dev_no), 0), NULL,

				"qvr_external_sensor_ioctl");

	if (sensor->dev == NULL) {

		class_destroy(sensor->class);

		cdev_del(&sensor->cdev);

		unregister_chrdev_region(sensor->dev_no, 1);

		return -ret;

	}

	return ret;

}



static void __exit qvr_external_sensor_exit(void)

{

	struct qvr_external_sensor *sensor = &qvr_external_sensor;



	device_destroy(sensor->class, MKDEV(MAJOR(sensor->dev_no), 0));

	class_destroy(sensor->class);

	cdev_del(&sensor->cdev);

	unregister_chrdev_region(sensor->dev_no, 1);

	kobject_put(qvr_external_sensor_kobj);

}



module_init(qvr_external_sensor_init);

module_exit(qvr_external_sensor_exit);

MODULE_LICENSE("GPL v2");

drivers/hid/hid-qvr.h

+0 −1
Original line number Diff line number Diff line
@@ -157,7 +157,6 @@ struct external_imu_format { 
	s16 mz3; //368 bytes

};



int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid);

void qvr_clear_def_parmeter(void);

void qvr_init(struct hid_device *hdev);

int qvr_input_init(void);