501 lines
12 KiB
C
501 lines
12 KiB
C
/*
|
|
* drivers/iio/imu/tsfw_icm20628/tsfw_icm20628.c
|
|
*
|
|
* Copyright (c) 2016, NVIDIA CORPORATION, All rights reserved.
|
|
*
|
|
* This software is licensed under the terms of the GNU General Public
|
|
* License version 2, as published by the Free Software Foundation, and
|
|
* may be copied, distributed, and modified under those terms.
|
|
*
|
|
* 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/errno.h>
|
|
#include <linux/nvs.h>
|
|
#include <linux/gfp.h>
|
|
#include <linux/hid.h>
|
|
#include <linux/iio/imu/tsfw_icm20628.h>
|
|
#include <linux/module.h>
|
|
#include <linux/byteorder/generic.h>
|
|
|
|
/* #define DEBUG_TSFW_ICM */
|
|
|
|
#define TSFW_SENSOR_NAME "tsfw_icm20628"
|
|
#define TSFW_SENSOR_VENDOR "Invensense"
|
|
#define TSFW_SENSOR_VERSION 1
|
|
|
|
#define AXIS_N 3 /* 3 channels of data (X, Y, Z) */
|
|
#define SENSOR_REPORT_SYN_MASK 0x80
|
|
|
|
/* TODO: hacky, declare in .h file of hid */
|
|
extern int atvr_ts_sensor_set(struct hid_device *hdev, bool enable);
|
|
|
|
/* Define sensor */
|
|
/* TODO: modify delay_us_min|max, implement batch */
|
|
/* TODO: comment delay_us_min|max */
|
|
static struct sensor_cfg tsfw_icm20628_cfg_dflt[] = {
|
|
{
|
|
.name = "accelerometer",
|
|
.snsr_id = SENSOR_TYPE_ACCELEROMETER,
|
|
.kbuf_sz = 32,
|
|
.ch_n = AXIS_N,
|
|
.ch_n_max = AXIS_N,
|
|
.ch_sz = -2, /* 2 bytes signed per channel */
|
|
.part = TSFW_SENSOR_NAME,
|
|
.vendor = TSFW_SENSOR_VENDOR,
|
|
.version = TSFW_SENSOR_VERSION,
|
|
.delay_us_min = 14222,
|
|
.delay_us_max = 14222,
|
|
.flags = DYNAMIC_SENSOR_MASK,
|
|
.float_significance = NVS_FLOAT_NANO,
|
|
.milliamp = {
|
|
.ival = 0,
|
|
.fval = 500000000,
|
|
},
|
|
.max_range = {
|
|
.ival = 19,
|
|
.fval = 613300,
|
|
},
|
|
.resolution = {
|
|
.ival = 0,
|
|
.fval = 598550,
|
|
},
|
|
.scales[0] = {
|
|
.ival = 0,
|
|
.fval = 598550,
|
|
},
|
|
.scales[1] = {
|
|
.ival = 0,
|
|
.fval = 598550,
|
|
},
|
|
.scales[2] = {
|
|
.ival = 0,
|
|
.fval = 598550,
|
|
},
|
|
.uuid = "ts_accel",
|
|
.matrix = { 0, -1, 0, 1, 0, 0, 0, 0, 1 },
|
|
},
|
|
{
|
|
.name = "gyroscope",
|
|
.snsr_id = SENSOR_TYPE_GYROSCOPE,
|
|
.kbuf_sz = 32,
|
|
.ch_n = AXIS_N,
|
|
.ch_n_max = AXIS_N,
|
|
.ch_sz = -2, /* 2 bytes signed per channel */
|
|
.part = TSFW_SENSOR_NAME,
|
|
.vendor = TSFW_SENSOR_VENDOR,
|
|
.version = TSFW_SENSOR_VERSION,
|
|
.delay_us_min = 14222,
|
|
.delay_us_max = 14222,
|
|
.flags = DYNAMIC_SENSOR_MASK,
|
|
.float_significance = NVS_FLOAT_NANO,
|
|
.milliamp = {
|
|
.ival = 3,
|
|
.fval = 700000000,
|
|
},
|
|
.max_range = {
|
|
.ival = 34,
|
|
.fval = 906585040,
|
|
},
|
|
.resolution = {
|
|
.ival = 0,
|
|
.fval = 1064225,
|
|
},
|
|
.scales[0] = {
|
|
.ival = 0,
|
|
.fval = 1064225,
|
|
},
|
|
.scales[1] = {
|
|
.ival = 0,
|
|
.fval = 1064225,
|
|
},
|
|
.scales[2] = {
|
|
.ival = 0,
|
|
.fval = 1064225,
|
|
},
|
|
.uuid = "ts_gyro",
|
|
.matrix = { 0, 1, 0, -1, 0, 0, 0, 0, 1 },
|
|
},
|
|
{
|
|
.name = "magnetic_field",
|
|
.snsr_id = SENSOR_TYPE_MAGNETIC_FIELD,
|
|
.kbuf_sz = 32,
|
|
.ch_n = AXIS_N,
|
|
.ch_n_max = AXIS_N,
|
|
.ch_sz = -2, /* 2 bytes signed per channel */
|
|
.part = TSFW_SENSOR_NAME,
|
|
.vendor = TSFW_SENSOR_VENDOR,
|
|
.version = TSFW_SENSOR_VERSION,
|
|
.delay_us_min = 14222,
|
|
.delay_us_max = 14222,
|
|
.flags = DYNAMIC_SENSOR_MASK,
|
|
.float_significance = NVS_FLOAT_NANO,
|
|
.milliamp = {
|
|
.ival = 2,
|
|
.fval = 800000,
|
|
},
|
|
.max_range = {
|
|
.ival = 4912,
|
|
.fval = 0,
|
|
},
|
|
.resolution = {
|
|
.ival = 0,
|
|
.fval = 600000,
|
|
},
|
|
.scales[0] = {
|
|
.ival = 0,
|
|
.fval = 600000,
|
|
},
|
|
.scales[1] = {
|
|
.ival = 0,
|
|
.fval = 600000,
|
|
},
|
|
.scales[2] = {
|
|
.ival = 0,
|
|
.fval = 600000,
|
|
},
|
|
.uuid = "ts_mag",
|
|
.matrix = { -1, 0, 0, 0, -1, 0, 0, 0, 1 },
|
|
}
|
|
};
|
|
|
|
#ifdef DEBUG_TSFW_ICM
|
|
#define pr_debugg(...) pr_info(__VA_ARGS__)
|
|
/* TODO: remove magic numbers */
|
|
static void pr_debugg_buffer(u8 *data, size_t len)
|
|
{
|
|
char qwerty[136] = { 0 };
|
|
size_t i, pos = 0;
|
|
if (len > 43)
|
|
len = 43;
|
|
for (i = 0; i < len; i++) {
|
|
if (i != 0 && i % 8 == 0)
|
|
pos += sprintf(qwerty + pos, "\n");
|
|
pos += sprintf(qwerty + pos, "%02x ", data[i]);
|
|
}
|
|
pos += sprintf(qwerty + pos, "\n");
|
|
pr_info("%s", qwerty);
|
|
}
|
|
#else
|
|
#define pr_debugg(...) do {} while (0)
|
|
static inline void pr_debugg_buffer(u8 *data, size_t len)
|
|
{
|
|
}
|
|
#endif
|
|
|
|
/* Different types of fragmentation
|
|
* report_id=5, syn=1, 1 full set (18B)
|
|
* report_id=5, syn=1, 1 full set + 1 partial set (23B)
|
|
* report_id=5, syn=0, 1 partial set (1-17B)
|
|
* report_id=5, syn=0, 1 full set (18B) does this exist?
|
|
* report_id=5, syn=0, 1 partial set + 1 full set (19-25B)
|
|
* report_id=5, syn=0, 1 partial set + 1 full set + 1 partial set (20-25B)
|
|
* report_id=6 (=syn=0), ???
|
|
* report_id=7 (=syn=1), 1 full set (18B)
|
|
* report_id=7 (=syn=1), 2 full sets (36B)
|
|
* report_id=7 (=syn=1), 2 full sets + 1 partial set (41B)
|
|
*/
|
|
|
|
/* only accepts full 18B buffers */
|
|
static void pass_to_nvs(struct tsfw_icm20628_state *st, u8 *data)
|
|
{
|
|
u16 tmp[SNSR_N * AXIS_N];
|
|
size_t i;
|
|
s64 ts;
|
|
ts = nvs_timestamp(); /* ns */
|
|
pr_debugg("Passing sensor data to nvs ts=%lld\n", ts);
|
|
pr_debugg_buffer(data, SENSOR_DATA_SET_SIZE);
|
|
|
|
/* TODO: st->nvs->nvs_mutex_lock(st->nvs_st); */
|
|
memcpy(tmp, data, SENSOR_DATA_SET_SIZE);
|
|
/* swap endianness */
|
|
for (i = 0; i < SNSR_N * AXIS_N; i++)
|
|
tmp[i] = ntohs(tmp[i]);
|
|
|
|
/* pass to nvs */
|
|
if (st && st->nvs && st->nvs->handler)
|
|
for (i = 0; i < SNSR_N; i++)
|
|
st->nvs->handler(st->snsr[i].nvs_st, tmp+(AXIS_N*i),
|
|
ts);
|
|
/* TODO: st->nvs->nvs_mutex_unlock(st->nvs_st); */
|
|
}
|
|
|
|
static int handle_sensor_data(struct tsfw_icm20628_state *st,
|
|
u8 report_counter, bool syn, u16 header,
|
|
u8 *data, size_t len)
|
|
{
|
|
u8 rem, *tmp;
|
|
pr_debugg("%s #%d ln=%zu syn=%d\n", __func__, report_counter, len, syn);
|
|
pr_debugg_buffer(data, len);
|
|
|
|
/* handle initial partial data if any */
|
|
if (syn) {
|
|
tmp = data;
|
|
} else {
|
|
/* TODO: handle first half-packet */
|
|
tmp = data + (SENSOR_DATA_SET_SIZE - st->buffered_cnt);
|
|
}
|
|
|
|
/* pass full sets of data to nvs */
|
|
while (tmp + SENSOR_DATA_SET_SIZE <= data + len) {
|
|
pass_to_nvs(st, tmp);
|
|
tmp += SENSOR_DATA_SET_SIZE;
|
|
}
|
|
|
|
/* copy remainder to buffer */
|
|
rem = data + len - tmp;
|
|
if (rem) {
|
|
pr_debugg("Copying remaining %d bytes to buffer\n", rem);
|
|
pr_debugg_buffer(tmp, rem);
|
|
memcpy(st->buffered_data, tmp, rem);
|
|
}
|
|
st->buffered_cnt = rem;
|
|
/* TODO: st->prev_report_counter = report_counter; */
|
|
return 0;
|
|
}
|
|
|
|
/* Report #1a
|
|
* data[0]: report id = 0x05
|
|
* data[1]: report counter
|
|
* data[2-18]: HID button data
|
|
* data[18] & 0x80: syn flag (= 0)
|
|
* data[19]: length of sensor data
|
|
* data[20-44]: up to 25 bytes of sensor data
|
|
*/
|
|
#define RPT1A_DATA_OFFSET 20
|
|
#define RPT1A_DATA_MAX_LEN 25
|
|
|
|
/* Report #1b
|
|
* data[0]: report id = 0x05
|
|
* data[1]: report counter
|
|
* data[2-18]: HID button data
|
|
* data[18] & 0x80: syn flag (= 1)
|
|
* data[19]: length of sensor data
|
|
* data[20-21]: header of which sensors enabled
|
|
* data[22-44]: up to 23 bytes of sensor data
|
|
*/
|
|
#define RPT1B_DATA_OFFSET 22
|
|
#define RPT1B_DATA_MAX_LEN 23
|
|
|
|
/* Report #2
|
|
* data[0]: report id = 0x06 (syn = 0)
|
|
* data[1]: report counter
|
|
* data[2]: length of sensor data
|
|
* data[3-44]: up to 42 bytes of sensor data
|
|
*/
|
|
#define RPT2_DATA_OFFSET 3
|
|
#define RPT2_DATA_MAX_LEN 42
|
|
|
|
/* Report #3
|
|
* data[0]: report id = 0x07 (syn = 1)
|
|
* data[1]: report counter
|
|
* data[2]: length of sensor data
|
|
* data[3-4]: header of which sensors enabled
|
|
* data[5-44]: up to 40 bytes of sensor data
|
|
*/
|
|
#define RPT3_DATA_OFFSET 5
|
|
#define RPT3_DATA_MAX_LEN 40
|
|
|
|
static int recv(struct tsfw_icm20628_state *st, u8 *data, size_t size)
|
|
{
|
|
u8 report_id = data[0];
|
|
u8 report_counter = data[1];
|
|
u16 sensor_header = 0;
|
|
u8 *sensor_data;
|
|
u8 len, len_max;
|
|
bool syn;
|
|
|
|
if (!st)
|
|
return -ENODEV;
|
|
|
|
pr_debugg("%s received %zu bytes\n", __func__, size);
|
|
pr_debugg_buffer(data, size);
|
|
|
|
if (report_id == SENSOR_REPORT_ID_COMBINED) {
|
|
syn = data[18] & SENSOR_REPORT_SYN_MASK;
|
|
len = data[19];
|
|
if (!syn) {
|
|
pr_debugg("Detected sensor packet #1a\n");
|
|
sensor_data = data + RPT1A_DATA_OFFSET;
|
|
len_max = RPT1A_DATA_MAX_LEN;
|
|
} else {
|
|
pr_debugg("Detected sensor packet #1b\n");
|
|
sensor_header = (data[22] << 8) | data[21];
|
|
sensor_data = data + RPT1B_DATA_OFFSET;
|
|
len_max = RPT1B_DATA_MAX_LEN;
|
|
}
|
|
} else if (report_id == SENSOR_REPORT_ID) {
|
|
pr_debugg("Detected sensor packet #2\n");
|
|
syn = false;
|
|
sensor_data = data + RPT2_DATA_OFFSET;
|
|
len = data[2];
|
|
len_max = RPT2_DATA_MAX_LEN;
|
|
} else if (report_id == SENSOR_REPORT_ID_SYN) {
|
|
pr_debugg("Detected sensor packet #3\n");
|
|
syn = true;
|
|
sensor_header = (data[4] << 8) | data[3];
|
|
sensor_data = data + RPT3_DATA_OFFSET;
|
|
len = data[2];
|
|
len_max = RPT3_DATA_MAX_LEN;
|
|
} else {
|
|
pr_err("unexpected sensor report data\n");
|
|
return -EINVAL;
|
|
}
|
|
|
|
if (len > len_max) {
|
|
pr_err("invalid len %d sensor report", len);
|
|
return -EINVAL;
|
|
}
|
|
|
|
handle_sensor_data(st, report_counter, syn, sensor_header, sensor_data,
|
|
len);
|
|
/* TODO: return error code if fail */
|
|
return 0;
|
|
}
|
|
|
|
static int of_dt(struct tsfw_icm20628_state *st)
|
|
{
|
|
size_t i;
|
|
for (i = 0; i < ARRAY_SIZE(tsfw_icm20628_cfg_dflt); i++)
|
|
memcpy(&st->snsr[i].cfg, &tsfw_icm20628_cfg_dflt[i],
|
|
sizeof(st->snsr[i].cfg));
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
static int enable_hw(struct tsfw_icm20628_state *st)
|
|
{
|
|
int ret;
|
|
ret = atvr_ts_sensor_set(st->hdev, true);
|
|
if (ret)
|
|
pr_err("%s failed to enable ts sensors\n", __func__);
|
|
return 0;
|
|
}
|
|
|
|
static int disable_hw(struct tsfw_icm20628_state *st)
|
|
{
|
|
atvr_ts_sensor_set(st->hdev, false);
|
|
/* TODO: ret check */
|
|
return 0;
|
|
}
|
|
|
|
/* TODO: needed?
|
|
static int set_rate_hw(struct tsfw_icm20628_state *st, unsigned int period)
|
|
{ return 0; }
|
|
*/
|
|
|
|
static int tsfw_icm_enable(void *client, int snsr_id, int enable)
|
|
{
|
|
struct tsfw_icm20628_state *st = (struct tsfw_icm20628_state *)client;
|
|
int ret;
|
|
|
|
pr_info("%s snsr_id: %d enable: %d\n", __func__, snsr_id, enable);
|
|
if (enable < 0)
|
|
return st->enabled;
|
|
|
|
/* TODO: make use of snsr_id for enabling/disabling specific sensors
|
|
* snsr_id will be SENSOR_TYPE_* from nvs.h
|
|
*/
|
|
if (enable)
|
|
ret = enable_hw(st);
|
|
else
|
|
ret = disable_hw(st);
|
|
if (!ret)
|
|
st->enabled = enable;
|
|
return ret;
|
|
}
|
|
|
|
static int tsfw_icm_batch(void *client, int snsr_id, int flags,
|
|
unsigned int period, unsigned int timeout)
|
|
{
|
|
pr_info("%s\n", __func__);
|
|
/* TODO: remove?
|
|
struct tsfw_icm20628_state *st = (struct tsfw_icm20628_state *)client;
|
|
return set_rate_hw(st, period);
|
|
*/
|
|
return 0;
|
|
}
|
|
|
|
static struct nvs_fn_dev tsfw_icm20628_fn_dev = {
|
|
.enable = tsfw_icm_enable,
|
|
.batch = tsfw_icm_batch,
|
|
};
|
|
|
|
static int probe(struct hid_device *hdev, struct tsfw_icm20628_state **pst)
|
|
{
|
|
struct tsfw_icm20628_state *st;
|
|
int ret;
|
|
size_t i;
|
|
|
|
pr_info("tsfw_icm %s\n", __func__);
|
|
/* TODO: use devm_kzalloc? */
|
|
*pst = kzalloc(sizeof(struct tsfw_icm20628_state), GFP_KERNEL);
|
|
st = *pst;
|
|
if (!st)
|
|
return -ENOMEM;
|
|
|
|
ret = of_dt(st);
|
|
if (ret) {
|
|
pr_err("failed to init of_dt\n");
|
|
goto probe_failed;
|
|
}
|
|
|
|
st->hdev = hdev;
|
|
tsfw_icm20628_fn_dev.errs = &st->errs;
|
|
tsfw_icm20628_fn_dev.sts = &st->sts;
|
|
st->nvs = nvs_iio();
|
|
if (!st->nvs || !st->nvs->probe) {
|
|
pr_err("failed to get nvs_iio\n");
|
|
ret = -ENODEV;
|
|
goto probe_failed;
|
|
}
|
|
|
|
for (i = 0; i < SNSR_N; i++) {
|
|
ret = st->nvs->probe(&st->snsr[i].nvs_st, st, &hdev->dev,
|
|
&tsfw_icm20628_fn_dev, &st->snsr[i].cfg);
|
|
if (ret)
|
|
goto nvs_probe_failed;
|
|
}
|
|
|
|
return 0;
|
|
|
|
|
|
nvs_probe_failed:
|
|
if (st && st->nvs && st->nvs->remove)
|
|
for (i = 0; i < SNSR_N; i++)
|
|
st->nvs->remove(st->snsr[i].nvs_st);
|
|
probe_failed:
|
|
kfree(st);
|
|
return ret;
|
|
}
|
|
|
|
static int remove(struct tsfw_icm20628_state *st)
|
|
{
|
|
size_t i;
|
|
if (st && st->nvs && st->nvs->remove)
|
|
for (i = 0; i < SNSR_N; i++)
|
|
st->nvs->remove(st->snsr[i].nvs_st);
|
|
|
|
kfree(st);
|
|
return 0;
|
|
}
|
|
|
|
static struct tsfw_icm20628_fn_dev fns = {
|
|
.probe = probe,
|
|
.recv = recv,
|
|
.remove = remove,
|
|
};
|
|
|
|
struct tsfw_icm20628_fn_dev *tsfw_icm20628_fns()
|
|
{
|
|
return &fns;
|
|
}
|
|
|
|
MODULE_LICENSE("GPL v2");
|
|
MODULE_DESCRIPTION("Thunderstrike FW Accel/Gyro/Mag sensors");
|
|
MODULE_AUTHOR("NVIDIA Corporation");
|