#include <linux/hid.h>
#include <linux/hiddev.h>
#include <linux/hardirq.h>
+#include <linux/iio/imu/tsfw_icm20628.h>
#include <linux/jiffies.h>
#include <linux/slab.h>
#include <linux/time.h>
struct shdr_device {
struct hid_device *hdev;
struct snd_card *shdr_card;
+ struct tsfw_icm20628_fn_dev *snsr_fns;
+ struct tsfw_icm20628_state *st;
};
static int num_remotes;
static struct mutex snd_cards_lock;
return ret;
}
+int atvr_ts_sensor_set(struct hid_device *hdev, bool enable)
+{
+ u8 report[TS_HOSTCMD_REPORT_SIZE] = { 0x04, 0x5a };
+ u8 sample_rate = 70;
+ int ret;
+
+ hid_info(hdev, "%s enable: %d\n", __func__, enable);
+
+ if (enable) {
+ report[3] = 0x01; /* set */
+ report[4] = 0x01; /* enable */
+ report[5] = 0x00; /* real data source, 0x01 for dummy */
+ report[6] = sample_rate;
+ hid_info(hdev, "enable ts sensor %dHz\n", sample_rate);
+ } else {
+ report[3] = 0x01; /* set */
+ hid_info(hdev, "disable ts sensor\n");
+ }
+
+ ret = hdev->hid_output_raw_report(hdev, report,
+ TS_HOSTCMD_REPORT_SIZE, HID_OUTPUT_REPORT);
+ if (ret < 0)
+ hid_info(hdev, "failed to send ts sensor ctrl report, err=%d\n", ret);
+ else
+ ret = 0;
+
+ return ret;
+}
+EXPORT_SYMBOL(atvr_ts_sensor_set);
+
/***************************************************************************/
/************* Atomic FIFO *************************************************/
/***************************************************************************/
#define JAR_AUDIO_REPORT_SIZE 233
#define JAR_AUDIO_FRAME_SIZE 0x3A
+#define TS_BUTTON_REPORT_SIZE 19
+
static int atvr_jarvis_break_events(struct hid_device *hdev,
struct hid_report *report,
u8 *data, int size)
#if (DEBUG_HID_RAW_INPUT == 1)
pr_info("%s: report->id = 0x%x, size = %d\n",
__func__, report->id, size);
- if (size < 22) {
+ if (size <= 22) {
u32 i;
- for (i = 1; i < sizeof(i); i++)
+ for (i = 0; i < size; i++)
pr_info("data[%d] = 0x%02x\n", i, data[i]);
}
#endif
pr_info("Report ID 10: PID: %d, report %s", hdev->product,
(char *)debug_info);
kfree(debug_info);
+ } else if (hdev->product == USB_DEVICE_ID_NVIDIA_THUNDERSTRIKE &&
+ (report->id == SENSOR_REPORT_ID ||
+ report->id == SENSOR_REPORT_ID_SYN ||
+ report->id == SENSOR_REPORT_ID_COMBINED) &&
+ shdr_dev->snsr_fns && shdr_dev->snsr_fns->recv) {
+ shdr_dev->snsr_fns->recv(shdr_dev->st, data, size);
+ /* TODO: check ret */
+ /* TODO: TEST data goes to hid_report_raw_event */
+ /* combined data report also has button data
+ * call hid_report_raw_event to handle button data
+ */
+ if (report->id == SENSOR_REPORT_ID_COMBINED)
+ hid_report_raw_event(hdev, 0, data,
+ TS_BUTTON_REPORT_SIZE, 0);
+ /* we've handled the event */
+ return 1;
}
/* let the event through for regular input processing */
return 0;
int ret;
struct shdr_device *shdr_dev;
struct snd_card *shdr_card;
+ struct tsfw_icm20628_state *st;
shdr_dev = kzalloc(sizeof(*shdr_dev), GFP_KERNEL);
if (shdr_dev == NULL) {
num_remotes++;
mutex_unlock(&snd_cards_lock);
+
+ shdr_dev->snsr_fns = tsfw_icm20628_fns();
+ if (shdr_dev->snsr_fns && shdr_dev->snsr_fns->probe)
+ shdr_dev->snsr_fns->probe(hdev, &st);
+ /* TODO: ret check */
+ shdr_dev->st = st;
+
return 0;
err_stop:
hid_hw_stop(hdev);
if (shdr_card == NULL)
return -EIO;
+ if (shdr_dev->snsr_fns && shdr_dev->snsr_fns->remove)
+ shdr_dev->snsr_fns->remove(shdr_dev->st);
+ /* TODO: ret check */
+
mutex_lock(&snd_cards_lock);
atvr_snd = shdr_card->private_data;
mutex_lock(&atvr_snd->hdev_lock);
--- /dev/null
+/*
+ * 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>
+
+/* #define DEBUG_TSFW_ICM */
+
+#define TSFW_SENSOR_NAME "tsfw_icm20628"
+#define TSFW_SENSOR_VENDOR "Invensense"
+#define TSFW_SENSOR_VERSION 1
+
+#define ID_ACCEL 0
+#define ID_GYRO 1
+#define ID_MAG 2
+
+#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",
+ },
+ {
+ .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",
+ },
+ {
+ .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,
+ /* TODO: milliamp */
+ .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",
+ }
+};
+
+#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(...) \
+({ \
+ if (0) \
+ pr_info(__VA_ARGS__); \
+ 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)
+ */
+static int handle_sensor_data(struct tsfw_icm20628_state *st,
+ u8 report_counter, bool syn, u16 header,
+ u8 *data, size_t len)
+{
+ size_t i;
+ s64 ts;
+ pr_debugg("%s #%d len=%zu syn=%d\n", __func__, report_counter, len, syn);
+ pr_debugg_buffer(data, len);
+ if (syn) {
+ u8 rem, *tmp = data;
+ while (tmp + SENSOR_DATA_SET_SIZE <= data + len) {
+ u8 t2[SENSOR_DATA_SET_SIZE];
+ pr_debugg("Passing sensor data to nvs ts=%ld\n", ts);
+ pr_debugg_buffer(tmp, SENSOR_DATA_SET_SIZE);
+
+ ts = nvs_timestamp(); /* ns */
+ /* TODO: st->nvs->nvs_mutex_lock(st->nvs_st); */
+ for (i = 0; i < 17; i += 2) {
+ /* Adjust for endianness */
+ t2[i] = tmp[i+1];
+ t2[i+1] = tmp[i];
+ }
+ st->nvs->handler(st->snsr[ID_ACCEL].nvs_st, t2, ts);
+ st->nvs->handler(st->snsr[ID_GYRO].nvs_st, t2+6, ts);
+ st->nvs->handler(st->snsr[ID_MAG].nvs_st, t2+12, ts);
+ /* TODO: st->nvs->nvs_mutex_unlock(st->nvs_st); */
+ tmp += SENSOR_DATA_SET_SIZE;
+ }
+ rem = data + len - tmp;
+ pr_debugg("Copying remainder to buffer\n");
+ pr_debugg_buffer(tmp, rem);
+ memcpy(st->buffered_data, tmp, rem);
+ st->buffered_cnt = rem;
+ /* TODO: st->prev_report_counter = report_counter; */
+ } else {
+ /* TODO: need to do something with non-sync packets */
+ }
+ return 0;
+}
+
+/* Report #1a
+ * data[0]: report id = 0x05
+ * data[1]: report counter
+ * data[2-18]: HID button data
+ * data[19]: syn flag (= 0)
+ * data[20]: length of sensor data
+ * data[21-45]: up to 25 bytes of sensor data
+ */
+#define RPT1A_DATA_OFFSET 21
+#define RPT1A_DATA_MAX_LEN 25
+
+/* Report #1b
+ * data[0]: report id = 0x05
+ * data[1]: report counter
+ * data[2-18]: HID button data
+ * data[19]: syn flag (= 1)
+ * data[20]: length of sensor data
+ * data[21-22]: header of which sensors enabled
+ * data[23-45]: up to 23 bytes of sensor data
+ */
+#define RPT1B_DATA_OFFSET 23
+#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-45]: up to 43 bytes of sensor data
+ */
+#define RPT2_DATA_OFFSET 3
+#define RPT2_DATA_MAX_LEN 43
+
+/* 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-45]: up to 41 bytes of sensor data
+ */
+#define RPT3_DATA_OFFSET 5
+#define RPT3_DATA_MAX_LEN 41
+
+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;
+ bool syn;
+
+ pr_debugg("%s received %zu bytes\n", __func__, size);
+ pr_debugg_buffer(data, size);
+
+ if (report_id == SENSOR_REPORT_ID_COMBINED) {
+ syn = data[19] & SENSOR_REPORT_SYN_MASK;
+ len = data[20];
+ if (!syn) {
+ pr_debugg("Detected sensor packet #1a\n");
+ sensor_data = data + RPT1A_DATA_OFFSET;
+ if (len > RPT1A_DATA_MAX_LEN)
+ len = RPT1A_DATA_MAX_LEN;
+ } else {
+ pr_debugg("Detected sensor packet #1b\n");
+ /* TODO: might be able to just (u16) data[21] */
+ sensor_header = (data[22] << 8) | data[21];
+ sensor_data = data + RPT1B_DATA_OFFSET;
+ if (len > RPT1B_DATA_MAX_LEN)
+ len = RPT1B_DATA_MAX_LEN;
+ }
+ handle_sensor_data(st, report_counter, syn, sensor_header,
+ sensor_data, 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];
+ if (len > RPT2_DATA_MAX_LEN)
+ len = RPT2_DATA_MAX_LEN;
+ handle_sensor_data(st, report_counter, syn, sensor_header,
+ sensor_data, len);
+ } else if (report_id == SENSOR_REPORT_ID_SYN) {
+ pr_debugg("Detected sensor packet #3\n");
+ syn = true;
+ /* TODO: might be able to just (u16) data[3] */
+ sensor_header = (data[4] << 8) | data[3];
+ sensor_data = data + RPT3_DATA_OFFSET;
+ len = data[2];
+ if (len > RPT3_DATA_MAX_LEN)
+ len = RPT3_DATA_MAX_LEN;
+ handle_sensor_data(st, report_counter, syn, sensor_header,
+ sensor_data, len);
+ } else {
+ pr_err("unexpected sensor report data\n");
+ }
+ /* TODO: return error code if fail */
+ return 0;
+}
+
+/* TODO: rename, struct device_node *dn needed? */
+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 read_hw(struct tsfw_icm20628_state *st, int32_t data)
+{ return 0; }
+ */
+
+static int 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 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 = enable,
+ .batch = batch,
+};
+
+static int probe(struct hid_device *hdev, struct tsfw_icm20628_state **pst)
+{
+ struct tsfw_icm20628_state *st;
+ int ret;
+ size_t i;
+
+ /* 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");
projects / hercules2020 / nv-tegra / linux-4.4.git / commitdiff