From 1638a863d3e587fab0d6ddcdbd09c8c1f84f653a Mon Sep 17 00:00:00 2001 From: Chris Lew Date: Thu, 30 Jul 2020 20:30:15 -0700 Subject: [PATCH] net: qrtr: haven: Add fragmentation support Clients may need to send packets larger than the physical transport allows. Add fragmantation support into the transport layer so packets larger than the shared memory fifo can be sent. This change increases the max packet size to 64k. The send function will now block until the entire packet has been sent across the shared fifo. Change-Id: I4e36671fbdd6a4de57abdb3d49b3757d8ceb0233 Signed-off-by: Chris Lew --- net/qrtr/haven.c | 175 ++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 143 insertions(+), 32 deletions(-) diff --git a/net/qrtr/haven.c b/net/qrtr/haven.c index b59ae4957eb0..8065eb404865 100644 --- a/net/qrtr/haven.c +++ b/net/qrtr/haven.c @@ -4,6 +4,7 @@ #define pr_fmt(fmt) "%s: " fmt, __func__ #include +#include #include #include #include @@ -17,6 +18,7 @@ #define HAVEN_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 HAVEN_MAGIC_IDX 0x0 @@ -24,11 +26,22 @@ #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 haven_ring { + void *buf; + size_t len; + u32 offset; +}; + struct haven_pipe { __le32 *tail; __le32 *head; + __le32 *read_notify; void *fifo; size_t length; @@ -38,7 +51,7 @@ struct haven_pipe { * qrtr_haven_dev - qrtr haven transport structure * @ep: qrtr endpoint specific info. * @dev: device from platform_device. - * @buf: buf for reading from fifo. + * @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. @@ -55,7 +68,7 @@ struct haven_pipe { struct qrtr_haven_dev { struct qrtr_endpoint ep; struct device *dev; - void *buf; + struct haven_ring ring; struct resource res; u32 memparcel; @@ -72,6 +85,7 @@ struct qrtr_haven_dev { struct haven_pipe tx_pipe; struct haven_pipe rx_pipe; + wait_queue_head_t tx_avail_notify; }; static void qrtr_haven_read(struct qrtr_haven_dev *qdev); @@ -105,6 +119,7 @@ static void qrtr_haven_cb(int irq, void *data) static size_t haven_rx_avail(struct haven_pipe *pipe) { + size_t len; u32 head; u32 tail; @@ -112,9 +127,14 @@ static size_t haven_rx_avail(struct haven_pipe *pipe) tail = le32_to_cpu(*pipe->tail); if (head < tail) - return pipe->length - tail + head; + len = pipe->length - tail + head; + else + len = head - tail; - return head - tail; + if (WARN_ON_ONCE(len > pipe->length)) + len = 0; + + return len; } static void haven_rx_peak(struct haven_pipe *pipe, void *data, @@ -143,8 +163,8 @@ static void haven_rx_advance(struct haven_pipe *pipe, size_t count) tail = le32_to_cpu(*pipe->tail); tail += count; - if (tail > pipe->length) - tail -= pipe->length; + if (tail >= pipe->length) + tail %= pipe->length; *pipe->tail = cpu_to_le32(tail); } @@ -163,6 +183,11 @@ static size_t haven_tx_avail(struct haven_pipe *pipe) else avail = tail - head; + if (avail < FIFO_FULL_RESERVE) + avail = 0; + else + avail -= FIFO_FULL_RESERVE; + return avail; } @@ -191,10 +216,37 @@ static void haven_tx_write(struct haven_pipe *pipe, *pipe->head = cpu_to_le32(head); } +static void haven_set_tx_notify(struct qrtr_haven_dev *qdev) +{ + *qdev->tx_pipe.read_notify = cpu_to_le32(1); +} + +static void haven_clr_tx_notify(struct qrtr_haven_dev *qdev) +{ + *qdev->tx_pipe.read_notify = 0; +} + +static bool haven_get_read_notify(struct qrtr_haven_dev *qdev) +{ + return le32_to_cpu(*qdev->rx_pipe.read_notify); +} + +static void haven_wait_for_tx_avail(struct qrtr_haven_dev *qdev) +{ + haven_set_tx_notify(qdev); + wait_event_timeout(qdev->tx_avail_notify, + haven_tx_avail(&qdev->tx_pipe), 10 * HZ); +} + /* from qrtr to haven */ static int qrtr_haven_send(struct qrtr_endpoint *ep, struct sk_buff *skb) { struct qrtr_haven_dev *qdev; + size_t tx_avail; + int chunk_size; + int left_size; + int offset; + int rc; qdev = container_of(ep, struct qrtr_haven_dev, ep); @@ -205,47 +257,100 @@ static int qrtr_haven_send(struct qrtr_endpoint *ep, struct sk_buff *skb) return rc; } - if (haven_tx_avail(&qdev->tx_pipe) < skb->len) { - pr_err("No Space in haven\n"); - return -EAGAIN; + left_size = skb->len; + offset = 0; + while (left_size > 0) { + tx_avail = haven_tx_avail(&qdev->tx_pipe); + if (!tx_avail) { + haven_wait_for_tx_avail(qdev); + continue; + } + if (tx_avail < left_size) + chunk_size = tx_avail; + else + chunk_size = left_size; + + haven_tx_write(&qdev->tx_pipe, skb->data + offset, chunk_size); + offset += chunk_size; + left_size -= chunk_size; + + qrtr_haven_kick(qdev); } - - haven_tx_write(&qdev->tx_pipe, skb->data, skb->len); + haven_clr_tx_notify(qdev); kfree_skb(skb); - qrtr_haven_kick(qdev); - return 0; } -static void qrtr_haven_read(struct qrtr_haven_dev *qdev) +static void qrtr_haven_read_new(struct qrtr_haven_dev *qdev) { + struct haven_ring *ring = &qdev->ring; size_t rx_avail; size_t pkt_len; u32 hdr[8]; int rc; size_t hdr_len = sizeof(hdr); - while (haven_rx_avail(&qdev->rx_pipe)) { - haven_rx_peak(&qdev->rx_pipe, &hdr, 0, hdr_len); - pkt_len = qrtr_peek_pkt_size((void *)&hdr); - if ((int)pkt_len < 0) { - dev_err(qdev->dev, "invalid pkt_len %zu\n", pkt_len); - break; - } + haven_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 = haven_rx_avail(&qdev->rx_pipe); - if (rx_avail < pkt_len) { - pr_err_ratelimited("Not FULL pkt in haven %zu %zu\n", - rx_avail, pkt_len); - break; - } - haven_rx_peak(&qdev->rx_pipe, qdev->buf, 0, pkt_len); - haven_rx_advance(&qdev->rx_pipe, pkt_len); + rx_avail = haven_rx_avail(&qdev->rx_pipe); + if (rx_avail > pkt_len) + rx_avail = pkt_len; - rc = qrtr_endpoint_post(&qdev->ep, qdev->buf, pkt_len); + haven_rx_peak(&qdev->rx_pipe, ring->buf, 0, rx_avail); + haven_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_haven_read_frag(struct qrtr_haven_dev *qdev) +{ + struct haven_ring *ring = &qdev->ring; + size_t rx_avail; + int rc; + + rx_avail = haven_rx_avail(&qdev->rx_pipe); + if (rx_avail + ring->offset > ring->len) + rx_avail = ring->len - ring->offset; + + haven_rx_peak(&qdev->rx_pipe, ring->buf + ring->offset, 0, rx_avail); + haven_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_haven_read(struct qrtr_haven_dev *qdev) +{ + wake_up_all(&qdev->tx_avail_notify); + + while (haven_rx_avail(&qdev->rx_pipe)) { + if (qdev->ring.offset) + qrtr_haven_read_frag(qdev); + else + qrtr_haven_read_new(qdev); + + if (haven_get_read_notify(qdev)) + qrtr_haven_kick(qdev); } } @@ -347,25 +452,30 @@ static void qrtr_haven_fifo_init(struct qrtr_haven_dev *qdev) 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; } @@ -456,8 +566,8 @@ static int qrtr_haven_probe(struct platform_device *pdev) qdev->dev = &pdev->dev; dev_set_drvdata(&pdev->dev, qdev); - qdev->buf = devm_kzalloc(&pdev->dev, FIFO_SIZE, GFP_KERNEL); - if (!qdev->buf) + qdev->ring.buf = devm_kzalloc(&pdev->dev, MAX_PKT_SZ, GFP_KERNEL); + if (!qdev->ring.buf) return -ENOMEM; ret = of_property_read_u32(node, "haven-label", &qdev->label); @@ -472,6 +582,7 @@ static int qrtr_haven_probe(struct platform_device *pdev) return ret; qrtr_haven_fifo_init(qdev); + init_waitqueue_head(&qdev->tx_avail_notify); if (qdev->master) { ret = of_property_read_u32(node, "peer-name", &qdev->peer_name);