From 9c95dfb712a18f2d8b4d18b58d896e2509278d93 Mon Sep 17 00:00:00 2001
From: Jason Kotzin <jasonkotzin@gmail.com>
Date: Mon, 1 Aug 2022 17:45:49 -0700
Subject: [PATCH] adding adiv5 support

---
 src/target/adiv5.c | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 8d869363..bf310554 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -246,6 +246,23 @@ static uint32_t adiv5_mem_read32(ADIv5_AP_t *ap, uint32_t addr)
 	return ret;
 }
 
+static uint32_t adiv5_ap_read_id(ADIv5_AP_t *ap, uint32_t addr)
+{
+	uint32_t res = 0;
+	for (int i = 0; i < 4; i++) {
+		uint32_t x = adiv5_mem_read32(ap, addr + 4 * i);
+		res |= (x & 0xff) << (i * 8);
+	}
+	return res;
+}
+
+uint64_t adiv5_ap_read_pidr(ADIv5_AP_t *ap, uint32_t addr)
+{
+	uint64_t pidr = adiv5_ap_read_id(ap, addr + PIDR4_OFFSET);
+	pidr = pidr << 32 |     adiv5_ap_read_id(ap, addr + PIDR0_OFFSET);
+	return pidr;
+}
+
 static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 {
 	addr &= ~3;