From 5b68fe4b963d888a63c71eaa8cc3ef1920b7230f Mon Sep 17 00:00:00 2001
From: Stefan Krulj <powertomato (-at-) gmail.com>
Date: Mon, 10 Nov 2014 02:28:24 +0100
Subject: [PATCH] Reading CPU-ID and registers seems to work

---
 src/DAP_config.h | 45 ++++++++++++++++++++++-----------------------
 src/Debug.h      |  6 +++++-
 src/SW_DP.c      | 15 ++++++++-------
 3 files changed, 35 insertions(+), 31 deletions(-)

diff --git a/src/DAP_config.h b/src/DAP_config.h
index f21e224..cc7b126 100644
--- a/src/DAP_config.h
+++ b/src/DAP_config.h
@@ -247,12 +247,9 @@ static __inline void FTDI_BEGIN_READ(){
 }
 
 static __inline void FTDI_END_READ(){
-	int success, remaining;
-	uint32_t read_size;
-
-	read_size = remaining = out_state_buff_pos;
+	int success, remaining, i;
+	remaining = out_state_buff_pos;
 	FTDI_WRITE_OUT_READ_MODE();
-	out_state_buff_pos = 0;
 
     success = 0;
     uint8_t *ptr = in_buff;
@@ -264,27 +261,18 @@ static __inline void FTDI_END_READ(){
         }
         ptr+=success;
         remaining-=success;
-        printf("try %d\n", success);
     }
-    printf("Read %d/%d bytes\n",(int)(ptr-in_buff),read_size);
-
-	printf("%x\n",dir_state);
-	int i;
-	for(i=0;i<success;i++){
-		printf("%x ",in_buff[i]>>7);
-	}
-	printf("\n");
+    DEBUG("Read %d/%d bytes\n",(int)(ptr-in_buff),out_state_buff_pos);
+	out_state_buff_pos = 0;
 
 	for(i=0;i<pin_read_pos;i++){
 		uint8_t is_set = (in_buff[pin_read_buff[i].pos] & pin_read_buff[i].read_bitmask) != 0;
-		printf("%d ",is_set);
 		if( is_set ) {
 			(*pin_read_buff[i].ptr) |= pin_read_buff[i].write_bitmask;
 		}else{
 			(*pin_read_buff[i].ptr) &= ~pin_read_buff[i].write_bitmask;
 		}
 	}
-	printf("\n");
 	pin_read_pos = is_in_read_mode = 0;
 
 	success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
@@ -353,8 +341,6 @@ static __inline void DAP_SETUP (void) {
 	}
 	/*uint8_t foo;
     success = ftdi_read_data(&__ftdic,&foo,1);*/
-
-    RESET_TARGET();
 }
 
 ///@}
@@ -502,7 +488,7 @@ static __forceinline void     PIN_SWDIO_TMS_CLR (void) {
 }
 
 
