[PATCH 4/6] mailbox: arm_mhu: re-factor data structure to add subchannel support

[Sudeep Holla]

In order to support subchannels, we need a bit of reword around data
structures that are per-channel. Since the number of subchannels are
not fixed though restricted to maximum of 20, the channel assignment
and initialization is move to xlate function.

This patch also adds the platform data for the existing support of one
channel per physical channel.

Cc: Alexey Klimov <alexey.klimov@xxxxxxx>
Cc: Jassi Brar <jaswinder.singh@xxxxxxxxxx>
Signed-off-by: Sudeep Holla <sudeep.holla@xxxxxxx>
---
 drivers/mailbox/arm_mhu.c | 208 +++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 186 insertions(+), 22 deletions(-)

diff --git a/drivers/mailbox/arm_mhu.c b/drivers/mailbox/arm_mhu.c
index ebe17c097f43..4e0f690b97fd 100644
--- a/drivers/mailbox/arm_mhu.c
+++ b/drivers/mailbox/arm_mhu.c
@@ -20,6 +20,8 @@
 #include <linux/io.h>
 #include <linux/mailbox_controller.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 #define INTR_STAT_OFS	0x0
 #define INTR_SET_OFS	0x8
@@ -30,7 +32,8 @@
 #define MHU_SEC_OFFSET	0x200
 #define TX_REG_OFFSET	0x100
 
