From b5758dacead936d7e8fac8f292dfcdfd88a107b7 Mon Sep 17 00:00:00 2001 From: Chris Lew Date: Mon, 17 Oct 2022 13:17:01 -0700 Subject: [PATCH] net: qrtr: Add gunyah transport Add the QRTR gunyah transport that facilitates sending and receiving IPC Router messages between virtual machines that use the haven hypervisor. Change-Id: I805ee5b144c5a8fc92e4a045061770d06495f63c Signed-off-by: Chris Lew --- net/qrtr/Kconfig | 8 + net/qrtr/Makefile | 2 + net/qrtr/gunyah.c | 670 ++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 680 insertions(+) create mode 100644 net/qrtr/gunyah.c diff --git a/net/qrtr/Kconfig b/net/qrtr/Kconfig index b4020b84760f..81f1cd100cab 100644 --- a/net/qrtr/Kconfig +++ b/net/qrtr/Kconfig @@ -35,4 +35,12 @@ config QRTR_MHI Say Y here to support MHI based ipcrouter channels. MHI is the transport used for communicating to external modems. +config QRTR_GUNYAH + tristate "Gunyah IPC Router channels" + help + Say Y here to support a fifo based ipcrouter channel with gunyah + hypervisor signaling. The gunyah transport layer enables IPC + Router communication between two virtual machines. The transport + uses dynamically shared memory and gunyah doorbells. + endif # QRTR diff --git a/net/qrtr/Makefile b/net/qrtr/Makefile index 8e0605f88a73..ec959e64c89c 100644 --- a/net/qrtr/Makefile +++ b/net/qrtr/Makefile @@ -8,3 +8,5 @@ obj-$(CONFIG_QRTR_TUN) += qrtr-tun.o qrtr-tun-y := tun.o obj-$(CONFIG_QRTR_MHI) += qrtr-mhi.o qrtr-mhi-y := mhi.o +obj-$(CONFIG_QRTR_GUNYAH) += qrtr-gunyah.o +qrtr-gunyah-y := gunyah.o diff --git a/net/qrtr/gunyah.c b/net/qrtr/gunyah.c new file mode 100644 index 000000000000..4631fa917b3f --- /dev/null +++ b/net/qrtr/gunyah.c @@ -0,0 +1,670 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2020-2022 Qualcomm Innovation Center, Inc. All rights reserved. + */ + +#define pr_fmt(fmt) "%s: " fmt, __func__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "qrtr.h" + +#define GUNYAH_MAGIC_KEY 0x24495043 /* "$IPC" */ +#define FIFO_SIZE 0x4000 +#define FIFO_FULL_RESERVE 8 +#define FIFO_0_START 0x1000 +#define FIFO_1_START (FIFO_0_START + FIFO_SIZE) +#define GUNYAH_MAGIC_IDX 0x0 +#define TAIL_0_IDX 0x1 +#define HEAD_0_IDX 0x2 +#define TAIL_1_IDX 0x3 +#define HEAD_1_IDX 0x4 +#define NOTIFY_0_IDX 0x5 +#define NOTIFY_1_IDX 0x6 +#define QRTR_DBL_MASK 0x1 + +#define MAX_PKT_SZ SZ_64K + +struct gunyah_ring { + void *buf; + size_t len; + u32 offset; +}; + +struct gunyah_pipe { + __le32 *tail; + __le32 *head; + __le32 *read_notify; + + void *fifo; + size_t length; +}; + +/** + * qrtr_gunyah_dev - qrtr gunyah transport structure + * @ep: qrtr endpoint specific info. + * @dev: device from platform_device. + * @pkt: buf for reading from fifo. + * @res: resource of reserved mem region + * @memparcel: memparcel handle returned from sharing mem + * @base: Base of the shared fifo. + * @size: fifo size. + * @master: primary vm indicator. + * @peer_name: name of vm peer. + * @rm_nb: notifier block for vm status from rm + * @label: label for gunyah resources + * @tx_dbl: doorbell for tx notifications. + * @rx_dbl: doorbell for rx notifications. + * @tx_pipe: TX gunyah specific info. + * @rx_pipe: RX gunyah specific info. + */ +struct qrtr_gunyah_dev { + struct qrtr_endpoint ep; + struct device *dev; + struct gunyah_ring ring; + + struct resource res; + u32 memparcel; + void *base; + size_t size; + bool master; + u32 peer_name; + struct notifier_block rm_nb; + + u32 label; + void *tx_dbl; + void *rx_dbl; + struct work_struct work; + + struct gunyah_pipe tx_pipe; + struct gunyah_pipe rx_pipe; + wait_queue_head_t tx_avail_notify; +}; + +static void qrtr_gunyah_read(struct qrtr_gunyah_dev *qdev); + +static void qrtr_gunyah_kick(struct qrtr_gunyah_dev *qdev) +{ + gh_dbl_flags_t dbl_mask = QRTR_DBL_MASK; + int ret; + + ret = gh_dbl_send(qdev->tx_dbl, &dbl_mask, GH_DBL_NONBLOCK); + if (ret) { + dev_err(qdev->dev, "failed to raise doorbell %d\n", ret); + if (!qdev->master) + schedule_work(&qdev->work); + } +} + +static void qrtr_gunyah_retry_work(struct work_struct *work) +{ + struct qrtr_gunyah_dev *qdev = container_of(work, struct qrtr_gunyah_dev, + work); + gh_dbl_flags_t dbl_mask = QRTR_DBL_MASK; + + gh_dbl_send(qdev->tx_dbl, &dbl_mask, 0); +} + +static void qrtr_gunyah_cb(int irq, void *data) +{ + qrtr_gunyah_read((struct qrtr_gunyah_dev *)data); +} + +static size_t gunyah_rx_avail(struct gunyah_pipe *pipe) +{ + size_t len; + u32 head; + u32 tail; + + head = le32_to_cpu(*pipe->head); + tail = le32_to_cpu(*pipe->tail); + + if (head < tail) + len = pipe->length - tail + head; + else + len = head - tail; + + if (WARN_ON_ONCE(len > pipe->length)) + len = 0; + + return len; +} + +static void gunyah_rx_peak(struct gunyah_pipe *pipe, void *data, + unsigned int offset, size_t count) +{ + size_t len; + u32 tail; + + tail = le32_to_cpu(*pipe->tail); + tail += offset; + if (tail >= pipe->length) + tail -= pipe->length; + + len = min_t(size_t, count, pipe->length - tail); + if (len) + memcpy_fromio(data, pipe->fifo + tail, len); + + if (len != count) + memcpy_fromio(data + len, pipe->fifo, (count - len)); +} + +static void gunyah_rx_advance(struct gunyah_pipe *pipe, size_t count) +{ + u32 tail; + + tail = le32_to_cpu(*pipe->tail); + + tail += count; + if (tail >= pipe->length) + tail %= pipe->length; + + *pipe->tail = cpu_to_le32(tail); +} + +static size_t gunyah_tx_avail(struct gunyah_pipe *pipe) +{ + u32 avail; + u32 head; + u32 tail; + + head = le32_to_cpu(*pipe->head); + tail = le32_to_cpu(*pipe->tail); + + if (tail <= head) + avail = pipe->length - head + tail; + else + avail = tail - head; + + if (avail < FIFO_FULL_RESERVE) + avail = 0; + else + avail -= FIFO_FULL_RESERVE; + + return avail; +} + +static void gunyah_tx_write(struct gunyah_pipe *pipe, const void *data, + size_t count) +{ + size_t len; + u32 head; + + head = le32_to_cpu(*pipe->head); + + len = min_t(size_t, count, pipe->length - head); + if (len) + memcpy_toio(pipe->fifo + head, data, len); + + if (len != count) + memcpy_toio(pipe->fifo, data + len, count - len); + + head += count; + if (head >= pipe->length) + head -= pipe->length; + + /* Ensure ordering of fifo and head update */ + smp_wmb(); + + *pipe->head = cpu_to_le32(head); +} + +static void gunyah_set_tx_notify(struct qrtr_gunyah_dev *qdev) +{ + *qdev->tx_pipe.read_notify = cpu_to_le32(1); +} + +static void gunyah_clr_tx_notify(struct qrtr_gunyah_dev *qdev) +{ + *qdev->tx_pipe.read_notify = 0; +} + +static bool gunyah_get_read_notify(struct qrtr_gunyah_dev *qdev) +{ + return le32_to_cpu(*qdev->rx_pipe.read_notify); +} + +static void gunyah_wait_for_tx_avail(struct qrtr_gunyah_dev *qdev) +{ + gunyah_set_tx_notify(qdev); + wait_event_timeout(qdev->tx_avail_notify, + gunyah_tx_avail(&qdev->tx_pipe), 10 * HZ); +} + +/* from qrtr to gunyah */ +static int qrtr_gunyah_send(struct qrtr_endpoint *ep, struct sk_buff *skb) +{ + struct qrtr_gunyah_dev *qdev; + size_t tx_avail; + int chunk_size; + int left_size; + int offset; + + int rc; + + qdev = container_of(ep, struct qrtr_gunyah_dev, ep); + + rc = skb_linearize(skb); + if (rc) { + kfree_skb(skb); + return rc; + } + + left_size = skb->len; + offset = 0; + while (left_size > 0) { + tx_avail = gunyah_tx_avail(&qdev->tx_pipe); + if (!tx_avail) { + gunyah_wait_for_tx_avail(qdev); + continue; + } + if (tx_avail < left_size) + chunk_size = tx_avail; + else + chunk_size = left_size; + + gunyah_tx_write(&qdev->tx_pipe, skb->data + offset, chunk_size); + offset += chunk_size; + left_size -= chunk_size; + + qrtr_gunyah_kick(qdev); + } + gunyah_clr_tx_notify(qdev); + kfree_skb(skb); + + return 0; +} + +static void qrtr_gunyah_read_new(struct qrtr_gunyah_dev *qdev) +{ + struct gunyah_ring *ring = &qdev->ring; + size_t rx_avail; + size_t pkt_len; + u32 hdr[8]; + int rc; + size_t hdr_len = sizeof(hdr); + + gunyah_rx_peak(&qdev->rx_pipe, &hdr, 0, hdr_len); + pkt_len = qrtr_peek_pkt_size((void *)&hdr); + if ((int)pkt_len < 0 || pkt_len > MAX_PKT_SZ) { + dev_err(qdev->dev, "invalid pkt_len %zu\n", pkt_len); + return; + } + + rx_avail = gunyah_rx_avail(&qdev->rx_pipe); + if (rx_avail > pkt_len) + rx_avail = pkt_len; + + gunyah_rx_peak(&qdev->rx_pipe, ring->buf, 0, rx_avail); + gunyah_rx_advance(&qdev->rx_pipe, rx_avail); + + if (rx_avail == pkt_len) { + rc = qrtr_endpoint_post(&qdev->ep, ring->buf, pkt_len); + if (rc == -EINVAL) + dev_err(qdev->dev, "invalid ipcrouter packet\n"); + } else { + ring->len = pkt_len; + ring->offset = rx_avail; + } +} + +static void qrtr_gunyah_read_frag(struct qrtr_gunyah_dev *qdev) +{ + struct gunyah_ring *ring = &qdev->ring; + size_t rx_avail; + int rc; + + rx_avail = gunyah_rx_avail(&qdev->rx_pipe); + if (rx_avail + ring->offset > ring->len) + rx_avail = ring->len - ring->offset; + + gunyah_rx_peak(&qdev->rx_pipe, ring->buf + ring->offset, 0, rx_avail); + gunyah_rx_advance(&qdev->rx_pipe, rx_avail); + + if (rx_avail + ring->offset == ring->len) { + rc = qrtr_endpoint_post(&qdev->ep, ring->buf, ring->len); + if (rc == -EINVAL) + dev_err(qdev->dev, "invalid ipcrouter packet\n"); + ring->offset = 0; + ring->len = 0; + } else { + ring->offset += rx_avail; + } +} + +static void qrtr_gunyah_read(struct qrtr_gunyah_dev *qdev) +{ + wake_up_all(&qdev->tx_avail_notify); + + while (gunyah_rx_avail(&qdev->rx_pipe)) { + if (qdev->ring.offset) + qrtr_gunyah_read_frag(qdev); + else + qrtr_gunyah_read_new(qdev); + + if (gunyah_get_read_notify(qdev)) + qrtr_gunyah_kick(qdev); + } +} + +static int qrtr_gunyah_share_mem(struct qrtr_gunyah_dev *qdev, gh_vmid_t self, + gh_vmid_t peer) +{ + u32 src_vmlist[1] = {self}; + int src_perms[2] = {PERM_READ | PERM_WRITE | PERM_EXEC}; + int dst_vmlist[2] = {self, peer}; + int dst_perms[2] = {PERM_READ | PERM_WRITE, PERM_READ | PERM_WRITE}; + struct gh_acl_desc *acl; + struct gh_sgl_desc *sgl; + int ret; + + ret = hyp_assign_phys(qdev->res.start, resource_size(&qdev->res), + src_vmlist, 1, + dst_vmlist, dst_perms, 2); + if (ret) { + pr_err("%s: hyp_assign_phys failed addr=%x size=%u err=%d\n", + __func__, qdev->res.start, qdev->size, ret); + return ret; + } + + acl = kzalloc(offsetof(struct gh_acl_desc, acl_entries[2]), GFP_KERNEL); + if (!acl) + return -ENOMEM; + sgl = kzalloc(offsetof(struct gh_sgl_desc, sgl_entries[1]), GFP_KERNEL); + if (!sgl) { + kfree(acl); + return -ENOMEM; + } + acl->n_acl_entries = 2; + acl->acl_entries[0].vmid = (u16)self; + acl->acl_entries[0].perms = GH_RM_ACL_R | GH_RM_ACL_W; + acl->acl_entries[1].vmid = (u16)peer; + acl->acl_entries[1].perms = GH_RM_ACL_R | GH_RM_ACL_W; + + sgl->n_sgl_entries = 1; + sgl->sgl_entries[0].ipa_base = qdev->res.start; + sgl->sgl_entries[0].size = resource_size(&qdev->res); + + ret = gh_rm_mem_share(GH_RM_MEM_TYPE_NORMAL, 0, qdev->label, + acl, sgl, NULL, &qdev->memparcel); + if (ret) { + pr_err("%s: gh_rm_mem_share failed addr=%x size=%u err=%d\n", + __func__, qdev->res.start, qdev->size, ret); + /* Attempt to give resource back to HLOS */ + hyp_assign_phys(qdev->res.start, resource_size(&qdev->res), + dst_vmlist, 2, + src_vmlist, src_perms, 1); + } + + kfree(acl); + kfree(sgl); + + return ret; +} + +static int qrtr_gunyah_rm_cb(struct notifier_block *nb, unsigned long cmd, + void *data) +{ + struct gh_rm_notif_vm_status_payload *vm_status_payload; + struct qrtr_gunyah_dev *qdev; + gh_vmid_t peer_vmid; + gh_vmid_t self_vmid; + + qdev = container_of(nb, struct qrtr_gunyah_dev, rm_nb); + + if (cmd != GH_RM_NOTIF_VM_STATUS) + return NOTIFY_DONE; + + vm_status_payload = data; + if (vm_status_payload->vm_status != GH_RM_VM_STATUS_READY) + return NOTIFY_DONE; + if (gh_rm_get_vmid(qdev->peer_name, &peer_vmid)) + return NOTIFY_DONE; + if (gh_rm_get_vmid(GH_PRIMARY_VM, &self_vmid)) + return NOTIFY_DONE; + if (peer_vmid != vm_status_payload->vmid) + return NOTIFY_DONE; + + if (qrtr_gunyah_share_mem(qdev, self_vmid, peer_vmid)) + pr_err("%s: failed to share memory\n", __func__); + + return NOTIFY_DONE; +} + +/** + * qrtr_gunyah_fifo_init() - init gunyah xprt configs + * + * @return: 0 on success, standard Linux error codes on error. + * + * This function is called to initialize the gunyah XPRT pointer with + * the gunyah XPRT configurations either from device tree or static arrays. + */ +static void qrtr_gunyah_fifo_init(struct qrtr_gunyah_dev *qdev) +{ + __le32 *descs; + + if (qdev->master) + memset(qdev->base, 0, sizeof(*descs) * 10); + + descs = qdev->base; + descs[GUNYAH_MAGIC_IDX] = GUNYAH_MAGIC_KEY; + + if (qdev->master) { + qdev->tx_pipe.tail = &descs[TAIL_0_IDX]; + qdev->tx_pipe.head = &descs[HEAD_0_IDX]; + qdev->tx_pipe.fifo = qdev->base + FIFO_0_START; + qdev->tx_pipe.length = FIFO_SIZE; + qdev->tx_pipe.read_notify = &descs[NOTIFY_0_IDX]; + + qdev->rx_pipe.tail = &descs[TAIL_1_IDX]; + qdev->rx_pipe.head = &descs[HEAD_1_IDX]; + qdev->rx_pipe.fifo = qdev->base + FIFO_1_START; + qdev->rx_pipe.length = FIFO_SIZE; + qdev->rx_pipe.read_notify = &descs[NOTIFY_1_IDX]; + } else { + qdev->tx_pipe.tail = &descs[TAIL_1_IDX]; + qdev->tx_pipe.head = &descs[HEAD_1_IDX]; + qdev->tx_pipe.fifo = qdev->base + FIFO_1_START; + qdev->tx_pipe.length = FIFO_SIZE; + qdev->tx_pipe.read_notify = &descs[NOTIFY_1_IDX]; + + qdev->rx_pipe.tail = &descs[TAIL_0_IDX]; + qdev->rx_pipe.head = &descs[HEAD_0_IDX]; + qdev->rx_pipe.fifo = qdev->base + FIFO_0_START; + qdev->rx_pipe.length = FIFO_SIZE; + qdev->rx_pipe.read_notify = &descs[NOTIFY_0_IDX]; + } + + /* Reset respective index */ + *qdev->tx_pipe.head = 0; + *qdev->tx_pipe.read_notify = 0; + *qdev->rx_pipe.tail = 0; +} + +static struct device_node *qrtr_gunyah_svm_of_parse(struct qrtr_gunyah_dev *qdev) +{ + const char *compat = "qcom,qrtr-gunyah-gen"; + struct device_node *np = NULL; + struct device_node *shm_np; + u32 label; + int ret; + + while ((np = of_find_compatible_node(np, NULL, compat))) { + ret = of_property_read_u32(np, "qcom,label", &label); + if (ret) { + of_node_put(np); + continue; + } + if (label == qdev->label) + break; + + of_node_put(np); + } + if (!np) + return NULL; + + shm_np = of_parse_phandle(np, "memory-region", 0); + if (!shm_np) + dev_err(qdev->dev, "can't parse svm shared mem node!\n"); + + of_node_put(np); + return shm_np; +} + +static int qrtr_gunyah_map_memory(struct qrtr_gunyah_dev *qdev) +{ + struct device *dev = qdev->dev; + struct device_node *np; + resource_size_t size; + int ret; + + np = of_parse_phandle(dev->of_node, "shared-buffer", 0); + if (!np) { + np = qrtr_gunyah_svm_of_parse(qdev); + if (!np) { + dev_err(dev, "can't parse shared mem node!\n"); + return -EINVAL; + } + } + + ret = of_address_to_resource(np, 0, &qdev->res); + of_node_put(np); + if (ret) { + dev_err(dev, "of_address_to_resource failed!\n"); + return -EINVAL; + } + size = resource_size(&qdev->res); + + qdev->base = devm_ioremap_resource(dev, &qdev->res); + if (IS_ERR(qdev->base)) { + dev_err(dev, "ioremap failed!\n"); + return PTR_ERR(qdev->base); + } + qdev->size = size; + + return 0; +} + +/** + * qrtr_gunyah_probe() - Probe a gunyah xprt + * + * @pdev: Platform device corresponding to gunyah xprt. + * + * @return: 0 on success, standard Linux error codes on error. + * + * This function is called when the underlying device tree driver registers + * a platform device, mapped to a gunyah transport. + */ +static int qrtr_gunyah_probe(struct platform_device *pdev) +{ + struct device_node *node = pdev->dev.of_node; + struct qrtr_gunyah_dev *qdev; + enum gh_dbl_label dbl_label; + int ret; + + qdev = devm_kzalloc(&pdev->dev, sizeof(*qdev), GFP_KERNEL); + if (!qdev) + return -ENOMEM; + qdev->dev = &pdev->dev; + dev_set_drvdata(&pdev->dev, qdev); + + qdev->ring.buf = devm_kzalloc(&pdev->dev, MAX_PKT_SZ, GFP_KERNEL); + if (!qdev->ring.buf) + return -ENOMEM; + + ret = of_property_read_u32(node, "gunyah-label", &qdev->label); + if (ret) { + dev_err(qdev->dev, "failed to read label info %d\n", ret); + return ret; + } + qdev->master = of_property_read_bool(node, "qcom,master"); + + ret = qrtr_gunyah_map_memory(qdev); + if (ret) + return ret; + + qrtr_gunyah_fifo_init(qdev); + init_waitqueue_head(&qdev->tx_avail_notify); + + if (qdev->master) { + ret = of_property_read_u32(node, "peer-name", &qdev->peer_name); + if (ret) + qdev->peer_name = GH_SELF_VM; + + qdev->rm_nb.notifier_call = qrtr_gunyah_rm_cb; + qdev->rm_nb.priority = INT_MAX; + gh_rm_register_notifier(&qdev->rm_nb); + } + + dbl_label = qdev->label; + qdev->tx_dbl = gh_dbl_tx_register(dbl_label); + if (IS_ERR_OR_NULL(qdev->tx_dbl)) { + ret = PTR_ERR(qdev->tx_dbl); + dev_err(qdev->dev, "failed to get gunyah tx dbl %d\n", ret); + return ret; + } + INIT_WORK(&qdev->work, qrtr_gunyah_retry_work); + + qdev->rx_dbl = gh_dbl_rx_register(dbl_label, qrtr_gunyah_cb, qdev); + if (IS_ERR_OR_NULL(qdev->rx_dbl)) { + ret = PTR_ERR(qdev->rx_dbl); + dev_err(qdev->dev, "failed to get gunyah rx dbl %d\n", ret); + goto fail_rx_dbl; + } + + qdev->ep.xmit = qrtr_gunyah_send; + ret = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NET_ID_AUTO, false); + if (ret) + goto register_fail; + + if (gunyah_rx_avail(&qdev->rx_pipe)) + qrtr_gunyah_read(qdev); + + return 0; + +register_fail: + gh_dbl_rx_unregister(qdev->rx_dbl); +fail_rx_dbl: + cancel_work_sync(&qdev->work); + gh_dbl_tx_unregister(qdev->tx_dbl); + + return ret; +} + +static int qrtr_gunyah_remove(struct platform_device *pdev) +{ + struct qrtr_gunyah_dev *qdev = dev_get_drvdata(&pdev->dev); + + cancel_work_sync(&qdev->work); + gh_dbl_tx_unregister(qdev->tx_dbl); + gh_dbl_rx_unregister(qdev->rx_dbl); + + return 0; +} + +static const struct of_device_id qrtr_gunyah_match_table[] = { + { .compatible = "qcom,qrtr-gunyah" }, + {} +}; + +static struct platform_driver qrtr_gunyah_driver = { + .driver = { + .name = "qcom_gunyah_qrtr", + .of_match_table = qrtr_gunyah_match_table, + }, + .probe = qrtr_gunyah_probe, + .remove = qrtr_gunyah_remove, +}; +module_platform_driver(qrtr_gunyah_driver); + +MODULE_DESCRIPTION("QTI IPC-Router Gunyah interface driver"); +MODULE_LICENSE("GPL");