diff --git a/net/qrtr/gunyah.c b/net/qrtr/gunyah.c index 38118890eed6..7f5a1898d3e2 100644 --- a/net/qrtr/gunyah.c +++ b/net/qrtr/gunyah.c @@ -219,6 +219,60 @@ static void gunyah_tx_write(struct gunyah_pipe *pipe, const void *data, *pipe->head = cpu_to_le32(head); } +static size_t gunyah_sg_copy_toio(struct scatterlist *sg, unsigned int nents, + void *buf, size_t buflen, off_t skip) +{ + unsigned int sg_flags = SG_MITER_ATOMIC | SG_MITER_FROM_SG; + struct sg_mapping_iter miter; + unsigned int offset = 0; + + sg_miter_start(&miter, sg, nents, sg_flags); + + if (!sg_miter_skip(&miter, skip)) + return 0; + + while ((offset < buflen) && sg_miter_next(&miter)) { + unsigned int len; + + len = min(miter.length, buflen - offset); + memcpy_toio(buf + offset, miter.addr, len); + offset += len; + } + + sg_miter_stop(&miter); + + return offset; +} + +static void gunyah_sg_write(struct gunyah_pipe *pipe, struct scatterlist *sg, + int offset, size_t count) +{ + size_t len; + u32 head; + int rc = 0; + + head = le32_to_cpu(*pipe->head); + + len = min_t(size_t, count, pipe->length - head); + if (len) { + rc = gunyah_sg_copy_toio(sg, sg_nents(sg), pipe->fifo + head, + len, offset); + offset += rc; + } + + if (len != count) + rc = gunyah_sg_copy_toio(sg, sg_nents(sg), pipe->fifo, + count - len, offset); + + head += count; + if (head >= pipe->length) + head -= pipe->length; + + 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); @@ -249,17 +303,10 @@ static int qrtr_gunyah_send(struct qrtr_endpoint *ep, struct sk_buff *skb) 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) { @@ -273,7 +320,22 @@ static int qrtr_gunyah_send(struct qrtr_endpoint *ep, struct sk_buff *skb) else chunk_size = left_size; - gunyah_tx_write(&qdev->tx_pipe, skb->data + offset, chunk_size); + if (skb_is_nonlinear(skb)) { + struct scatterlist sg[MAX_SKB_FRAGS + 1]; + + sg_init_table(sg, skb_shinfo(skb)->nr_frags + 1); + rc = skb_to_sgvec(skb, sg, 0, skb->len); + if (rc < 0) { + pr_err("failed skb_to_sgvec rc:%d\n", rc); + break; + } + gunyah_sg_write(&qdev->tx_pipe, sg, offset, + chunk_size); + } else { + gunyah_tx_write(&qdev->tx_pipe, skb->data + offset, + chunk_size); + } + offset += chunk_size; left_size -= chunk_size;