Initial commit: Linux kernel 4.19 for Unplugged OS

This commit is contained in:
ziv committed 2025-10-06 13:52:42 +00:00
commit d30235b04c
80519 files changed
+35800681

No files matched your search

@@ -0,0 +1,7 @@
config MTK_SENSORHUB
tristate "sensorhub architecture for MediaTek package"
help
It support mediatek sensorhub architecture.
If this option is set,
it will support
mediatek sensorhub architecture.
@@ -0,0 +1,17 @@
ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/sensorhub
ccflags-y += -I$(srctree)/drivers/misc/mediatek/scp/include
obj-$(CONFIG_MTK_SENSORHUB) += sensorhub.o
sensorhub-objs += tiny_crc8.o
sensorhub-objs += ready.o
sensorhub-objs += ipi_comm.o
sensorhub-objs += sensor_comm.o
sensorhub-objs += sensor_list.o
sensorhub-objs += transceiver.o
sensorhub-objs += share_memory.o
sensorhub-objs += timesync.o
sensorhub-objs += debug.o
sensorhub-objs += custom_cmd.o
#obj-y += test/
@@ -0,0 +1,293 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2021 MediaTek Inc.
*/
#define pr_fmt(fmt) "cust_cmd " fmt
#include <linux/spinlock.h>
#include <linux/completion.h>
#include <linux/mutex.h>
#include <linux/atomic.h>
#include "sensor_comm.h"
#include "custom_cmd.h"
#include "share_memory.h"
static DEFINE_MUTEX(bus_user_lock);
static atomic_t cust_cmd_sequence;
static DECLARE_COMPLETION(cust_cmd_done);
static DEFINE_SPINLOCK(rx_notify_lock);
static struct sensor_comm_notify rx_notify;
static DECLARE_COMPLETION(cust_cmd_fast_done);
static DEFINE_SPINLOCK(rx_fast_notify_lock);
static struct sensor_comm_notify rx_fast_notify;
static struct share_mem cust_cmd_shm_rx;
static struct share_mem cust_cmd_shm_tx;
static void custom_cmd_notify_handler(struct sensor_comm_notify *n,
void *private_data)
{
spin_lock(&rx_notify_lock);
memcpy(&rx_notify, n, sizeof(rx_notify));
spin_unlock(&rx_notify_lock);
complete(&cust_cmd_done);
}
static void custom_cmd_fast_notify_handler(struct sensor_comm_notify *n,
void *private_data)
{
spin_lock(&rx_fast_notify_lock);
memcpy(&rx_fast_notify, n, sizeof(rx_fast_notify));
spin_unlock(&rx_fast_notify_lock);
complete(&cust_cmd_fast_done);
}
static int custom_cmd_slow_seq(int sensor_type, struct share_mem_cmd *shm_cmd)
{
int ret = 0;
int timeout = 0;
unsigned long flags = 0;
uint32_t write_position = 0;
struct share_mem_notify notify;
/*
* NOTE: must reinit_completion before sensor_comm_notify
* wrong sequence:
* sensor_comm_notify ---> reinit_completion -> wait_for_completion
* |
* complete
* if complete before reinit_completion, will lose this complete
* right sequence:
* reinit_completion -> sensor_comm_notify -> wait_for_completion
*/
reinit_completion(&cust_cmd_done);
share_mem_write_reset(&cust_cmd_shm_tx);
ret = share_mem_write(&cust_cmd_shm_tx, shm_cmd, sizeof(*shm_cmd));
if (ret < 0)
return ret;
if (!ret)
return -ENOMEM;
/* safe sequence given by atomic, round from 0 to 255 */
notify.sequence = atomic_inc_return(&cust_cmd_sequence);
notify.sensor_type = sensor_type;
notify.notify_cmd = SENS_COMM_NOTIFY_CUSTOM_CMD;
ret = share_mem_flush(&cust_cmd_shm_tx, &notify);
if (ret < 0)
return ret;
timeout = wait_for_completion_timeout(&cust_cmd_done,
msecs_to_jiffies(100));
if (!timeout)
return -ETIMEDOUT;
spin_lock_irqsave(&rx_notify_lock, flags);
if (rx_notify.sequence != notify.sequence &&
rx_notify.sensor_type != notify.sensor_type &&
rx_notify.command != notify.notify_cmd) {
spin_unlock_irqrestore(&rx_notify_lock, flags);
return -EREMOTEIO;
}
write_position = rx_notify.value[0];
spin_unlock_irqrestore(&rx_notify_lock, flags);
ret = share_mem_seek(&cust_cmd_shm_rx, write_position);
if (ret < 0)
return ret;
ret = share_mem_read(&cust_cmd_shm_rx, shm_cmd, sizeof(*shm_cmd));
if (ret < 0)
return ret;
if (!ret)
return -ENOMEM;
share_mem_read_reset(&cust_cmd_shm_rx);
return 0;
}
static int custom_cmd_slow_comm(int sensor_type, struct custom_cmd *cust_cmd)
{
int retry = 0, ret = 0;
const int max_retry = 3;
struct share_mem_cmd shm_cmd;
if (sizeof(shm_cmd) != sizeof(*cust_cmd) ||
sizeof(shm_cmd.data) != sizeof(cust_cmd->data) ||
offsetof(typeof(shm_cmd), data) !=
offsetof(typeof(*cust_cmd), data)) {
pr_err("%u %u struct not match\n",
sensor_type, cust_cmd->command);
return -EINVAL;
}
shm_cmd.command = cust_cmd->command;
shm_cmd.tx_len = cust_cmd->tx_len;
shm_cmd.rx_len = cust_cmd->rx_len;
memcpy(shm_cmd.data, cust_cmd->data, shm_cmd.tx_len);
mutex_lock(&bus_user_lock);
do {
ret = custom_cmd_slow_seq(sensor_type, &shm_cmd);
} while (retry++ < max_retry && ret < 0);
mutex_unlock(&bus_user_lock);
if (ret < 0) {
pr_err("%u %u comm fail %d\n",
sensor_type, cust_cmd->command, ret);
return ret;
}
if (shm_cmd.command != cust_cmd->command ||
shm_cmd.rx_len > sizeof(shm_cmd.data)) {
pr_err("%u %u param error\n",
sensor_type, cust_cmd->command);
return -EINVAL;
}
cust_cmd->rx_len = shm_cmd.rx_len;
memcpy(cust_cmd->data, shm_cmd.data, cust_cmd->rx_len);
return 0;
}
static int custom_cmd_fast_seq(int sensor_type, struct custom_cmd *cust_cmd)
{
int ret = 0;
int timeout = 0;
unsigned long flags = 0;
struct sensor_comm_notify notify;
struct custom_cmd *rx_cmd = NULL;
/*
* NOTE: must reinit_completion before sensor_comm_notify
* wrong sequence:
* sensor_comm_notify ---> reinit_completion -> wait_for_completion
* |
* complete
* if complete before reinit_completion, will lose this complete
* right sequence:
* reinit_completion -> sensor_comm_notify -> wait_for_completion
*/
reinit_completion(&cust_cmd_fast_done);
/* safe sequence given by atomic, round from 0 to 255 */
notify.sequence = atomic_inc_return(&cust_cmd_sequence);
notify.sensor_type = sensor_type;
notify.command = SENS_COMM_NOTIFY_FAST_CUST_CMD;
notify.length = offsetof(typeof(*cust_cmd), data) + cust_cmd->tx_len;
memcpy(notify.value, cust_cmd, notify.length);
ret = sensor_comm_notify(&notify);
if (ret < 0)
return ret;
timeout = wait_for_completion_timeout(&cust_cmd_fast_done,
msecs_to_jiffies(100));
if (!timeout)
return -ETIMEDOUT;
spin_lock_irqsave(&rx_fast_notify_lock, flags);
if (rx_fast_notify.sequence != notify.sequence &&
rx_fast_notify.sensor_type != notify.sensor_type &&
rx_fast_notify.command != notify.command) {
spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
return -EREMOTEIO;
}
rx_cmd = (struct custom_cmd *)rx_fast_notify.value;
if (rx_cmd->command != cust_cmd->command ||
rx_cmd->rx_len > sizeof(cust_cmd->data) ||
rx_cmd->rx_len > cust_cmd->rx_len) {
spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
return -EIO;
}
cust_cmd->rx_len = rx_cmd->rx_len;
memcpy(cust_cmd->data, rx_cmd->data, cust_cmd->rx_len);
spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
return 0;
}
static int custom_cmd_fast_comm(int sensor_type, struct custom_cmd *cust_cmd)
{
int retry = 0, ret = 0;
const int max_retry = 3;
mutex_lock(&bus_user_lock);
do {
ret = custom_cmd_fast_seq(sensor_type, cust_cmd);
} while (retry++ < max_retry && ret < 0);
mutex_unlock(&bus_user_lock);
if (ret < 0)
pr_err("%u %u fast comm fail %d\n",
sensor_type, cust_cmd->command, ret);
return ret;
}
int custom_cmd_comm_with(int sensor_type, struct custom_cmd *cust_cmd)
{
int ret = 0;
uint8_t header_len = offsetof(typeof(*cust_cmd), data);
uint16_t tx_len = header_len + cust_cmd->tx_len;
uint16_t rx_len = header_len + cust_cmd->rx_len;
if (tx_len > sizeof(rx_fast_notify.value) ||
rx_len > sizeof(rx_fast_notify.value))
ret = custom_cmd_slow_comm(sensor_type, cust_cmd);
else
ret = custom_cmd_fast_comm(sensor_type, cust_cmd);
return ret;
}
static int custom_cmd_w_shm_cfg(struct share_mem_config *cfg,
void *private_data)
{
cust_cmd_shm_tx.name = "cust_cmd_w";
cust_cmd_shm_tx.item_size = sizeof(struct share_mem_cmd);
cust_cmd_shm_tx.buffer_full_detect = false;
return share_mem_init(&cust_cmd_shm_tx, cfg);
}
static int custom_cmd_r_shm_cfg(struct share_mem_config *cfg,
void *private_data)
{
cust_cmd_shm_rx.name = "cust_cmd_r";
cust_cmd_shm_rx.item_size = sizeof(struct share_mem_cmd);
cust_cmd_shm_rx.buffer_full_detect = false;
return share_mem_init(&cust_cmd_shm_rx, cfg);
}
int custom_cmd_init(void)
{
unsigned long flags = 0;
atomic_set(&cust_cmd_sequence, 0);
spin_lock_irqsave(&rx_notify_lock, flags);
memset(&rx_notify, 0, sizeof(rx_notify));
spin_unlock_irqrestore(&rx_notify_lock, flags);
spin_lock_irqsave(&rx_fast_notify_lock, flags);
memset(&rx_fast_notify, 0, sizeof(rx_fast_notify));
spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_CUSTOM_CMD,
custom_cmd_notify_handler, NULL);
sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_FAST_CUST_CMD,
custom_cmd_fast_notify_handler, NULL);
share_mem_config_handler_register(SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE,
custom_cmd_w_shm_cfg, NULL);
share_mem_config_handler_register(SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE,
custom_cmd_r_shm_cfg, NULL);
return 0;
}
void custom_cmd_exit(void)
{
sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_CUSTOM_CMD);
sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_FAST_CUST_CMD);
share_mem_config_handler_unregister(SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE);
share_mem_config_handler_unregister(SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE);
}
@@ -0,0 +1,15 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2021 MediaTek Inc.
*/
#ifndef _CUSTOM_CMD_H_
#define _CUSTOM_CMD_H_
#include "hf_sensor_io.h"
int custom_cmd_comm_with(int sensor_type, struct custom_cmd *cust_cmd);
int custom_cmd_init(void);
void custom_cmd_exit(void);
#endif
@@ -0,0 +1,166 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "debug " fmt
#include <linux/spinlock.h>
#include <linux/completion.h>
#include <linux/mutex.h>
#include <linux/atomic.h>
#include <linux/slab.h>
#include "hf_sensor_type.h"
#include "sensor_comm.h"
#include "debug.h"
#include "share_memory.h"
static DEFINE_MUTEX(bus_user_lock);
static atomic_t debug_sequence;
static DECLARE_COMPLETION(debug_done);
static DEFINE_SPINLOCK(rx_notify_lock);
static struct sensor_comm_notify rx_notify;
static struct share_mem debug_shm_reader;
static void debug_notify_handler(struct sensor_comm_notify *n,
void *private_data)
{
spin_lock(&rx_notify_lock);
memcpy(&rx_notify, n, sizeof(rx_notify));
spin_unlock(&rx_notify_lock);
complete(&debug_done);
}
static int debug_seq_get_debug(uint8_t sensor_type, uint8_t *buffer,
unsigned int len)
{
int ret = 0;
int timeout = 0;
unsigned long flags = 0;
struct sensor_comm_ctrl *ctrl = NULL;
struct share_mem_debug *shm_debug = NULL;
uint32_t write_position = 0;
ret = share_mem_read_reset(&debug_shm_reader);
if (ret < 0) {
pr_err("%u reset fail %d\n", sensor_type, ret);
return ret;
}
/*
* NOTE: must reinit_completion before sensor_comm_notify
* wrong sequence:
* sensor_comm_ctrl_send ---> reinit_completion -> wait_for_completion
* |
* complete
* if complete before reinit_completion, will lose this complete
* right sequence:
* reinit_completion -> sensor_comm_ctrl_send -> wait_for_completion
*/
reinit_completion(&debug_done);
ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
ctrl->sensor_type = sensor_type;
ctrl->command = SENS_COMM_CTRL_DEBUG_CMD;
ctrl->length = sizeof(ctrl->data[0]);
/* safe sequence given by atomic, round from 0 to 255 */
ctrl->data[0] = atomic_inc_return(&debug_sequence);
ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
if (ret < 0) {
pr_err("%u send fail %d\n", sensor_type, ret);
goto out1;
}
timeout = wait_for_completion_timeout(&debug_done,
msecs_to_jiffies(100));
if (!timeout) {
pr_err("%u wait completion timeout\n", sensor_type);
ret = -ETIMEDOUT;
goto out1;
}
spin_lock_irqsave(&rx_notify_lock, flags);
if (rx_notify.sequence != ctrl->data[0] &&
rx_notify.sensor_type != ctrl->sensor_type &&
rx_notify.command != ctrl->command) {
pr_err("%u reply fail\n", sensor_type);
spin_unlock_irqrestore(&rx_notify_lock, flags);
ret = -EREMOTEIO;
goto out1;
}
write_position = rx_notify.value[0];
spin_unlock_irqrestore(&rx_notify_lock, flags);
ret = share_mem_seek(&debug_shm_reader, write_position);
if (ret < 0) {
pr_err("%u seek fail %d\n", sensor_type, ret);
goto out1;
}
shm_debug = kzalloc(sizeof(*shm_debug), GFP_KERNEL);
ret = share_mem_read(&debug_shm_reader, shm_debug, sizeof(*shm_debug));
if (ret < 0) {
pr_err("%u read fail %d\n", sensor_type, ret);
goto out2;
} else if (ret != sizeof(*shm_debug)) {
pr_err("%u size fail %d\n", sensor_type, ret);
ret = -EPROTO;
goto out2;
}
len = len < shm_debug->written ? len : shm_debug->written;
strlcpy(buffer, shm_debug->buffer, len);
ret = len;
out2:
kfree(shm_debug);
out1:
kfree(ctrl);
return ret;
}
/* SENSOR_TYPE_INVALID for get sensor_manager debug information */
int debug_get_debug(uint8_t sensor_type, uint8_t *buffer, unsigned int len)
{
int retry = 0, ret = 0;
const int max_retry = 3;
mutex_lock(&bus_user_lock);
do {
ret = debug_seq_get_debug(sensor_type, buffer, len);
} while (retry++ < max_retry && ret < 0);
mutex_unlock(&bus_user_lock);
return ret;
}
static int debug_share_mem_cfg(struct share_mem_config *cfg,
void *private_data)
{
debug_shm_reader.name = "debug_r";
debug_shm_reader.item_size = sizeof(struct share_mem_debug);
debug_shm_reader.buffer_full_detect = false;
return share_mem_init(&debug_shm_reader, cfg);
}
int debug_init(void)
{
unsigned long flags = 0;
atomic_set(&debug_sequence, 0);
spin_lock_irqsave(&rx_notify_lock, flags);
memset(&rx_notify, 0, sizeof(rx_notify));
spin_unlock_irqrestore(&rx_notify_lock, flags);
sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_DEBUG_CMD,
debug_notify_handler, NULL);
share_mem_config_handler_register(SHARE_MEM_DEBUG_PAYLOAD_TYPE,
debug_share_mem_cfg, NULL);
return 0;
}
void debug_exit(void)
{
sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_DEBUG_CMD);
share_mem_config_handler_unregister(SHARE_MEM_DEBUG_PAYLOAD_TYPE);
}
@@ -0,0 +1,13 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _SENSOR_DEBUG_H_
#define _SENSOR_DEBUG_H_
int debug_get_debug(uint8_t sensor_type, uint8_t *buffer, unsigned int len);
int debug_init(void);
void debug_exit(void);
#endif
@@ -0,0 +1,314 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "ipi_comm " fmt
#include <linux/module.h>
#include <linux/workqueue.h>
#include <linux/spinlock.h>
#include <linux/completion.h>
#include <linux/delay.h>
#include "scp.h"
#include "ipi_comm.h"
struct ipi_controller {
spinlock_t lock;
bool running;
struct list_head head;
struct workqueue_struct *workqueue;
struct work_struct work;
void (*notify_callback)(int id, void *data, unsigned int len);
};
struct ipi_hw_transfer {
struct completion done;
int count;
/* data buffers */
int id;
const unsigned char *tx;
unsigned char *rx;
unsigned int tx_len;
unsigned int rx_len;
void *context;
};
#define ipi_len(x) (((x) + MBOX_SLOT_SIZE - 1) / MBOX_SLOT_SIZE)
static struct ipi_controller controller;
static struct ipi_hw_transfer hw_transfer;
static DEFINE_SPINLOCK(hw_transfer_lock);
static uint8_t ctrl_payload[PIN_IN_SIZE_SENSOR_CTRL * MBOX_SLOT_SIZE];
static uint8_t notify_payload[PIN_IN_SIZE_SENSOR_NOTIFY * MBOX_SLOT_SIZE];
static inline int ipi_retry_transfer(int id, void *tx, int tx_len)
{
int ret = 0, retry = 0;
do {
ret = mtk_ipi_send(&scp_ipidev, id, 0, tx, ipi_len(tx_len), 0);
if (ret < 0 && ret != IPI_PIN_BUSY)
return -EIO;
if (ret == IPI_PIN_BUSY) {
if (retry++ >= 1000)
return -EBUSY;
if (retry % 100 == 0)
usleep_range(1000, 2000);
}
} while (ret == IPI_PIN_BUSY);
return 0;
}
static int ipi_transfer_buffer(struct ipi_transfer *t)
{
int ret = 0;
int timeout;
unsigned long flags;
struct ipi_hw_transfer *hw = &hw_transfer;
spin_lock_irqsave(&hw_transfer_lock, flags);
hw->id = t->id;
hw->tx = t->tx_buf;
hw->rx = t->rx_buf;
hw->tx_len = t->tx_len;
hw->rx_len = t->rx_len;
reinit_completion(&hw->done);
hw->context = &hw->done;
spin_unlock_irqrestore(&hw_transfer_lock, flags);
ret = ipi_retry_transfer(hw->id, (void *)hw->tx, hw->tx_len);
if (ret < 0)
return ret;
timeout = wait_for_completion_timeout(&hw->done,
msecs_to_jiffies(100));
spin_lock_irqsave(&hw_transfer_lock, flags);
if (!timeout) {
hw->count = -ETIMEDOUT;
pr_err_ratelimited("timeout %u %u %u %u %u\n",
hw->tx[0], hw->tx[1], hw->tx[2],
hw->tx[3], hw->tx[4]);
}
hw->context = NULL;
spin_unlock_irqrestore(&hw_transfer_lock, flags);
return hw->count;
}
static void ipi_complete(void *arg)
{
complete(arg);
}
static void ipi_transfer_messages(void)
{
struct ipi_message *m;
struct ipi_transfer *t = NULL;
int status = 0;
unsigned long flags;
spin_lock_irqsave(&controller.lock, flags);
if (list_empty(&controller.head) || controller.running)
goto out;
controller.running = true;
while (!list_empty(&controller.head)) {
m = list_first_entry(&controller.head,
struct ipi_message, list);
list_del(&m->list);
spin_unlock_irqrestore(&controller.lock, flags);
list_for_each_entry(t, &m->transfers, transfer_list) {
if (!t->tx_buf && t->tx_len) {
status = -EINVAL;
break;
}
if (t->tx_len)
status = ipi_transfer_buffer(t);
if (status < 0) {
break;
} else if (status != t->rx_len) {
status = -EBADMSG;
break;
}
status = 0;
}
m->status = status;
m->complete(m->context);
spin_lock_irqsave(&controller.lock, flags);
}
controller.running = false;
out:
spin_unlock_irqrestore(&controller.lock, flags);
}
static void ipi_prefetch_messages(void)
{
ipi_transfer_messages();
}
static void ipi_work(struct work_struct *work)
{
ipi_transfer_messages();
}
static int __ipi_transfer(struct ipi_message *m)
{
unsigned long flags;
m->status = -EINPROGRESS;
spin_lock_irqsave(&controller.lock, flags);
list_add_tail(&m->list, &controller.head);
queue_work(controller.workqueue, &controller.work);
spin_unlock_irqrestore(&controller.lock, flags);
return 0;
}
static int __ipi_xfer(struct ipi_message *message)
{
DECLARE_COMPLETION_ONSTACK(done);
int status;
message->complete = ipi_complete;
message->context = &done;
status = __ipi_transfer(message);
if (status == 0) {
ipi_prefetch_messages();
wait_for_completion(&done);
status = message->status;
}
message->context = NULL;
return status;
}
static int ipi_sync(int id, const unsigned char *txbuf, unsigned int n_tx,
unsigned char *rxbuf, unsigned int n_rx)
{
struct ipi_transfer t;
struct ipi_message m;
t.id = id;
t.tx_buf = txbuf;
t.tx_len = n_tx;
t.rx_buf = rxbuf;
t.rx_len = n_rx;
ipi_message_init(&m);
ipi_message_add_tail(&t, &m);
return __ipi_xfer(&m);
}
static int ipi_async(struct ipi_message *m)
{
return __ipi_transfer(m);
}
int ipi_comm_sync(int id, unsigned char *tx, unsigned int n_tx,
unsigned char *rx, unsigned int n_rx)
{
return ipi_sync(id, tx, n_tx, rx, n_rx);
}
int ipi_comm_async(struct ipi_message *m)
{
return ipi_async(m);
}
int ipi_comm_noack(int id, unsigned char *tx, unsigned int n_tx)
{
return ipi_retry_transfer(id, tx, n_tx);
}
static void ipi_comm_complete(unsigned char *buffer, unsigned int len)
{
struct ipi_hw_transfer *hw = &hw_transfer;
spin_lock(&hw_transfer_lock);
if (!hw->context) {
pr_err("dropped transfer\n");
goto out;
}
/* only copy hw->rx_len bytes to hw->rx to avoid memory corruption */
memcpy(hw->rx, buffer, hw->rx_len);
/* hw->count give real len */
hw->count = len;
complete(hw->context);
out:
spin_unlock(&hw_transfer_lock);
}
static int ipi_comm_ctrl_handler(unsigned int id, void *prdata,
void *data, unsigned int len)
{
WARN_ON(id != IPI_IN_SENSOR_CTRL);
ipi_comm_complete(data, len);
return 0;
}
static int ipi_comm_notify_handler(unsigned int id, void *prdata,
void *data, unsigned int len)
{
WARN_ON(id != IPI_IN_SENSOR_NOTIFY);
if (controller.notify_callback)
controller.notify_callback(id, data, len);
return 0;
}
int get_ctrl_id(void)
{
return IPI_OUT_SENSOR_CTRL;
}
int get_notify_id(void)
{
return IPI_OUT_SENSOR_NOTIFY;
}
void ipi_comm_notify_handler_register(
void (*f)(int id, void *data, unsigned int len))
{
controller.notify_callback = f;
}
void ipi_comm_notify_handler_unregister(void)
{
controller.notify_callback = NULL;
}
int ipi_comm_init(void)
{
int ret = 0;
init_completion(&hw_transfer.done);
INIT_WORK(&controller.work, ipi_work);
INIT_LIST_HEAD(&controller.head);
spin_lock_init(&controller.lock);
controller.workqueue = alloc_workqueue("ipi_comm",
WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
if (controller.workqueue == NULL) {
pr_err("create workqueue fail\n");
return -1;
}
ret = mtk_ipi_register(&scp_ipidev, IPI_IN_SENSOR_CTRL,
ipi_comm_ctrl_handler, NULL, ctrl_payload);
if (ret < 0)
pr_err("register ipi %u fail %d\n", IPI_IN_SENSOR_CTRL, ret);
ret = mtk_ipi_register(&scp_ipidev, IPI_IN_SENSOR_NOTIFY,
ipi_comm_notify_handler, NULL, notify_payload);
if (ret < 0)
pr_err("register ipi %u fail %d\n", IPI_IN_SENSOR_NOTIFY, ret);
return 0;
}
void ipi_comm_exit(void)
{
mtk_ipi_unregister(&scp_ipidev, IPI_IN_SENSOR_CTRL);
mtk_ipi_unregister(&scp_ipidev, IPI_IN_SENSOR_NOTIFY);
flush_workqueue(controller.workqueue);
destroy_workqueue(controller.workqueue);
}
@@ -0,0 +1,52 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _MTK_NANOHUB_IPI_H_
#define _MTK_NANOHUB_IPI_H_
#include <linux/list.h>
struct ipi_transfer {
int id;
const unsigned char *tx_buf;
unsigned char *rx_buf;
unsigned int tx_len;
unsigned int rx_len;
struct list_head transfer_list;
};
struct ipi_message {
struct list_head transfers;
struct list_head list;
void *context;
int status;
void (*complete)(void *context);
};
static inline void ipi_message_init(struct ipi_message *m)
{
memset(m, 0, sizeof(*m));
INIT_LIST_HEAD(&m->transfers);
}
static inline void ipi_message_add_tail(struct ipi_transfer *t,
struct ipi_message *m)
{
list_add_tail(&t->transfer_list, &m->transfers);
}
int get_ctrl_id(void);
int get_notify_id(void);
int ipi_comm_sync(int id, unsigned char *tx, unsigned int n_tx,
unsigned char *rx, unsigned int n_rx);
int ipi_comm_async(struct ipi_message *m);
int ipi_comm_noack(int id, unsigned char *tx, unsigned int n_tx);
void ipi_comm_notify_handler_register(
void (*f)(int id, void *data, unsigned int len));
void ipi_comm_notify_handler_unregister(void);
int ipi_comm_init(void);
void ipi_comm_exit(void);
#endif
@@ -0,0 +1,158 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "sensor_ready " fmt
#include <linux/spinlock.h>
#include <linux/workqueue.h>
#include "scp.h"
#include "hf_sensor_type.h"
#include "sensor_comm.h"
#include "ready.h"
static DEFINE_SPINLOCK(sensor_ready_lock);
static bool scp_platform_ready;
static bool scp_sensor_ready;
static bool sensor_ready;
static BLOCKING_NOTIFIER_HEAD(sensor_ready_notifier_head);
static struct workqueue_struct *sensor_ready_workqueue;
static struct work_struct sensor_ready_work;
static struct delayed_work sensor_rescure_work;
void sensor_ready_notifier_chain_register(struct notifier_block *nb)
{
unsigned long flags = 0;
bool status = false;
blocking_notifier_chain_register(&sensor_ready_notifier_head, nb);
/*
* NOTE: must copy sensor_ready to status to avoid sensor_ready
* modified during notifier calling, keep notify the same status.
*/
spin_lock_irqsave(&sensor_ready_lock, flags);
status = sensor_ready;
spin_unlock_irqrestore(&sensor_ready_lock, flags);
if (status)
nb->notifier_call(nb, status, NULL);
}
void sensor_ready_notifier_chain_unregister(struct notifier_block *nb)
{
blocking_notifier_chain_unregister(&sensor_ready_notifier_head, nb);
}
static void scp_sensor_ready_notify_handler(struct sensor_comm_notify *n,
void *private_data)
{
unsigned long flags = 0;
spin_lock_irqsave(&sensor_ready_lock, flags);
scp_sensor_ready = true;
if (scp_platform_ready && scp_sensor_ready) {
sensor_ready = true;
cancel_delayed_work(&sensor_rescure_work);
queue_work(sensor_ready_workqueue, &sensor_ready_work);
}
spin_unlock_irqrestore(&sensor_ready_lock, flags);
}
static int scp_platform_ready_notifier_call(struct notifier_block *this,
unsigned long event, void *ptr)
{
unsigned long flags = 0;
int ret = 0;
struct sensor_comm_notify notify;
if (event == SCP_EVENT_STOP) {
cancel_delayed_work(&sensor_rescure_work);
spin_lock_irqsave(&sensor_ready_lock, flags);
scp_platform_ready = false;
scp_sensor_ready = false;
sensor_ready = false;
queue_work(sensor_ready_workqueue, &sensor_ready_work);
spin_unlock_irqrestore(&sensor_ready_lock, flags);
} else if (event == SCP_EVENT_READY) {
notify.sequence = 0;
notify.sensor_type = SENSOR_TYPE_INVALID;
notify.command = SENS_COMM_NOTIFY_READY_CMD;
notify.length = 0;
ret = sensor_comm_notify_bypass(&notify);
if (ret < 0)
pr_err("notify ready to scp fail %d\n", ret);
queue_delayed_work(sensor_ready_workqueue,
&sensor_rescure_work, msecs_to_jiffies(5000));
spin_lock_irqsave(&sensor_ready_lock, flags);
scp_platform_ready = true;
if (scp_platform_ready && scp_sensor_ready) {
sensor_ready = true;
queue_work(sensor_ready_workqueue,
&sensor_ready_work);
}
spin_unlock_irqrestore(&sensor_ready_lock, flags);
}
return NOTIFY_DONE;
}
static struct notifier_block scp_platform_ready_notifier = {
.notifier_call = scp_platform_ready_notifier_call,
};
static void sensor_ready_work_fn(struct work_struct *work)
{
unsigned long flags = 0;
bool status = false;
/*
* NOTE: must copy sensor_ready to status to avoid sensor_ready
* modified during notifier calling, keep notify the same status.
*/
spin_lock_irqsave(&sensor_ready_lock, flags);
status = sensor_ready;
spin_unlock_irqrestore(&sensor_ready_lock, flags);
blocking_notifier_call_chain(&sensor_ready_notifier_head,
status, NULL);
}
static void sensor_rescure_work_fn(struct work_struct *work)
{
unsigned long flags = 0;
spin_lock_irqsave(&sensor_ready_lock, flags);
if (scp_platform_ready && !scp_sensor_ready) {
pr_alert("rescure sensor by scp reset due to no ready ack\n");
scp_wdt_reset(0);
}
spin_unlock_irqrestore(&sensor_ready_lock, flags);
}
int host_ready_init(void)
{
INIT_WORK(&sensor_ready_work, sensor_ready_work_fn);
INIT_DELAYED_WORK(&sensor_rescure_work, sensor_rescure_work_fn);
sensor_ready_workqueue = alloc_workqueue("sensor_ready",
WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
if (!sensor_ready_workqueue) {
pr_err("alloc workqueue fail\n");
return -ENOMEM;
}
/*
* NOTE: sensor comm notify must before scp register notify
* to avoid lost sensor ready notify.
*/
sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_READY_CMD,
scp_sensor_ready_notify_handler, NULL);
scp_A_register_notify(&scp_platform_ready_notifier);
return 0;
}
void host_ready_exit(void)
{
scp_A_unregister_notify(&scp_platform_ready_notifier);
sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_READY_CMD);
flush_workqueue(sensor_ready_workqueue);
destroy_workqueue(sensor_ready_workqueue);
}
@@ -0,0 +1,22 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _READY_H_
#define _READY_H_
#include <linux/notifier.h>
enum sensor_ready_priority {
READY_STDPRI, /* others use std */
READY_HIGHPRI, /* transceiver use high */
READY_HIGHESTPRI, /* sensor comm use highest */
};
void sensor_ready_notifier_chain_register(struct notifier_block *nb);
void sensor_ready_notifier_chain_unregister(struct notifier_block *nb);
int host_ready_init(void);
void host_ready_exit(void);
#endif
@@ -0,0 +1,181 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "sensor_comm " fmt
#include <linux/err.h>
#include <linux/device.h>
#include <linux/printk.h>
#include <linux/atomic.h>
#include <linux/string.h>
#include <linux/timekeeping.h>
#include "tiny_crc8.h"
#include "sensor_comm.h"
#include "ready.h"
struct sensor_notify_handle {
void (*handler)(struct sensor_comm_notify *n, void *private_data);
void *private_data;
};
static bool scp_status;
static atomic_t sensor_comm_sequence;
static
struct sensor_notify_handle sens_notify_handle[MAX_SENS_COMM_NOTIFY_CMD];
static void sensor_comm_notify_handler(int id, void *data, unsigned int len)
{
uint8_t crc = 0;
struct sensor_comm_notify *notify = data;
struct sensor_notify_handle *handle = NULL;
crc = tiny_crc8((uint8_t *)notify, offsetof(typeof(*notify), crc8));
if (notify->crc8 != crc) {
pr_err("unrecognized packet %u %u %u %u %u %u\n",
notify->sequence, notify->sensor_type, notify->command,
notify->length, notify->crc8, crc);
return;
}
if (sizeof(*notify) != len)
return;
if (notify->command >= MAX_SENS_COMM_NOTIFY_CMD)
return;
handle = &sens_notify_handle[notify->command];
if (!handle->handler)
return;
handle->handler(notify, handle->private_data);
}
static int sensor_comm_ctrl_seq_send(struct sensor_comm_ctrl *ctrl,
unsigned int size)
{
int ret = 0;
uint8_t crc = 0;
struct sensor_comm_ack ack;
memset(&ack, 0, sizeof(ack));
/* safe sequence given by atomic, round from 0 to 255 */
ctrl->sequence = atomic_inc_return(&sensor_comm_sequence);
ctrl->crc8 = tiny_crc8((uint8_t *)ctrl, offsetof(typeof(*ctrl), crc8));
ret = ipi_comm_sync(get_ctrl_id(), (unsigned char *)ctrl, size,
(unsigned char *)&ack, sizeof(ack));
if (ret < 0)
return ret;
crc = tiny_crc8((uint8_t *)&ack, offsetof(typeof(ack), crc8));
if (ack.crc8 != crc) {
pr_err("unrecognized packet %u %u %u %u %u %u\n",
ack.sequence, ack.sensor_type, ack.command,
ack.ret_val, ack.crc8, crc);
return -EBADMSG;
}
if (ctrl->sequence != ack.sequence)
return -EILSEQ;
if (ctrl->sensor_type != ack.sensor_type)
return -EPROTO;
if (ctrl->command != ack.command)
return -EPROTO;
if (ack.ret_val < 0)
return -EREMOTEIO;
return 0;
}
int sensor_comm_ctrl_send(struct sensor_comm_ctrl *ctrl, unsigned int size)
{
int retry = 0, ret = 0;
const int max_retry = 10;
const int64_t timeout = 10000000000LL;
int64_t start_time = 0, duration = 0;
start_time = ktime_get_boottime_ns();
if (!READ_ONCE(scp_status)) {
pr_err_ratelimited("dropped comm %u %u\n",
ctrl->sensor_type, ctrl->command);
return 0;
}
do {
ret = sensor_comm_ctrl_seq_send(ctrl, size);
} while (retry++ < max_retry && ret < 0);
duration = ktime_get_boottime_ns() - start_time;
if (duration > timeout)
pr_notice("running time %lld, type %u, cmd %u, retries %d\n",
duration, ctrl->sensor_type, ctrl->command, retry);
return ret;
}
int sensor_comm_notify(struct sensor_comm_notify *notify)
{
if (!READ_ONCE(scp_status)) {
pr_err_ratelimited("dropped comm %u %u\n",
notify->sensor_type, notify->command);
return 0;
}
notify->crc8 =
tiny_crc8((uint8_t *)notify, offsetof(typeof(*notify), crc8));
return ipi_comm_noack(get_notify_id(), (unsigned char *)notify,
sizeof(*notify));
}
/*
* no need check scp_status due to send ready to scp.
*/
int sensor_comm_notify_bypass(struct sensor_comm_notify *notify)
{
notify->crc8 =
tiny_crc8((uint8_t *)notify, offsetof(typeof(*notify), crc8));
return ipi_comm_noack(get_notify_id(), (unsigned char *)notify,
sizeof(*notify));
}
void sensor_comm_notify_handler_register(uint8_t cmd,
void (*f)(struct sensor_comm_notify *n, void *private_data),
void *private_data)
{
if (cmd >= MAX_SENS_COMM_NOTIFY_CMD)
return;
sens_notify_handle[cmd].private_data = private_data;
sens_notify_handle[cmd].handler = f;
}
void sensor_comm_notify_handler_unregister(uint8_t cmd)
{
if (cmd >= MAX_SENS_COMM_NOTIFY_CMD)
return;
sens_notify_handle[cmd].handler = NULL;
sens_notify_handle[cmd].private_data = NULL;
}
static int sensor_comm_ready_notifier_call(struct notifier_block *this,
unsigned long event, void *ptr)
{
WRITE_ONCE(scp_status, !!event);
return NOTIFY_DONE;
}
static struct notifier_block sensor_comm_ready_notifier = {
.notifier_call = sensor_comm_ready_notifier_call,
.priority = READY_HIGHESTPRI,
};
int sensor_comm_init(void)
{
atomic_set(&sensor_comm_sequence, 0);
ipi_comm_init();
ipi_comm_notify_handler_register(sensor_comm_notify_handler);
sensor_ready_notifier_chain_register(&sensor_comm_ready_notifier);
return 0;
}
void sensor_comm_exit(void)
{
sensor_ready_notifier_chain_unregister(&sensor_comm_ready_notifier);
ipi_comm_notify_handler_unregister();
ipi_comm_exit();
}
@@ -0,0 +1,101 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _SENSOR_COMM_H_
#define _SENSOR_COMM_H_
#include "ipi_comm.h"
enum sensor_comm_ctrl_cmd {
SENS_COMM_CTRL_DISABLE_CMD,
SENS_COMM_CTRL_ENABLE_CMD,
SENS_COMM_CTRL_FLUSH_CMD,
SENS_COMM_CTRL_CALI_CMD,
SENS_COMM_CTRL_CONFIG_CMD,
SENS_COMM_CTRL_SELF_TEST_CMD,
SENS_COMM_CTRL_ENABLE_RAW_CMD,
SENS_COMM_CTRL_DISABLE_RAW_CMD,
SENS_COMM_CTRL_MASK_NOTIFY_CMD,
SENS_COMM_CTRL_UNMASK_NOTIFY_CMD,
SENS_COMM_CTRL_TIMESYNC_CMD,
SENS_COMM_CTRL_SHARE_MEMORY_CMD,
SENS_COMM_CTRL_DEBUG_CMD,
MAX_SENS_COMM_CTRL_CMD,
};
enum sensor_comm_notify_cmd {
SENS_COMM_NOTIFY_DATA_CMD,
SENS_COMM_NOTIFY_FULL_CMD,
SENS_COMM_NOTIFY_SUPER_DATA_CMD,
SENS_COMM_NOTIFY_SUPER_FULL_CMD,
SENS_COMM_NOTIFY_READY_CMD,
SENS_COMM_NOTIFY_LIST_CMD,
SENS_COMM_NOTIFY_DEBUG_CMD,
SENS_COMM_NOTIFY_CUSTOM_CMD,
SENS_COMM_NOTIFY_FAST_CUST_CMD,
MAX_SENS_COMM_NOTIFY_CMD,
};
struct sensor_comm_batch {
int64_t delay;
int64_t latency;
} __packed __aligned(4);
struct sensor_comm_timesync {
int64_t host_timestamp;
int64_t host_archcounter;
} __packed __aligned(4);
struct sensor_comm_share_mem {
uint8_t available_num;
struct {
uint8_t payload_type;
uint32_t payload_base;
} __aligned(4) base_info[4];
} __packed __aligned(4);
struct sensor_comm_ctrl {
uint8_t sequence;
uint8_t sensor_type;
uint8_t command;
uint8_t length;
uint8_t crc8;
uint8_t data[0] __aligned(4);
} __packed __aligned(4);
struct sensor_comm_ack {
uint8_t sequence;
uint8_t sensor_type;
uint8_t command;
int8_t ret_val;
uint8_t crc8;
} __packed __aligned(4);
struct data_notify {
int32_t write_position;
int64_t scp_timestamp;
int64_t scp_archcounter;
} __packed __aligned(4);
struct sensor_comm_notify {
uint8_t sequence;
uint8_t sensor_type;
uint8_t command;
uint8_t length;
uint8_t crc8;
int32_t value[5] __aligned(4);
} __packed __aligned(4);
int sensor_comm_ctrl_send(struct sensor_comm_ctrl *ctrl, unsigned int size);
int sensor_comm_notify(struct sensor_comm_notify *notify);
int sensor_comm_notify_bypass(struct sensor_comm_notify *notify);
void sensor_comm_notify_handler_register(uint8_t cmd,
void (*f)(struct sensor_comm_notify *n, void *private_data),
void *private_data);
void sensor_comm_notify_handler_unregister(uint8_t cmd);
int sensor_comm_init(void);
void sensor_comm_exit(void);
#endif
@@ -0,0 +1,167 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "sensor_list " fmt
#include <linux/spinlock.h>
#include <linux/completion.h>
#include <linux/mutex.h>
#include <linux/atomic.h>
#include <linux/slab.h>
#include "hf_sensor_type.h"
#include "sensor_comm.h"
#include "sensor_list.h"
#include "share_memory.h"
static DEFINE_MUTEX(bus_user_lock);
static atomic_t sensor_list_sequence;
static DECLARE_COMPLETION(sensor_list_done);
static DEFINE_SPINLOCK(rx_notify_lock);
static struct sensor_comm_notify rx_notify;
static struct share_mem sensor_list_shm_reader;
static void sensor_list_notify_handler(struct sensor_comm_notify *n,
void *private_data)
{
spin_lock(&rx_notify_lock);
memcpy(&rx_notify, n, sizeof(rx_notify));
spin_unlock(&rx_notify_lock);
complete(&sensor_list_done);
}
static int sensor_list_seq_get_list(struct sensor_info *list,
unsigned int num)
{
int ret = 0;
int timeout = 0;
unsigned long flags = 0;
struct sensor_comm_notify notify;
uint32_t write_position = 0;
struct share_mem_info info;
unsigned int i = 0;
ret = share_mem_read_reset(&sensor_list_shm_reader);
if (ret < 0) {
pr_err("reset fail %d\n", ret);
return ret;
}
/*
* NOTE: must reinit_completion before sensor_comm_notify
* wrong sequence:
* sensor_comm_notify ---> reinit_completion -> wait_for_completion
* |
* complete
* if complete before reinit_completion, will lose this complete
* right sequence:
* reinit_completion -> sensor_comm_notify -> wait_for_completion
*/
reinit_completion(&sensor_list_done);
/* safe sequence given by atomic, round from 0 to 255 */
notify.sequence = atomic_inc_return(&sensor_list_sequence);
notify.sensor_type = SENSOR_TYPE_INVALID;
notify.command = SENS_COMM_NOTIFY_LIST_CMD;
notify.length = 0;
ret = sensor_comm_notify(&notify);
if (ret < 0) {
pr_err("notify fail %d\n", ret);
return ret;
}
timeout = wait_for_completion_timeout(&sensor_list_done,
msecs_to_jiffies(100));
if (!timeout) {
pr_err("wait completion timeout\n");
return -ETIMEDOUT;
}
spin_lock_irqsave(&rx_notify_lock, flags);
if (rx_notify.sequence != notify.sequence &&
rx_notify.sensor_type != notify.sensor_type &&
rx_notify.command != notify.command) {
pr_err("reply fail\n");
spin_unlock_irqrestore(&rx_notify_lock, flags);
return -EREMOTEIO;
}
write_position = rx_notify.value[0];
spin_unlock_irqrestore(&rx_notify_lock, flags);
ret = share_mem_seek(&sensor_list_shm_reader, write_position);
if (ret < 0) {
pr_err("seek fail %d\n", ret);
return ret;
}
/* NOTE: i++ must not put into for loop */
for (i = 0; i < num; ) {
ret = share_mem_read(&sensor_list_shm_reader,
&info, sizeof(info));
if (ret == 0)
break;
if (ret != sizeof(info)) {
pr_err("read fail %d\n", ret);
break;
}
if (info.sensor_type >= SENSOR_TYPE_SENSOR_MAX ||
!info.sensor_type || !info.gain) {
pr_err("read wrong sensor info\n");
continue;
}
list[i].sensor_type = info.sensor_type;
list[i].gain = info.gain;
strlcpy(list[i].name, info.name, sizeof(list[i].name));
strlcpy(list[i].vendor, info.vendor, sizeof(list[i].vendor));
i++;
}
return i;
}
int sensor_list_get_list(struct sensor_info *list, unsigned int num)
{
int retry = 0, ret = 0;
const int max_retry = 3;
mutex_lock(&bus_user_lock);
do {
ret = sensor_list_seq_get_list(list, num);
} while (retry++ < max_retry && ret < 0);
mutex_unlock(&bus_user_lock);
return ret;
}
static int sensor_list_share_mem_cfg(struct share_mem_config *cfg,
void *private_data)
{
sensor_list_shm_reader.name = "sensor_list_r";
sensor_list_shm_reader.item_size = sizeof(struct share_mem_info);
sensor_list_shm_reader.buffer_full_detect = false;
return share_mem_init(&sensor_list_shm_reader, cfg);
}
int sensor_list_init(void)
{
unsigned long flags = 0;
atomic_set(&sensor_list_sequence, 0);
spin_lock_irqsave(&rx_notify_lock, flags);
memset(&rx_notify, 0, sizeof(rx_notify));
spin_unlock_irqrestore(&rx_notify_lock, flags);
sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_LIST_CMD,
sensor_list_notify_handler, NULL);
share_mem_config_handler_register(SHARE_MEM_LIST_PAYLOAD_TYPE,
sensor_list_share_mem_cfg, NULL);
return 0;
}
void sensor_list_exit(void)
{
sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_LIST_CMD);
share_mem_config_handler_unregister(SHARE_MEM_LIST_PAYLOAD_TYPE);
}
@@ -0,0 +1,15 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _SENSOR_LIST_H_
#define _SENSOR_LIST_H_
#include "hf_manager.h"
int sensor_list_get_list(struct sensor_info *list, unsigned int size);
int sensor_list_init(void);
void sensor_list_exit(void);
#endif
@@ -0,0 +1,417 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "share_mem " fmt
#include <linux/err.h>
#include <linux/string.h>
#include <linux/printk.h>
#include <linux/io.h>
#include <linux/slab.h>
#include "sensor_comm.h"
#include "share_memory.h"
#include "hf_sensor_type.h"
struct share_mem_config_handle {
int (*handler)(struct share_mem_config *cfg, void *private_data);
void *private_data;
};
struct share_mem_usage {
uint8_t payload_type;
bool init_status;
int id;
};
static struct share_mem_config_handle shm_handle[MAX_SHARE_MEM_PAYLOAD_TYPE];
static struct share_mem_usage shm_usage_table[] = {
{
.payload_type = SHARE_MEM_DATA_PAYLOAD_TYPE,
.id = SENS_MEM_ID,
},
{
.payload_type = SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE,
.id = SENS_SUPER_MEM_ID,
},
{
.payload_type = SHARE_MEM_LIST_PAYLOAD_TYPE,
.id = SENS_LIST_MEM_ID,
},
{
.payload_type = SHARE_MEM_DEBUG_PAYLOAD_TYPE,
.id = SENS_DEBUG_MEM_ID,
},
{
.payload_type = SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE,
.id = SENS_CUSTOM_W_MEM_ID,
},
{
.payload_type = SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE,
.id = SENS_CUSTOM_R_MEM_ID,
},
};
static int share_mem_notify(struct share_mem *shm,
struct share_mem_notify *notify)
{
int ret = 0;
struct sensor_comm_notify n;
if (shm->write_position == shm->last_write_position)
return 0;
n.sequence = notify->sequence;
n.sensor_type = notify->sensor_type;
n.command = notify->notify_cmd;
n.value[0] = shm->write_position;
n.length = sizeof(n.value[0]);
ret = sensor_comm_notify(&n);
if (ret < 0)
return ret;
shm->last_write_position = shm->write_position;
return ret;
}
static void share_mem_buffer_full_detect(struct share_mem *shm,
uint32_t curr_written)
{
int ret = 0;
uint32_t rp = 0, wp = 0, buffer_size = 0;
struct share_mem_notify notify;
shm->buffer_full_written += curr_written;
if (shm->buffer_full_written < shm->buffer_full_threshold)
return;
rp = shm->base->rp;
wp = shm->base->wp;
buffer_size = shm->base->buffer_size;
shm->buffer_full_written = (wp > rp) ?
(wp - rp) : (buffer_size - rp + wp);
if (shm->buffer_full_written >= shm->buffer_full_threshold) {
notify.sequence = 0;
notify.sensor_type = SENSOR_TYPE_INVALID;
notify.notify_cmd = shm->buffer_full_cmd;
ret = share_mem_notify(shm, &notify);
if (ret < 0)
pr_err("%s buffer full notify fail %d\n",
shm->name, ret);
else
shm->buffer_full_written = 0;
}
}
int share_mem_seek(struct share_mem *shm, uint32_t write_position)
{
if (!shm->base)
return -EINVAL;
mutex_lock(&shm->lock);
shm->write_position = write_position;
mutex_unlock(&shm->lock);
return 0;
}
int share_mem_read_reset(struct share_mem *shm)
{
if (!shm->base)
return -EINVAL;
mutex_lock(&shm->lock);
shm->base->rp = 0;
shm->write_position = 0;
mutex_unlock(&shm->lock);
return 0;
}
int share_mem_write_reset(struct share_mem *shm)
{
if (!shm->base)
return -EINVAL;
mutex_lock(&shm->lock);
shm->base->wp = 0;
shm->write_position = 0;
shm->last_write_position = 0;
shm->buffer_full_written = 0;
mutex_unlock(&shm->lock);
return 0;
}
static int share_mem_read_dram(struct share_mem *shm,
void *buf, uint32_t count)
{
uint32_t rp = 0, wp = 0, buffer_size = 0, item_size = 0;
uint8_t *src = NULL, *dst = buf;
uint32_t first = 0, second = 0, read = 0;
if (!shm->item_size || count % shm->item_size)
return -EINVAL;
wp = shm->write_position;
rp = shm->base->rp;
buffer_size = shm->base->buffer_size;
item_size = shm->base->item_size;
src = (uint8_t *)shm->base + offsetof(struct share_mem_base, data);
if (item_size != shm->item_size)
return -EIO;
if (wp == rp)
return 0;
if (wp > rp) {
first = wp - rp;
second = 0;
} else {
first = buffer_size - rp;
second = wp;
}
first = min(first, count);
second = min(count - first, second);
memcpy_fromio(dst, src + rp, first);
memcpy_fromio(dst + first, src, second);
read = first + second;
rp += read;
rp %= buffer_size;
/*
* make sure that the data is copied before
* incrementing the rp index counter
*/
smp_wmb();
shm->base->rp = rp;
return read;
}
int share_mem_read(struct share_mem *shm, void *buf, uint32_t count)
{
int ret = 0;
if (!shm->base || !buf || !count)
return -EINVAL;
mutex_lock(&shm->lock);
ret = share_mem_read_dram(shm, buf, count);
mutex_unlock(&shm->lock);
return ret;
}
static int share_mem_write_dram(struct share_mem *shm,
void *buf, uint32_t count)
{
uint32_t rp = 0, wp = 0, buffer_size = 0, item_size = 0, write = 0;
uint8_t *src = buf, *dst = NULL;
if (!shm->item_size || count % shm->item_size)
return -EINVAL;
rp = shm->base->rp;
wp = shm->base->wp;
buffer_size = shm->base->buffer_size;
item_size = shm->base->item_size;
dst = (uint8_t *)shm->base + offsetof(struct share_mem_base, data);
if (item_size != shm->item_size)
return -EIO;
/* remain 1 count */
while ((write < count) && ((wp + item_size) % buffer_size != rp)) {
memcpy_toio(dst + wp, src + write, item_size);
write += item_size;
wp += item_size;
wp %= buffer_size;
}
if (!write)
return 0;
/*
* make sure that the data is copied before
* incrementing the wp index counter
*/
smp_wmb();
shm->base->wp = wp;
shm->write_position = wp;
if (shm->buffer_full_detect)
share_mem_buffer_full_detect(shm, write);
return write;
}
int share_mem_write(struct share_mem *shm, void *buf, uint32_t count)
{
int ret = 0;
if (!shm->base || !buf || !count)
return -EINVAL;
mutex_lock(&shm->lock);
ret = share_mem_write_dram(shm, buf, count);
mutex_unlock(&shm->lock);
return ret;
}
int share_mem_flush(struct share_mem *shm, struct share_mem_notify *notify)
{
int ret = 0;
if (!shm->base)
return -EINVAL;
mutex_lock(&shm->lock);
ret = share_mem_notify(shm, notify);
mutex_unlock(&shm->lock);
return ret;
}
int share_mem_init(struct share_mem *shm, struct share_mem_config *cfg)
{
int ret = 0;
if (!shm->name || !shm->item_size)
return -EINVAL;
mutex_init(&shm->lock);
cfg->base->wp = 0;
cfg->base->rp = 0;
cfg->base->item_size = shm->item_size;
cfg->base->buffer_size =
(((long)cfg->buffer_size -
offsetof(struct share_mem_base, data)) /
shm->item_size) * shm->item_size;
shm->write_position = 0;
shm->last_write_position = 0;
if (shm->buffer_full_detect) {
shm->buffer_full_written = 0;
shm->buffer_full_threshold =
((uint32_t)(((cfg->base->buffer_size - shm->item_size) /
shm->item_size) * 8 / 10)) *
shm->item_size;
if (shm->buffer_full_threshold <= shm->item_size) {
ret = -EINVAL;
goto exit;
}
}
shm->base = cfg->base;
exit:
return ret;
}
static int share_mem_send_config(void)
{
int ret = 0;
uint32_t i = 0, index = 0;
struct share_mem_usage *usage = NULL;
struct sensor_comm_ctrl *ctrl = NULL;
struct sensor_comm_share_mem *comm_shm = NULL;
ctrl = kzalloc(sizeof(*ctrl) + sizeof(*comm_shm), GFP_KERNEL);
if (!ctrl)
return -ENOMEM;
ctrl->sensor_type = 0;
ctrl->command = SENS_COMM_CTRL_SHARE_MEMORY_CMD;
ctrl->length = sizeof(*comm_shm);
comm_shm = (struct sensor_comm_share_mem *)ctrl->data;
memset(comm_shm, 0, sizeof(*comm_shm));
for (i = 0; i < ARRAY_SIZE(shm_usage_table); i++) {
usage = &shm_usage_table[i];
if (usage->payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE)
continue;
/* host init share mem ready we can send share mem to scp */
if (usage->init_status) {
comm_shm->base_info[index].payload_type =
usage->payload_type;
comm_shm->base_info[index].payload_base =
(uint32_t)scp_get_reserve_mem_phys(usage->id);
WARN_ON(!comm_shm->base_info[index].payload_base);
comm_shm->available_num = ++index;
}
if (index == ARRAY_SIZE(comm_shm->base_info) ||
(i == (ARRAY_SIZE(shm_usage_table) - 1) && index)) {
ret = sensor_comm_ctrl_send(ctrl,
sizeof(*ctrl) + ctrl->length);
if (ret < 0)
break;
index = 0;
memset(comm_shm, 0, sizeof(*comm_shm));
}
}
kfree(ctrl);
return ret;
}
int share_mem_config(void)
{
int ret = 0;
uint32_t i = 0;
struct share_mem_config cfg;
struct share_mem_config_handle *handle = NULL;
struct share_mem_usage *usage = NULL;
for (i = 0; i < ARRAY_SIZE(shm_usage_table); i++) {
usage = &shm_usage_table[i];
/* must reset init_status to false scp reset each times */
usage->init_status = false;
if (usage->payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE) {
pr_err("payload type %u invalid index %u\n",
usage->payload_type, i);
BUG_ON(1);
}
handle = &shm_handle[usage->payload_type];
if (!handle->handler) {
pr_err("payload type %u handler NULL index %u\n",
usage->payload_type, i);
BUG_ON(1);
}
memset(&cfg, 0, sizeof(cfg));
cfg.payload_type = usage->payload_type;
cfg.base =
(void *)(long)scp_get_reserve_mem_virt(usage->id);
cfg.buffer_size =
(uint32_t)scp_get_reserve_mem_size(usage->id);
BUG_ON(!cfg.base);
ret = handle->handler(&cfg, handle->private_data);
if (ret < 0)
continue;
usage->init_status = true;
}
return share_mem_send_config();
}
void share_mem_config_handler_register(uint8_t payload_type,
int (*f)(struct share_mem_config *cfg, void *private_data),
void *private_data)
{
if (payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE)
return;
shm_handle[payload_type].private_data = private_data;
shm_handle[payload_type].handler = f;
}
void share_mem_config_handler_unregister(uint8_t payload_type)
{
if (payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE)
return;
shm_handle[payload_type].handler = NULL;
shm_handle[payload_type].private_data = NULL;
}
@@ -0,0 +1,112 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _SHARE_MEMORY_H_
#define _SHARE_MEMORY_H_
#include "scp.h"
enum share_mem_payload_type {
SHARE_MEM_DATA_PAYLOAD_TYPE,
SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE,
SHARE_MEM_LIST_PAYLOAD_TYPE,
SHARE_MEM_DEBUG_PAYLOAD_TYPE,
SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE,
SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE,
MAX_SHARE_MEM_PAYLOAD_TYPE,
};
struct share_mem_data {
uint8_t sensor_type;
uint8_t action;
uint8_t accurancy;
uint8_t padding[1];
int64_t timestamp;
int32_t value[6] __aligned(4);
} __packed __aligned(4);
struct share_mem_super_data {
uint8_t sensor_type;
uint8_t action;
uint8_t accurancy;
uint8_t padding[1];
int64_t timestamp;
int32_t value[16] __aligned(4);
} __packed __aligned(4);
struct share_mem_debug {
uint8_t sensor_type;
uint8_t padding[3];
uint32_t written;
uint8_t buffer[4032] __aligned(4); //2048+1024+512+256+128+64
} __packed __aligned(4);
struct share_mem_info {
uint8_t sensor_type;
uint8_t padding[3];
uint32_t gain;
uint8_t name[16];
uint8_t vendor[16];
} __packed __aligned(4);
struct share_mem_cmd {
uint8_t command;
uint8_t tx_len;
uint8_t rx_len;
uint8_t padding[1];
int32_t data[15] __aligned(4);
} __packed __aligned(4);
struct share_mem_base {
uint32_t rp;
uint32_t wp;
uint32_t buffer_size;
uint32_t item_size;
uint8_t data[0] __aligned(4);
} __packed __aligned(4);
struct share_mem {
struct share_mem_base *base;
struct mutex lock;
uint32_t item_size;
uint32_t write_position;
uint32_t last_write_position;
bool buffer_full_detect;
uint8_t buffer_full_cmd;
uint32_t buffer_full_written;
uint32_t buffer_full_threshold;
char *name;
};
struct share_mem_notify {
uint8_t sequence;
uint8_t sensor_type;
uint8_t notify_cmd;
};
struct share_mem_config {
uint8_t payload_type;
struct share_mem_base *base;
uint32_t buffer_size;
};
int share_mem_seek(struct share_mem *shm, uint32_t write_position);
int share_mem_read_reset(struct share_mem *shm);
int share_mem_write_reset(struct share_mem *shm);
int share_mem_read(struct share_mem *shm, void *buf, uint32_t count);
int share_mem_write(struct share_mem *shm, void *buf, uint32_t count);
int share_mem_flush(struct share_mem *shm, struct share_mem_notify *notify);
int share_mem_init(struct share_mem *shm, struct share_mem_config *cfg);
int share_mem_config(void);
void share_mem_config_handler_register(uint8_t payload_type,
int (*f)(struct share_mem_config *cfg, void *private_data),
void *private_data);
void share_mem_config_handler_unregister(uint8_t payload_type);
#endif
@@ -0,0 +1,98 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "test_sensor " fmt
#include <linux/module.h>
#include <linux/string.h>
#include <linux/kthread.h>
#include <linux/slab.h>
#include <linux/workqueue.h>
#include <linux/hrtimer.h>
#include "hf_sensor_type.h"
#include "sensor_comm.h"
#include "ready.h"
struct test_sensor_t {
char *name;
struct hrtimer test_timer;
struct work_struct test_work;
struct workqueue_struct *workqueue;
};
static struct test_sensor_t test_sensor = {
.name = "test_sensor",
};
static int enable_disable_count;
static void test_work_func(struct work_struct *work)
{
int ret = 0;
struct sensor_comm_ctrl *ctrl;
struct sensor_comm_batch *batch;
if (enable_disable_count++ % 2) {
ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
ctrl->sensor_type = SENSOR_TYPE_ACCELEROMETER;
ctrl->command = SENS_COMM_CTRL_DISABLE_CMD;
ctrl->length = 0;
} else {
ctrl = kzalloc(sizeof(*ctrl) + sizeof(*batch), GFP_KERNEL);
ctrl->sensor_type = SENSOR_TYPE_ACCELEROMETER;
ctrl->command = SENS_COMM_CTRL_ENABLE_CMD;
ctrl->length = sizeof(*batch);
batch = (struct sensor_comm_batch *)ctrl->data;
batch->delay = 20000000;
batch->latency = 1000000000;
}
ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
if (ret < 0)
pr_err("enable disable fail %d\n", ret);
kfree(ctrl);
}
static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
{
queue_work(test_sensor.workqueue, &test_sensor.test_work);
hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
return HRTIMER_RESTART;
}
static int test_ready_notifier_call(struct notifier_block *this,
unsigned long event, void *ptr)
{
return NOTIFY_DONE;
}
static struct notifier_block test_ready_notifier = {
.notifier_call = test_ready_notifier_call,
.priority = READY_STDPRI,
};
static int __init test_init(void)
{
INIT_WORK(&test_sensor.test_work, test_work_func);
test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
hrtimer_init(&test_sensor.test_timer,
CLOCK_MONOTONIC, HRTIMER_MODE_REL);
test_sensor.test_timer.function = test_timer_func;
hrtimer_start(&test_sensor.test_timer,
ns_to_ktime(10000000000), HRTIMER_MODE_REL);
sensor_ready_notifier_chain_register(&test_ready_notifier);
return 0;
}
static void __exit test_exit(void)
{
}
module_init(test_init);
module_exit(test_exit);
MODULE_AUTHOR("Mediatek");
MODULE_DESCRIPTION("test driver");
MODULE_LICENSE("GPL");
@@ -0,0 +1,80 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "test_sensor1 " fmt
#include <linux/module.h>
#include <linux/string.h>
#include <linux/kthread.h>
#include <linux/slab.h>
#include <linux/workqueue.h>
#include <linux/hrtimer.h>
#include <linux/time.h>
#include <asm/arch_timer.h>
#include "hf_sensor_type.h"
#include "sensor_comm.h"
struct test_sensor_t {
char *name;
struct hrtimer test_timer;
struct work_struct test_work;
struct workqueue_struct *workqueue;
};
static struct test_sensor_t test_sensor = {
.name = "test_sensor1",
};
static void test_work_func(struct work_struct *work)
{
int ret = 0;
struct sensor_comm_ctrl *ctrl = NULL;
struct sensor_comm_timesync *timesync = NULL;
ctrl = kzalloc(sizeof(*ctrl) + sizeof(*timesync), GFP_KERNEL);
ctrl->sensor_type = SENSOR_TYPE_INVALID;
ctrl->command = SENS_COMM_CTRL_TIMESYNC_CMD;
ctrl->length = sizeof(*timesync);
timesync = (struct sensor_comm_timesync *)ctrl->data;
local_irq_disable();
timesync->host_timestamp = ktime_get_boot_ns();
timesync->host_archcounter = arch_counter_get_cntvct();
local_irq_enable();
ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
if (ret < 0)
pr_err("timesync fail %d\n", ret);
kfree(ctrl);
}
static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
{
queue_work(test_sensor.workqueue, &test_sensor.test_work);
hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
return HRTIMER_RESTART;
}
static int __init test_init(void)
{
INIT_WORK(&test_sensor.test_work, test_work_func);
test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
hrtimer_init(&test_sensor.test_timer,
CLOCK_MONOTONIC, HRTIMER_MODE_REL);
test_sensor.test_timer.function = test_timer_func;
hrtimer_start(&test_sensor.test_timer,
ns_to_ktime(10000000000), HRTIMER_MODE_REL);
return 0;
}
static void __exit test_exit(void)
{
}
module_init(test_init);
module_exit(test_exit);
MODULE_AUTHOR("Mediatek");
MODULE_DESCRIPTION("test driver");
MODULE_LICENSE("GPL");
@@ -0,0 +1,85 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "test_sensor2 " fmt
#include <linux/module.h>
#include <linux/string.h>
#include <linux/kthread.h>
#include <linux/slab.h>
#include <linux/workqueue.h>
#include <linux/hrtimer.h>
#include "hf_sensor_type.h"
#include "sensor_comm.h"
struct test_sensor_t {
char *name;
struct hrtimer test_timer;
struct work_struct test_work;
struct workqueue_struct *workqueue;
};
static struct test_sensor_t test_sensor = {
.name = "test_sensor2",
};
static int enable_disable_count;
static void test_work_func(struct work_struct *work)
{
int ret = 0;
struct sensor_comm_ctrl *ctrl;
struct sensor_comm_batch *batch;
if (enable_disable_count++ % 2) {
ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
ctrl->sensor_type = SENSOR_TYPE_MAGNETIC_FIELD;
ctrl->command = SENS_COMM_CTRL_DISABLE_CMD;
ctrl->length = 0;
} else {
ctrl = kzalloc(sizeof(*ctrl) + sizeof(*batch), GFP_KERNEL);
ctrl->sensor_type = SENSOR_TYPE_MAGNETIC_FIELD;
ctrl->command = SENS_COMM_CTRL_ENABLE_CMD;
ctrl->length = sizeof(*batch);
batch = (struct sensor_comm_batch *)ctrl->data;
batch->delay = 20000000;
batch->latency = 2000000000;
}
ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
if (ret < 0)
pr_err("enable disable fail %d\n", ret);
kfree(ctrl);
}
static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
{
queue_work(test_sensor.workqueue, &test_sensor.test_work);
hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
return HRTIMER_RESTART;
}
static int __init test_init(void)
{
INIT_WORK(&test_sensor.test_work, test_work_func);
test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
hrtimer_init(&test_sensor.test_timer,
CLOCK_MONOTONIC, HRTIMER_MODE_REL);
test_sensor.test_timer.function = test_timer_func;
hrtimer_start(&test_sensor.test_timer,
ns_to_ktime(10000000000), HRTIMER_MODE_REL);
return 0;
}
static void __exit test_exit(void)
{
}
module_init(test_init);
module_exit(test_exit);
MODULE_AUTHOR("Mediatek");
MODULE_DESCRIPTION("test driver");
MODULE_LICENSE("GPL");
@@ -0,0 +1,90 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "test_sensor3 " fmt
#include <linux/module.h>
#include <linux/string.h>
#include <linux/kthread.h>
#include <linux/slab.h>
#include <linux/workqueue.h>
#include <linux/hrtimer.h>
#include <linux/time.h>
#include <asm/arch_timer.h>
#include "hf_sensor_type.h"
#include "sensor_comm.h"
#include "debug.h"
struct test_sensor_t {
char *name;
struct hrtimer test_timer;
struct work_struct test_work;
struct workqueue_struct *workqueue;
};
static struct test_sensor_t test_sensor = {
.name = "test_sensor3",
};
static void test_config(void)
{
int ret = 0;
struct sensor_comm_ctrl *ctrl = NULL;
int32_t cfg[3] = {500, 100, 203};
ctrl = kzalloc(sizeof(*ctrl) + sizeof(cfg), GFP_KERNEL);
ctrl->sensor_type = SENSOR_TYPE_ACCELEROMETER;
ctrl->command = SENS_COMM_CTRL_CONFIG_CMD;
ctrl->length = sizeof(cfg);
memcpy(ctrl->data, cfg, sizeof(cfg));
ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
if (ret < 0)
pr_err("config fail %d\n", ret);
kfree(ctrl);
}
static void test_work_func(struct work_struct *work)
{
int ret = 0;
unsigned int len = 1024;
uint8_t *buffer = kzalloc(len, GFP_KERNEL);
ret = debug_get_debug(SENSOR_TYPE_INVALID, buffer, len);
if (ret > 0)
pr_err("\n%s\n", buffer);
kfree(buffer);
test_config();
}
static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
{
queue_work(test_sensor.workqueue, &test_sensor.test_work);
hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
return HRTIMER_RESTART;
}
static int __init test_init(void)
{
INIT_WORK(&test_sensor.test_work, test_work_func);
test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
hrtimer_init(&test_sensor.test_timer,
CLOCK_MONOTONIC, HRTIMER_MODE_REL);
test_sensor.test_timer.function = test_timer_func;
hrtimer_start(&test_sensor.test_timer,
ns_to_ktime(10000000000), HRTIMER_MODE_REL);
return 0;
}
static void __exit test_exit(void)
{
}
module_init(test_init);
module_exit(test_exit);
MODULE_AUTHOR("Mediatek");
MODULE_DESCRIPTION("test driver");
MODULE_LICENSE("GPL");
@@ -0,0 +1,102 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "test_sensor4 " fmt
#include <linux/module.h>
#include <linux/string.h>
#include <linux/kthread.h>
#include <linux/slab.h>
#include <linux/workqueue.h>
#include <linux/hrtimer.h>
#include "hf_sensor_type.h"
#include "hf_manager.h"
#include "sensor_comm.h"
#include "sensor_list.h"
struct test_sensor_t {
int count;
char *name;
struct hrtimer test_timer;
struct work_struct test_work;
struct workqueue_struct *workqueue;
};
static struct test_sensor_t test_sensor = {
.name = "test_sensor4",
};
static struct sensor_info sensor_list[SENSOR_TYPE_SENSOR_MAX];
static void test_work_func(struct work_struct *work)
{
int ret = 0;
struct sensor_comm_notify notify;
unsigned int i = 0;
notify.sequence = 0;
notify.sensor_type = SENSOR_TYPE_ACCELEROMETER;
notify.command = SENS_COMM_NOTIFY_TEST_CMD;
notify.length = sizeof(notify.value[0]);
notify.value[0] = test_sensor.count++;
ret = sensor_comm_notify(&notify);
if (ret < 0)
pr_err("notify failed %d\n", ret);
memset(sensor_list, 0, sizeof(sensor_list));
ret = sensor_list_get_list(sensor_list, ARRAY_SIZE(sensor_list));
if (ret < 0) {
pr_err("get sensor list failed %d\n", ret);
return;
}
for (i = 0; i < ret; i++) {
if (sensor_list[i].sensor_type == SENSOR_TYPE_INVALID)
continue;
pr_err("sensor list type:%u, gain:%u, name:%s, vendor:%s\n",
sensor_list[i].sensor_type,
sensor_list[i].gain, sensor_list[i].name,
sensor_list[i].vendor);
}
}
static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
{
queue_work(test_sensor.workqueue, &test_sensor.test_work);
return HRTIMER_NORESTART;
}
static void notify_func(struct sensor_comm_notify *n, void *private_data)
{
hrtimer_start(&test_sensor.test_timer,
ns_to_ktime(10000000000), HRTIMER_MODE_REL);
}
static int __init test_init(void)
{
sensor_comm_notify_handler_register(
SENS_COMM_NOTIFY_TEST_CMD, notify_func, NULL);
INIT_WORK(&test_sensor.test_work, test_work_func);
test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
hrtimer_init(&test_sensor.test_timer,
CLOCK_MONOTONIC, HRTIMER_MODE_REL);
test_sensor.test_timer.function = test_timer_func;
hrtimer_start(&test_sensor.test_timer,
ns_to_ktime(10000000000), HRTIMER_MODE_REL);
return 0;
}
static void __exit test_exit(void)
{
}
module_init(test_init);
module_exit(test_exit);
MODULE_AUTHOR("Mediatek");
MODULE_DESCRIPTION("test driver");
MODULE_LICENSE("GPL");
@@ -0,0 +1,230 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "timesync " fmt
#include <linux/slab.h>
#include <linux/timekeeping.h>
#include <linux/timer.h>
#include <linux/suspend.h>
#include <asm/arch_timer.h>
#include <linux/workqueue.h>
#include <linux/atomic.h>
#include <linux/spinlock.h>
#include "timesync.h"
#include "sensor_comm.h"
static bool timesync_suspend_flag;
static struct timer_list timesync_timer;
static struct work_struct timesync_work;
static struct wakeup_source *wakeup_src;
/* arch counter is 13M, mult is 161319385, shift is 21 */
static inline int64_t arch_counter_to_ns(int64_t cyc)
{
#define ARCH_TIMER_MULT 161319385
#define ARCH_TIMER_SHIFT 21
return (cyc * ARCH_TIMER_MULT) >> ARCH_TIMER_SHIFT;
}
static void timesync_filter_calculate(struct timesync_filter *filter,
int64_t host_time, int64_t scp_time)
{
int i = 0;
int64_t avg = 0, delta = 0;
unsigned long flags;
spin_lock_irqsave(&filter->lock, flags);
if (host_time > filter->last_time + filter->max_diff ||
filter->last_time == 0) {
filter->tail = 0;
filter->cnt = 0;
} else if (host_time < filter->last_time + filter->min_diff) {
spin_unlock_irqrestore(&filter->lock, flags);
return;
}
filter->last_time = host_time;
filter->buffer[filter->tail++] = host_time - scp_time;
filter->tail &= (filter->bufsize - 1);
if (filter->cnt < filter->bufsize)
filter->cnt++;
for (i = 1, avg = 0; i < filter->cnt; i++)
avg += (filter->buffer[i] - filter->buffer[0]);
filter->offset = div_s64(avg, filter->cnt) + filter->buffer[0];
if (!filter->offset_debug) {
filter->offset_debug = filter->offset;
} else {
delta = filter->offset_debug - filter->offset;
if (unlikely(delta >= 2500000) || unlikely(delta <= -2500000))
pr_err("%s host with scp jump too large %lld\n",
filter->name, delta);
filter->offset_debug = filter->offset;
}
spin_unlock_irqrestore(&filter->lock, flags);
}
static int timesync_comm_with_nolock(void)
{
int ret = 0;
unsigned long flags;
struct sensor_comm_ctrl *ctrl = NULL;
struct sensor_comm_timesync *time = NULL;
int64_t now_time = 0, arch_counter = 0;
if (READ_ONCE(timesync_suspend_flag))
return 0;
ctrl = kzalloc(sizeof(*ctrl) + sizeof(*time), GFP_KERNEL);
if (!ctrl)
return -ENOMEM;
ctrl->sensor_type = 0;
ctrl->command = SENS_COMM_CTRL_TIMESYNC_CMD;
ctrl->length = sizeof(*time);
time = (struct sensor_comm_timesync *)ctrl->data;
local_irq_save(flags);
now_time = ktime_get_boottime_ns();
arch_counter = __arch_counter_get_cntvct();
local_irq_restore(flags);
pr_info("host boottime %lld\n", now_time);
time->host_timestamp = now_time;
time->host_archcounter = arch_counter;
ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
kfree(ctrl);
return ret;
}
static int timesync_comm_with(void)
{
int ret = 0;
if (READ_ONCE(timesync_suspend_flag))
return 0;
__pm_stay_awake(wakeup_src);
ret = timesync_comm_with_nolock();
__pm_relax(wakeup_src);
return ret;
}
static void timesync_work_func(struct work_struct *work)
{
timesync_comm_with();
}
static void timesync_timer_func(struct timer_list *list)
{
schedule_work(&timesync_work);
timesync_start();
}
void timesync_filter_set(struct timesync_filter *filter,
int64_t scp_timestamp, int64_t scp_archcounter)
{
unsigned long flags;
int64_t host_timestamp = 0, host_archcounter = 0;
int64_t ipi_transfer_time = 0;
/*
*if (!timekeeping_rtc_skipresume()) {
* if (READ_ONCE(timesync_suspend_flag))
* return;
*}
*/
local_irq_save(flags);
host_timestamp = ktime_get_boottime_ns();
host_archcounter = __arch_counter_get_cntvct();
local_irq_restore(flags);
ipi_transfer_time = arch_counter_to_ns(host_archcounter -
scp_archcounter);
scp_timestamp = scp_timestamp + ipi_transfer_time;
timesync_filter_calculate(filter, host_timestamp, scp_timestamp);
}
int64_t timesync_filter_get(struct timesync_filter *filter)
{
unsigned long flags;
int64_t offset = 0;
spin_lock_irqsave(&filter->lock, flags);
offset = filter->offset;
spin_unlock_irqrestore(&filter->lock, flags);
return offset;
}
int timesync_filter_init(struct timesync_filter *filter)
{
if (!filter->max_diff)
filter->max_diff = 10000000000LL;
if (!filter->min_diff)
filter->min_diff = 10000000LL;
if (!filter->bufsize)
filter->bufsize = 16;
WARN_ON(!filter->name);
spin_lock_init(&filter->lock);
filter->bufsize = roundup_pow_of_two(filter->bufsize);
filter->buffer = kcalloc(filter->bufsize, sizeof(*filter->buffer),
GFP_KERNEL);
if (!filter->buffer)
return -ENOMEM;
return 0;
}
void timesync_filter_exit(struct timesync_filter *filter)
{
kfree(filter->buffer);
filter->buffer = NULL;
}
void timesync_start(void)
{
mod_timer(&timesync_timer, jiffies + msecs_to_jiffies(10000));
}
void timesync_stop(void)
{
del_timer_sync(&timesync_timer);
}
void timesync_resume(void)
{
pr_info("host resume boottime %lld\n", ktime_get_boottime_ns());
WRITE_ONCE(timesync_suspend_flag, false);
timesync_comm_with();
}
void timesync_suspend(void)
{
pr_info("host suspend boottime %lld\n", ktime_get_boottime_ns());
WRITE_ONCE(timesync_suspend_flag, true);
}
int timesync_init(void)
{
INIT_WORK(&timesync_work, timesync_work_func);
timer_setup(&timesync_timer, timesync_timer_func, 0);
wakeup_src = wakeup_source_register(NULL, "timesync");
if (!wakeup_src) {
pr_err("timesync wakeup source init fail\n");
return -ENOMEM;
}
return 0;
}
void timesync_exit(void)
{
timesync_stop();
wakeup_source_unregister(wakeup_src);
flush_work(&timesync_work);
}
@@ -0,0 +1,37 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _TIMESYNC_H_
#define _TIMESYNC_H_
#include <linux/spinlock.h>
struct timesync_filter {
spinlock_t lock;
int64_t last_time;
int64_t max_diff;
int64_t min_diff;
uint32_t cnt;
uint32_t tail;
uint32_t bufsize;
int64_t *buffer;
int64_t offset;
int64_t offset_debug;
char *name;
};
void timesync_filter_set(struct timesync_filter *filter,
int64_t scp_timestamp, int64_t scp_archcounter);
int64_t timesync_filter_get(struct timesync_filter *filter);
int timesync_filter_init(struct timesync_filter *filter);
void timesync_filter_exit(struct timesync_filter *filter);
void timesync_start(void);
void timesync_stop(void);
void timesync_resume(void);
void timesync_suspend(void);
int timesync_init(void);
void timesync_exit(void);
#endif
@@ -0,0 +1,91 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#define pr_fmt(fmt) "tiny_crc8 " fmt
#include <linux/printk.h>
#include "tiny_crc8.h"
#define TINY_CRC8
#ifndef TINY_CRC8
/*
* CRC using polynomial:
* 1. X^8 + X^5 + X^4 + X^0
* 2. Little Endian XOR
* 3. Polynomial = 0x8c
*/
static const uint8_t tiny_crc8_table[] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0,
0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d,
0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58,
0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6,
0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f,
0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92,
0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1,
0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49,
0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a,
0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7,
0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35
};
uint8_t tiny_crc8(uint8_t *ptr, uint8_t len)
{
uint8_t crc = 0;
while (len-- > 0)
crc = tiny_crc8_table[*ptr++ ^ crc];
return crc;
}
#else
/*
* CRC using polynomial:
* 1. X^8 + X^5 + X^4 + X^0
* 2. Little Endian XOR
* 3. Polynomial = 0x8c
*/
static const uint8_t tiny_crc8_table[] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
0x00, 0x9d, 0x23, 0xbe, 0x46, 0xdb, 0x65, 0xf8,
0x8c, 0x11, 0xaf, 0x32, 0xca, 0x57, 0xe9, 0x74
};
uint8_t tiny_crc8(uint8_t *ptr, uint8_t len)
{
uint8_t crc = 0;
while (len-- > 0) {
crc = *ptr++ ^ crc;
crc = tiny_crc8_table[crc & 0x0f] ^
tiny_crc8_table[16 + ((crc >> 4) & 0x0f)];
}
return crc;
}
#endif
@@ -0,0 +1,11 @@
/* SPDX-License-Identifier: GPL-2.0 */
/*
* Copyright (C) 2020 MediaTek Inc.
*/
#ifndef _TINY_CRC8_H_
#define _TINY_CRC8_H_
uint8_t tiny_crc8(uint8_t *ptr, uint8_t len);
#endif
File diff suppressed because it is too large. Load diff