threeeye
bc1d038eac
Squashed backup commit (no shallow-clone history issues) of xiaomi-mt6833-dev/kernel_xiaomi_mt6833 (lineage-23.2 branch, upstream commit 9eece662e) plus all fixes applied to get everpal_defconfig building clean against a modern (GCC 15.2.0) toolchain -- see the previous commit message on the lineage-23.2 branch (also in this repo history is not available here since this is a squashed snapshot; full incremental history lives in the local DL380 checkout and the upstream GitHub remote). Upstream: https://github.com/xiaomi-mt6833-dev/kernel_xiaomi_mt6833 (lineage-23.2)
1089 lines
26 KiB
C
1089 lines
26 KiB
C
/*
|
|
* Copyright (C) 2017 MediaTek Inc.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License version 2 as
|
|
* published by the Free Software Foundation.
|
|
*
|
|
* 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 "adsp_ipi_queue.h"
|
|
|
|
#include <linux/types.h>
|
|
#include <linux/errno.h>
|
|
|
|
#include <linux/slab.h> /* needed by kmalloc */
|
|
|
|
#include <linux/kthread.h>
|
|
#include <linux/wait.h>
|
|
#include <linux/spinlock.h>
|
|
|
|
#include <linux/delay.h>
|
|
#include <linux/jiffies.h>
|
|
|
|
#include "adsp_ipi.h"
|
|
#include "mtk_hifixdsp_common.h"
|
|
|
|
#ifdef CONFIG_MTK_AEE_FEATURE
|
|
#include <mt-plat/aee.h>
|
|
#endif
|
|
|
|
#ifdef scp_debug
|
|
#undef scp_debug
|
|
#endif
|
|
#if 0 /* debug only. might make performace degrade */
|
|
#define scp_debug(x...) pr_info(x)
|
|
#else
|
|
#define scp_debug(x...)
|
|
#endif
|
|
|
|
|
|
/*
|
|
* =============================================================================
|
|
* MACRO
|
|
* =============================================================================
|
|
*/
|
|
|
|
#define MAX_ADSP_COUNT 1
|
|
#define MAX_SCP_MSG_NUM_IN_QUEUE (16)
|
|
#define SHARE_BUF_SIZE 288
|
|
#define SCP_MSG_BUFFER_SIZE ((SHARE_BUF_SIZE) - 16)
|
|
|
|
/*
|
|
* =============================================================================
|
|
* struct def
|
|
* =============================================================================
|
|
*/
|
|
|
|
struct scp_msg_t {
|
|
uint32_t ipi_id;
|
|
uint8_t buf[SCP_MSG_BUFFER_SIZE]; /* TODO: use ring buf? */
|
|
uint32_t len;
|
|
void (*ipi_handler)(int ipi_id, void *buf, unsigned int len);
|
|
};
|
|
|
|
|
|
struct scp_queue_element_t {
|
|
struct scp_msg_t msg;
|
|
|
|
spinlock_t element_lock;
|
|
wait_queue_head_t element_wq;
|
|
|
|
bool wait_in_thread;
|
|
bool signal_arrival;
|
|
int send_retval;
|
|
uint32_t queue_counter;
|
|
};
|
|
|
|
|
|
enum { /* scp_path_t */
|
|
SCP_PATH_A2S = 0, /* AP to SCP */
|
|
SCP_PATH_S2A = 1, /* SCP to AP */
|
|
SCP_NUM_PATH
|
|
};
|
|
|
|
|
|
struct scp_msg_queue_t {
|
|
uint32_t opendsp_id; /* enum opendsp_id */
|
|
uint32_t scp_path; /* enum scp_dir_t */
|
|
|
|
struct scp_queue_element_t element[MAX_SCP_MSG_NUM_IN_QUEUE];
|
|
|
|
uint32_t size;
|
|
uint32_t idx_r;
|
|
uint32_t idx_w;
|
|
|
|
uint32_t queue_counter;
|
|
|
|
spinlock_t queue_lock;
|
|
wait_queue_head_t queue_wq;
|
|
|
|
/* scp_send_msg_to_scp() / scp_process_msg_from_scp() */
|
|
int (*scp_process_msg_func)(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t *p_scp_msg);
|
|
|
|
struct task_struct *scp_thread_task;
|
|
bool thread_enable;
|
|
|
|
bool init;
|
|
bool enable;
|
|
};
|
|
|
|
struct adsp_device_t {
|
|
uint32_t opendsp_id;
|
|
send_handler_t send_handler;
|
|
struct scp_msg_queue_t scp_msg_queue[SCP_NUM_PATH];
|
|
};
|
|
|
|
/*
|
|
* =============================================================================
|
|
* private global members
|
|
* =============================================================================
|
|
*/
|
|
|
|
static struct adsp_device_t g_adsp_devices[MAX_ADSP_COUNT];
|
|
|
|
/*
|
|
* =============================================================================
|
|
* utilities
|
|
* =============================================================================
|
|
*/
|
|
|
|
inline bool scp_check_idx_msg_valid(
|
|
const struct scp_msg_queue_t *msg_queue,
|
|
const uint32_t idx_msg);
|
|
|
|
inline bool scp_check_queue_empty(
|
|
const struct scp_msg_queue_t *msg_queue);
|
|
|
|
inline bool scp_check_queue_to_be_full(
|
|
const struct scp_msg_queue_t *msg_queue);
|
|
|
|
inline uint32_t scp_get_num_messages_in_queue(
|
|
const struct scp_msg_queue_t *msg_queue);
|
|
|
|
|
|
#ifdef CONFIG_MTK_AEE_FEATURE
|
|
#define IPI_ASSERT(exp) \
|
|
do { \
|
|
if (!(exp)) { \
|
|
aee_kernel_exception_api(__FILE__, \
|
|
__LINE__, \
|
|
DB_OPT_DEFAULT, \
|
|
"[IPI]", \
|
|
"ASSERT("#exp") fail!!"); \
|
|
} \
|
|
} while (0)
|
|
#else
|
|
#define IPI_ASSERT(exp) \
|
|
do { \
|
|
if (!(exp)) { \
|
|
pr_notice("ASSERT("#exp")!! \"" __FILE__ "\", %uL\n", \
|
|
__LINE__); \
|
|
} \
|
|
} while (0)
|
|
#endif
|
|
|
|
|
|
|
|
/*
|
|
* =============================================================================
|
|
* private function declaration
|
|
* =============================================================================
|
|
*/
|
|
|
|
static void scp_dump_msg_in_queue(struct scp_msg_queue_t *msg_queue);
|
|
|
|
static int scp_push_msg(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
uint32_t ipi_id,
|
|
void *buf,
|
|
uint32_t len,
|
|
void (*ipi_handler)(int ipi_id, void *buf, unsigned int len),
|
|
bool wait_in_thread,
|
|
uint32_t *p_idx_msg,
|
|
uint32_t *p_queue_counter);
|
|
|
|
static int scp_pop_msg(struct scp_msg_queue_t *msg_queue);
|
|
|
|
static int scp_front_msg(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t **pp_scp_msg,
|
|
uint32_t *p_idx_msg);
|
|
|
|
|
|
static int scp_get_queue_element(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t **pp_scp_msg,
|
|
uint32_t *p_idx_msg);
|
|
|
|
|
|
static int scp_init_single_msg_queue(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
const uint32_t opendsp_id,
|
|
const uint32_t scp_path);
|
|
|
|
static int scp_process_msg_thread(void *data);
|
|
|
|
|
|
static int scp_send_msg_to_scp(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t *p_scp_msg);
|
|
|
|
static int scp_process_msg_from_scp(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t *p_scp_msg);
|
|
|
|
|
|
|
|
/*
|
|
* =============================================================================
|
|
* utilities
|
|
* =============================================================================
|
|
*/
|
|
|
|
inline bool scp_check_idx_msg_valid(
|
|
const struct scp_msg_queue_t *msg_queue,
|
|
const uint32_t idx_msg)
|
|
{
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), msg_queue == NULL!! return\n", __func__);
|
|
return false;
|
|
}
|
|
|
|
return (idx_msg < msg_queue->size) ? true : false;
|
|
}
|
|
|
|
|
|
inline bool scp_check_queue_empty(const struct scp_msg_queue_t *msg_queue)
|
|
{
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), msg_queue == NULL!! return\n", __func__);
|
|
return false;
|
|
}
|
|
|
|
return (msg_queue->idx_r == msg_queue->idx_w);
|
|
}
|
|
|
|
|
|
inline bool scp_check_queue_to_be_full(const struct scp_msg_queue_t *msg_queue)
|
|
{
|
|
uint32_t idx_w_to_be = 0;
|
|
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), msg_queue == NULL!! return\n", __func__);
|
|
return false;
|
|
}
|
|
|
|
idx_w_to_be = msg_queue->idx_w + 1;
|
|
if (idx_w_to_be == msg_queue->size)
|
|
idx_w_to_be = 0;
|
|
|
|
return (idx_w_to_be == msg_queue->idx_r) ? true : false;
|
|
}
|
|
|
|
|
|
inline uint32_t scp_get_num_messages_in_queue(
|
|
const struct scp_msg_queue_t *msg_queue)
|
|
{
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), msg_queue == NULL!! return\n", __func__);
|
|
return 0;
|
|
}
|
|
|
|
return (msg_queue->idx_w >= msg_queue->idx_r) ?
|
|
(msg_queue->idx_w - msg_queue->idx_r) :
|
|
((msg_queue->size - msg_queue->idx_r) + msg_queue->idx_w);
|
|
}
|
|
|
|
inline struct adsp_device_t *get_adsp_device_by_id(uint32_t opendsp_id)
|
|
{
|
|
int idx = 0;
|
|
|
|
for (idx = 0; idx < MAX_ADSP_COUNT; idx++) {
|
|
if (g_adsp_devices[idx].opendsp_id == opendsp_id)
|
|
return &g_adsp_devices[idx];
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
/*
|
|
* compatibility interface for old adsp queue implementation
|
|
*
|
|
*/
|
|
send_handler_t get_send_handler_by_id(uint32_t opendsp_id)
|
|
{
|
|
return NULL; // dummy function
|
|
}
|
|
|
|
/*
|
|
* =============================================================================
|
|
* create/destroy/init/deinit functions
|
|
* =============================================================================
|
|
*/
|
|
int scp_ipi_queue_init_ex(uint32_t opendsp_id, send_handler_t send_handler)
|
|
{
|
|
static atomic_t opendsp_idx = ATOMIC_INIT(0);
|
|
struct scp_msg_queue_t *msg_queue = NULL;
|
|
int idx_curr = 0;
|
|
uint32_t scp_path = 0;
|
|
int ret = 0;
|
|
|
|
idx_curr = atomic_add_return(1, &opendsp_idx) - 1;
|
|
|
|
if (idx_curr >= MAX_ADSP_COUNT) {
|
|
pr_info("%s(), opendsp_id: %u error, adsp count overflow!!\n",
|
|
__func__,
|
|
opendsp_id);
|
|
return -EFAULT;
|
|
}
|
|
|
|
|
|
g_adsp_devices[idx_curr].opendsp_id = opendsp_id;
|
|
g_adsp_devices[idx_curr].send_handler = send_handler;
|
|
for (scp_path = 0; scp_path < SCP_NUM_PATH; scp_path++) {
|
|
msg_queue = &g_adsp_devices[idx_curr].scp_msg_queue[scp_path];
|
|
ret = scp_init_single_msg_queue(msg_queue,
|
|
g_adsp_devices[idx_curr].opendsp_id,
|
|
scp_path);
|
|
if (ret != 0)
|
|
WARN_ON(1);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int scp_ipi_queue_init(uint32_t opendsp_id)
|
|
{
|
|
send_handler_t send_handler = get_send_handler_by_id(opendsp_id);
|
|
|
|
if (send_handler == NULL) {
|
|
pr_info("%s(), opendsp_id: could not found send handler for dsp %u!!\n",
|
|
__func__,
|
|
opendsp_id);
|
|
return -EFAULT;
|
|
}
|
|
|
|
return scp_ipi_queue_init_ex(opendsp_id, send_handler);
|
|
}
|
|
|
|
int scp_flush_msg_queue(uint32_t opendsp_id)
|
|
{
|
|
struct scp_msg_queue_t *msg_queue = NULL;
|
|
unsigned long flags = 0;
|
|
struct adsp_device_t *p_adsp_device = NULL;
|
|
|
|
p_adsp_device = get_adsp_device_by_id(opendsp_id);
|
|
|
|
if (!p_adsp_device) {
|
|
pr_err("%s(), opendsp_id: could not found adsp_device for %u!!\n",
|
|
__func__, opendsp_id);
|
|
return -EFAULT;
|
|
}
|
|
|
|
msg_queue = &p_adsp_device->scp_msg_queue[SCP_PATH_A2S];
|
|
msg_queue->enable = false;
|
|
|
|
spin_lock_irqsave(&msg_queue->queue_lock, flags);
|
|
|
|
while (scp_check_queue_empty(msg_queue) == false) {
|
|
wake_up_interruptible(
|
|
&msg_queue->element[msg_queue->idx_r].element_wq);
|
|
scp_pop_msg(msg_queue);
|
|
}
|
|
|
|
spin_unlock_irqrestore(&msg_queue->queue_lock, flags);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/*
|
|
* =============================================================================
|
|
* public functions
|
|
* =============================================================================
|
|
*/
|
|
|
|
int scp_send_msg_to_queue(
|
|
uint32_t opendsp_id, /* enum opendsp_id */
|
|
uint32_t ipi_id,
|
|
void *buf,
|
|
uint32_t len,
|
|
uint32_t wait_ms)
|
|
{
|
|
struct scp_msg_queue_t *msg_queue = NULL;
|
|
struct scp_queue_element_t *p_element = NULL;
|
|
struct adsp_device_t *p_adsp_device = NULL;
|
|
|
|
uint32_t idx_msg = 0;
|
|
uint32_t queue_counter = 0;
|
|
|
|
int retval = 0;
|
|
unsigned long flags = 0;
|
|
|
|
uint32_t try_cnt = 0;
|
|
const uint32_t k_max_try_cnt = 20;
|
|
const uint32_t k_restart_sleep_min_us = 20 * 1000;
|
|
const uint32_t k_restart_sleep_max_us = (k_restart_sleep_min_us + 200);
|
|
|
|
scp_debug("%s(+), opendsp_id: %u, ipi_id: %u, buf: %p, len: %u, wait_ms: %u\n",
|
|
__func__, opendsp_id, ipi_id, buf, len, wait_ms);
|
|
|
|
p_adsp_device = get_adsp_device_by_id(opendsp_id);
|
|
|
|
if (!p_adsp_device) {
|
|
pr_err("%s(), opendsp_id: could not found adsp_device for %u!!\n",
|
|
__func__, opendsp_id);
|
|
return -EFAULT;
|
|
}
|
|
if (buf == NULL || len > SCP_MSG_BUFFER_SIZE) {
|
|
pr_err("%s(), buf: %p, len: %u!! return\n",
|
|
__func__, buf, len);
|
|
return -EFAULT;
|
|
}
|
|
|
|
|
|
msg_queue = &p_adsp_device->scp_msg_queue[SCP_PATH_A2S];
|
|
if (msg_queue->enable == false) {
|
|
pr_err("%s(), queue disabled!! return\n", __func__);
|
|
return -1;
|
|
}
|
|
|
|
if (!hifixdsp_run_status() || is_from_wdt) {
|
|
pr_info("DSP shutdown, close ipi and empty scp queue!\n");
|
|
msg_queue->idx_w = 0;
|
|
msg_queue->idx_r = 0;
|
|
return -1;
|
|
}
|
|
|
|
/* NEVER sleep in ISR */
|
|
if (in_interrupt() && wait_ms != 0) {
|
|
pr_info("%s(), in_interrupt()!! wait_ms %u => 0!!\n",
|
|
__func__, wait_ms);
|
|
wait_ms = 0;
|
|
}
|
|
|
|
/* push message to queue */
|
|
spin_lock_irqsave(&msg_queue->queue_lock, flags);
|
|
retval = scp_push_msg(msg_queue,
|
|
ipi_id,
|
|
buf,
|
|
len,
|
|
NULL, /* only for recv msg from scp */
|
|
(wait_ms != 0),
|
|
&idx_msg,
|
|
&queue_counter);
|
|
spin_unlock_irqrestore(&msg_queue->queue_lock, flags);
|
|
if (retval != 0) {
|
|
pr_info("%s(), opendsp_id: %u, push fail!!\n",
|
|
__func__, opendsp_id);
|
|
return retval;
|
|
}
|
|
|
|
/* notify queue thread to process it */
|
|
wake_up_interruptible(&msg_queue->queue_wq);
|
|
|
|
/* no need to wait */
|
|
if (wait_ms == 0) {
|
|
scp_debug("%s(-), wait_ms == 0, exit\n", __func__);
|
|
return 0;
|
|
}
|
|
|
|
|
|
/* wait until message processed */
|
|
p_element = &msg_queue->element[idx_msg];
|
|
for (try_cnt = 0; try_cnt < k_max_try_cnt; try_cnt++) {
|
|
retval = wait_event_interruptible_timeout(
|
|
p_element->element_wq,
|
|
(p_element->signal_arrival == true ||
|
|
msg_queue->enable == false),
|
|
msecs_to_jiffies(wait_ms));
|
|
|
|
if (msg_queue->enable == false) {
|
|
pr_debug("%s(), queue disable\n", __func__);
|
|
retval = -1;
|
|
break;
|
|
}
|
|
if (retval > 0) {
|
|
retval = 0;
|
|
break;
|
|
}
|
|
if (retval == 0) { /* timeout; will still send to scp later */
|
|
pr_info("%s(), wait timeout, retval: %d, wait_ms: %u\n",
|
|
__func__, retval, wait_ms);
|
|
break;
|
|
}
|
|
if (retval == -ERESTARTSYS) {
|
|
pr_info("%s(), -ERESTARTSYS, retval: %d, wait_ms: %u\n",
|
|
__func__, retval, wait_ms);
|
|
retval = -EINTR;
|
|
usleep_range(k_restart_sleep_min_us,
|
|
k_restart_sleep_max_us);
|
|
}
|
|
}
|
|
|
|
/* get ipc result */
|
|
spin_lock_irqsave(&p_element->element_lock, flags);
|
|
if (p_element->queue_counter == queue_counter) {
|
|
/* keep retval of scp_send_msg_to_scp */
|
|
if (p_element->signal_arrival == true)
|
|
retval = p_element->send_retval;
|
|
p_element->wait_in_thread = false;
|
|
}
|
|
spin_unlock_irqrestore(&p_element->element_lock, flags);
|
|
|
|
|
|
|
|
scp_debug("%s(-), opendsp_id: %u, ipi_id: %u, buf: %p, len: %u, wait_ms: %u\n",
|
|
__func__, opendsp_id, ipi_id, buf, len, wait_ms);
|
|
|
|
return retval;
|
|
}
|
|
|
|
|
|
int scp_dispatch_ipi_hanlder_to_queue(
|
|
uint32_t opendsp_id,
|
|
uint32_t ipi_id,
|
|
void *buf,
|
|
uint32_t len,
|
|
void (*ipi_handler)(int ipi_id, void *buf, unsigned int len))
|
|
{
|
|
struct scp_msg_queue_t *msg_queue = NULL;
|
|
struct adsp_device_t *p_adsp_device = NULL;
|
|
|
|
uint32_t idx_msg = 0;
|
|
uint32_t queue_counter = 0;
|
|
|
|
int retval = 0;
|
|
unsigned long flags = 0;
|
|
|
|
|
|
scp_debug("%s(+), opendsp_id: %u, ipi_id: %u, buf: %p, len: %u, ipi_handler: %p\n",
|
|
__func__, opendsp_id, ipi_id, buf, len, ipi_handler);
|
|
|
|
p_adsp_device = get_adsp_device_by_id(opendsp_id);
|
|
|
|
if (!p_adsp_device) {
|
|
pr_err("%s(), opendsp_id: could not found adsp_device for %u!!\n",
|
|
__func__, opendsp_id);
|
|
return -EFAULT;
|
|
}
|
|
if (buf == NULL || len > SCP_MSG_BUFFER_SIZE) {
|
|
pr_info("%s(), buf: %p, len: %u!! return\n",
|
|
__func__, buf, len);
|
|
return -EFAULT;
|
|
}
|
|
if (ipi_handler == NULL) {
|
|
pr_info("%s(), NULL!! ipi_handler: %p\n",
|
|
__func__, ipi_handler);
|
|
return -EFAULT;
|
|
}
|
|
|
|
msg_queue = &p_adsp_device->scp_msg_queue[SCP_PATH_S2A];
|
|
if (msg_queue->enable == false) {
|
|
pr_info("%s(), queue disabled!! return\n", __func__);
|
|
return -1;
|
|
}
|
|
|
|
|
|
/* push message to queue */
|
|
spin_lock_irqsave(&msg_queue->queue_lock, flags);
|
|
retval = scp_push_msg(msg_queue,
|
|
ipi_id,
|
|
buf,
|
|
len,
|
|
ipi_handler,
|
|
false,
|
|
&idx_msg,
|
|
&queue_counter);
|
|
spin_unlock_irqrestore(&msg_queue->queue_lock, flags);
|
|
if (retval != 0) {
|
|
pr_info("%s(), opendsp_id: %u, push fail!!\n",
|
|
__func__, opendsp_id);
|
|
return retval;
|
|
}
|
|
|
|
/* notify queue thread to process it */
|
|
wake_up_interruptible(&msg_queue->queue_wq);
|
|
|
|
scp_debug("%s(-), opendsp_id: %u, ipi_id: %u, buf: %p, len: %u, ipi_handler: %p\n",
|
|
__func__, opendsp_id, ipi_id, buf, len, ipi_handler);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
* =============================================================================
|
|
* private implementation
|
|
* =============================================================================
|
|
*/
|
|
|
|
static void scp_dump_msg_in_queue(struct scp_msg_queue_t *msg_queue)
|
|
{
|
|
struct scp_msg_t *p_scp_msg = NULL;
|
|
uint32_t idx_dump = msg_queue->idx_r;
|
|
|
|
pr_info("%s(), opendsp_id: %u, idx_r: %u, idx_w: %u, queue(%u/%u)\n",
|
|
__func__,
|
|
msg_queue->opendsp_id,
|
|
msg_queue->idx_r,
|
|
msg_queue->idx_w,
|
|
scp_get_num_messages_in_queue(msg_queue),
|
|
msg_queue->size);
|
|
|
|
while (idx_dump != msg_queue->idx_w) {
|
|
/* get head msg */
|
|
p_scp_msg = &msg_queue->element[idx_dump].msg;
|
|
|
|
pr_info("element[%u], ipi_id: %u, len: %u\n",
|
|
idx_dump, p_scp_msg->ipi_id, p_scp_msg->len);
|
|
|
|
/* update dump index */
|
|
idx_dump++;
|
|
if (idx_dump == msg_queue->size)
|
|
idx_dump = 0;
|
|
}
|
|
}
|
|
|
|
|
|
static int scp_push_msg(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
uint32_t ipi_id,
|
|
void *buf,
|
|
uint32_t len,
|
|
void (*ipi_handler)(int ipi_id, void *buf, unsigned int len),
|
|
bool wait_in_thread,
|
|
uint32_t *p_idx_msg,
|
|
uint32_t *p_queue_counter)
|
|
{
|
|
struct scp_msg_t *p_scp_msg = NULL;
|
|
struct scp_queue_element_t *p_element = NULL;
|
|
|
|
unsigned long flags = 0;
|
|
|
|
if (msg_queue == NULL || buf == NULL ||
|
|
p_idx_msg == NULL || p_queue_counter == NULL) {
|
|
pr_info("%s(), NULL!! msg_queue: %p, buf: %p, p_idx_msg: %p, p_queue_counter: %p\n",
|
|
__func__, msg_queue, buf, p_idx_msg, p_queue_counter);
|
|
return -EFAULT;
|
|
}
|
|
|
|
/* check queue full */
|
|
if (scp_check_queue_to_be_full(msg_queue) == true) {
|
|
pr_info("opendsp_id: %u, ipi_id: %u, queue overflow, idx_r: %u, idx_w: %u, drop it\n",
|
|
msg_queue->opendsp_id,
|
|
ipi_id, msg_queue->idx_r,
|
|
msg_queue->idx_w);
|
|
scp_dump_msg_in_queue(msg_queue);
|
|
WARN_ON(1);
|
|
return -EOVERFLOW;
|
|
}
|
|
|
|
if (scp_check_idx_msg_valid(msg_queue, msg_queue->idx_w) == false) {
|
|
pr_info("%s(), idx_w %u is invalid!! return\n",
|
|
__func__, msg_queue->idx_w);
|
|
return -1;
|
|
}
|
|
|
|
/* push */
|
|
*p_idx_msg = msg_queue->idx_w;
|
|
msg_queue->idx_w++;
|
|
if (msg_queue->idx_w == msg_queue->size)
|
|
msg_queue->idx_w = 0;
|
|
|
|
/* copy */
|
|
p_element = &msg_queue->element[*p_idx_msg];
|
|
spin_lock_irqsave(&p_element->element_lock, flags);
|
|
|
|
p_scp_msg = &p_element->msg;
|
|
p_scp_msg->ipi_id = ipi_id;
|
|
memcpy((void *)p_scp_msg->buf, buf, len);
|
|
p_scp_msg->len = len;
|
|
p_scp_msg->ipi_handler = ipi_handler;
|
|
|
|
p_element->wait_in_thread = wait_in_thread;
|
|
p_element->signal_arrival = false;
|
|
p_element->send_retval = 0;
|
|
p_element->queue_counter = msg_queue->queue_counter;
|
|
|
|
*p_queue_counter = msg_queue->queue_counter;
|
|
if (msg_queue->queue_counter == 0xFFFFFFFF)
|
|
msg_queue->queue_counter = 0;
|
|
else
|
|
msg_queue->queue_counter++;
|
|
spin_unlock_irqrestore(&p_element->element_lock, flags);
|
|
|
|
scp_debug("%s(), opendsp_id: %u, scp_path: %u, ipi_id: %u, idx_r: %u, idx_w: %u, queue(%u/%u), *p_idx_msg: %u\n",
|
|
__func__,
|
|
msg_queue->opendsp_id,
|
|
msg_queue->scp_path,
|
|
p_scp_msg->ipi_id,
|
|
msg_queue->idx_r,
|
|
msg_queue->idx_w,
|
|
scp_get_num_messages_in_queue(msg_queue),
|
|
msg_queue->size,
|
|
*p_idx_msg);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static int scp_pop_msg(struct scp_msg_queue_t *msg_queue)
|
|
{
|
|
struct scp_msg_t *p_scp_msg = NULL;
|
|
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), NULL!! msg_queue: %p\n", __func__, msg_queue);
|
|
return -EFAULT;
|
|
}
|
|
|
|
/* check queue empty */
|
|
if (scp_check_queue_empty(msg_queue) == true) {
|
|
pr_info("%s(), opendsp_id: %u, queue is empty, idx_r: %u, idx_w: %u\n",
|
|
__func__,
|
|
msg_queue->opendsp_id,
|
|
msg_queue->idx_r,
|
|
msg_queue->idx_w);
|
|
return -1;
|
|
}
|
|
|
|
/* pop */
|
|
p_scp_msg = &msg_queue->element[msg_queue->idx_r].msg;
|
|
msg_queue->idx_r++;
|
|
if (msg_queue->idx_r == msg_queue->size)
|
|
msg_queue->idx_r = 0;
|
|
|
|
|
|
scp_debug("%s(), opendsp_id: %u, scp_path: %u, ipi_id: %u, idx_r: %u, idx_w: %u, queue(%u/%u)\n",
|
|
__func__,
|
|
msg_queue->opendsp_id,
|
|
msg_queue->scp_path,
|
|
p_scp_msg->ipi_id,
|
|
msg_queue->idx_r,
|
|
msg_queue->idx_w,
|
|
scp_get_num_messages_in_queue(msg_queue),
|
|
msg_queue->size);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static int scp_front_msg(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t **pp_scp_msg,
|
|
uint32_t *p_idx_msg)
|
|
{
|
|
if (msg_queue == NULL || pp_scp_msg == NULL || p_idx_msg == NULL) {
|
|
pr_info("%s(), NULL!! msg_queue: %p, pp_scp_msg: %p, p_idx_msg: %p\n",
|
|
__func__, msg_queue, pp_scp_msg, p_idx_msg);
|
|
return -EFAULT;
|
|
}
|
|
|
|
*pp_scp_msg = NULL;
|
|
*p_idx_msg = 0xFFFFFFFF;
|
|
|
|
/* check queue empty */
|
|
if (scp_check_queue_empty(msg_queue) == true) {
|
|
pr_info("%s(), opendsp_id: %u, queue empty, idx_r: %u, idx_w: %u\n",
|
|
__func__, msg_queue->opendsp_id,
|
|
msg_queue->idx_r, msg_queue->idx_w);
|
|
return -ENOMEM;
|
|
}
|
|
|
|
/* front */
|
|
if (scp_check_idx_msg_valid(msg_queue, msg_queue->idx_r) == false) {
|
|
pr_info("%s(), idx_r %u is invalid!! return\n",
|
|
__func__, msg_queue->idx_r);
|
|
return -1;
|
|
}
|
|
*p_idx_msg = msg_queue->idx_r;
|
|
*pp_scp_msg = &msg_queue->element[*p_idx_msg].msg;
|
|
|
|
#if 0
|
|
scp_debug("%s(), opendsp_id: %u, scp_path: %u, ipi_id: %u, idx_r: %u, idx_w: %u, queue(%u/%u), *p_idx_msg: %u\n",
|
|
__func__,
|
|
msg_queue->opendsp_id,
|
|
msg_queue->scp_path,
|
|
msg_queue->element[*p_idx_msg].msg.ipi_id,
|
|
msg_queue->idx_r,
|
|
msg_queue->idx_w,
|
|
scp_get_num_messages_in_queue(msg_queue),
|
|
msg_queue->size,
|
|
*p_idx_msg);
|
|
#endif
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static int scp_get_queue_element(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t **pp_scp_msg,
|
|
uint32_t *p_idx_msg)
|
|
{
|
|
bool is_empty = true;
|
|
|
|
unsigned long flags = 0;
|
|
int retval = 0;
|
|
|
|
uint32_t try_cnt = 0;
|
|
const uint32_t k_max_try_cnt = 20;
|
|
const uint32_t k_restart_sleep_min_us = 1000;
|
|
const uint32_t k_restart_sleep_max_us = (k_restart_sleep_min_us + 200);
|
|
|
|
spin_lock_irqsave(&msg_queue->queue_lock, flags);
|
|
is_empty = scp_check_queue_empty(msg_queue);
|
|
spin_unlock_irqrestore(&msg_queue->queue_lock, flags);
|
|
|
|
/* wait until message is pushed to queue */
|
|
if (is_empty == true) {
|
|
for (try_cnt = 0; try_cnt < k_max_try_cnt; try_cnt++) {
|
|
retval = wait_event_interruptible(
|
|
msg_queue->queue_wq,
|
|
(!scp_check_queue_empty(msg_queue) ||
|
|
!msg_queue->thread_enable));
|
|
if (msg_queue->thread_enable == false) {
|
|
pr_debug("%s(), thread disable\n", __func__);
|
|
retval = -1;
|
|
break;
|
|
}
|
|
if (retval == 0) /* got msg in queue */
|
|
break;
|
|
if (retval == -ERESTARTSYS) {
|
|
pr_info("%s(), -ERESTARTSYS, retval: %d\n",
|
|
__func__, retval);
|
|
retval = -EINTR;
|
|
usleep_range(k_restart_sleep_min_us,
|
|
k_restart_sleep_max_us);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (retval == 0) {
|
|
spin_lock_irqsave(&msg_queue->queue_lock, flags);
|
|
retval = scp_front_msg(msg_queue, pp_scp_msg, p_idx_msg);
|
|
spin_unlock_irqrestore(&msg_queue->queue_lock, flags);
|
|
}
|
|
|
|
return retval;
|
|
}
|
|
|
|
|
|
static int scp_init_single_msg_queue(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
const uint32_t opendsp_id,
|
|
const uint32_t scp_path)
|
|
{
|
|
struct scp_queue_element_t *p_element = NULL;
|
|
struct scp_msg_t *p_scp_msg = NULL;
|
|
|
|
char thread_name[32] = {0};
|
|
|
|
int i = 0;
|
|
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), NULL!! msg_queue: %p\n", __func__, msg_queue);
|
|
return -EFAULT;
|
|
}
|
|
|
|
if (scp_path >= SCP_NUM_PATH) {
|
|
pr_info("%s(), opendsp_id: %u, scp_path: %u error!!\n",
|
|
__func__, opendsp_id, scp_path);
|
|
return -EFAULT;
|
|
}
|
|
|
|
|
|
/* check double init */
|
|
if (msg_queue->init) {
|
|
pr_info("%s(), opendsp_id: %u already init!!\n",
|
|
__func__, opendsp_id);
|
|
return 0;
|
|
}
|
|
msg_queue->init = true;
|
|
|
|
/* init var */
|
|
msg_queue->opendsp_id = opendsp_id;
|
|
msg_queue->scp_path = scp_path;
|
|
|
|
for (i = 0; i < MAX_SCP_MSG_NUM_IN_QUEUE; i++) {
|
|
p_element = &msg_queue->element[i];
|
|
|
|
p_scp_msg = &p_element->msg;
|
|
p_scp_msg->ipi_id = 0;
|
|
memset((void *)p_scp_msg->buf, 0, SCP_MSG_BUFFER_SIZE);
|
|
p_scp_msg->len = 0;
|
|
p_scp_msg->ipi_handler = NULL;
|
|
|
|
spin_lock_init(&p_element->element_lock);
|
|
init_waitqueue_head(&p_element->element_wq);
|
|
|
|
p_element->wait_in_thread = false;
|
|
p_element->signal_arrival = false;
|
|
p_element->send_retval = 0;
|
|
p_element->queue_counter = 0;
|
|
}
|
|
|
|
msg_queue->size = MAX_SCP_MSG_NUM_IN_QUEUE;
|
|
msg_queue->idx_r = 0;
|
|
msg_queue->idx_w = 0;
|
|
|
|
msg_queue->queue_counter = 0;
|
|
|
|
spin_lock_init(&msg_queue->queue_lock);
|
|
init_waitqueue_head(&msg_queue->queue_wq);
|
|
|
|
|
|
if (scp_path == SCP_PATH_A2S) {
|
|
msg_queue->scp_process_msg_func = scp_send_msg_to_scp;
|
|
snprintf(thread_name,
|
|
sizeof(thread_name),
|
|
"scp_send_thread_id_%u", opendsp_id);
|
|
} else if (scp_path == SCP_PATH_S2A) {
|
|
msg_queue->scp_process_msg_func = scp_process_msg_from_scp;
|
|
snprintf(thread_name,
|
|
sizeof(thread_name),
|
|
"scp_recv_thread_id_%u", opendsp_id);
|
|
} else
|
|
WARN_ON(1);
|
|
|
|
/* lunch thread */
|
|
msg_queue->scp_thread_task = kthread_create(
|
|
scp_process_msg_thread,
|
|
msg_queue,
|
|
thread_name);
|
|
if (IS_ERR(msg_queue->scp_thread_task)) {
|
|
pr_info("can not create %s kthread\n", thread_name);
|
|
WARN_ON(1);
|
|
msg_queue->thread_enable = false;
|
|
} else {
|
|
msg_queue->thread_enable = true;
|
|
wake_up_process(msg_queue->scp_thread_task);
|
|
}
|
|
|
|
/* enable */
|
|
msg_queue->enable = true;
|
|
|
|
|
|
return (msg_queue->enable && msg_queue->thread_enable) ? 0 : -EFAULT;
|
|
}
|
|
|
|
|
|
static int scp_process_msg_thread(void *data)
|
|
{
|
|
struct scp_msg_queue_t *msg_queue = (struct scp_msg_queue_t *)data;
|
|
struct scp_queue_element_t *p_element = NULL;
|
|
struct scp_msg_t *p_scp_msg = NULL;
|
|
uint32_t idx_msg = 0;
|
|
|
|
unsigned long flags = 0;
|
|
int retval = 0;
|
|
|
|
if (msg_queue == NULL) {
|
|
pr_info("%s(), msg_queue == NULL!! return\n", __func__);
|
|
return -EFAULT;
|
|
}
|
|
|
|
|
|
while (msg_queue->thread_enable && !kthread_should_stop()) {
|
|
/* wait until element pushed */
|
|
retval = scp_get_queue_element(msg_queue, &p_scp_msg, &idx_msg);
|
|
if (retval != 0) {
|
|
pr_info("%s(), scp_get_queue_element retval %d\n",
|
|
__func__, retval);
|
|
continue;
|
|
}
|
|
p_element = &msg_queue->element[idx_msg];
|
|
|
|
/* send to scp */
|
|
retval = msg_queue->scp_process_msg_func(msg_queue, p_scp_msg);
|
|
if (retval != 0)
|
|
WARN_ON(1);
|
|
|
|
/* notify element if need */
|
|
spin_lock_irqsave(&p_element->element_lock, flags);
|
|
if (p_element->wait_in_thread == true) {
|
|
p_element->send_retval = retval;
|
|
p_element->signal_arrival = true;
|
|
wake_up_interruptible(&p_element->element_wq);
|
|
}
|
|
spin_unlock_irqrestore(&p_element->element_lock, flags);
|
|
|
|
|
|
/* pop message from queue */
|
|
spin_lock_irqsave(&msg_queue->queue_lock, flags);
|
|
scp_pop_msg(msg_queue);
|
|
spin_unlock_irqrestore(&msg_queue->queue_lock, flags);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static int scp_send_msg_to_scp(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t *p_scp_msg)
|
|
{
|
|
const int k_max_try_count = 1500;
|
|
int try_count = 0;
|
|
bool send_fail = false;
|
|
struct adsp_device_t *p_adsp_device = NULL;
|
|
int retval = 0;
|
|
|
|
p_adsp_device = get_adsp_device_by_id(msg_queue->opendsp_id);
|
|
|
|
if (msg_queue == NULL || p_scp_msg == NULL) {
|
|
pr_info("%s(), NULL!! msg_queue: %p, p_scp_msg: %p\n",
|
|
__func__, msg_queue, p_scp_msg);
|
|
return -EFAULT;
|
|
}
|
|
|
|
if (p_adsp_device == NULL) {
|
|
pr_info("%s(), NULL!! p_adsp_device: %p\n",
|
|
__func__, p_adsp_device);
|
|
return -EFAULT;
|
|
}
|
|
|
|
for (try_count = 0; try_count < k_max_try_count; try_count++) {
|
|
retval = p_adsp_device->send_handler(
|
|
p_scp_msg->ipi_id,
|
|
p_scp_msg->buf,
|
|
p_scp_msg->len);
|
|
|
|
if (retval == 0)
|
|
break;
|
|
send_fail = true;
|
|
msleep(20);
|
|
}
|
|
if (send_fail) {
|
|
pr_info("%s(), opendsp_id %u retry #%d, ret %d\n",
|
|
__func__, msg_queue->opendsp_id,
|
|
try_count, retval);
|
|
}
|
|
|
|
IPI_ASSERT(retval == 0);
|
|
|
|
return retval;
|
|
}
|
|
|
|
|
|
static int scp_process_msg_from_scp(
|
|
struct scp_msg_queue_t *msg_queue,
|
|
struct scp_msg_t *p_scp_msg)
|
|
{
|
|
if (msg_queue == NULL || p_scp_msg == NULL) {
|
|
pr_info("%s(), NULL!! msg_queue: %p, p_scp_msg: %p\n",
|
|
__func__, msg_queue, p_scp_msg);
|
|
return -EFAULT;
|
|
}
|
|
|
|
if (p_scp_msg->ipi_handler == NULL) {
|
|
pr_info("%s(), NULL!! p_scp_msg->ipi_handler: %p\n",
|
|
__func__, p_scp_msg->ipi_handler);
|
|
return -EFAULT;
|
|
}
|
|
|
|
if (p_scp_msg->len == 0) {
|
|
pr_info("%s(), p_scp_msg->buf: %p, p_scp_msg->len: %u\n",
|
|
__func__, p_scp_msg->buf, p_scp_msg->len);
|
|
return -EFAULT;
|
|
}
|
|
|
|
/* TODO: add time info here */
|
|
p_scp_msg->ipi_handler(
|
|
p_scp_msg->ipi_id,
|
|
p_scp_msg->buf,
|
|
p_scp_msg->len);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|