diff --git a/drivers/hid/hid-qvr.c b/drivers/hid/hid-qvr.c index b17ab1e9bc1d..f5b1ea0e27ad 100644 --- a/drivers/hid/hid-qvr.c +++ b/drivers/hid/hid-qvr.c @@ -36,32 +36,28 @@ #include #include #include +#include +#include +#include +#include +#include +#include #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 WAIT_EVENT_INT_TOUT 1000 + +#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,38 +79,203 @@ struct qvr_sensor_t { uint8_t padding[4]; }; +struct qvr_calib_data { + __u64 data_ptr; +}; -int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid) +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; +}; + +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] = QVR_HID_REPORT_ID_CAL; + hid_buf[1] = QVR_CMD_ID_CALIBRATION_DATA_SIZE; + + ret = hid_hw_raw_request(sensor->hdev, hid_buf[0], + hid_buf, + QVR_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(WAIT_EVENT_INT_TOUT)); + 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] = QVR_HID_REPORT_ID_CAL; + hid_buf[1] = QVR_CMD_ID_CALIBRATION_BLOCK_DATA; + + 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, + QVR_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(WAIT_EVENT_INT_TOUT)); + 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] = QVR_HID_REPORT_ID_CAL; + hid_buf[1] = QVR_CMD_ID_IMU_CONTROL; + hid_buf[2] = status; + + ret = hid_hw_raw_request(sensor->hdev, hid_buf[0], + hid_buf, + QVR_HID_REQUEST_REPORT_SIZE, + HID_FEATURE_REPORT, + HID_REQ_SET_REPORT); + ret = wait_event_interruptible_timeout(wq, sensor->ext_ack == 1, + msecs_to_jiffies(WAIT_EVENT_INT_TOUT)); + if (ret && status) { + pr_debug("qvr: falling back - start IMU stream failed\n"); + hid_buf[0] = QVR_HID_REPORT_ID_CAL; + hid_buf[1] = QVR_CMD_ID_IMU_CONTROL_FALLBACK; + ret = hid_hw_raw_request(sensor->hdev, hid_buf[0], hid_buf, + QVR_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; struct external_imu_format imuData = { 0 }; struct qvr_buf_index *index_buf; - /* - * Actual message size is 369 bytes - * to make it 8 byte aligned we created a structure of size 368 bytes. - * Ignoring the first byte 'report id' (which is always 1) - * - */ - memcpy((void *)&imuData, (void *)message + 1, msg_size); + if (msize != sizeof(struct external_imu_format)) { + pr_err("%s: data size mismatch %d\n", __func__, msize); + return -EPROTO; + } - 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)); + memcpy((void *)&imuData, (void *)message, + sizeof(struct external_imu_format)); + + 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; else - data->ats = ts_base + ((imuData.gts0 - ts_offset) * 100); + data->ats = sensor->ts_base + + imuData.gts0 - sensor->ts_offset; + if (imuData.mts0 == 0) + data->mts = 0; + else + data->mts = data->ats; data->gts = data->ats; - data->mts = data->ats; data->ax = -imuData.ax0; data->ay = imuData.ay0; data->az = -imuData.az0; @@ -159,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; @@ -199,13 +361,15 @@ done: 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, @@ -219,17 +383,18 @@ 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; } @@ -237,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, @@ -250,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); } static ssize_t ts_offset_store(struct kobject *kobj, @@ -286,11 +451,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\n", __func__); goto err_free; @@ -305,20 +471,7 @@ static int qvr_external_sensor_probe(struct hid_device *hdev, pr_err("%s: hid_hw_start failed\n", __func__); goto err_free; } - if (hdev->vendor == USB_VENDOR_ID_QVR5) { - 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 +480,96 @@ err_free: } +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 ((hid->vendor == USB_VENDOR_ID_QVR5) && (vaddr != NULL)) { + 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; } @@ -357,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, @@ -369,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 = @@ -383,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\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"); - diff --git a/drivers/hid/hid-qvr.h b/drivers/hid/hid-qvr.h index bc0755d37745..72d27da72171 100644 --- a/drivers/hid/hid-qvr.h +++ b/drivers/hid/hid-qvr.h @@ -20,146 +20,53 @@ #define USB_DEVICE_ID_QVR5 0x0659 #define QVR_EXTERNAL_SENSOR_REPORT_ID 0x1 -struct external_imu_format { - s16 temp0; - s16 temp1; - s16 temp2; - s16 temp3; - u64 gts0; - u64 gts1; - u64 gts2; - u64 gts3; - s16 gx0; - s16 gx1; - s16 gx2; - s16 gx3; - s16 gx4; - s16 gx5; - s16 gx6; - s16 gx7; - s16 gx8; - s16 gx9; - s16 gx10; - s16 gx11; - s16 gx12; - s16 gx13; - s16 gx14; - s16 gx15; - s16 gx16; - s16 gx17; - s16 gx18; - s16 gx19; - s16 gx20; - s16 gx21; - s16 gx22; - s16 gx23; - s16 gx24; - s16 gx25; - s16 gx26; - s16 gx27; - s16 gx28; - s16 gx29; - s16 gx30; - s16 gx31; - s16 gy0; - s16 gy1; - s16 gy2; - s16 gy3; - s16 gy4; - s16 gy5; - s16 gy6; - s16 gy7; - s16 gy8; - s16 gy9; - s16 gy10; - s16 gy11; - s16 gy12; - s16 gy13; - s16 gy14; - s16 gy15; - s16 gy16; - s16 gy17; - s16 gy18; - s16 gy19; - s16 gy20; - s16 gy21; - s16 gy22; - s16 gy23; - s16 gy24; - s16 gy25; - s16 gy26; - s16 gy27; - s16 gy28; - s16 gy29; - s16 gy30; - s16 gy31; - s16 gz0; - s16 gz1; - s16 gz2; - s16 gz3; - s16 gz4; - s16 gz5; - s16 gz6; - s16 gz7; - s16 gz8; - s16 gz9; - s16 gz10; - s16 gz11; - s16 gz12; - s16 gz13; - s16 gz14; - s16 gz15; - s16 gz16; - s16 gz17; - s16 gz18; - s16 gz19; - s16 gz20; - s16 gz21; - s16 gz22; - s16 gz23; - s16 gz24; - s16 gz25; - s16 gz26; - s16 gz27; - s16 gz28; - s16 gz29; - s16 gz30; - s16 gz31; - u64 ats0; - u64 ats1; - u64 ats2; - u64 ats3; - s32 ax0; - s32 ax1; - s32 ax2; - s32 ax3; - s32 ay0; - s32 ay1; - s32 ay2; - s32 ay3; - s32 az0; - s32 az1; - s32 az2; - s32 az3; - u64 mts0; - u64 mts1; - u64 mts2; - u64 mts3; - s16 mx0; - s16 mx1; - s16 mx2; - s16 mx3; - s16 my0; - s16 my1; - s16 my2; - s16 my3; - s16 mz0; - s16 mz1; - s16 mz2; - s16 mz3; //368 bytes -}; +//CMD IDs +#define QVR_CMD_ID_CALIBRATION_DATA_SIZE 20 +#define QVR_CMD_ID_CALIBRATION_BLOCK_DATA 21 +#define QVR_CMD_ID_START_CALIBRATION_UPDATE 22 +#define QVR_CMD_ID_UPDATE_CALIBRATION_BLOCK 23 +#define QVR_CMD_ID_FINISH_CALIBRATION_UPDATE 24 +#define QVR_CMD_ID_IMU_CONTROL 25 +#define QVR_CMD_ID_IMU_CONTROL_FALLBACK 7 + +#define QVR_HID_REPORT_ID_CAL 2 +#define QVR_HID_REQUEST_REPORT_SIZE 64 + +struct external_imu_format { + u8 reportID; + u8 padding; + u16 version; + u16 numIMUs; + u16 numSamplesPerImuPacket; + u16 totalPayloadSize; + u8 reservedPadding[28]; + + s16 imuID; + s16 sampleID; + s16 temperature; + + u64 gts0; + u32 gNumerator; + u32 gDenominator; + s32 gx0; + s32 gy0; + s32 gz0; + + u64 ats0; + u32 aNumerator; + u32 aDenominator; + s32 ax0; + s32 ay0; + s32 az0; + + u64 mts0; + u32 mNumerator; + u32 mDenominator; + s32 mx0; + s32 my0; + s32 mz0; +} __packed; -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);