Jetpack/kernel/nvidia/drivers/misc/nvs/nvs_dsm.c

527 lines
12 KiB
C

/* Copyright (c) 2016-2017, 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.
*/
/* The NVS = NVidia Sensor framework */
/* See nvs.h for documentation */
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/platform_device.h>
#include <linux/err.h>
#include <linux/of.h>
#include <linux/nvs.h>
#define DSM_DRIVER_VERSION (3)
#define DSM_VENDOR "NVIDIA Corporation"
#define DSM_NAME "nvs_dsm"
#define DSM_DEV_PATH_N (64)
#define DSM_PUSH_DELAY_MS (100)
#define DSM_PUSH_DELAY_N (10)
enum DSM_KIF {
DSM_KIF_RELAY = 0,
DSM_KIF_IIO,
DSM_KIF_INPUT,
DSM_KIF_N,
};
static const char *dsm_kif_str[DSM_KIF_N] = {
[DSM_KIF_RELAY] = "RELAY",
[DSM_KIF_IIO] = "IIO",
[DSM_KIF_INPUT] = "INPUT",
};
static struct nvs_fn_if *(*nvs_kif[])(void) = {
#ifdef NVS_KIF_RELAY
nvs_relay,
#else
NULL,
#endif
#ifdef NVS_KIF_IIO
nvs_iio,
#else
NULL,
#endif
#ifdef NVS_KIF_INPUT
nvs_input,
#else
NULL,
#endif
};
struct dsm_state {
struct platform_device *pd;
struct mutex mutex;
struct delayed_work dw;
struct nvs_fn_if *nvs[ARRAY_SIZE(nvs_kif)];
void *nvs_st[ARRAY_SIZE(nvs_kif)];
struct sensor_cfg cfg;
struct nvs_dsm_msg msg;
s64 msg_ts;
int msg_err;
unsigned int sts;
unsigned int delay_ms;
unsigned int delay_n;
unsigned int connect_n;
unsigned int disconnect_n;
unsigned int kif;
bool mutex_locked;
char dev_path[DSM_DEV_PATH_N];
};
enum DSM_DBG {
DSM_DBG_STS = 0,
DSM_DBG_PUSH = 0xC6, /* use 0xD0 on cmd line */
DSM_DBG_MSG_FLAGS,
DSM_DBG_MSG_DEV_ID,
DSM_DBG_MSG_SNSR_ID,
DSM_DBG_UUID,
DSM_DBG_PUSH_DELAY_MS,
DSM_DBG_PUSH_DELAY_N,
DSM_DBG_N,
};
static struct dsm_state *dsm_state_local;
static void dsm_mutex_lock(struct dsm_state *st)
{
mutex_lock(&st->mutex);
st->mutex_locked = true;
}
static void dsm_mutex_unlock(struct dsm_state *st)
{
if (st->mutex_locked) {
st->mutex_locked = false;
mutex_unlock(&st->mutex);
}
}
static int dsm_push_msg(struct dsm_state *st, unsigned int kif_i,
char *dev_path)
{
int ret = 0;
if (dev_path) {
ret = 1;
if (st->sts & (NVS_STS_SPEW_MSG | NVS_STS_SPEW_DATA)) {
if (ret)
dev_info(&st->pd->dev, "%s NO %s\n",
__func__, dev_path);
else
dev_info(&st->pd->dev, "%s %s FOUND\n",
__func__, dev_path);
}
}
if (st->msg_err <= 0 || !ret) {
st->msg_ts = nvs_timestamp();
ret = st->nvs[kif_i]->handler(st->nvs_st[kif_i],
&st->msg, st->msg_ts);
if (ret > 0)
ret = 0;
st->msg_err = ret;
dsm_mutex_unlock(st);
} else {
if (st->sts & (NVS_STS_SPEW_MSG | NVS_STS_SPEW_DATA))
dev_info(&st->pd->dev,
"%s schedule_delayed_work=%ums (n=%u)\n",
__func__, st->delay_ms, st->msg_err);
if (st->msg_err > 0)
st->msg_err--;
schedule_delayed_work(&st->dw, msecs_to_jiffies(st->delay_ms));
ret = 0;
}
return ret;
}
static void dsm_work(struct work_struct *ws)
{
struct dsm_state *st = container_of((struct delayed_work *)ws,
struct dsm_state, dw);
dsm_push_msg(st, st->kif, st->dev_path);
}
static int dsm_push(struct dsm_state *st, int dev_id, bool connect,
int snsr_id, unsigned char *uuid, unsigned int kif_i)
{
int ret = 0;
dsm_mutex_lock(st);
st->kif = kif_i;
memset(&st->msg, 0, sizeof(st->msg));
st->msg.ver = sizeof(st->msg);
st->msg.dev_id = dev_id;
st->msg.snsr_id = snsr_id;
if (uuid)
memcpy(&st->msg.uuid, uuid, sizeof(st->msg.uuid));
if (connect) {
st->msg.flags = 1 << NVS_DSM_MSG_FLAGS_CONNECT;
st->connect_n++;
if (!st->connect_n)
st->connect_n--;
if (dev_id >= 0) {
switch (kif_i) {
case DSM_KIF_RELAY:
ret = snprintf(st->dev_path,
sizeof(st->dev_path),
"/dev/nvs:device%d", dev_id);
break;
case DSM_KIF_IIO:
ret = snprintf(st->dev_path,
sizeof(st->dev_path),
"/dev/iio:device%d", dev_id);
break;
case DSM_KIF_INPUT:
ret = snprintf(st->dev_path,
sizeof(st->dev_path),
"/dev/input/event%d", dev_id);
break;
}
if (ret > 0) {
st->msg_err = st->delay_n;
return dsm_push_msg(st, kif_i, st->dev_path);
}
} else {
st->msg_err = 0;
return dsm_push_msg(st, kif_i, NULL);
}
dsm_mutex_unlock(st);
return -ENOMEM;
}
/* disconnect */
st->disconnect_n++;
if (!st->disconnect_n)
st->disconnect_n--;
st->msg_err = 0;
return dsm_push_msg(st, kif_i, NULL);
}
int nvs_dsm_relay(int dev_id, bool connect, int snsr_id, unsigned char *uuid)
{
struct dsm_state *st = dsm_state_local;
int ret = -EPERM;
if (st == NULL)
return -EPERM;
if (st->nvs[DSM_KIF_RELAY] && st->nvs_st[DSM_KIF_RELAY])
ret = dsm_push(st, dev_id, connect, snsr_id, uuid,
DSM_KIF_RELAY);
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&st->pd->dev, "%s err=%d\n", __func__, ret);
return ret;
}
int nvs_dsm_iio(int dev_id, bool connect, int snsr_id, unsigned char *uuid)
{
struct dsm_state *st = dsm_state_local;
int ret = -EPERM;
if (st == NULL)
return -EPERM;
if (st->nvs[DSM_KIF_IIO] && st->nvs_st[DSM_KIF_IIO])
ret = dsm_push(st, dev_id, connect, snsr_id, uuid,
DSM_KIF_IIO);
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&st->pd->dev, "%s err=%d\n", __func__, ret);
return ret;
}
EXPORT_SYMBOL_GPL(nvs_dsm_iio);
int nvs_dsm_input(int dev_id, bool connect, int snsr_id, unsigned char *uuid)
{
struct dsm_state *st = dsm_state_local;
int ret = -EPERM;
if (st == NULL)
return -EPERM;
if (st->nvs[DSM_KIF_INPUT] && st->nvs_st[DSM_KIF_INPUT])
ret = dsm_push(st, dev_id, connect, snsr_id, uuid,
DSM_KIF_INPUT);
if (st->sts & NVS_STS_SPEW_MSG)
dev_info(&st->pd->dev, "%s err=%d\n", __func__, ret);
return ret;
}
static int dsm_nvs_write(void *client, int snsr_id, unsigned int nvs)
{
struct dsm_state *st = (struct dsm_state *)client;
unsigned int val;
switch (nvs & 0xFF) {
case DSM_DBG_STS:
return 0;
case DSM_DBG_PUSH:
val = (nvs >> 8) & 0xFF;
if (val)
/* 0 based index */
val--;
else
/* kif = auto: use last used */
val = st->kif;
if (val < ARRAY_SIZE(nvs_kif) && nvs_kif[val])
return dsm_push_msg(st, val, NULL);
return -EINVAL;
case DSM_DBG_MSG_FLAGS:
st->msg.flags = (nvs >> 8) & 0xFF;
val = (nvs >> 16) & 0xFF;
if (val)
st->msg.ver = val;
else
st->msg.ver = sizeof(st->msg);
return 0;
case DSM_DBG_MSG_DEV_ID:
st->msg.dev_id = nvs >> 8;
return 0;
case DSM_DBG_MSG_SNSR_ID:
st->msg.snsr_id = nvs >> 8;
return 0;
case DSM_DBG_UUID:
val = (nvs >> 16) & 0xF;
st->msg.uuid[val] = (nvs >> 8) & 0xFF;
return 0;
case DSM_DBG_PUSH_DELAY_MS:
st->delay_ms = nvs >> 8;
return 0;
case DSM_DBG_PUSH_DELAY_N:
st->delay_n = nvs >> 8;
return 0;
}
return -EINVAL;
}
static int dsm_nvs_read(void *client, int snsr_id, char *buf)
{
struct dsm_state *st = (struct dsm_state *)client;
int i;
ssize_t t;
t = snprintf(buf, PAGE_SIZE, "driver v.%u\n", DSM_DRIVER_VERSION);
t += snprintf(buf + t, PAGE_SIZE - t, "Device tree:\n");
t += snprintf(buf + t, PAGE_SIZE - t,
" push_delay_ms=%ums\n", st->delay_ms);
t += snprintf(buf + t, PAGE_SIZE - t,
" push_delay_count=%u\n", st->delay_n);
t += snprintf(buf + t, PAGE_SIZE - t,
"connections=%u\n", st->connect_n);
t += snprintf(buf + t, PAGE_SIZE - t,
"disconnections=%u\n", st->disconnect_n);
if (st->kif < DSM_KIF_N) {
t += snprintf(buf + t, PAGE_SIZE - t, "Last DSM message:\n");
t += snprintf(buf + t, PAGE_SIZE - t,
" msg ver/size: %u\n", st->msg.ver);
t += snprintf(buf + t, PAGE_SIZE - t,
" flags: 0x%X\n", st->msg.flags);
t += snprintf(buf + t, PAGE_SIZE - t,
" dev_id: %d\n", st->msg.dev_id);
t += snprintf(buf + t, PAGE_SIZE - t,
" snsr_id: %d\n", st->msg.snsr_id);
t += snprintf(buf + t, PAGE_SIZE - t, " uuid: ");
for (i = 0; i < 16; i++)
t += snprintf(buf + t, PAGE_SIZE - t, "%02x ",
st->msg.uuid[i]);
t += snprintf(buf + t, PAGE_SIZE - t,
"\nmessage timestamp=%lld\n", st->msg_ts);
if (st->msg_err > 0)
t += snprintf(buf + t, PAGE_SIZE - t,
"message delay pending=%dms\n",
st->msg_err * st->delay_ms);
else
t += snprintf(buf + t, PAGE_SIZE - t,
"message error=%d\n", st->msg_err);
t += snprintf(buf + t, PAGE_SIZE - t, "KIF[%u] %s\n",
st->kif + 1, dsm_kif_str[st->kif]);
}
t += snprintf(buf + t, PAGE_SIZE - t, "Supported KIFs:\n");
for (i = 0; i < DSM_KIF_N; i++) {
if (nvs_kif[i])
t += snprintf(buf + t, PAGE_SIZE - t,
" KIF[%u]=%s \n", i + 1,
dsm_kif_str[i]);
}
return t;
}
static struct nvs_fn_dev dsm_fn_dev = {
.nvs_write = dsm_nvs_write,
.nvs_read = dsm_nvs_read,
};
static void dsm_shutdown(struct platform_device *pd)
{
struct dsm_state *st = (struct dsm_state *)dev_get_drvdata(&pd->dev);
unsigned int i;
dsm_state_local = NULL;
for (i = 0; i < ARRAY_SIZE(nvs_kif); i++) {
if (st->nvs[i] && st->nvs_st[i])
st->nvs[i]->shutdown(st->nvs_st[i]);
}
}
static int dsm_remove(struct platform_device *pd)
{
struct dsm_state *st = (struct dsm_state *)dev_get_drvdata(&pd->dev);
unsigned int i;
if (st != NULL) {
dsm_shutdown(pd);
for (i = 0; i < ARRAY_SIZE(nvs_kif); i++) {
if (st->nvs[i] && st->nvs_st[i])
st->nvs[i]->remove(st->nvs_st[i]);
}
}
dev_info(&pd->dev, "%s\n", __func__);
return 0;
}
static struct sensor_cfg dsm_sensor_cfg = {
.name = "dynamic_sensor_meta",
.kbuf_sz = 32,
.ch_n = 1,
.ch_sz = sizeof(struct nvs_dsm_msg),
.part = DSM_NAME,
.vendor = DSM_VENDOR,
.version = DSM_DRIVER_VERSION,
.flags = SENSOR_FLAG_SPECIAL_REPORTING_MODE,
};
static int dsm_of_dt(struct dsm_state *st)
{
st->kif = -1;
st->delay_ms = DSM_PUSH_DELAY_MS;
st->delay_n = DSM_PUSH_DELAY_N;
of_property_read_u32(st->pd->dev.of_node, "dsm_push_delay_ms",
&st->delay_ms);
of_property_read_u32(st->pd->dev.of_node, "dsm_push_delay_count",
&st->delay_n);
memcpy(&st->cfg, &dsm_sensor_cfg, sizeof(st->cfg));
return nvs_of_dt(st->pd->dev.of_node, &st->cfg, NULL);
}
static int dsm_init(struct dsm_state *st)
{
unsigned int i;
unsigned int n;
int ret;
ret = dsm_of_dt(st);
if (ret < 0) {
if (ret == -ENODEV) {
dev_info(&st->pd->dev, "%s DT disabled\n", __func__);
} else {
dev_err(&st->pd->dev, "%s _of_dt ERR\n", __func__);
ret = -ENODEV;
}
return ret;
}
n = 0;
for (i = 0; i < ARRAY_SIZE(nvs_kif); i++) {
if (nvs_kif[i]) {
st->nvs[i] = nvs_kif[i]();
if (!st->nvs[i]) {
dev_err(&st->pd->dev, "%s nvs_kif[%u] ERR\n",
__func__, i);
continue;
}
ret = st->nvs[i]->probe(&st->nvs_st[i],
st, &st->pd->dev,
&dsm_fn_dev, &st->cfg);
if (ret) {
dev_err(&st->pd->dev, "%s nvs_probe ERR\n",
__func__);
st->nvs[i] = NULL;
st->nvs_st[i] = NULL;
continue;
}
n++;
}
}
if (!n)
return -ENODEV;
INIT_DELAYED_WORK(&st->dw, dsm_work);
return 0;
}
static int dsm_probe(struct platform_device *pd)
{
struct dsm_state *st;
int ret;
dev_info(&pd->dev, "%s\n", __func__);
st = devm_kzalloc(&pd->dev, sizeof(*st), GFP_KERNEL);
if (!st) {
dev_err(&pd->dev, "%s devm_kzalloc ERR\n", __func__);
return -ENOMEM;
}
dev_set_drvdata(&pd->dev, st);
st->pd = pd;
dsm_fn_dev.sts = &st->sts;
mutex_init(&st->mutex);
ret = dsm_init(st);
if (ret < 0)
dsm_remove(pd);
else
dsm_state_local = st;
dev_info(&pd->dev, "%s done\n", __func__);
return ret;
}
static const struct of_device_id dsm_of_match[] = {
{ .compatible = "nvidia,nvs_dsm", },
{},
};
MODULE_DEVICE_TABLE(of, dsm_of_match);
static struct platform_driver dsm_driver = {
.probe = dsm_probe,
.remove = dsm_remove,
.shutdown = dsm_shutdown,
.driver = {
.name = DSM_NAME,
.owner = THIS_MODULE,
.of_match_table = of_match_ptr(dsm_of_match),
},
};
module_platform_driver(dsm_driver);
MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("NVidia Sensor Dynamic Sensor Meta driver");
MODULE_AUTHOR("NVIDIA Corporation");