diff mbox series

[V8,10/10] remoteproc: imx_proc: enable virtio/mailbox

Message ID 1611041711-15902-11-git-send-email-peng.fan@nxp.com
State Superseded
Headers show
Series remoteproc: imx_rproc: support iMX8MQ/M | expand

Commit Message

Peng Fan Jan. 19, 2021, 7:35 a.m. UTC
From: Peng Fan <peng.fan@nxp.com>


Use virtio/mailbox to build connection between Remote Proccessors
and Linux. Add work queue to handle incoming messages.

Reviewed-by: Richard Zhu <hongxing.zhu@nxp.com>

Signed-off-by: Peng Fan <peng.fan@nxp.com>

---
 drivers/remoteproc/imx_rproc.c | 116 ++++++++++++++++++++++++++++++++-
 1 file changed, 113 insertions(+), 3 deletions(-)

-- 
2.28.0

Comments

Mathieu Poirier Jan. 19, 2021, 4:39 p.m. UTC | #1
On Tue, Jan 19, 2021 at 03:35:11PM +0800, peng.fan@nxp.com wrote:
> From: Peng Fan <peng.fan@nxp.com>

> 

> Use virtio/mailbox to build connection between Remote Proccessors

> and Linux. Add work queue to handle incoming messages.

> 

> Reviewed-by: Richard Zhu <hongxing.zhu@nxp.com>

> Signed-off-by: Peng Fan <peng.fan@nxp.com>

> ---

>  drivers/remoteproc/imx_rproc.c | 116 ++++++++++++++++++++++++++++++++-

>  1 file changed, 113 insertions(+), 3 deletions(-)


Reviewed-by: Mathieu Poirier <mathieu.poirer@linaro.org>


I will pickup this set when Rob has reviewed the DT bindings.  In the mean time
you might want to fix the title section in the the .yaml file, the Kconfig
and the MODULE_DESCRIPTION and add a reference to i.MX8.

Thanks,
Mathieu

> 

> diff --git a/drivers/remoteproc/imx_rproc.c b/drivers/remoteproc/imx_rproc.c

> index 54ac143ba033..c16a91f8e410 100644

> --- a/drivers/remoteproc/imx_rproc.c

> +++ b/drivers/remoteproc/imx_rproc.c

> @@ -7,6 +7,7 @@

>  #include <linux/err.h>

>  #include <linux/interrupt.h>

>  #include <linux/kernel.h>

> +#include <linux/mailbox_client.h>

>  #include <linux/mfd/syscon.h>

>  #include <linux/module.h>

>  #include <linux/of_address.h>

> @@ -15,6 +16,9 @@

>  #include <linux/platform_device.h>

>  #include <linux/regmap.h>

>  #include <linux/remoteproc.h>

> +#include <linux/workqueue.h>

> +

> +#include "remoteproc_internal.h"

>  

>  #define IMX7D_SRC_SCR			0x0C

>  #define IMX7D_ENABLE_M4			BIT(3)

> @@ -86,6 +90,11 @@ struct imx_rproc {

>  	const struct imx_rproc_dcfg	*dcfg;

>  	struct imx_rproc_mem		mem[IMX7D_RPROC_MEM_MAX];

>  	struct clk			*clk;

> +	struct mbox_client		cl;

> +	struct mbox_chan		*tx_ch;

> +	struct mbox_chan		*rx_ch;

> +	struct work_struct		rproc_work;

> +	struct workqueue_struct		*workqueue;

>  };

>  

>  static const struct imx_rproc_att imx_rproc_att_imx8mq[] = {

> @@ -366,9 +375,33 @@ static int imx_rproc_parse_fw(struct rproc *rproc, const struct firmware *fw)

>  	return 0;

>  }

>  

> +static void imx_rproc_kick(struct rproc *rproc, int vqid)