-static __inline uint8_t bit_count(uint8_t v){
+static __inline uint8_t bit_count8(uint8_t v){
 	/*
 	 * http://stackoverflow.com/questions/3815165/how-to-implement-bitcount-using-only-bitwise-operators
 	 * 0x55=01010101, 0x33=00110011, 0x0F=00001111
@@ -524,15 +510,29 @@ static __inline uint8_t bit_count(uint8_t v){
 	return c;
 }
 
+static __inline uint8_t bit_count32(uint32_t v){
+	uint32_t c = 0;
+	c = (v & 0x55555555) + ((v >> 1) & 0x55555555) ;
+	c = (c & 0x33333333) + ((c >> 2) & 0x33333333) ;
+	c = (c & 0x0F0F0F0F) + ((c >> 4) & 0x0F0F0F0F) ;
+	c = (c & 0x00FF00FF) + ((c >> 8) & 0x00FF00FF) ;
+	c = (c & 0x0000FFFF) + ((c >>16) & 0x0000FFFF) ;
+
+	return c;
+}
+
 /** SWDIO I/O pin: Get Input (used in SWD mode only).
 \return Current status of the SWDIO DAP hardware I/O pin.
 */
 static __forceinline void PIN_SWDIO_IN      (uint8_t *ptr,uint8_t bit){
-
+	if( !is_in_read_mode ){
+		DEBUG("Not in read mode!\n");
+		return;
+	}
 	if( ptr!=NULL ){
 		DEBUG("setting read-to-pointer for pin reads!\n");
 		read_slot = ptr;
-		if( bit_count(bit)==1 ){
+		if( bit_count8(bit)==1 ){
 			pin_read_mask=bit;
 		}else{
 			pin_read_mask=1;
@@ -544,13 +544,12 @@ static __forceinline void PIN_SWDIO_IN      (uint8_t *ptr,uint8_t bit){
 	pin_read_buff[pin_read_pos].write_bitmask = pin_read_mask;
 	pin_read_buff[pin_read_pos].read_bitmask = TMS_SWDIO;
 
+	pin_read_buff[pin_read_pos].ptr = read_slot;
 	pin_read_mask = pin_read_mask<<1;
 	if( !pin_read_mask ){
 		pin_read_mask = 1;
 		read_slot += 1;
 	}
-
-	pin_read_buff[pin_read_pos].ptr = read_slot;
 	pin_read_pos++;
 
 	if( pin_read_pos==BUFFER_SIZE ){
diff --git a/src/Debug.h b/src/Debug.h
index 53ba487..0b9e51b 100644
--- a/src/Debug.h
+++ b/src/Debug.h
@@ -10,6 +10,10 @@
 #ifndef SRC_DEBUG_H_
 #define SRC_DEBUG_H_
 
-	#define DEBUG(format, ...) fprintf (stderr, format, ##__VA_ARGS__)
+#ifdef DEBUG_TARGET
+#  define DEBUG(format, ...) fprintf (stderr, format, ##__VA_ARGS__)
+#else
+#  define DEBUG(format, ...)
+#endif
 
 #endif //SRC_DEBUG_H_
diff --git a/src/SW_DP.c b/src/SW_DP.c
index 728f0fc..8ed9d38 100644
--- a/src/SW_DP.c
+++ b/src/SW_DP.c
@@ -36,7 +36,6 @@
   PIN_DELAY()
 
 #define SW_WRITE_BIT(bit)               \
-	printf("swdio=%d\n",bit);\
   PIN_SWDIO_OUT(bit);                   \
   PIN_SWCLK_CLR();                      \
   PIN_DELAY();                          \
@@ -103,10 +102,10 @@ uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {
   uint8_t ack;                                                                  \
   uint32_t bit;                                                                 \
   uint8_t val[4];                                                               \
-  uint32_t parity;                                                              \
+  uint8_t parity;                                                               \
+  uint8_t calc_parity;                                                          \
                                                                                 \
   uint32_t n;                                                                   \
-                       printf("req %d\n",request);                                                         \
   /* Packet Request */                                                          \
   parity = 0;                                                                   \
   SW_WRITE_BIT(1);                      /* Start Bit */                         \
@@ -142,19 +141,21 @@ uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {
   if (ack == DAP_TRANSFER_OK) {         /* OK response */                       \
     /* Data transfer */                                                         \
     if (request & DAP_TRANSFER_RnW) {                                           \
+      FTDI_BEGIN_READ();                                                        \
       /* Read data */                                                           \
-      memset( val, 0, 4);                                                       \
+      val[0]=val[1]=val[2]=val[3]=0;                                            \
       parity = 0;                                                               \
       SW_READ_BIT(val,1);                                                       \
       for (n = 31; n; n--) {                                                    \
         SW_READ_BIT(NULL,0);     /* Read RDATA[0:31] */                         \
-      }  /*TODO calc parity */                                                  \
+      }                                                                         \
 	  SW_READ_BIT((uint8_t*)&parity,1);  /* Read parity bit */                  \
 	  FTDI_END_READ();                                                          \
-      if ((parity ^ bit) & 1) {                                                 \
+	  calc_parity = bit_count32(val[3]<<24 | val[2]<<16 | val[1]<<8 | val[0])%2==0 ? 0 : 1; \
+      if ((parity & 1)!=calc_parity) {                                          \
         ack = DAP_TRANSFER_ERROR;                                               \
       }                                                                         \
-      if (data){ *data = val[0]<<24 | val[1]<<16 | val[2]<<8 | val[3];  }       \
+      if (data){ *data = val[3]<<24 | val[2]<<16 | val[1]<<8 | val[0];  }       \
       /* Turnaround */                                                          \
       for (n = DAP_Data.swd_conf.turnaround; n; n--) {                          \
         SW_CLOCK_CYCLE();                                                       \