mirror of
https://github.com/BobTheBlinker/android_kernel_motorola_sm6375.git
synced 2026-10-25 06:03:36 -04:00
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: rbandi <rohitbandi@codeaurora.org> Signed-off-by: Sathish Ambley <sathishambley@codeaurora.org>
This commit is contained in:
Sathish Ambley
Gerrit - the friendly Code Review server
parent
64108d4ba0
commit
002053fd87
2 changed files with 338 additions and 74 deletions
|
|
@ -36,31 +36,25 @@
|
|||
#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"
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
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;
|
||||
|
||||
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
|
||||
|
|
@ -158,38 +316,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;
|
||||
|
||||
|
|
@ -198,13 +357,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,
|
||||
|
|
@ -218,17 +379,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;
|
||||
}
|
||||
|
|
@ -236,7 +398,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,
|
||||
|
|
@ -249,7 +411,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,
|
||||
|
|
@ -285,11 +447,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;
|
||||
|
|
@ -304,20 +467,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;
|
||||
|
||||
|
|
@ -326,22 +476,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;
|
||||
}
|
||||
|
||||
|
|
@ -356,6 +580,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,
|
||||
|
|
@ -368,6 +600,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 =
|
||||
|
|
@ -382,15 +615,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");
|
||||
|
||||
|
|
|
|||
|
|
@ -159,7 +159,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);
|
||||
|
|
|
|||
Loading…
Reference in a new issue