X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fsvf%2Fsvf.c;h=ac7c2c68d01256cdedee8a412018bcd2bfc20795;hp=4107aa2c0eb26d1bda6535698996448f6707395b;hb=2eb8a31a6bc91a645ed6aa06752835f4c61cd703;hpb=b1beaa36e2d6ae6ffdf5bddbaae6ac165f984510

diff --git a/src/svf/svf.c b/src/svf/svf.c
index 4107aa2c0e..ac7c2c68d0 100644
--- a/src/svf/svf.c
+++ b/src/svf/svf.c
@@ -241,6 +241,37 @@ static long svf_total_lines;
 static int svf_percentage;
 static int svf_last_printed_percentage = -1;
 
+/*
+ * macro is used to print the svf hex buffer at desired debug level
+ * DEBUG, INFO, ERROR, USER
+ */
+#define SVF_BUF_LOG(_lvl, _buf, _nbits, _desc)							\
+	svf_hexbuf_print(LOG_LVL_##_lvl ,  __FILE__, __LINE__, __func__, _buf, _nbits, _desc)
+
+static void svf_hexbuf_print(int dbg_lvl, const char *file, unsigned line,
+							 const char *function, const uint8_t *buf,
+							 int bit_len, const char *desc)
+{
+	int j, len = 0;
+	int byte_len = DIV_ROUND_UP(bit_len, 8);
+	int msbits = bit_len % 8;
+
+	/* allocate 2 bytes per hex digit */
+	char *prbuf = malloc((byte_len * 2) + 1);
+	if (!prbuf)
+		return;
+
+	/* print correct number of bytes, mask excess bits where applicable */
+	uint8_t msb = buf[byte_len - 1] & (msbits ? (1 << msbits) - 1 : 0xff);
+	len = sprintf(prbuf, msbits <= 4 ? "0x%01"PRIx8 : "0x%02"PRIx8, msb);
+	for (j = byte_len - 2; j >= 0; j--)
+		len += sprintf(prbuf + len, "%02"PRIx8, buf[j]);
+
+	log_printf_lf(dbg_lvl, file, line, function, "%8s = %s", desc ? desc : " ", prbuf);
+
+	free(prbuf);
+}
+
 static int svf_realloc_buffers(size_t len)
 {
 	void *ptr;
@@ -287,20 +318,6 @@ static void svf_free_xxd_para(struct svf_xxr_para *para)
 	}
 }
 
-static unsigned svf_get_mask_u32(int bitlen)
-{
-	uint32_t bitmask;
-
-	if (bitlen < 0)
-		bitmask = 0;
-	else if (bitlen >= 32)
-		bitmask = 0xFFFFFFFF;
-	else
-		bitmask = (1 << bitlen) - 1;
-
-	return bitmask;
-}
-
 int svf_add_statemove(tap_state_t state_to)
 {
 	tap_state_t state_from = cmd_queue_cur_state;
@@ -838,19 +855,11 @@ static int svf_check_tdo(void)
 		if ((svf_check_tdo_para[i].enabled)
 				&& buf_cmp_mask(&svf_tdi_buffer[index_var], &svf_tdo_buffer[index_var],
 				&svf_mask_buffer[index_var], len)) {
-			unsigned bitmask;
-			unsigned received, expected, tapmask;
-			bitmask = svf_get_mask_u32(svf_check_tdo_para[i].bit_len);
-
-			memcpy(&received, svf_tdi_buffer + index_var, sizeof(unsigned));
-			memcpy(&expected, svf_tdo_buffer + index_var, sizeof(unsigned));
-			memcpy(&tapmask, svf_mask_buffer + index_var, sizeof(unsigned));
 			LOG_ERROR("tdo check error at line %d",
 				svf_check_tdo_para[i].line_num);
-			LOG_ERROR("read = 0x%X, want = 0x%X, mask = 0x%X",
-				received & bitmask,
-				expected & bitmask,
-				tapmask & bitmask);
+			SVF_BUF_LOG(ERROR, &svf_tdi_buffer[index_var], len, "READ");
+			SVF_BUF_LOG(ERROR, &svf_tdo_buffer[index_var], len, "WANT");
+			SVF_BUF_LOG(ERROR, &svf_mask_buffer[index_var], len, "MASK");
 			return ERROR_FAIL;
 		}
 	}
@@ -1045,8 +1054,7 @@ XXR_common:
 					LOG_ERROR("fail to parse hex value");
 					return ERROR_FAIL;
 				}
-				LOG_DEBUG("\t%s = 0x%X", argus[i],
-						(**(int **)pbuffer_tmp) & svf_get_mask_u32(xxr_para_tmp->len));
+				SVF_BUF_LOG(DEBUG, *pbuffer_tmp, xxr_para_tmp->len, argus[i]);
 			}
 			/* If a command changes the length of the last scan of the same type and the
 			 * MASK parameter is absent, */
@@ -1509,11 +1517,7 @@ XXR_common:
 
 			/* output debug info */
 			if ((SIR == command) || (SDR == command)) {
-				int read_value;
-				memcpy(&read_value, svf_tdi_buffer, sizeof(int));
-				/* in debug mode, data is from index 0 */
-				int read_mask = svf_get_mask_u32(svf_check_tdo_para[0].bit_len);
-				LOG_DEBUG("\tTDO read = 0x%X", read_value & read_mask);
+				SVF_BUF_LOG(DEBUG, svf_tdi_buffer, svf_check_tdo_para[0].bit_len, "TDO read");
 			}
 		}
 	} else {