-#define MHU_CHANS	3
+#define MHU_NUM_PCHANS	3	/* Secure, Non-Secure High and Low Priority */
+#define MHU_CHAN_MAX	20	/* Max channels to save on unused RAM */
 
 struct mhu_link {
 	unsigned irq;
@@ -40,53 +43,175 @@ struct mhu_link {
 
 struct arm_mhu {
 	void __iomem *base;
-	struct mhu_link mlink[MHU_CHANS];
-	struct mbox_chan chan[MHU_CHANS];
+	struct mhu_link mlink[MHU_NUM_PCHANS];
 	struct mbox_controller mbox;
+	struct device *dev;
 };
 
+/**
+ * ARM MHU Mailbox platform specific configuration
+ *
+ * @num_pchans: Maximum number of physical channels
+ * @num_subchans: Maximum number of sub-channels per physical channel
+ */
+struct mhu_mbox_pdata {
+	unsigned int num_pchans;
+	unsigned int num_subchans;
+	bool support_subchans;
+};
+
+/**
+ * ARM MHU Mailbox allocated channel information
+ *
+ * @mhu: Pointer to parent mailbox device
+ * @pchan: Physical channel within which this sub-channel resides in
+ * @channel: Sub-channel number pertaining to this container
+ */
+struct mhu_channel {
+	struct arm_mhu *mhu;
+	unsigned int pchan;
+	unsigned int subchan;
+};
+
+static inline struct mbox_chan *
+mhu_mbox_to_channel(struct mbox_controller *mbox,
+		    unsigned int pchan, unsigned int subchan)
+{
+	int i;
+	struct mhu_channel *chan_info;
+
+	for (i = 0; i < mbox->num_chans; i++) {
+		chan_info = mbox->chans[i].con_priv;
+		if (chan_info && chan_info->pchan == pchan &&
+		    chan_info->subchan == subchan)
+			return &mbox->chans[i];
+	}
+
+	dev_err(mbox->dev,
+		"Channel not registered: physical channel: %d subchan: %d\n",
+		pchan, subchan);
+
+	return NULL;
+}
+
+static unsigned int mhu_mbox_irq_to_pchan_num(struct arm_mhu *mhu, int irq)
+{
+	unsigned int pchan;
+	struct mhu_mbox_pdata *pdata = dev_get_platdata(mhu->dev);
+
+	for (pchan = 0; pchan < pdata->num_pchans; pchan++)
+		if (mhu->mlink[pchan].irq == irq)
+			break;
+	return pchan;
+}
+
+static struct mbox_chan *mhu_mbox_xlate(struct mbox_controller *mbox,
+					const struct of_phandle_args *spec)
+{
+	struct arm_mhu *mhu = dev_get_drvdata(mbox->dev);
+	struct mhu_mbox_pdata *pdata = dev_get_platdata(mhu->dev);
+	struct mhu_channel *chan_info;
+	struct mbox_chan *chan = NULL;
+	unsigned int pchan = spec->args[0];
+	unsigned int subchan = pdata->support_subchans ? spec->args[1] : 0;
+	int i;
+
+	/* Bounds checking */
+	if (pchan >= pdata->num_pchans || subchan >= pdata->num_subchans) {
+		dev_err(mbox->dev,
+			"Invalid channel requested pchan: %d subchan: %d\n",
+			pchan, subchan);
+		return ERR_PTR(-EINVAL);
+	}
+
+	for (i = 0; i < mbox->num_chans; i++) {
+		chan_info = mbox->chans[i].con_priv;
+
+		/* Is requested channel free? */
+		if (chan_info &&
+		    mbox->dev == chan_info->mhu->dev &&
+		    pchan == chan_info->pchan &&
+		    subchan == chan_info->subchan) {
+			dev_err(mbox->dev, "Channel in use\n");
+			return ERR_PTR(-EBUSY);
+		}
+
+		/*
+		 * Find the first free slot, then continue checking
+		 * to see if requested channel is in use
+		 */
+		if (!chan && !chan_info)
+			chan = &mbox->chans[i];
+	}
+
+	if (!chan) {
+		dev_err(mbox->dev, "No free channels left\n");
+		return ERR_PTR(-EBUSY);
+	}
+
+	chan_info = devm_kzalloc(mbox->dev, sizeof(*chan_info), GFP_KERNEL);
+	if (!chan_info)
+		return ERR_PTR(-ENOMEM);
+
+	chan_info->mhu = mhu;
+	chan_info->pchan = pchan;
+	chan_info->subchan = subchan;
+
+	chan->con_priv = chan_info;
+
+	dev_dbg(mbox->dev, "mbox: created channel - physical: %d sub: %d\n",
+		pchan, subchan);
+
+	return chan;
+}
+
 static irqreturn_t mhu_rx_interrupt(int irq, void *p)
 {
-	struct mbox_chan *chan = p;
-	struct mhu_link *mlink = chan->con_priv;
+	struct arm_mhu *mhu = p;
+	unsigned int pchan = mhu_mbox_irq_to_pchan_num(mhu, irq);
+	struct mbox_chan *chan = mhu_mbox_to_channel(&mhu->mbox, pchan, 0);
+	void __iomem *base = mhu->mlink[pchan].rx_reg;
 	u32 val;
 
-	val = readl_relaxed(mlink->rx_reg + INTR_STAT_OFS);
+	val = readl_relaxed(base + INTR_STAT_OFS);
 	if (!val)
 		return IRQ_NONE;
 
 	mbox_chan_received_data(chan, (void *)&val);
 
-	writel_relaxed(val, mlink->rx_reg + INTR_CLR_OFS);
+	writel_relaxed(val, base + INTR_CLR_OFS);
 
 	return IRQ_HANDLED;
 }
 
 static bool mhu_last_tx_done(struct mbox_chan *chan)
 {
-	struct mhu_link *mlink = chan->con_priv;
-	u32 val = readl_relaxed(mlink->tx_reg + INTR_STAT_OFS);
+	struct mhu_channel *chan_info = chan->con_priv;
+	void __iomem *base = chan_info->mhu->mlink[chan_info->pchan].tx_reg;
+	u32 val = readl_relaxed(base + INTR_STAT_OFS);
 
 	return (val == 0);
 }
 
 static int mhu_send_data(struct mbox_chan *chan, void *data)
 {
-	struct mhu_link *mlink = chan->con_priv;
+	struct mhu_channel *chan_info = chan->con_priv;
+	void __iomem *base = chan_info->mhu->mlink[chan_info->pchan].tx_reg;
 	u32 *arg = data;
 
-	writel_relaxed(*arg, mlink->tx_reg + INTR_SET_OFS);
+	writel_relaxed(*arg, base + INTR_SET_OFS);
 
 	return 0;
 }
 
 static int mhu_startup(struct mbox_chan *chan)
 {
-	struct mhu_link *mlink = chan->con_priv;
+	struct mhu_channel *chan_info = chan->con_priv;
+	void __iomem *base = chan_info->mhu->mlink[chan_info->pchan].tx_reg;
 	u32 val;
 
-	val = readl_relaxed(mlink->tx_reg + INTR_STAT_OFS);
-	writel_relaxed(val, mlink->tx_reg + INTR_CLR_OFS);
+	val = readl_relaxed(base + INTR_STAT_OFS);
+	writel_relaxed(val, base + INTR_CLR_OFS);
 
 	return 0;
 }
@@ -102,14 +227,46 @@ static const struct mbox_chan_ops mhu_ops = {
 	.last_tx_done = mhu_last_tx_done,
 };
 
+static const struct mhu_mbox_pdata arm_mhu_pdata = {
+	.num_pchans = 3,
+	.num_subchans = 1,
+	.support_subchans = false,
+};
+
+static const struct of_device_id mhu_mbox_match[] = {
+	{ .compatible = "arm,mhu", .data = (void *)&arm_mhu_pdata },
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, mhu_mbox_match);
+
 static int mhu_probe(struct amba_device *adev, const struct amba_id *id)
 {
-	int i, err;
+	int i, err, max_chans;
 	struct arm_mhu *mhu;
+	struct mbox_chan *chans;
+	struct mhu_mbox_pdata *pdata;
+	const struct of_device_id *match;
 	struct device *dev = &adev->dev;
-	int mhu_reg[MHU_CHANS] = {MHU_LP_OFFSET, MHU_HP_OFFSET, MHU_SEC_OFFSET};
+	int mhu_reg[MHU_NUM_PCHANS] = {
+		MHU_LP_OFFSET, MHU_HP_OFFSET, MHU_SEC_OFFSET,
+	};
+
+	match = of_match_device(mhu_mbox_match, dev);
+	if (!match) {
+		dev_err(dev, "No configuration found\n");
+		return -ENODEV;
+	}
+	pdata = (struct mhu_mbox_pdata *)match->data;
+
+	if (pdata->num_pchans > MHU_NUM_PCHANS) {
+		dev_err(dev, "Number of physical channel can't exceed %d\n",
+			MHU_NUM_PCHANS);
+		return -EINVAL;
+	}
+
+	max_chans = pdata->support_subchans ? MHU_CHAN_MAX : MHU_NUM_PCHANS;
 
-	/* Allocate memory for device */
 	mhu = devm_kzalloc(dev, sizeof(*mhu), GFP_KERNEL);
 	if (!mhu)
 		return -ENOMEM;
@@ -120,14 +277,22 @@ static int mhu_probe(struct amba_device *adev, const struct amba_id *id)
 		return PTR_ERR(mhu->base);
 	}
 
+	chans = devm_kcalloc(dev, max_chans, sizeof(*chans), GFP_KERNEL);
+	if (!chans)
+		return -ENOMEM;
+
+	dev->platform_data = pdata;
+
+	mhu->dev = dev;
 	mhu->mbox.dev = dev;
-	mhu->mbox.chans = &mhu->chan[0];
-	mhu->mbox.num_chans = MHU_CHANS;
+	mhu->mbox.chans = chans;
+	mhu->mbox.num_chans = max_chans;
 	mhu->mbox.ops = &mhu_ops;
 	mhu->mbox.txdone_irq = false;
 	mhu->mbox.txdone_poll = true;
 	mhu->mbox.txpoll_period = 1;
 
+	mhu->mbox.of_xlate = mhu_mbox_xlate;
 	amba_set_drvdata(adev, mhu);
 
 	err = mbox_controller_register(&mhu->mbox);
@@ -136,7 +301,7 @@ static int mhu_probe(struct amba_device *adev, const struct amba_id *id)
 		return err;
 	}
 
-	for (i = 0; i < MHU_CHANS; i++) {
+	for (i = 0; i < pdata->num_pchans; i++) {
 		int irq = mhu->mlink[i].irq = adev->irq[i];
 
 		if (irq <= 0) {
@@ -144,13 +309,12 @@ static int mhu_probe(struct amba_device *adev, const struct amba_id *id)
 			continue;
 		}
 
-		mhu->chan[i].con_priv = &mhu->mlink[i];
 		mhu->mlink[i].rx_reg = mhu->base + mhu_reg[i];
 		mhu->mlink[i].tx_reg = mhu->mlink[i].rx_reg + TX_REG_OFFSET;
 
 		err = devm_request_threaded_irq(dev, irq, NULL,
 						mhu_rx_interrupt, IRQF_ONESHOT,
-						"mhu_link", &mhu->chan[i]);
+						"mhu_link", mhu);
 		if (err) {
 			dev_err(dev, "Can't claim IRQ %d\n", irq);
 			mbox_controller_unregister(&mhu->mbox);
-- 
2.7.4