Blackfin: jtag-console: handle newline stuffing

Serial devices currently have to manually stuff \r after every \n found,
but this is a bit more difficult with the jtag console since we process
everything in chunks of 4 bit.  So we have to scan & stuff the whole
string rather than what most serial drivers do which is output on a byte
per byte basis.

Signed-off-by: Mike Frysinger <vapier@gentoo.org>

diff --git a/arch/blackfin/cpu/jtag-console.c b/arch/blackfin/cpu/jtag-console.c
index bde8beeca54a3864dabec9fae0df2e4d44b33337..e0f2975603b67abd5785eac9ac99377237620245 100644 (file)
--- a/arch/blackfin/cpu/jtag-console.c
+++ b/arch/blackfin/cpu/jtag-console.c
@@ -7,6 +7,7 @@
  */
 
 #include <common.h>
+#include <malloc.h>
 #include <stdio_dev.h>
 #include <asm/blackfin.h>
 
@@ -61,33 +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(c, len);
+       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)
 {