diff --git a/tools/kwboot.c b/tools/kwboot.c
index e773f01..c47970e 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -1,5 +1,6 @@
 /*
- * Boot a Marvell Kirkwood SoC, with Xmodem over UART0.
+ * Boot a Marvell SoC, with Xmodem over UART0.
+ *  supports Kirkwood, Dove, Armada 370, Armada XP
  *
  * (c) 2012 Daniel Stodden <daniel.stodden@gmail.com>
  *
@@ -37,8 +38,12 @@ static unsigned char kwboot_msg_boot[] = {
 	0xBB, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77
 };
 
-#define KWBOOT_MSG_REQ_DELAY	10 /* ms */
-#define KWBOOT_MSG_RSP_TIMEO	50 /* ms */
+static unsigned char kwboot_msg_debug[] = {
+	0xDD, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77
+};
+
+#define KWBOOT_MSG_REQ_DELAY	1000 /* ms */
+#define KWBOOT_MSG_RSP_TIMEO	1000 /* ms */
 
 /*
  * Xmodem Transfers
@@ -184,6 +189,9 @@ kwboot_tty_send(int fd, const void *buf, size_t len)
 	int rc;
 	ssize_t n;
 
+	if (!buf)
+		return 0;
+
 	rc = -1;
 
 	do {
@@ -268,7 +276,10 @@ kwboot_bootmsg(int tty, void *msg)
 	int rc;
 	char c;
 
-	kwboot_printv("Sending boot message. Please reboot the target...");
+	if (msg == NULL)
+		kwboot_printv("Please reboot the target into UART boot mode...");
+	else
+		kwboot_printv("Sending boot message. Please reboot the target...");
 
 	do {
 		rc = tcflush(tty, TCIOFLUSH);
@@ -293,6 +304,37 @@ kwboot_bootmsg(int tty, void *msg)
 }
 
 static int
+kwboot_debugmsg(int tty, void *msg)
+{
+	int rc;
+
+	kwboot_printv("Sending debug message. Please reboot the target...");
+
+	do {
+		char buf[16];
+
+		rc = tcflush(tty, TCIOFLUSH);
+		if (rc)
+			break;
+
+		rc = kwboot_tty_send(tty, msg, 8);
+		if (rc) {
+			usleep(KWBOOT_MSG_REQ_DELAY * 1000);
+			continue;
+		}
+
+		rc = kwboot_tty_recv(tty, buf, 16, KWBOOT_MSG_RSP_TIMEO);
+
+		kwboot_spinner();
+
+	} while (rc);
+
+	kwboot_printv("\n");
+
+	return rc;
+}
+
+static int
 kwboot_xm_makeblock(struct kwboot_block *block, const void *data,
 		    size_t size, int pnum)
 {
@@ -300,6 +342,7 @@ kwboot_xm_makeblock(struct kwboot_block *block, const void *data,
 	size_t n;
 	int i;
 
+	block->soh = SOH;
 	block->pnum = pnum;
 	block->_pnum = ~block->pnum;
 
@@ -326,9 +369,15 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block)
 		if (rc)
 			break;
 
-		rc = kwboot_tty_recv(fd, &c, 1, KWBOOT_BLK_RSP_TIMEO);
-		if (rc)
-			break;
+		do {
+			rc = kwboot_tty_recv(fd, &c, 1, KWBOOT_BLK_RSP_TIMEO);
+			if (rc)
+				break;
+
+			if (c != ACK && c != NAK && c != CAN)
+				printf("%c", c);
+
+		} while (c != ACK && c != NAK && c != CAN);
 
 		if (c != ACK)
 			kwboot_progress(-1, '+');
@@ -511,7 +560,6 @@ kwboot_mmap_image(const char *path, size_t *size, int prot)
 	void *img;
 
 	rc = -1;
-	fd = -1;
 	img = NULL;
 
 	fd = open(path, O_RDONLY);
@@ -601,11 +649,15 @@ static void
 kwboot_usage(FILE *stream, char *progname)
 {
 	fprintf(stream,
-		"Usage: %s -b <image> [ -p ] [ -t ] "
-		"[-B <baud> ] <TTY>\n", progname);
+		"Usage: %s [-d | -b <image> | -D <image> ] [ -t ] [-B <baud> ] <TTY>\n",
+		progname);
 	fprintf(stream, "\n");
-	fprintf(stream, "  -b <image>: boot <image>\n");
+	fprintf(stream,
+		"  -b <image>: boot <image> with preamble (Kirkwood, Armada 370/XP)\n");
 	fprintf(stream, "  -p: patch <image> to type 0x69 (uart boot)\n");
+	fprintf(stream,
+		"  -D <image>: boot <image> without preamble (Dove)\n");
+	fprintf(stream, "  -d: enter debug mode\n");
 	fprintf(stream, "\n");
 	fprintf(stream, "  -t: mini terminal\n");
 	fprintf(stream, "\n");
@@ -619,6 +671,7 @@ main(int argc, char **argv)
 	const char *ttypath, *imgpath;
 	int rv, rc, tty, term, prot, patch;
 	void *bootmsg;
+	void *debugmsg;
 	void *img;
 	size_t size;
 	speed_t speed;
@@ -626,6 +679,7 @@ main(int argc, char **argv)
 	rv = 1;
 	tty = -1;
 	bootmsg = NULL;
+	debugmsg = NULL;
 	imgpath = NULL;
 	img = NULL;
 	term = 0;
@@ -636,7 +690,7 @@ main(int argc, char **argv)
 	kwboot_verbose = isatty(STDOUT_FILENO);
 
 	do {
-		int c = getopt(argc, argv, "hb:ptB:");
+		int c = getopt(argc, argv, "hb:ptB:dD:");
 		if (c < 0)
 			break;
 
@@ -646,6 +700,15 @@ main(int argc, char **argv)
 			imgpath = optarg;
 			break;
 
+		case 'D':
+			bootmsg = NULL;
+			imgpath = optarg;
+			break;
+
+		case 'd':
+			debugmsg = kwboot_msg_debug;
+			break;
+
 		case 'p':
 			patch = 1;
 			break;
@@ -667,7 +730,7 @@ main(int argc, char **argv)
 		}
 	} while (1);
 
-	if (!bootmsg && !term)
+	if (!bootmsg && !term && !debugmsg)
 		goto usage;
 
 	if (patch && !imgpath)
@@ -702,7 +765,13 @@ main(int argc, char **argv)
 		}
 	}
 
-	if (bootmsg) {
+	if (debugmsg) {
+		rc = kwboot_debugmsg(tty, debugmsg);
+		if (rc) {
+			perror("debugmsg");
+			goto out;
+		}
+	} else {
 		rc = kwboot_bootmsg(tty, bootmsg);
 		if (rc) {
 			perror("bootmsg");
