| /* |
| * Copyright (c) 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 |
| * only version 2 as published by the Free Software Foundation. |
| * |
| * 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/kobject.h> |
| #include <linux/string.h> |
| #include <linux/sysfs.h> |
| #include <linux/module.h> |
| #include <linux/init.h> |
| #include <linux/msm_ion.h> |
| #include <linux/usb.h> |
| #include <linux/slab.h> |
| #include <linux/hid.h> |
| #include <linux/module.h> |
| #include <linux/kernel.h> |
| #include <linux/input.h> |
| #include <linux/hiddev.h> |
| #include <linux/hid-debug.h> |
| #include <linux/hidraw.h> |
| #include <linux/device.h> |
| #include <linux/of_gpio.h> |
| #include <linux/of_irq.h> |
| #include <linux/gpio.h> |
| #include <linux/timekeeping.h> |
| #include <linux/ion.h> |
| #include "../soc/qcom/smp2p_private.h" |
| #include "hid-ids.h" |
| #include "hid-qvr.h" |
| #include "hid-trace.h" |
| |
| static struct ion_handle *handle; |
| static struct ion_client *client; |
| static void *vaddr; |
| static size_t vsize; |
| static uint64_t ts_base; |
| static uint64_t ts_offset; |
| static int msg_size = 368; |
| |
| struct gpio_info { |
| int gpio_base_id; |
| int irq_base_id; |
| }; |
| |
| |
| /* GPIO Inbound/Outbound callback info */ |
| struct gpio_inout { |
| struct gpio_info in; |
| struct gpio_info out; |
| }; |
| |
| static struct gpio_inout gpio_info[SMP2P_NUM_PROCS]; |
| static struct gpio_info *in_gpio_info_ptr; |
| static struct gpio_info *out_gpio_info_ptr; |
| |
| |
| static struct hid_driver qvr_external_sensor_driver; |
| static int fd; |
| |
| |
| struct ion_handle { |
| struct kref ref; |
| unsigned int user_ref_count; |
| struct ion_client *client; |
| struct ion_buffer *buffer; |
| struct rb_node node; |
| unsigned int kmap_cnt; |
| int id; |
| }; |
| |
| struct qvr_buf_index { |
| int most_recent_index; |
| uint8_t padding[60]; |
| }; |
| |
| struct qvr_sensor_t { |
| uint64_t gts; |
| uint64_t ats; |
| uint64_t mts; |
| s32 gx; |
| s32 gy; |
| s32 gz; |
| s32 ax; |
| s32 ay; |
| s32 az; |
| s32 mx; |
| s32 my; |
| s32 mz; |
| uint8_t padding[4]; |
| }; |
| |
| |
| int qvr_send_package_wrap(u8 *message, int msize, struct hid_device *hid) |
| { |
| 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 (!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)); |
| |
| data = (struct qvr_sensor_t *)&(sensor_buf[buf_index]); |
| if (ts_offset > imuData.gts0) |
| data->ats = ts_base + ((ts_offset - imuData.gts0) * 100); |
| else |
| data->ats = ts_base + ((imuData.gts0 - ts_offset) * 100); |
| if (imuData.mts0 == 0) |
| data->mts = 0; |
| else |
| data->mts = data->ats; |
| data->gts = data->ats; |
| data->ax = -imuData.ax0; |
| data->ay = imuData.ay0; |
| data->az = -imuData.az0; |
| data->gx = -imuData.gx0; |
| data->gy = imuData.gy0; |
| data->gz = -imuData.gz0; |
| data->mx = -imuData.my0; |
| data->my = -imuData.mx0; |
| data->mz = -imuData.mz0; |
| |
| trace_qvr_recv_sensor("gyro", data->gts, data->gx, data->gy, data->gz); |
| trace_qvr_recv_sensor("accel", data->ats, data->ax, data->ay, data->az); |
| |
| pr_debug("%s: gts= %llu, gx= %d, gy=%d, gz=%d", __func__, |
| data->gts, data->gx, data->gy, data->gz); |
| pr_debug("%s: ats= %llu, ax= %d, ay=%d, az=%d", __func__, |
| data->ats, data->ax, data->ay, data->az); |
| pr_debug("%s: mts= %llu, mx= %d, my=%d, mz=%d", __func__, |
| data->mts, data->mx, data->my, data->mz); |
| |
| index_buf->most_recent_index = buf_index; |
| buf_index = (buf_index == (8 - 1)) ? 0 : buf_index + 1; |
| return 0; |
| } |
| |
| static int register_smp2p(char *node_name, struct gpio_info *gpio_info_ptr) |
| { |
| struct device_node *node = NULL; |
| int cnt = 0; |
| int id = 0; |
| |
| node = of_find_compatible_node(NULL, NULL, node_name); |
| if (node) { |
| cnt = of_gpio_count(node); |
| if (cnt && gpio_info_ptr) { |
| id = of_get_gpio(node, 0); |
| if (id == -EPROBE_DEFER) |
| return id; |
| gpio_info_ptr->gpio_base_id = id; |
| gpio_info_ptr->irq_base_id = gpio_to_irq(id); |
| return 0; |
| } |
| } |
| return -EINVAL; |
| } |
| |
| |
| |
| static int kernel_map_gyro_buffer(int fd) |
| { |
| handle = ion_import_dma_buf_fd(client, fd); |
| if (IS_ERR(handle)) { |
| pr_err("%s: ion_import_dma_buf_fd failed\n", __func__); |
| return -EINVAL; |
| } |
| |
| if (ion_handle_get_size(client, handle, &vsize)) { |
| pr_err("%s: Could not dma buf %d size\n", __func__, fd); |
| return -EINVAL; |
| } |
| |
| vaddr = ion_map_kernel(client, handle); |
| if (IS_ERR_OR_NULL(vaddr)) { |
| ion_free(client, handle); |
| return -EINVAL; |
| } |
| |
| return 0; |
| } |
| |
| |
| static void kernel_unmap_gyro_buffer(void) |
| { |
| if (!IS_ERR_OR_NULL(vaddr)) { |
| ion_unmap_kernel(client, handle); |
| ion_free(client, handle); |
| vaddr = NULL; |
| } |
| } |
| |
| static ssize_t fd_show(struct kobject *kobj, |
| struct kobj_attribute *attr, |
| char *buf) |
| { |
| return snprintf(buf, 16, "%d\n", fd); |
| } |
| |
| static ssize_t fd_store(struct kobject *kobj, |
| struct kobj_attribute *attr, |
| const char *buf, size_t count) |
| { |
| int ret; |
| |
| ret = kstrtoint(buf, 10, &fd); |
| if (ret < 0) |
| return ret; |
| if (fd == -1) |
| kernel_unmap_gyro_buffer(); |
| else |
| kernel_map_gyro_buffer(fd); |
| ts_base = 0; |
| ts_offset = 0; |
| |
| return count; |
| } |
| |
| static ssize_t ts_base_show(struct kobject *kobj, |
| struct kobj_attribute *attr, char *buf) |
| { |
| return snprintf(buf, 16, "%lld\n", ts_base); |
| } |
| |
| static ssize_t ts_base_store(struct kobject *kobj, |
| struct kobj_attribute *attr, |
| const char *buf, size_t count) |
| { |
| return 0; |
| } |
| |
| static ssize_t ts_offset_show(struct kobject *kobj, |
| struct kobj_attribute *attr, char *buf) |
| { |
| return snprintf(buf, 16, "%lld\n", ts_offset * 100); |
| } |
| |
| static ssize_t ts_offset_store(struct kobject *kobj, |
| struct kobj_attribute *attr, |
| const char *buf, size_t count) |
| { |
| return 0; |
| } |
| |
| static struct kobj_attribute fd_attribute = __ATTR(fd, 0664, |
| fd_show, |
| fd_store); |
| static struct kobj_attribute ts_base_attribute = __ATTR(ts_base, 0664, |
| ts_base_show, |
| ts_base_store); |
| static struct kobj_attribute ts_offset_attribute = __ATTR(ts_offset, 0664, |
| ts_offset_show, |
| ts_offset_store); |
| |
| static struct attribute *attrs[] = { |
| &fd_attribute.attr, |
| &ts_base_attribute.attr, |
| &ts_offset_attribute.attr, |
| NULL, |
| }; |
| |
| static struct attribute_group attr_group = { |
| .attrs = attrs, |
| }; |
| |
| static struct kobject *qvr_external_sensor_kobj; |
| |
| static int qvr_external_sensor_probe(struct hid_device *hdev, |
| const struct hid_device_id *id) |
| { |
| int ret; |
| char *in_node_name = "qcom,smp2pgpio_client_qvrexternal_5_in"; |
| char *out_node_name = "qcom,smp2pgpio_client_qvrexternal_5_out"; |
| __u8 hid_buf[255] = { 0 }; |
| size_t hid_count = 64; |
| |
| ret = register_smp2p(in_node_name, in_gpio_info_ptr); |
| if (ret) { |
| pr_err("%s: register_smp2p failed", __func__); |
| goto err_free; |
| } |
| ret = register_smp2p(out_node_name, out_gpio_info_ptr); |
| if (ret) { |
| pr_err("%s: register_smp2p failed", __func__); |
| goto err_free; |
| } |
| ret = hid_open_report(hdev); |
| if (ret) { |
| pr_err("%s: hid_open_report failed", __func__); |
| goto err_free; |
| } |
| ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT); |
| if (ret) { |
| pr_err("%s: hid_hw_start failed", __func__); |
| goto err_free; |
| } |
| hid_buf[0] = 2; |
| hid_buf[1] = 7; |
| ret = hid_hw_raw_request(hdev, hid_buf[0], |
| hid_buf, |
| hid_count, |
| HID_FEATURE_REPORT, |
| HID_REQ_SET_REPORT); |
| return 0; |
| err_free: |
| return ret; |
| |
| } |
| |
| static int qvr_external_sensor_raw_event(struct hid_device *hid, |
| struct hid_report *report, |
| u8 *data, int size) |
| { |
| int val; |
| int ret = -1; |
| |
| if (vaddr != NULL && report->id == 0x1) { |
| ret = qvr_send_package_wrap(data/*hid_value*/, size, hid); |
| if (ret != 0) { |
| pr_err("%s: qvr_send_package_wrap failed", __func__); |
| return ret; |
| } |
| val = 1 ^ gpio_get_value(out_gpio_info_ptr->gpio_base_id + 0); |
| gpio_set_value(out_gpio_info_ptr->gpio_base_id + 0, val); |
| ret = -1; |
| } |
| return ret; |
| } |
| |
| static void qvr_external_sensor_device_remove(struct hid_device *hdev) |
| { |
| hid_hw_stop(hdev); |
| } |
| |
| static struct hid_device_id qvr_external_sensor_table[] = { |
| { HID_USB_DEVICE(USB_VENDOR_ID_QVR5, USB_DEVICE_ID_QVR5) }, |
| { HID_USB_DEVICE(USB_VENDOR_ID_QVR32A, USB_DEVICE_ID_QVR32A) }, |
| { } |
| }; |
| MODULE_DEVICE_TABLE(hid, qvr_external_sensor_table); |
| |
| static struct hid_driver qvr_external_sensor_driver = { |
| .name = "qvr_external_sensor", |
| .id_table = qvr_external_sensor_table, |
| .probe = qvr_external_sensor_probe, |
| .raw_event = qvr_external_sensor_raw_event, |
| .remove = qvr_external_sensor_device_remove, |
| }; |
| |
| module_hid_driver(qvr_external_sensor_driver); |
| |
| static int __init qvr_external_sensor_init(void) |
| { |
| const char *device_name = "aoe"; |
| int ret = 0; |
| |
| in_gpio_info_ptr = &gpio_info[SMP2P_CDSP_PROC].in; |
| in_gpio_info_ptr->gpio_base_id = -1; |
| out_gpio_info_ptr = &gpio_info[SMP2P_CDSP_PROC].out; |
| out_gpio_info_ptr->gpio_base_id = -1; |
| |
| |
| qvr_external_sensor_kobj = |
| kobject_create_and_add("qvr_external_sensor", kernel_kobj); |
| |
| if (!qvr_external_sensor_kobj) { |
| pr_err("%s: kobject_create_and_add() fail\n", __func__); |
| return -ENOMEM; |
| } |
| |
| ret = sysfs_create_group(qvr_external_sensor_kobj, &attr_group); |
| if (ret) { |
| pr_err("%s: can't register sysfs\n", __func__); |
| return -ENOMEM; |
| } |
| |
| client = msm_ion_client_create(device_name); |
| if (client == NULL) { |
| pr_err("msm_ion_client_create failed in %s", __func__); |
| return -EINVAL; |
| } |
| |
| return ret; |
| } |
| |
| static void __exit qvr_external_sensor_exit(void) |
| { |
| kobject_put(qvr_external_sensor_kobj); |
| } |
| |
| module_init(qvr_external_sensor_init); |
| module_exit(qvr_external_sensor_exit); |
| MODULE_LICENSE("GPL v2"); |
| |