From 954644d8f5008610e21ef289b22979fdd737fbcc Mon Sep 17 00:00:00 2001
From: Wei Fu <wefu@redhat.com>
Date: Sun, 19 Sep 2021 14:13:15 +0800
Subject: sifive_cache: add flush_range func in sifive cache driver

Signed-off-by: Wei Fu <wefu@redhat.com>
---
 arch/riscv/lib/sifive_cache.c       | 21 +++++++++++++
 drivers/cache/cache-sifive-ccache.c | 61 ++++++++++++++++++++++++++++++++++++-
 2 files changed, 81 insertions(+), 1 deletion(-)

diff --git a/arch/riscv/lib/sifive_cache.c b/arch/riscv/lib/sifive_cache.c
index 28154878fc..de4aba6630 100644
--- a/arch/riscv/lib/sifive_cache.c
+++ b/arch/riscv/lib/sifive_cache.c
@@ -25,3 +25,24 @@ void enable_caches(void)
 			log_debug("ccache enable failed");
 	}
 }
+
+
+void flush_dcache_range(unsigned long start, unsigned long end)
+{
+	struct udevice *dev;
+	int ret;
+
+	/* Enable ways of ccache */
+	ret = uclass_get_device_by_driver(UCLASS_CACHE,
+					  DM_DRIVER_GET(sifive_ccache),
+					  &dev);
+	if (ret) {
+		log_debug("Cannot flush dcache in %p - %p",
+			  (void *)start, (void *)end);
+	} else {
+		ret = flush_range(dev, start, end);
+		if (ret)
+			log_debug("ccache flush failed in %p - %p",
+				  (void *)start, (void *)end);
+	}
+}
\ No newline at end of file
diff --git a/drivers/cache/cache-sifive-ccache.c b/drivers/cache/cache-sifive-ccache.c
index c8766f6242..a4889f1bc5 100644
--- a/drivers/cache/cache-sifive-ccache.c
+++ b/drivers/cache/cache-sifive-ccache.c
@@ -7,16 +7,22 @@
 #include <cache.h>
 #include <dm.h>
 #include <asm/io.h>
-#include <dm/device.h>
 #include <linux/bitfield.h>
+#include <fdt_support.h>
 
 #define SIFIVE_CCACHE_CONFIG		0x000
 #define SIFIVE_CCACHE_CONFIG_WAYS	GENMASK(15, 8)
 
 #define SIFIVE_CCACHE_WAY_ENABLE	0x008
+#define SIFIVE_CCACHE_FLUSH64		0x200
+
+static bool range_check = false;
 
 struct sifive_ccache {
 	void __iomem *base;
+	int cache_line_size;
+	phys_addr_t flush_start; /* Start physical address of flush range limit. */
+	phys_addr_t flush_end; /* End physical address of flush range limit. */
 };
 
 static int sifive_ccache_enable(struct udevice *dev)
@@ -43,19 +49,72 @@ static int sifive_ccache_get_info(struct udevice *dev, struct cache_info *info)
 	return 0;
 }
 
+static int sifive_ccache_flush_range(struct udevice *dev,
+				     unsigned long start, unsigned long end)
+{
+	unsigned long line;
+	volatile unsigned long *flush64;
+	struct sifive_ccache *priv = dev_get_priv(dev);
+
+	if (range_check) {
+		/* make sure the address is in the range */
+		if(start > end ||
+		   start < priv->flush_start || end > priv->flush_end)
+			return -EINVAL;
+	} else {
+		pr_warn("skip checking range.");
+	}
+
+	/* make sure we won't get into infinite loop below */
+	if (!priv->cache_line_size) {
+		pr_warn("missing cache_line_size, skip flush.");
+		return -EINVAL;
+	}
+
+	flush64 = (volatile unsigned long *)(priv->base + SIFIVE_CCACHE_FLUSH64);
+
+	/* memory barrier */
+	mb();
+	for (line = start; line < end; line += priv->cache_line_size)
+		(*flush64) = line;
+	/* memory barrier */
+	mb();
+
+	return 0;
+}
+
 static const struct cache_ops sifive_ccache_ops = {
 	.enable = sifive_ccache_enable,
 	.get_info = sifive_ccache_get_info,
+	.flush_range = sifive_ccache_flush_range,
 };
 
 static int sifive_ccache_probe(struct udevice *dev)
 {
+	int ret;
+	uint64_t addr, len;
 	struct sifive_ccache *priv = dev_get_priv(dev);
 
 	priv->base = dev_read_addr_ptr(dev);
 	if (!priv->base)
 		return -EINVAL;
 
+	priv->cache_line_size =
+		dev_read_u32_default(dev, "cache-line-size",
+				     CONFIG_SYS_CACHELINE_SIZE);
+
+	/* only read range index 0 */
+	ret = fdt_read_range((void *)gd->fdt_blob, dev_of_offset(dev), 0,
+			     NULL, &addr, &len);
+	if (ret) {
+		pr_warn("missing flush range, ignore range check.");
+		return 0;
+	}
+
+	range_check = true;
+	priv->flush_start = addr;
+	priv->flush_end = addr + len - 1;
+
 	return 0;
 }
 
-- 
cgit v1.2.3