From 31def064871336af7780d01a6ed8c5ed075b7e48 Mon Sep 17 00:00:00 2001
From: Shengjiu Wang <shengjiu.wang@nxp.com>
Date: Thu, 7 Mar 2019 11:01:06 +0800
Subject: [PATCH] MLK-21107-2: ASoC: Drop msg if msg queue is full
We designed length of msg queue to be 0x18, which is too
small if the period size = 512bytes, sometimes the queue
will be full, which cause the msg is replaced by new one.
which is not acceptable.
So we need to enlarge the length of msg queue and add
protection when msg queue is full, if the msg queue is full
with the cmd (START, STOP, PAUSE...), driver will return
error, with the cmd (I2S_TX_POINTER, I2S_TX_PERIOD_DONE)
driver will drop the cmd.
Signed-off-by: Shengjiu Wang <shengjiu.wang@nxp.com>
---
sound/soc/fsl/fsl_rpmsg_i2s.c | 4 ++
sound/soc/fsl/fsl_rpmsg_i2s.h | 6 +-
sound/soc/fsl/imx-pcm-rpmsg.c | 127 +++++++++++++++++++++-------------
3 files changed, 88 insertions(+), 49 deletions(-)
diff --git a/sound/soc/fsl/fsl_rpmsg_i2s.c b/sound/soc/fsl/fsl_rpmsg_i2s.c
index 192802ae339be4..c6836736662b73 100644
--- a/sound/soc/fsl/fsl_rpmsg_i2s.c
+++ b/sound/soc/fsl/fsl_rpmsg_i2s.c
@@ -149,6 +149,9 @@ static void rpmsg_i2s_work(struct work_struct *work)
i2s_info = work_of_rpmsg->i2s_info;
i2s_send_message(&work_of_rpmsg->msg, i2s_info);
+
+ i2s_info->work_read_index++;
+ i2s_info->work_read_index %= WORK_MAX_NUM;
}
static int fsl_rpmsg_i2s_probe(struct platform_device *pdev)
@@ -179,6 +182,7 @@ static int fsl_rpmsg_i2s_probe(struct platform_device *pdev)
return -ENOMEM;
}
+ i2s_info->work_write_index = 1;
i2s_info->send_message = i2s_send_message;
for (i = 0; i < WORK_MAX_NUM; i++) {
diff --git a/sound/soc/fsl/fsl_rpmsg_i2s.h b/sound/soc/fsl/fsl_rpmsg_i2s.h
index 36d6c5f5136003..14922f1a769080 100644
--- a/sound/soc/fsl/fsl_rpmsg_i2s.h
+++ b/sound/soc/fsl/fsl_rpmsg_i2s.h
@@ -309,7 +309,7 @@
#define I2S_TYPE_A_NUM 0x18
-#define WORK_MAX_NUM 0x18
+#define WORK_MAX_NUM 0x30
#define I2S_TX_PERIOD_DONE 0x0
#define I2S_RX_PERIOD_DONE 0x1
@@ -397,7 +397,9 @@ struct i2s_info {
struct workqueue_struct *rpmsg_wq;
struct work_of_rpmsg work_list[WORK_MAX_NUM];
- int work_index;
+ int work_write_index;
+ int work_read_index;
+ int msg_drop_count[2];
int num_period[2];
void *callback_param[2];
int (*send_message)(struct i2s_rpmsg *msg, struct i2s_info *info);
diff --git a/sound/soc/fsl/imx-pcm-rpmsg.c b/sound/soc/fsl/imx-pcm-rpmsg.c
index c61dc98dd33538..1cbdf4215c11e6 100644
--- a/sound/soc/fsl/imx-pcm-rpmsg.c
+++ b/sound/soc/fsl/imx-pcm-rpmsg.c
@@ -127,7 +127,7 @@ static void imx_rpmsg_timer_callback(unsigned long data)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
int time_msec;
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
@@ -140,11 +140,14 @@ static void imx_rpmsg_timer_callback(unsigned long data)
else
rpmsg->send_msg.header.cmd = I2S_RX_POINTER;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
+ queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ i2s_info->msg_drop_count[substream->stream]++;
if (rpmsg_i2s->force_lpa) {
time_msec = (int)(runtime->period_size*1000/runtime->rate);
@@ -208,6 +211,7 @@ static int imx_rpmsg_pcm_open(struct snd_pcm_substream *substream)
if (ret < 0)
return ret;
+ i2s_info->msg_drop_count[substream->stream] = 0;
/*create thread*/
setup_timer(&i2s_info->stream_timer[substream->stream],
@@ -243,6 +247,10 @@ static int imx_rpmsg_pcm_close(struct snd_pcm_substream *substream)
kfree(prtd);
+ if (i2s_info->msg_drop_count[substream->stream])
+ dev_warn(rtd->dev, "Msg is dropped!, number is %d\n",
+ i2s_info->msg_drop_count[substream->stream]);
+
return ret;
}
@@ -324,7 +332,7 @@ static int imx_rpmsg_pcm_prepare_and_submit(struct snd_pcm_substream *substream)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
rpmsg = &i2s_info->rpmsg[I2S_TX_BUFFER];
@@ -346,18 +354,21 @@ static int imx_rpmsg_pcm_prepare_and_submit(struct snd_pcm_substream *substream)
rpmsg->send_msg.param.buffer_size /
rpmsg->send_msg.param.period_size;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
+ queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ return -EPIPE;
i2s_info->callback[substream->stream] = imx_rpmsg_pcm_dma_complete;
i2s_info->callback_param[substream->stream] = substream;
return 0;
}
-static void imx_rpmsg_async_issue_pending(struct snd_pcm_substream *substream)
+static int imx_rpmsg_async_issue_pending(struct snd_pcm_substream *substream)
{
struct snd_pcm_runtime *runtime = substream->runtime;
struct snd_soc_pcm_runtime *rtd = substream->private_data;
@@ -365,7 +376,7 @@ static void imx_rpmsg_async_issue_pending(struct snd_pcm_substream *substream)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
int time_msec;
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
@@ -378,17 +389,22 @@ static void imx_rpmsg_async_issue_pending(struct snd_pcm_substream *substream)
else
rpmsg->send_msg.header.cmd = I2S_RX_START;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
- sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ sizeof(struct i2s_rpmsg_s));
+ queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ return -EPIPE;
if (rpmsg_i2s->force_lpa) {
time_msec = (int)(runtime->period_size*1000/runtime->rate);
mod_timer(&i2s_info->stream_timer[substream->stream],
jiffies + msecs_to_jiffies(time_msec));
}
+
+ return 0;
}
static int imx_rpmsg_restart(struct snd_pcm_substream *substream)
@@ -398,7 +414,7 @@ static int imx_rpmsg_restart(struct snd_pcm_substream *substream)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
rpmsg = &i2s_info->rpmsg[I2S_TX_RESTART];
@@ -410,11 +426,14 @@ static int imx_rpmsg_restart(struct snd_pcm_substream *substream)
else
rpmsg->send_msg.header.cmd = I2S_RX_RESTART;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
+ queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ return -EPIPE;
return 0;
}
@@ -426,7 +445,7 @@ static int imx_rpmsg_pause(struct snd_pcm_substream *substream)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
rpmsg = &i2s_info->rpmsg[I2S_TX_PAUSE];
@@ -438,11 +457,15 @@ static int imx_rpmsg_pause(struct snd_pcm_substream *substream)
else
rpmsg->send_msg.header.cmd = I2S_RX_PAUSE;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
- sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ sizeof(struct i2s_rpmsg_s));
+ queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ return -EPIPE;
+
return 0;
}
@@ -453,7 +476,7 @@ static int imx_rpmsg_terminate_all(struct snd_pcm_substream *substream)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
int cmd;
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
@@ -466,12 +489,6 @@ static int imx_rpmsg_terminate_all(struct snd_pcm_substream *substream)
else
rpmsg->send_msg.header.cmd = I2S_RX_TERMINATE;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
- sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
-
if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
cmd = I2S_TX_PERIOD_DONE + I2S_TYPE_A_NUM;
i2s_info->rpmsg[cmd].send_msg.param.buffer_tail = 0;
@@ -485,6 +502,16 @@ static int imx_rpmsg_terminate_all(struct snd_pcm_substream *substream)
}
del_timer(&i2s_info->stream_timer[substream->stream]);
+
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ sizeof(struct i2s_rpmsg_s));
+ queue_work(i2s_info->rpmsg_wq, &i2s_info->work_list[index].work);
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ return -EPIPE;
+
return 0;
}
@@ -495,14 +522,14 @@ int imx_rpmsg_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
- int ret;
+ int ret = 0;
switch (cmd) {
case SNDRV_PCM_TRIGGER_START:
ret = imx_rpmsg_pcm_prepare_and_submit(substream);
if (ret)
return ret;
- imx_rpmsg_async_issue_pending(substream);
+ ret = imx_rpmsg_async_issue_pending(substream);
break;
case SNDRV_PCM_TRIGGER_RESUME:
if (rpmsg_i2s->force_lpa) {
@@ -514,27 +541,30 @@ int imx_rpmsg_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
break;
}
case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
- imx_rpmsg_restart(substream);
+ ret = imx_rpmsg_restart(substream);
break;
case SNDRV_PCM_TRIGGER_SUSPEND:
if (!rpmsg_i2s->force_lpa) {
if (runtime->info & SNDRV_PCM_INFO_PAUSE)
- imx_rpmsg_pause(substream);
+ ret = imx_rpmsg_pause(substream);
else
- imx_rpmsg_terminate_all(substream);
+ ret = imx_rpmsg_terminate_all(substream);
} else
del_timer(&i2s_info->stream_timer[substream->stream]);
break;
case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
- imx_rpmsg_pause(substream);
+ ret = imx_rpmsg_pause(substream);
break;
case SNDRV_PCM_TRIGGER_STOP:
- imx_rpmsg_terminate_all(substream);
+ ret = imx_rpmsg_terminate_all(substream);
break;
default:
return -EINVAL;
}
+ if (ret)
+ return ret;
+
return 0;
}
@@ -547,7 +577,7 @@ int imx_rpmsg_pcm_ack(struct snd_pcm_substream *substream)
struct fsl_rpmsg_i2s *rpmsg_i2s = dev_get_drvdata(cpu_dai->dev);
struct i2s_info *i2s_info = &rpmsg_i2s->i2s_info;
struct i2s_rpmsg *rpmsg;
- u8 index = i2s_info->work_index;
+ int index = i2s_info->work_write_index;
int buffer_tail = 0;
if (!rpmsg_i2s->force_lpa)
@@ -571,12 +601,15 @@ int imx_rpmsg_pcm_ack(struct snd_pcm_substream *substream)
if (buffer_tail != rpmsg->send_msg.param.buffer_tail) {
rpmsg->send_msg.param.buffer_tail = buffer_tail;
- memcpy(&i2s_info->work_list[index].msg, rpmsg,
+ if (i2s_info->work_write_index != i2s_info->work_read_index) {
+ memcpy(&i2s_info->work_list[index].msg, rpmsg,
sizeof(struct i2s_rpmsg_s));
- queue_work(i2s_info->rpmsg_wq,
+ queue_work(i2s_info->rpmsg_wq,
&i2s_info->work_list[index].work);
- i2s_info->work_index++;
- i2s_info->work_index %= WORK_MAX_NUM;
+ i2s_info->work_write_index++;
+ i2s_info->work_write_index %= WORK_MAX_NUM;
+ } else
+ i2s_info->msg_drop_count[substream->stream]++;
}
return 0;
--
GitLab