From 95d4bdad3bf5340b0bf1e53e28fcd487d9253315 Mon Sep 17 00:00:00 2001
From: Maxime Ripard <maxime@cerno.tech>
Date: Mon, 27 Mar 2023 17:10:07 +0200
Subject: [PATCH] dmaengine: bcm2835: Fix position reporting for 40
 bits channels

For 40 bits channels, the position is reported by reading the upper byte
in the SRCI/DESTI registers. However the driver adds that upper byte
with an 8-bits left shift, while it should be 32.

Fixes: 9a52a9918306 ("bcm2835-dma: Add proper 40-bit DMA support")
Signed-off-by: Maxime Ripard <maxime@cerno.tech>
---
 drivers/dma/bcm2835-dma.c | 27 ++++++++++++++++-----------
 1 file changed, 16 insertions(+), 11 deletions(-)

--- a/drivers/dma/bcm2835-dma.c
+++ b/drivers/dma/bcm2835-dma.c
@@ -819,20 +819,25 @@ static enum dma_status bcm2835_dma_tx_st
 		struct bcm2835_desc *d = c->desc;
 		dma_addr_t pos;
 
-		if (d->dir == DMA_MEM_TO_DEV && c->is_40bit_channel)
-			pos = readl(c->chan_base + BCM2711_DMA40_SRC) +
-				((readl(c->chan_base + BCM2711_DMA40_SRCI) &
-				  0xff) << 8);
-		else if (d->dir == DMA_MEM_TO_DEV && !c->is_40bit_channel)
+		if (d->dir == DMA_MEM_TO_DEV && c->is_40bit_channel) {
+			u64 lo_bits, hi_bits;
+
+			lo_bits = readl(c->chan_base + BCM2711_DMA40_SRC);
+			hi_bits = readl(c->chan_base + BCM2711_DMA40_SRCI) & 0xff;
+			pos = (hi_bits << 32) | lo_bits;
+		} else if (d->dir == DMA_MEM_TO_DEV && !c->is_40bit_channel) {
 			pos = readl(c->chan_base + BCM2835_DMA_SOURCE_AD);
-		else if (d->dir == DMA_DEV_TO_MEM && c->is_40bit_channel)
-			pos = readl(c->chan_base + BCM2711_DMA40_DEST) +
-				((readl(c->chan_base + BCM2711_DMA40_DESTI) &
-				  0xff) << 8);
-		else if (d->dir == DMA_DEV_TO_MEM && !c->is_40bit_channel)
+		} else if (d->dir == DMA_DEV_TO_MEM && c->is_40bit_channel) {
+			u64 lo_bits, hi_bits;
+
+			lo_bits = readl(c->chan_base + BCM2711_DMA40_DEST);
+			hi_bits = readl(c->chan_base + BCM2711_DMA40_DESTI) & 0xff;
+			pos = (hi_bits << 32) | lo_bits;
+		} else if (d->dir == DMA_DEV_TO_MEM && !c->is_40bit_channel) {
 			pos = readl(c->chan_base + BCM2835_DMA_DEST_AD);
-		else
+		} else {
 			pos = 0;
+		}
 
 		txstate->residue = bcm2835_dma_desc_size_pos(d, pos);
 	} else {
