Merge "HID: Read calibration data from external sensor" (d709492b) · Commits · e / devices / android_kernel_oneplus_sm8150

drivers/hid/hid-qvr.c

+339 −72

Original line number	Diff line number	Diff line
		@@ -35,32 +35,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];
		@@ -82,9 +76,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", __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", __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;
		@@ -99,19 +254,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
		@@ -161,38 +319,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;

		@@ -201,37 +360,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, sizeof(buf), "%d\n", fd);
		return snprintf(buf, sizeof(buf), "%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;
		}
		@@ -239,7 +401,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,
		@@ -252,7 +414,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,
		@@ -288,11 +450,12 @@ 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", __func__);
		goto err_free;
		@@ -307,18 +470,7 @@ static int qvr_external_sensor_probe(struct hid_device *hdev,
		pr_err("%s: hid_hw_start failed", __func__);
		goto err_free;
		}
		hid_buf = kzalloc(255, GFP_ATOMIC);
		if (hid_buf == NULL)
		return -ENOMEM;
		hid_buf[0] = hid_request_report_id;
		hid_buf[1] = 7;
		ret = hid_hw_raw_request(hdev, hid_buf[0], hid_buf,
		hid_request_report_size,
		HID_FEATURE_REPORT,
		HID_REQ_SET_REPORT);
		kfree(hid_buf);

		qvr_device = &hdev->dev;
		sensor->device = &hdev->dev;

		return 0;

		@@ -327,22 +479,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", __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", __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;
		}

		@@ -358,6 +584,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,
		@@ -370,6 +604,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 =
		@@ -384,15 +619,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");
		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");
		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

+1 −2

Original line number	Diff line number	Diff line
		/*
		* Copyright (c) 2018, The Linux Foundation. All rights reserved.
		* Copyright (c) 2018-2019, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -156,7 +156,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);