diff -drupN a/drivers/rpmsg/sunxi_msgbox.c b/drivers/rpmsg/sunxi_msgbox.c --- a/drivers/rpmsg/sunxi_msgbox.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/rpmsg/sunxi_msgbox.c 2022-06-12 05:28:14.000000000 +0300 @@ -0,0 +1,437 @@ +/* + * Copyright (c) 2015, allwinnertech Communications R329. + * Copyright (c) 2012-2013, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "rpmsg_internal.h" + +#define MSGBOX_CTRL_REG0_OFFSET 0 +#define MSGBOX_CTRL_REG1_OFFSET 0x0004 +#define MSGBOX_VER_REG_OFFSET 0x0010 +#define MSGBOX_IRQ_EN_REG_OFFSET 0x0040 +#define MSGBOX_IRQ_STATUS_REG_OFFSET 0x0050 +#define MSGBOX_FIFO_STATUS_REG_OFFSET 0x0100 +#define MSGBOX_MSG_STATUS_REG_OFFSET 0x0140 +#define MSGBOX_MSG_REG_OFFSET 0x0180 +#define MSGBOX_DEBUG_REG_OFFSET 0x01c0 + +/* + * there is 8 channel, but we use it dual channel + * so there's only 4 channel for 4 * 2 + * 0, 2, 4, 6 is trans + * 1, 3, 5, 7 is recv + */ +#define SUNXI_MSGBOX_MAX_EDP 4 + +#define TO_SUNXI_MSGBOX_ENDPOINT(x) \ + container_of(x, struct sunxi_msgbox_endpoint, edp) + +#define TO_SUNXI_MSGBOX_DEVICE(x) \ + container_of(x, struct sunxi_msgbox_device, rpmsg_dev) + +/* + * sunxi_msgbox_endpoint - warp struct of rpmsg_endpoint + * mtx_send: send mutex used to only on send + * send_done: infor send finish + * len: send length + * idx: current should send + * d: send buf + */ +struct sunxi_msgbox_endpoint { + struct rpmsg_endpoint edp; + struct mutex mtx_send; + struct completion send_done; + int len; + int idx; + uint8_t *d; +}; + +/* + * sunxi_msgbox_device - warp rpmsg_device of sunxi + * edp_bitmap: used edp + * base: base address of this messagebox + * irq: irq descriptor of this messagebox + * sunxi_edp: edp of this messagebox + * + * this rpmsg_dev has 8 msg_queue and group into 4 group + * every group has 2 msg_queue. + * so we have 4 rpmsg_endpoint every messagebox. + * every rpmsg_endpoint use two msg_queue. + * the little is for send the other is for rev. + */ +struct sunxi_msgbox_device { + struct rpmsg_device rpmsg_dev; + DECLARE_BITMAP(edp_bitmap, SUNXI_MSGBOX_MAX_EDP); + void __iomem *base; + int irq; + + /* managed endpoint */ + struct sunxi_msgbox_endpoint sunxi_edp[0]; +}; + +/* + * sunxi_msgbox_start_send - this function start interrupt of send + */ +static int sunxi_msgbox_start_send(struct sunxi_msgbox_endpoint *medp) +{ + struct sunxi_msgbox_device *mdev = + TO_SUNXI_MSGBOX_DEVICE(medp->edp.rpdev); + void __iomem *base = mdev->base; + void *regs = base + MSGBOX_IRQ_EN_REG_OFFSET; + uint32_t enable; + + /* enable interrupt */ + enable = readl(regs); + enable |= BIT(medp->edp.addr * 2 * 2 + 1); + writel(enable, regs); + + return 0; +} + +/* + * sunxi_msgbox_send - function of rpmsg_endpoint's callback + */ +static int sunxi_msgbox_send(struct rpmsg_endpoint *ept, void *data, int len) +{ + struct sunxi_msgbox_endpoint *medp = TO_SUNXI_MSGBOX_ENDPOINT(ept); + unsigned long ret; + + dev_dbg(&ept->rpdev->dev, "send msg start\n"); + + mutex_lock(&medp->mtx_send); + + medp->d = data; + medp->len = len; + medp->idx = 0; + + reinit_completion(&medp->send_done); + sunxi_msgbox_start_send(medp); + ret = wait_for_completion_timeout(&medp->send_done, + msecs_to_jiffies(15000)); + + mutex_unlock(&medp->mtx_send); + + if (!ret) + return -ERESTARTSYS; + + return 0; +} + +/* + * sunxi_msgbox_destory_edp - destory rpmsg_endpoint + */ +static void sunxi_msgbox_destory_edp(struct rpmsg_endpoint *ept) +{ + struct sunxi_msgbox_device *mdev = TO_SUNXI_MSGBOX_DEVICE(ept->rpdev); + + clear_bit(ept->addr, mdev->edp_bitmap); +} + +static struct rpmsg_endpoint_ops sunxi_msgbox_ep_ops = { + .destroy_ept = sunxi_msgbox_destory_edp, + .send = sunxi_msgbox_send, +}; + +/* + * sunxi_msgbox_direction_set - set rpmsg_endpoint direction + * + * channel is double, so on channel use two msgbox_mq; + * channel N used msgbox_mq is N * 2 and N * 2 + 1; + * N * 2 is trans of cpu, and N * 2 + 1 is rec of cpu. + */ +static int sunxi_msgbox_direction_set(struct sunxi_msgbox_endpoint *medp) +{ + struct sunxi_msgbox_device *mdev = + TO_SUNXI_MSGBOX_DEVICE(medp->edp.rpdev); + int channel = medp->edp.addr; + uint32_t *regs = mdev->base + (channel / 2 * 4); + uint32_t data; + uint32_t shift = (channel % 2) * 16; + + data = readl(regs); + data &= ~(0xffff << shift); + data |= (0x1001 << shift); + writel(data, regs); + + return 0; +} + +/* + * sunxi_create_ept - used to create rpmsg_endpoint + * rpdev: the rpmsg_device used to create rpmsg_endpoint + * cb: callback of this create rpmsg_endpoint + * priv: the cb add priv data + * chinfo: used to create rpmsg_endpoint information + * + * we use chinfo's src address only + */ +static struct rpmsg_endpoint *sunxi_create_ept(struct rpmsg_device *rpdev, + rpmsg_rx_cb_t cb, void *priv, + struct rpmsg_channel_info chinfo) +{ + struct sunxi_msgbox_endpoint *sunxi_edp; + struct sunxi_msgbox_device *mdev = TO_SUNXI_MSGBOX_DEVICE(rpdev); + void __iomem *base = mdev->base; + void *regs = base + MSGBOX_IRQ_EN_REG_OFFSET; + uint32_t enable; + + /* check is over max endpoint */ + if (chinfo.src >= SUNXI_MSGBOX_MAX_EDP) { + dev_err(&mdev->rpmsg_dev.dev, "error channel src addr\n"); + return NULL; + } + + /* check it is ept is used */ + if (test_bit(chinfo.src, mdev->edp_bitmap)) { + dev_err(&mdev->rpmsg_dev.dev, "channel is used\n"); + return NULL; + } + + sunxi_edp = &mdev->sunxi_edp[chinfo.src]; + if (!sunxi_edp) + return NULL; + + sunxi_edp->edp.addr = chinfo.src; + sunxi_edp->edp.rpdev = &mdev->rpmsg_dev; + sunxi_edp->edp.cb = cb; + sunxi_edp->edp.priv = priv; + sunxi_edp->edp.ops = &sunxi_msgbox_ep_ops; + + kref_init(&sunxi_edp->edp.refcount); + set_bit(chinfo.src, mdev->edp_bitmap); + mutex_init(&sunxi_edp->mtx_send); + init_completion(&sunxi_edp->send_done); + + sunxi_msgbox_direction_set(sunxi_edp); + + + /* enable interrupt */ + enable = readl(regs); + enable |= BIT((sunxi_edp->edp.addr * 2 + 1) * 2); + writel(enable, regs); + + return &sunxi_edp->edp; +} + +static struct rpmsg_device_ops sunxi_msgbox_dev_ops = { + .create_ept = sunxi_create_ept, +}; + +/* + * sunxi_msgbox_rec_int - receive interrupt handler + */ +static int sunxi_msgbox_rec_int(struct sunxi_msgbox_endpoint *sunxi_edp) +{ + struct sunxi_msgbox_device *mdev = + TO_SUNXI_MSGBOX_DEVICE(sunxi_edp->edp.rpdev); + int offset = (sunxi_edp->edp.addr * 2 + 1) * 4; + uint32_t data; + uint8_t d[4]; + void *reg_fifo = mdev->base + MSGBOX_MSG_REG_OFFSET + offset; + void *reg_num = mdev->base + MSGBOX_MSG_STATUS_REG_OFFSET + offset; + + if (sunxi_edp->edp.cb) { + while (readl(reg_num)) { + data = readl(reg_fifo); + d[0] = data & 0xff; + d[1] = (data >> 8) & 0xff; + d[2] = (data >> 16) & 0xff; + d[3] = (data >> 24) & 0xff; + + sunxi_edp->edp.cb(&mdev->rpmsg_dev, d, 4, + sunxi_edp->edp.priv, + sunxi_edp->edp.addr); + } + } + + return 0; +} + +/* + * sunxi_msgbox_send_int - send interrupt handler + */ +static int sunxi_msgbox_send_int(struct sunxi_msgbox_endpoint *sunxi_edp) +{ + struct sunxi_msgbox_device *mdev = + TO_SUNXI_MSGBOX_DEVICE(sunxi_edp->edp.rpdev); + int offset = (sunxi_edp->edp.addr * 2) * 4; + uint32_t data; + uint8_t d; + int i; + void *reg_en = mdev->base + MSGBOX_IRQ_EN_REG_OFFSET; + void *reg_full = mdev->base + MSGBOX_FIFO_STATUS_REG_OFFSET + offset; + void *reg_fifo = mdev->base + MSGBOX_MSG_REG_OFFSET + offset; + + while (!readl(reg_full)) { + if (sunxi_edp->idx >= sunxi_edp->len) { + /* disable interrupt */ + data = readl(reg_en); + data &= ~BIT(sunxi_edp->edp.addr * 2 * 2 + 1); + writel(data, reg_en); + + complete(&sunxi_edp->send_done); + + break; + } + + data = 0; + for (i = 0; i < sizeof(uint32_t); i++) { + if (sunxi_edp->idx < sunxi_edp->len) { + d = sunxi_edp->d[sunxi_edp->idx++]; + data |= d << (i * 8); + } + } + writel(data, reg_fifo); + } + + + return 0; +} + +/* + * sunxi_msgbox_irq - rpmsg_device irq handler + */ +static irqreturn_t sunxi_msgbox_irq(int irq, void *dev_id) +{ + struct sunxi_msgbox_device *msgbox = dev_id; + void __iomem *base = msgbox->base; + int msgbox_q; + uint32_t enable, status; + unsigned long handle, bit; + + enable = readl(base + MSGBOX_IRQ_EN_REG_OFFSET); + status = readl(base + MSGBOX_IRQ_STATUS_REG_OFFSET); + handle = enable & status & 0xffff; + + dev_dbg(&msgbox->rpmsg_dev.dev, "interrupt with %d n", (int)handle); + for_each_set_bit(bit, &handle, BITS_PER_LONG) { + msgbox_q = bit / 2; + /* test is this endpoint is used */ + if (!test_bit(msgbox_q / 2, msgbox->edp_bitmap)) { + continue; + } + + if (bit % 2) { + /* transmit */ + sunxi_msgbox_send_int(&msgbox->sunxi_edp[msgbox_q / 2]); + } else { + /* reception */ + sunxi_msgbox_rec_int(&msgbox->sunxi_edp[msgbox_q / 2]); + } + } + + writel(handle, base + MSGBOX_IRQ_STATUS_REG_OFFSET); + + return IRQ_HANDLED; +} + +static int sunxi_msgbox_probe(struct platform_device *pdev) +{ + struct sunxi_msgbox_device *msgbox; + struct resource *res; + int ret; + const char *rpmsg_id; + + msgbox = devm_kzalloc(&pdev->dev, + sizeof(struct sunxi_msgbox_device) + + sizeof(struct sunxi_msgbox_endpoint) * + SUNXI_MSGBOX_MAX_EDP, + GFP_KERNEL); + + res = platform_get_resource(pdev, IORESOURCE_MEM, 0); + if (!res) { + dev_err(&pdev->dev, "no memory resource\n"); + return -ENXIO; + } + msgbox->base = devm_ioremap(&pdev->dev, res->start, resource_size(res)); + if (!msgbox->base) { + dev_err(&pdev->dev, "not io map\n"); + return -ENXIO; + } + + msgbox->rpmsg_dev.dev.parent = &pdev->dev; + msgbox->rpmsg_dev.dst = RPMSG_ADDR_ANY; + msgbox->rpmsg_dev.src = 0; + + ret = of_property_read_string(pdev->dev.of_node, "rpmsg_id", &rpmsg_id); + if (ret) { + dev_err(&pdev->dev, "can not get rpmsg_id\n"); + return ret; + } + strncpy(msgbox->rpmsg_dev.id.name, rpmsg_id, RPMSG_NAME_SIZE - 1); + + platform_set_drvdata(pdev, msgbox); + + msgbox->irq = platform_get_irq(pdev, 0); + if (msgbox->irq < 0) + return msgbox->irq; + + msgbox->rpmsg_dev.ops = &sunxi_msgbox_dev_ops; + ret = devm_request_irq(&pdev->dev, msgbox->irq, sunxi_msgbox_irq, 0, + "sunxi-msgbox", msgbox); + if (ret < 0) { + dev_err(&pdev->dev, "error get irq\n"); + return ret; + } + + ret = rpmsg_register_device(&msgbox->rpmsg_dev); + if (ret) + return ret; + + + return 0; +} + +static int sunxi_msgbox_remove(struct platform_device *pdev) +{ + return 0; +} + +static const struct of_device_id sunxi_msgbox_table[] = { + { .compatible = "sunxi,msgbox" }, + {} +}; + +static struct platform_driver sunxi_msgbox_driver = { + .probe = sunxi_msgbox_probe, + .remove = sunxi_msgbox_remove, + .driver = { + .name = "sunxi-msgbox", + .of_match_table = sunxi_msgbox_table, + }, +}; + +static int __init sunxi_msgbox_init(void) +{ + return platform_driver_register(&sunxi_msgbox_driver); +} +subsys_initcall(sunxi_msgbox_init); + +static void __exit sunxi_msgbox_exit(void) +{ + platform_driver_unregister(&sunxi_msgbox_driver); +} +module_exit(sunxi_msgbox_exit); + +MODULE_AUTHOR("fuyao "); +MODULE_DESCRIPTION("msgbox communication channel"); +MODULE_LICENSE("GPL v2");