> +{

> +	struct imx_rproc *priv = rproc->priv;

> +	int err;

> +	__u32 mmsg;

> +

> +	if (!priv->tx_ch) {

> +		dev_err(priv->dev, "No initialized mbox tx channel\n");

> +		return;

> +	}

> +

> +	/*

> +	 * Send the index of the triggered virtqueue as the mu payload.

> +	 * Let remote processor know which virtqueue is used.

> +	 */

> +	mmsg = vqid << 16;

> +

> +	err = mbox_send_message(priv->tx_ch, (void *)&mmsg);

> +	if (err < 0)

> +		dev_err(priv->dev, "%s: failed (%d, err:%d)\n",

> +			__func__, vqid, err);

> +}

> +

>  static const struct rproc_ops imx_rproc_ops = {

>  	.start		= imx_rproc_start,

>  	.stop		= imx_rproc_stop,

> +	.kick		= imx_rproc_kick,

>  	.da_to_va       = imx_rproc_da_to_va,

>  	.load		= rproc_elf_load_segments,

>  	.parse_fw	= imx_rproc_parse_fw,

> @@ -444,6 +477,66 @@ static int imx_rproc_addr_init(struct imx_rproc *priv,

>  	return 0;

>  }

>  

> +static void imx_rproc_vq_work(struct work_struct *work)

> +{

> +	struct imx_rproc *priv = container_of(work, struct imx_rproc,

> +					      rproc_work);

> +

> +	rproc_vq_interrupt(priv->rproc, 0);

> +	rproc_vq_interrupt(priv->rproc, 1);

> +}

> +

> +static void imx_rproc_rx_callback(struct mbox_client *cl, void *msg)

> +{

> +	struct rproc *rproc = dev_get_drvdata(cl->dev);

> +	struct imx_rproc *priv = rproc->priv;

> +

> +	queue_work(priv->workqueue, &priv->rproc_work);

> +}

> +

> +static int imx_rproc_xtr_mbox_init(struct rproc *rproc)

> +{

> +	struct imx_rproc *priv = rproc->priv;

> +	struct device *dev = priv->dev;

> +	struct mbox_client *cl;

> +	int ret;

> +

> +	if (!of_get_property(dev->of_node, "mbox-names", NULL))

> +		return 0;

> +

> +	cl = &priv->cl;

> +	cl->dev = dev;

> +	cl->tx_block = true;

> +	cl->tx_tout = 100;

> +	cl->knows_txdone = false;

> +	cl->rx_callback = imx_rproc_rx_callback;

> +

> +	priv->tx_ch = mbox_request_channel_byname(cl, "tx");

> +	if (IS_ERR(priv->tx_ch)) {

> +		ret = PTR_ERR(priv->tx_ch);

> +		return dev_err_probe(cl->dev, ret,

> +				     "failed to request tx mailbox channel: %d\n", ret);

> +	}

> +

> +	priv->rx_ch = mbox_request_channel_byname(cl, "rx");

> +	if (IS_ERR(priv->rx_ch)) {

> +		mbox_free_channel(priv->tx_ch);

> +		ret = PTR_ERR(priv->rx_ch);

> +		return dev_err_probe(cl->dev, ret,

> +				     "failed to request rx mailbox channel: %d\n", ret);

> +	}

> +

> +	return 0;

> +}

> +

> +static void imx_rproc_free_mbox(struct rproc *rproc)

> +{

> +	struct imx_rproc *priv = rproc->priv;

> +

> +	mbox_free_channel(priv->tx_ch);

> +	mbox_free_channel(priv->rx_ch);

> +}

> +

>  static int imx_rproc_probe(struct platform_device *pdev)

>  {

>  	struct device *dev = &pdev->dev;

> @@ -481,18 +574,28 @@ static int imx_rproc_probe(struct platform_device *pdev)

>  	priv->dev = dev;

>  

>  	dev_set_drvdata(dev, rproc);

> +	priv->workqueue = create_workqueue(dev_name(dev));

> +	if (!priv->workqueue) {

> +		dev_err(dev, "cannot create workqueue\n");

> +		ret = -ENOMEM;

> +		goto err_put_rproc;

> +	}

> +

> +	ret = imx_rproc_xtr_mbox_init(rproc);

> +	if (ret)

> +		goto err_put_wkq;

>  

>  	ret = imx_rproc_addr_init(priv, pdev);

>  	if (ret) {

>  		dev_err(dev, "failed on imx_rproc_addr_init\n");

> -		goto err_put_rproc;

> +		goto err_put_mbox;

>  	}

>  

>  	priv->clk = devm_clk_get(dev, NULL);

>  	if (IS_ERR(priv->clk)) {

>  		dev_err(dev, "Failed to get clock\n");

>  		ret = PTR_ERR(priv->clk);

> -		goto err_put_rproc;

> +		goto err_put_mbox;

>  	}

>  

>  	/*

> @@ -502,9 +605,11 @@ static int imx_rproc_probe(struct platform_device *pdev)

>  	ret = clk_prepare_enable(priv->clk);

>  	if (ret) {

>  		dev_err(&rproc->dev, "Failed to enable clock\n");

> -		goto err_put_rproc;

> +		goto err_put_mbox;

>  	}

>  

> +	INIT_WORK(&(priv->rproc_work), imx_rproc_vq_work);

> +

>  	ret = rproc_add(rproc);

>  	if (ret) {

>  		dev_err(dev, "rproc_add failed\n");

> @@ -515,6 +620,10 @@ static int imx_rproc_probe(struct platform_device *pdev)

>  

>  err_put_clk:

>  	clk_disable_unprepare(priv->clk);

> +err_put_mbox:

> +	imx_rproc_free_mbox(rproc);

> +err_put_wkq:

> +	destroy_workqueue(priv->workqueue);

>  err_put_rproc:

>  	rproc_free(rproc);

>  

> @@ -528,6 +637,7 @@ static int imx_rproc_remove(struct platform_device *pdev)

>  

>  	clk_disable_unprepare(priv->clk);

>  	rproc_del(rproc);

> +	imx_rproc_free_mbox(rproc);

>  	rproc_free(rproc);

>  

>  	return 0;

> -- 

> 2.28.0

>
Peng Fan Jan. 21, 2021, 1:21 a.m. UTC | #2
> Subject: Re: [PATCH V8 10/10] remoteproc: imx_proc: enable virtio/mailbox

> 

> On Tue, Jan 19, 2021 at 03:35:11PM +0800, peng.fan@nxp.com wrote:

> > From: Peng Fan <peng.fan@nxp.com>

> >

> > Use virtio/mailbox to build connection between Remote Proccessors and

> > Linux. Add work queue to handle incoming messages.

> >

> > Reviewed-by: Richard Zhu <hongxing.zhu@nxp.com>

> > Signed-off-by: Peng Fan <peng.fan@nxp.com>

> > ---

> >  drivers/remoteproc/imx_rproc.c | 116

> > ++++++++++++++++++++++++++++++++-

> >  1 file changed, 113 insertions(+), 3 deletions(-)

> 

> Reviewed-by: Mathieu Poirier <mathieu.poirer@linaro.org>

> 

> I will pickup this set when Rob has reviewed the DT bindings.  In the mean

> time you might want to fix the title section in the the .yaml file, the Kconfig

> and the MODULE_DESCRIPTION and add a reference to i.MX8.


Thanks for your reviewing efforts.

I have updated patch 2, 8 in v9 to address the issue you mentioned.

Thanks,
Peng.

> 

> Thanks,

> Mathieu

> 

> >

> > diff --git a/drivers/remoteproc/imx_rproc.c

> > b/drivers/remoteproc/imx_rproc.c index 54ac143ba033..c16a91f8e410

> > 100644

> > --- a/drivers/remoteproc/imx_rproc.c

> > +++ b/drivers/remoteproc/imx_rproc.c

> > @@ -7,6 +7,7 @@

> >  #include <linux/err.h>

> >  #include <linux/interrupt.h>

> >  #include <linux/kernel.h>

> > +#include <linux/mailbox_client.h>

> >  #include <linux/mfd/syscon.h>

> >  #include <linux/module.h>

> >  #include <linux/of_address.h>

> > @@ -15,6 +16,9 @@

> >  #include <linux/platform_device.h>

> >  #include <linux/regmap.h>

> >  #include <linux/remoteproc.h>

> > +#include <linux/workqueue.h>

> > +

> > +#include "remoteproc_internal.h"

> >

> >  #define IMX7D_SRC_SCR			0x0C

> >  #define IMX7D_ENABLE_M4			BIT(3)

> > @@ -86,6 +90,11 @@ struct imx_rproc {

> >  	const struct imx_rproc_dcfg	*dcfg;

> >  	struct imx_rproc_mem		mem[IMX7D_RPROC_MEM_MAX];

> >  	struct clk			*clk;

> > +	struct mbox_client		cl;

> > +	struct mbox_chan		*tx_ch;

> > +	struct mbox_chan		*rx_ch;

> > +	struct work_struct		rproc_work;

> > +	struct workqueue_struct		*workqueue;

> >  };

> >

> >  static const struct imx_rproc_att imx_rproc_att_imx8mq[] = { @@

> > -366,9 +375,33 @@ static int imx_rproc_parse_fw(struct rproc *rproc,

> const struct firmware *fw)

> >  	return 0;

> >  }

> >

> > +static void imx_rproc_kick(struct rproc *rproc, int vqid) {

> > +	struct imx_rproc *priv = rproc->priv;

> > +	int err;

> > +	__u32 mmsg;

> > +

> > +	if (!priv->tx_ch) {

> > +		dev_err(priv->dev, "No initialized mbox tx channel\n");

> > +		return;

> > +	}

> > +

> > +	/*

> > +	 * Send the index of the triggered virtqueue as the mu payload.

> > +	 * Let remote processor know which virtqueue is used.

> > +	 */

> > +	mmsg = vqid << 16;

> > +

> > +	err = mbox_send_message(priv->tx_ch, (void *)&mmsg);

> > +	if (err < 0)

> > +		dev_err(priv->dev, "%s: failed (%d, err:%d)\n",

> > +			__func__, vqid, err);

> > +}

> > +

> >  static const struct rproc_ops imx_rproc_ops = {

> >  	.start		= imx_rproc_start,

> >  	.stop		= imx_rproc_stop,

> > +	.kick		= imx_rproc_kick,

> >  	.da_to_va       = imx_rproc_da_to_va,

> >  	.load		= rproc_elf_load_segments,

> >  	.parse_fw	= imx_rproc_parse_fw,

> > @@ -444,6 +477,66 @@ static int imx_rproc_addr_init(struct imx_rproc

> *priv,

> >  	return 0;

> >  }

> >

> > +static void imx_rproc_vq_work(struct work_struct *work) {

> > +	struct imx_rproc *priv = container_of(work, struct imx_rproc,

> > +					      rproc_work);

> > +

> > +	rproc_vq_interrupt(priv->rproc, 0);

> > +	rproc_vq_interrupt(priv->rproc, 1);

> > +}

> > +

> > +static void imx_rproc_rx_callback(struct mbox_client *cl, void *msg)

> > +{

> > +	struct rproc *rproc = dev_get_drvdata(cl->dev);

> > +	struct imx_rproc *priv = rproc->priv;

> > +

> > +	queue_work(priv->workqueue, &priv->rproc_work); }

> > +

> > +static int imx_rproc_xtr_mbox_init(struct rproc *rproc) {

> > +	struct imx_rproc *priv = rproc->priv;

> > +	struct device *dev = priv->dev;

> > +	struct mbox_client *cl;

> > +	int ret;

> > +

> > +	if (!of_get_property(dev->of_node, "mbox-names", NULL))

> > +		return 0;

> > +

> > +	cl = &priv->cl;

> > +	cl->dev = dev;

> > +	cl->tx_block = true;

> > +	cl->tx_tout = 100;

> > +	cl->knows_txdone = false;

> > +	cl->rx_callback = imx_rproc_rx_callback;

> > +

> > +	priv->tx_ch = mbox_request_channel_byname(cl, "tx");

> > +	if (IS_ERR(priv->tx_ch)) {

> > +		ret = PTR_ERR(priv->tx_ch);

> > +		return dev_err_probe(cl->dev, ret,

> > +				     "failed to request tx mailbox channel: %d\n", ret);

> > +	}

> > +

> > +	priv->rx_ch = mbox_request_channel_byname(cl, "rx");

> > +	if (IS_ERR(priv->rx_ch)) {

> > +		mbox_free_channel(priv->tx_ch);

> > +		ret = PTR_ERR(priv->rx_ch);

> > +		return dev_err_probe(cl->dev, ret,

> > +				     "failed to request rx mailbox channel: %d\n", ret);

> > +	}

> > +

> > +	return 0;

> > +}

> > +

> > +static void imx_rproc_free_mbox(struct rproc *rproc) {

> > +	struct imx_rproc *priv = rproc->priv;

> > +

> > +	mbox_free_channel(priv->tx_ch);

> > +	mbox_free_channel(priv->rx_ch);

> > +}

> > +

> >  static int imx_rproc_probe(struct platform_device *pdev)  {

> >  	struct device *dev = &pdev->dev;

> > @@ -481,18 +574,28 @@ static int imx_rproc_probe(struct

> platform_device *pdev)

> >  	priv->dev = dev;

> >

> >  	dev_set_drvdata(dev, rproc);

> > +	priv->workqueue = create_workqueue(dev_name(dev));

> > +	if (!priv->workqueue) {

> > +		dev_err(dev, "cannot create workqueue\n");

> > +		ret = -ENOMEM;

> > +		goto err_put_rproc;

> > +	}

> > +

> > +	ret = imx_rproc_xtr_mbox_init(rproc);

> > +	if (ret)

> > +		goto err_put_wkq;

> >

> >  	ret = imx_rproc_addr_init(priv, pdev);

> >  	if (ret) {

> >  		dev_err(dev, "failed on imx_rproc_addr_init\n");

> > -		goto err_put_rproc;

> > +		goto err_put_mbox;

> >  	}

> >

> >  	priv->clk = devm_clk_get(dev, NULL);

> >  	if (IS_ERR(priv->clk)) {

> >  		dev_err(dev, "Failed to get clock\n");

> >  		ret = PTR_ERR(priv->clk);

> > -		goto err_put_rproc;

> > +		goto err_put_mbox;

> >  	}

> >

> >  	/*

> > @@ -502,9 +605,11 @@ static int imx_rproc_probe(struct platform_device

> *pdev)

> >  	ret = clk_prepare_enable(priv->clk);

> >  	if (ret) {

> >  		dev_err(&rproc->dev, "Failed to enable clock\n");

> > -		goto err_put_rproc;

> > +		goto err_put_mbox;

> >  	}

> >

> > +	INIT_WORK(&(priv->rproc_work), imx_rproc_vq_work);

> > +

> >  	ret = rproc_add(rproc);

> >  	if (ret) {

> >  		dev_err(dev, "rproc_add failed\n"); @@ -515,6 +620,10 @@ static

> int

> > imx_rproc_probe(struct platform_device *pdev)

> >

> >  err_put_clk:

> >  	clk_disable_unprepare(priv->clk);

> > +err_put_mbox:

> > +	imx_rproc_free_mbox(rproc);

> > +err_put_wkq:

> > +	destroy_workqueue(priv->workqueue);

> >  err_put_rproc:

> >  	rproc_free(rproc);

> >

> > @@ -528,6 +637,7 @@ static int imx_rproc_remove(struct platform_device

> > *pdev)

> >

> >  	clk_disable_unprepare(priv->clk);

> >  	rproc_del(rproc);

> > +	imx_rproc_free_mbox(rproc);

> >  	rproc_free(rproc);

> >

> >  	return 0;

> > --

> > 2.28.0

> >
diff mbox series

Patch

diff --git a/drivers/remoteproc/imx_rproc.c b/drivers/remoteproc/imx_rproc.c
index 54ac143ba033..c16a91f8e410 100644
--- a/drivers/remoteproc/imx_rproc.c
+++ b/drivers/remoteproc/imx_rproc.c
@@ -7,6 +7,7 @@ 
 #include <linux/err.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/mailbox_client.h>
 #include <linux/mfd/syscon.h>
 #include <linux/module.h>
 #include <linux/of_address.h>
@@ -15,6 +16,9 @@ 
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/remoteproc.h>
+#include <linux/workqueue.h>
+
+#include "remoteproc_internal.h"
 
 #define IMX7D_SRC_SCR			0x0C
 #define IMX7D_ENABLE_M4			BIT(3)
@@ -86,6 +90,11 @@  struct imx_rproc {
 	const struct imx_rproc_dcfg	*dcfg;
 	struct imx_rproc_mem		mem[IMX7D_RPROC_MEM_MAX];
 	struct clk			*clk;
+	struct mbox_client		cl;
+	struct mbox_chan		*tx_ch;
+	struct mbox_chan		*rx_ch;
+	struct work_struct		rproc_work;
+	struct workqueue_struct		*workqueue;
 };
 
 static const struct imx_rproc_att imx_rproc_att_imx8mq[] = {
@@ -366,9 +375,33 @@  static int imx_rproc_parse_fw(struct rproc *rproc, const struct firmware *fw)
 	return 0;
 }
 
+static void imx_rproc_kick(struct rproc *rproc, int vqid)
+{
+	struct imx_rproc *priv = rproc->priv;
+	int err;
+	__u32 mmsg;
+
+	if (!priv->tx_ch) {
+		dev_err(priv->dev, "No initialized mbox tx channel\n");
+		return;
+	}
+
+	/*
+	 * Send the index of the triggered virtqueue as the mu payload.
+	 * Let remote processor know which virtqueue is used.
+	 */
+	mmsg = vqid << 16;
+
+	err = mbox_send_message(priv->tx_ch, (void *)&mmsg);
+	if (err < 0)
+		dev_err(priv->dev, "%s: failed (%d, err:%d)\n",
+			__func__, vqid, err);
+}
+
 static const struct rproc_ops imx_rproc_ops = {
 	.start		= imx_rproc_start,
 	.stop		= imx_rproc_stop,
+	.kick		= imx_rproc_kick,
 	.da_to_va       = imx_rproc_da_to_va,
 	.load		= rproc_elf_load_segments,
 	.parse_fw	= imx_rproc_parse_fw,
@@ -444,6 +477,66 @@  static int imx_rproc_addr_init(struct imx_rproc *priv,
 	return 0;
 }
 
+static void imx_rproc_vq_work(struct work_struct *work)
+{
+	struct imx_rproc *priv = container_of(work, struct imx_rproc,
+					      rproc_work);
+
+	rproc_vq_interrupt(priv->rproc, 0);
+	rproc_vq_interrupt(priv->rproc, 1);
+}
+
+static void imx_rproc_rx_callback(struct mbox_client *cl, void *msg)
+{
+	struct rproc *rproc = dev_get_drvdata(cl->dev);
+	struct imx_rproc *priv = rproc->priv;
+
+	queue_work(priv->workqueue, &priv->rproc_work);
+}
+
+static int imx_rproc_xtr_mbox_init(struct rproc *rproc)
+{
+	struct imx_rproc *priv = rproc->priv;
+	struct device *dev = priv->dev;
+	struct mbox_client *cl;
+	int ret;
+
+	if (!of_get_property(dev->of_node, "mbox-names", NULL))
+		return 0;
+
+	cl = &priv->cl;
+	cl->dev = dev;
+	cl->tx_block = true;
+	cl->tx_tout = 100;
+	cl->knows_txdone = false;
+	cl->rx_callback = imx_rproc_rx_callback;
+
+	priv->tx_ch = mbox_request_channel_byname(cl, "tx");
+	if (IS_ERR(priv->tx_ch)) {
+		ret = PTR_ERR(priv->tx_ch);
+		return dev_err_probe(cl->dev, ret,
+				     "failed to request tx mailbox channel: %d\n", ret);
+	}
+
+	priv->rx_ch = mbox_request_channel_byname(cl, "rx");
+	if (IS_ERR(priv->rx_ch)) {
+		mbox_free_channel(priv->tx_ch);
+		ret = PTR_ERR(priv->rx_ch);
+		return dev_err_probe(cl->dev, ret,
+				     "failed to request rx mailbox channel: %d\n", ret);
+	}
+
+	return 0;
+}
+
+static void imx_rproc_free_mbox(struct rproc *rproc)
+{
+	struct imx_rproc *priv = rproc->priv;
+
+	mbox_free_channel(priv->tx_ch);
+	mbox_free_channel(priv->rx_ch);
+}
+
 static int imx_rproc_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -481,18 +574,28 @@  static int imx_rproc_probe(struct platform_device *pdev)
 	priv->dev = dev;
 
 	dev_set_drvdata(dev, rproc);
+	priv->workqueue = create_workqueue(dev_name(dev));
+	if (!priv->workqueue) {
+		dev_err(dev, "cannot create workqueue\n");
+		ret = -ENOMEM;
+		goto err_put_rproc;
+	}
+
+	ret = imx_rproc_xtr_mbox_init(rproc);
+	if (ret)
+		goto err_put_wkq;
 
 	ret = imx_rproc_addr_init(priv, pdev);
 	if (ret) {
 		dev_err(dev, "failed on imx_rproc_addr_init\n");
-		goto err_put_rproc;
+		goto err_put_mbox;
 	}
 
 	priv->clk = devm_clk_get(dev, NULL);
 	if (IS_ERR(priv->clk)) {
 		dev_err(dev, "Failed to get clock\n");
 		ret = PTR_ERR(priv->clk);
-		goto err_put_rproc;
+		goto err_put_mbox;
 	}
 
 	/*
@@ -502,9 +605,11 @@  static int imx_rproc_probe(struct platform_device *pdev)
 	ret = clk_prepare_enable(priv->clk);
 	if (ret) {
 		dev_err(&rproc->dev, "Failed to enable clock\n");
-		goto err_put_rproc;
+		goto err_put_mbox;
 	}
 
+	INIT_WORK(&(priv->rproc_work), imx_rproc_vq_work);
+
 	ret = rproc_add(rproc);
 	if (ret) {
 		dev_err(dev, "rproc_add failed\n");
@@ -515,6 +620,10 @@  static int imx_rproc_probe(struct platform_device *pdev)
 
 err_put_clk:
 	clk_disable_unprepare(priv->clk);
+err_put_mbox:
+	imx_rproc_free_mbox(rproc);
+err_put_wkq:
+	destroy_workqueue(priv->workqueue);
 err_put_rproc:
 	rproc_free(rproc);
 
@@ -528,6 +637,7 @@  static int imx_rproc_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(priv->clk);
 	rproc_del(rproc);
+	imx_rproc_free_mbox(rproc);
 	rproc_free(rproc);
 
 	return 0;