Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 1638a863 authored by Chris Lew's avatar Chris Lew
Browse files

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: default avatarChris Lew <clew@codeaurora.org>
parent 63a58ef9

net/qrtr/haven.c

+143 −32
Original line number Diff line number Diff line
@@ -4,6 +4,7 @@ 
#define pr_fmt(fmt) "%s: " fmt, __func__



#include <linux/io.h>

#include <linux/sizes.h>

#include <linux/of_device.h>

#include <linux/of_address.h>

#include <linux/module.h>
@@ -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, skb->len);

	kfree_skb(skb);

		haven_tx_write(&qdev->tx_pipe, skb->data + offset, chunk_size);

		offset += chunk_size;

		left_size -= chunk_size;



		qrtr_haven_kick(qdev);

	}

	haven_clr_tx_notify(qdev);

	kfree_skb(skb);



	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) {

	if ((int)pkt_len < 0 || pkt_len > MAX_PKT_SZ) {

		dev_err(qdev->dev, "invalid pkt_len %zu\n", pkt_len);

			break;

		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;

	if (rx_avail > pkt_len)

		rx_avail = 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;

	}

		haven_rx_peak(&qdev->rx_pipe, qdev->buf, 0, pkt_len);

		haven_rx_advance(&qdev->rx_pipe, pkt_len);

}



static void qrtr_haven_read_frag(struct qrtr_haven_dev *qdev)

{

	struct haven_ring *ring = &qdev->ring;

	size_t rx_avail;

	int rc;



		rc = qrtr_endpoint_post(&qdev->ep, qdev->buf, pkt_len);

	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);