4 files changed, 16 insertions, 11 deletions

diff --git a/drivers/rpmsg/qcom_glink_native.c b/drivers/rpmsg/qcom_glink_native.c
index 5dccbf18046e..6d9985b719f9 100644
--- a/drivers/rpmsg/qcom_glink_native.c
+++ b/drivers/rpmsg/qcom_glink_native.c
@@ -233,9 +233,9 @@ static size_t qcom_glink_rx_avail(struct qcom_glink *glink)
 }
 
 static void qcom_glink_rx_peak(struct qcom_glink *glink,
-			       void *data, size_t count)
+			       void *data, unsigned int offset, size_t count)
 {
-	glink->rx_pipe->peak(glink->rx_pipe, data, count);
+	glink->rx_pipe->peak(glink->rx_pipe, data, offset, count);
 }
 
 static void qcom_glink_rx_advance(struct qcom_glink *glink, size_t count)
@@ -600,7 +600,7 @@ static int qcom_glink_rx_defer(struct qcom_glink *glink, size_t extra)
 
 	INIT_LIST_HEAD(&dcmd->node);
 
-	qcom_glink_rx_peak(glink, &dcmd->msg, sizeof(dcmd->msg) + extra);
+	qcom_glink_rx_peak(glink, &dcmd->msg, 0, sizeof(dcmd->msg) + extra);
 
 	spin_lock(&glink->rx_lock);
 	list_add_tail(&dcmd->node, &glink->rx_queue);
@@ -633,7 +633,7 @@ static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail)
 		return -EAGAIN;
 	}
 
-	qcom_glink_rx_peak(glink, &hdr, sizeof(hdr));
+	qcom_glink_rx_peak(glink, &hdr, 0, sizeof(hdr));
 	chunk_size = le32_to_cpu(hdr.chunk_size);
 	left_size = le32_to_cpu(hdr.left_size);
 
@@ -700,9 +700,8 @@ static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail)
 		goto advance_rx;
 	}
 
-	qcom_glink_rx_advance(glink, ALIGN(sizeof(hdr), 8));
 	qcom_glink_rx_peak(glink, intent->data + intent->offset,
-			   chunk_size);
+			   sizeof(hdr), chunk_size);
 	intent->offset += chunk_size;
 
 	/* Handle message when no fragments remain to be received */
@@ -722,7 +721,7 @@ static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail)
 	}
 
 advance_rx:
-	qcom_glink_rx_advance(glink, ALIGN(chunk_size, 8));
+	qcom_glink_rx_advance(glink, ALIGN(sizeof(hdr) + chunk_size, 8));
 
 	return ret;
 }
@@ -759,7 +758,7 @@ static irqreturn_t qcom_glink_native_intr(int irq, void *data)
 		if (avail < sizeof(msg))
 			break;
 
-		qcom_glink_rx_peak(glink, &msg, sizeof(msg));
+		qcom_glink_rx_peak(glink, &msg, 0, sizeof(msg));
 
 		cmd = le16_to_cpu(msg.cmd);
 		param1 = le16_to_cpu(msg.param1);
diff --git a/drivers/rpmsg/qcom_glink_native.h b/drivers/rpmsg/qcom_glink_native.h
index 92e088144128..0cae8a8199f8 100644
--- a/drivers/rpmsg/qcom_glink_native.h
+++ b/drivers/rpmsg/qcom_glink_native.h
@@ -24,7 +24,7 @@ struct qcom_glink_pipe {
 	size_t (*avail)(struct qcom_glink_pipe *glink_pipe);
 
 	void (*peak)(struct qcom_glink_pipe *glink_pipe, void *data,
-		     size_t count);
+		     unsigned int offset, size_t count);
 	void (*advance)(struct qcom_glink_pipe *glink_pipe, size_t count);
 
 	void (*write)(struct qcom_glink_pipe *glink_pipe,
diff --git a/drivers/rpmsg/qcom_glink_rpm.c b/drivers/rpmsg/qcom_glink_rpm.c
index 5a86e089a977..69b25d157d0f 100644
--- a/drivers/rpmsg/qcom_glink_rpm.c
+++ b/drivers/rpmsg/qcom_glink_rpm.c
@@ -77,13 +77,16 @@ static size_t glink_rpm_rx_avail(struct qcom_glink_pipe *glink_pipe)
 }
 
 static void glink_rpm_rx_peak(struct qcom_glink_pipe *glink_pipe,
-			      void *data, size_t count)
+			      void *data, unsigned int offset, size_t count)
 {
 	struct glink_rpm_pipe *pipe = to_rpm_pipe(glink_pipe);
 	unsigned int tail;
 	size_t len;
 
 	tail = readl(pipe->tail);
+	tail += offset;
+	if (tail >= pipe->native.length)
+		tail -= pipe->native.length;
 
 	len = min_t(size_t, count, pipe->native.length - tail);
 	if (len) {
diff --git a/drivers/rpmsg/qcom_glink_smem.c b/drivers/rpmsg/qcom_glink_smem.c
index 9d73ced075e7..5cdaa5f8fb61 100644
--- a/drivers/rpmsg/qcom_glink_smem.c
+++ b/drivers/rpmsg/qcom_glink_smem.c
@@ -87,13 +87,16 @@ static size_t glink_smem_rx_avail(struct qcom_glink_pipe *np)
 }
 
 static void glink_smem_rx_peak(struct qcom_glink_pipe *np,
-			       void *data, size_t count)
+			       void *data, unsigned int offset, size_t count)
 {
 	struct glink_smem_pipe *pipe = to_smem_pipe(np);
 	size_t len;
 	u32 tail;
 
 	tail = le32_to_cpu(*pipe->tail);
+	tail += offset;
+	if (tail >= pipe->native.length)
+		tail -= pipe->native.length;
 
 	len = min_t(size_t, count, pipe->native.length - tail);
 	if (len) {