From 0075abacbfc525518c4027df07e1f205a288174f Mon Sep 17 00:00:00 2001
From: Rafael Silva <perigoso@riseup.net>
Date: Sat, 6 Aug 2022 21:01:51 +0100
Subject: [PATCH] jlink_adiv5_swdp: remove duplicated logic on swdp low access

---
 src/platforms/hosted/jlink_adiv5_swdp.c | 13 +------------
 src/target/adiv5.h                      |  2 +-
 src/target/adiv5_swdp.c                 | 10 +++++++---
 3 files changed, 9 insertions(+), 16 deletions(-)

diff --git a/src/platforms/hosted/jlink_adiv5_swdp.c b/src/platforms/hosted/jlink_adiv5_swdp.c
index cc79943..df58122 100644
--- a/src/platforms/hosted/jlink_adiv5_swdp.c
+++ b/src/platforms/hosted/jlink_adiv5_swdp.c
@@ -178,9 +178,8 @@ static uint32_t jlink_adiv5_swdp_error(ADIv5_DP_t *dp)
 
 static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW, uint16_t addr, uint32_t value)
 {
+	uint8_t request = make_packet_request(RnW, addr);
 	bool APnDP = addr & ADIV5_APnDP;
-	uint8_t addr8 = addr & 0xffU;
-	uint8_t request = 0x81U;
 	uint32_t response = 0;
 	uint8_t ack;
 	platform_timeout timeout;
@@ -188,16 +187,6 @@ static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW, uint16_
 	if (APnDP && dp->fault)
 		return 0;
 
-	if (APnDP)
-		request ^= 0x22U;
-	if (RnW)
-		request ^= 0x24U;
-
-	addr8 &= 0xCU;
-	request |= (addr8 << 1U) & 0x18U;
-	if ((addr8 == 4U) || (addr8 == 8U))
-		request ^= 0x20U;
-
 	uint8_t cmd[16];
 	memset(cmd, 0, sizeof(cmd));
 
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index db08138..f248db8 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -298,7 +298,7 @@ struct ADIv5_AP_s {
 	uint16_t partno;
 };
 
-unsigned int make_packet_request(uint8_t RnW, uint16_t addr);
+uint8_t make_packet_request(uint8_t RnW, uint16_t addr);
 
 #if PC_HOSTED == 0
 static inline uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 27d3cd5..1a7fd6a 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -29,11 +29,14 @@
 #include "target.h"
 #include "target_internal.h"
 
-unsigned int make_packet_request(uint8_t RnW, uint16_t addr)
+uint8_t make_packet_request(uint8_t RnW, uint16_t addr)
 {
 	bool APnDP = addr & ADIV5_APnDP;
+
 	addr &= 0xffU;
-	unsigned int request = 0x81U; /* Park and Startbit */
+
+	uint8_t request = 0x81U; /* Park and Startbit */
+
 	if (APnDP)
 		request ^= 0x22U;
 	if (RnW)
@@ -43,6 +46,7 @@ unsigned int make_packet_request(uint8_t RnW, uint16_t addr)
 	request |= (addr << 1U) & 0x18U;
 	if (addr == 4U || addr == 8U)
 		request ^= 0x20U;
+
 	return request;
 }
 
@@ -235,7 +239,7 @@ uint32_t firmware_swdp_error(ADIv5_DP_t *dp)
 
 uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW, uint16_t addr, uint32_t value)
 {
-	uint32_t request = make_packet_request(RnW, addr);
+	uint8_t request = make_packet_request(RnW, addr);
 	uint32_t response = 0;
 	uint32_t ack = SWDP_ACK_WAIT;
 	platform_timeout timeout;