X-Git-Url: https://git.librecmc.org/?a=blobdiff_plain;f=arch%2Fblackfin%2Fcpu%2Fjtag-console.c;h=e0f2975603b67abd5785eac9ac99377237620245;hb=6ca803750e1e08fce26e508f1c35170bf9259bb4;hp=46b30a0f8c0b12137547947327ee734828d911f0;hpb=5079d8bbad77bf21ecc5f17e05141f04cfaaabb3;p=oweals%2Fu-boot.git

diff --git a/arch/blackfin/cpu/jtag-console.c b/arch/blackfin/cpu/jtag-console.c
index 46b30a0f8c..e0f2975603 100644
--- a/arch/blackfin/cpu/jtag-console.c
+++ b/arch/blackfin/cpu/jtag-console.c
@@ -7,9 +7,26 @@
  */
 
 #include <common.h>
+#include <malloc.h>
 #include <stdio_dev.h>
 #include <asm/blackfin.h>
 
+#ifdef DEBUG
+# define dprintf(...) serial_printf(__VA_ARGS__)
+#else
+# define dprintf(...) do { if (0) printf(__VA_ARGS__); } while (0)
+#endif
+
+static inline void dprintf_decode(const char *s, uint32_t len)
+{
+	uint32_t i;
+	for (i = 0; i < len; ++i)
+		if (s[i] < 0x20 || s[i] >= 0x7f)
+			dprintf("\\%o", s[i]);
+		else
+			dprintf("%c", s[i]);
+}
+
 static inline uint32_t bfin_write_emudat(uint32_t emudat)
 {
 	__asm__ __volatile__("emudat = %0;" : : "d"(emudat));
@@ -45,29 +62,55 @@ static bool jtag_write_emudat(uint32_t emudat)
 /* Transmit a buffer.  The format is:
  * [32bit length][actual data]
  */
-static void jtag_send(const char *c, uint32_t len)
+static void jtag_send(const char *raw_str, uint32_t len)
 {
-	uint32_t i;
+	const char *cooked_str;
+	uint32_t i, ex;
 
 	if (len == 0)
 		return;
 
+	/* Ugh, need to output \r after \n */
+	ex = 0;
+	for (i = 0; i < len; ++i)
+		if (raw_str[i] == '\n')
+			++ex;
+	if (ex) {
+		char *c = malloc(len + ex);
+		cooked_str = c;
+		for (i = 0; i < len; ++i) {
+			*c++ = raw_str[i];
+			if (raw_str[i] == '\n')
+				*c++ = '\r';
+		}
+		len += ex;
+	} else
+		cooked_str = raw_str;
+
+	dprintf("%s(\"", __func__);
+	dprintf_decode(cooked_str, len);
+	dprintf("\", %i)\n", len);
+
 	/* First send the length */
 	if (jtag_write_emudat(len))
-		return;
+		goto done;
 
 	/* Then send the data */
 	for (i = 0; i < len; i += 4) {
 		uint32_t emudat =
-			(c[i + 0] <<  0) |
-			(c[i + 1] <<  8) |
-			(c[i + 2] << 16) |
-			(c[i + 3] << 24);
+			(cooked_str[i + 0] <<  0) |
+			(cooked_str[i + 1] <<  8) |
+			(cooked_str[i + 2] << 16) |
+			(cooked_str[i + 3] << 24);
 		if (jtag_write_emudat(emudat)) {
 			bfin_write_emudat(0);
-			return;
+			goto done;
 		}
 	}
+
+ done:
+	if (cooked_str != raw_str)
+		free((char *)cooked_str);
 }
 static void jtag_putc(const char c)
 {
@@ -83,7 +126,10 @@ static size_t inbound_len, leftovers_len;
 /* Lower layers want to know when jtag has data */
 static int jtag_tstc_dbg(void)
 {
-	return (bfin_read_DBGSTAT() & 0x2);
+	int ret = (bfin_read_DBGSTAT() & 0x2);
+	if (ret)
+		dprintf("%s: ret:%i\n", __func__, ret);
+	return ret;
 }
 
 /* Higher layers want to know when any data is available */
@@ -101,6 +147,9 @@ static int jtag_getc(void)
 	int ret;
 	uint32_t emudat;
 
+	dprintf("%s: inlen:%zu leftlen:%zu left:%x\n", __func__,
+		inbound_len, leftovers_len, leftovers);
+
 	/* see if any data is left over */
 	if (leftovers_len) {
 		--leftovers_len;