[PATCH u-boot-marvell 25/29] tools: kwboot: Support higher baudrates when booting via UART

Marek Behún marek.behun at nic.cz
Wed Aug 25 15:46:30 CEST 2021


From: Pali Rohár <pali at kernel.org>

Add support for uploading the boot image (the data part only) at higher
baudrate than the standard one.

The kwboot utility already has -B option, but choosing other baudrate
than the standard one (115200 Bd) can only work for debug mode, not for
booting the device. The BootROM for kwboot supported platforms (Orion,
Kirkwood, Dove, Discovery, AXP, A37x, A38x, A39x) cannot change the
baudrate when uploading boot image via the Xmodem protocol, nor can it
be configured via strapping pins.

So instead we add this support by injecting baudrate changing code into
the kwbimage v1 header as a new optional binary extension. This code is
executed by BootROM after it receives the whole header. The code sends
the magic string "$baudratechange\0" just before changing the baudrate
to let kwboot know that it should also change it. This is because the
injected code is run as the last binary extension, and we do not want
to loose possible output from other possible binary extensions that
came before it (in most cases this is U-Boot SPL).

We also inject the code before the payload (the data part of the image),
to change the baudrate back to the standard value, in case the payload
does not reset UART.

This change improves boot time via UART significantly (depending on the
chosen baudrate), which is very useful when debugging.

Signed-off-by: Pali Rohár <pali at kernel.org>
[ major refactor ]
Signed-off-by: Marek Behún <marek.behun at nic.cz>
---
 tools/kwboot.c | 617 ++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 589 insertions(+), 28 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c
index 6d7d812416..dbe59833eb 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -70,6 +70,186 @@ struct kwboot_block {
 #define KWBOOT_BLK_RSP_TIMEO 1000 /* ms */
 #define KWBOOT_HDR_RSP_TIMEO 10000 /* ms */
 
+/* ARM code making baud rate changing function return to _start. */
+static unsigned char kwboot_pre_baud_code[] = {
+	0xf8, 0xe0, 0x8f, 0xe2, /* adr lr, _start */
+};
+
+/* ARM code for binary header injection to change baudrate */
+static unsigned char kwboot_baud_code[] = {
+				/* ; #define UART_BASE 0xd0012000             */
+				/* ; #define THR       0x00                   */
+				/* ; #define DLL       0x00                   */
+				/* ; #define DLH       0x04                   */
+				/* ; #define LCR       0x0c                   */
+				/* ; #define   DLAB    0x80                   */
+				/* ; #define LSR       0x14                   */
+				/* ; #define   THRE    0x20                   */
+				/* ; #define   TEMT    0x40                   */
+				/* ; #define DIV_ROUND(a, b) ((a + b/2) / b)  */
+				/* ;                                          */
+				/* ; u32 set_baudrate(u32 old_b, u32 new_b) { */
+				/* ;   const u8 *str = "$baudratechange";     */
+				/* ;   u8 c;                                  */
+				/* ;   do {                                   */
+				/* ;       c = *str++;                        */
+				/* ;       writel(UART_BASE + THR, c);        */
+				/* ;   } while (c);                           */
+				/* ;   while                                  */
+				/* ;      (!(readl(UART_BASE + LSR) & TEMT)); */
+				/* ;   u32 lcr = readl(UART_BASE + LCR);      */
+				/* ;   writel(UART_BASE + LCR, lcr | DLAB);   */
+				/* ;   u8 old_dll = readl(UART_BASE + DLL);   */
+				/* ;   u8 old_dlh = readl(UART_BASE + DLH);   */
+				/* ;   u16 old_dl = old_dll | (old_dlh << 8); */
+				/* ;   u32 clk = old_b * old_dl;              */
+				/* ;   u16 new_dl = DIV_ROUND(clk, new_b);    */
+				/* ;   u8 new_dll = new_dl & 0xff;            */
+				/* ;   u8 new_dlh = (new_dl >> 8) & 0xff;     */
+				/* ;   writel(UART_BASE + DLL, new_dll);      */
+				/* ;   writel(UART_BASE + DLH, new_dlh);      */
+				/* ;   writel(UART_BASE + LCR, lcr & ~DLAB);  */
+				/* ;   msleep(1);                             */
+				/* ;   return 0;                              */
+				/* ; }                                        */
+
+	0xfe, 0x5f, 0x2d, 0xe9, /* push  { r1 - r12, lr }                     */
+
+				/*  ; r0 = UART_BASE                          */
+	0x02, 0x0a, 0xa0, 0xe3, /* mov   r0, #0x2000                          */
+	0x01, 0x00, 0x4d, 0xe3, /* movt  r0, #0xd001                          */
+
+				/*  ; r2 = address of preamble string         */
+	0xd0, 0x20, 0x8f, 0xe2, /* adr   r2, preamble                         */
+
+				/*  ; Send preamble string over UART          */
+				/* .Lloop_preamble:                           */
+				/*                                            */
+				/*  ; Wait until Transmitter Holding is Empty */
+				/* .Lloop_thre:                               */
+				/*  ; r1 = UART_BASE[LSR] & THRE              */
+	0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
+	0x20, 0x00, 0x11, 0xe3, /* tst   r1, #0x20                            */
+	0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_thre                          */
+
+				/*  ; Put character into Transmitter FIFO     */
+				/*  ; r1 = *r2++                              */
+	0x01, 0x10, 0xd2, 0xe4, /* ldrb  r1, [r2], #1                         */
+				/*  ; UART_BASE[THR] = r1                     */
+	0x00, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0]                       */
+
+				/*  ; Loop until end of preamble string       */
+	0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
+	0xf8, 0xff, 0xff, 0x1a, /* bne   .Lloop_preamble                      */
+
+				/*  ; Wait until Transmitter FIFO is Empty    */
+				/* .Lloop_txempty:                            */
+				/*  ; r1 = UART_BASE[LSR] & TEMT              */
+	0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
+	0x40, 0x00, 0x11, 0xe3, /* tst   r1, #0x40                            */
+	0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_txempty                       */
+
+				/*  ; Set Divisor Latch Access Bit            */
+				/*  ; UART_BASE[LCR] |= DLAB                  */
+	0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
+	0x80, 0x10, 0x81, 0xe3, /* orr   r1, r1, #0x80                        */
+	0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
+
+				/*  ; Read current Divisor Latch              */
+				/*  ; r1 = UART_BASE[DLH]<<8 | UART_BASE[DLL] */
+	0x00, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x00]                      */
+	0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
+	0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
+	0x04, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x04]                      */
+	0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
+	0x41, 0x14, 0xa0, 0xe1, /* asr   r1, r1, #8                           */
+	0x02, 0x10, 0x81, 0xe1, /* orr   r1, r1, r2                           */
+
+				/*  ; Read old baudrate value                 */
+				/*  ; r2 = old_baudrate                       */
+	0x8c, 0x20, 0x9f, 0xe5, /* ldr   r2, old_baudrate                     */
+
+				/*  ; Calculate base clock                    */
+				/*  ; r1 = r2 * r1                            */
+	0x92, 0x01, 0x01, 0xe0, /* mul   r1, r2, r1                           */
+
+				/*  ; Read new baudrate value                 */
+				/*  ; r2 = baudrate                           */
+	0x88, 0x20, 0x9f, 0xe5, /* ldr   r2, baudrate                         */
+
+				/*  ; Calculate new Divisor Latch             */
+				/*  ; r1 = DIV_ROUND(r1, r2) =                */
+				/*  ;    = (r1 + r2/2) / r2                   */
+	0xa2, 0x10, 0x81, 0xe0, /* add   r1, r1, r2, lsr #1                   */
+	0x02, 0x40, 0xa0, 0xe1, /* mov   r4, r2                               */
+	0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
+				/* .Lloop_div1:                               */
+	0x84, 0x40, 0xa0, 0x91, /* movls r4, r4, lsl #1                       */
+	0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
+	0xfc, 0xff, 0xff, 0x9a, /* bls   .Lloop_div1                          */
+	0x00, 0x30, 0xa0, 0xe3, /* mov   r3, #0                               */
+				/* .Lloop_div2:                               */
+	0x04, 0x00, 0x51, 0xe1, /* cmp   r1, r4                               */
+	0x04, 0x10, 0x41, 0x20, /* subhs r1, r1, r4                           */
+	0x03, 0x30, 0xa3, 0xe0, /* adc   r3, r3, r3                           */
+	0xa4, 0x40, 0xa0, 0xe1, /* mov   r4, r4, lsr #1                       */
+	0x02, 0x00, 0x54, 0xe1, /* cmp   r4, r2                               */
+	0xf9, 0xff, 0xff, 0x2a, /* bhs   .Lloop_div2                          */
+	0x03, 0x10, 0xa0, 0xe1, /* mov   r1, r3                               */
+
+				/*  ; Set new Divisor Latch Low               */
+				/*  ; UART_BASE[DLL] = r1 & 0xff              */
+	0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
+	0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
+	0x00, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x00]                      */
+
+				/*  ; Set new Divisor Latch High              */
+				/*  ; UART_BASE[DLH] = r1>>8 & 0xff           */
+	0x41, 0x24, 0xa0, 0xe1, /* asr   r2, r1, #8                           */
+	0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
+	0x04, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x04]                      */
+
+				/*  ; Clear Divisor Latch Access Bit          */
+				/*  ; UART_BASE[LCR] &= ~DLAB                 */
+	0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
+	0x80, 0x10, 0xc1, 0xe3, /* bic   r1, r1, #0x80                        */
+	0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
+
+				/*  ; Sleep 1ms ~~ 600000 cycles at 1200 MHz  */
+				/*  ; r1 = 600000                             */
+	0x9f, 0x1d, 0xa0, 0xe3, /* mov   r1, #0x27c0                          */
+	0x09, 0x10, 0x40, 0xe3, /* movt  r1, #0x0009                          */
+				/* .Lloop_sleep:                              */
+	0x01, 0x10, 0x41, 0xe2, /* sub   r1, r1, #1                           */
+	0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
+	0xfc, 0xff, 0xff, 0x1a, /* bne   .Lloop_sleep                         */
+
+				/*  ; Return 0 - no error                     */
+	0x00, 0x00, 0xa0, 0xe3, /* mov   r0, #0                               */
+	0xfe, 0x9f, 0xbd, 0xe8, /* pop   { r1 - r12, pc }                     */
+
+				/*  ; Preamble string                         */
+				/* preamble:                                  */
+	0x24, 0x62, 0x61, 0x75, /* .asciz "$baudratechange"                   */
+	0x64, 0x72, 0x61, 0x74,
+	0x65, 0x63, 0x68, 0x61,
+	0x6e, 0x67, 0x65, 0x00,
+
+				/*  ; Placeholder for old baudrate value      */
+				/* old_baudrate:                              */
+	0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
+
+				/*  ; Placeholder for new baudrate value      */
+				/* new_baudrate:                              */
+	0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
+				/* _start:                                    */
+};
+
+#define KWBOOT_BAUDRATE_BIN_HEADER_SZ (sizeof(kwboot_baud_code) + \
+				       sizeof(struct opt_hdr_v1) + 8)
+
+static const char kwb_baud_magic[16] = "$baudratechange";
+
 static int kwboot_verbose;
 
 static int msg_req_delay = KWBOOT_MSG_REQ_DELAY;
@@ -233,26 +413,184 @@ kwboot_tty_send_char(int fd, unsigned char c)
 }
 
 static speed_t
-kwboot_tty_speed(int baudrate)
+kwboot_tty_baudrate_to_speed(int baudrate)
 {
 	switch (baudrate) {
+#ifdef B4000000
+	case 4000000:
+		return B4000000;
+#endif
+#ifdef B3500000
+	case 3500000:
+		return B3500000;
+#endif
+#ifdef B3000000
+	case 3000000:
+		return B3000000;
+#endif
+#ifdef B2500000
+	case 2500000:
+		return B2500000;
+#endif
+#ifdef B2000000
+	case 2000000:
+		return B2000000;
+#endif
+#ifdef B1500000
+	case 1500000:
+		return B1500000;
+#endif
+#ifdef B1152000
+	case 1152000:
+		return B1152000;
+#endif
+#ifdef B1000000
+	case 1000000:
+		return B1000000;
+#endif
+#ifdef B921600
+	case 921600:
+		return B921600;
+#endif
+#ifdef B614400
+	case 614400:
+		return B614400;
+#endif
+#ifdef B576000
+	case 576000:
+		return B576000;
+#endif
+#ifdef B500000
+	case 500000:
+		return B500000;
+#endif
+#ifdef B460800
+	case 460800:
+		return B460800;
+#endif
+#ifdef B307200
+	case 307200:
+		return B307200;
+#endif
+#ifdef B230400
+	case 230400:
+		return B230400;
+#endif
+#ifdef B153600
+	case 153600:
+		return B153600;
+#endif
+#ifdef B115200
 	case 115200:
 		return B115200;
+#endif
+#ifdef B76800
+	case 76800:
+		return B76800;
+#endif
+#ifdef B57600
 	case 57600:
 		return B57600;
+#endif
+#ifdef B38400
 	case 38400:
 		return B38400;
+#endif
+#ifdef B19200
 	case 19200:
 		return B19200;
+#endif
+#ifdef B9600
 	case 9600:
 		return B9600;
+#endif
+#ifdef B4800
+	case 4800:
+		return B4800;
+#endif
+#ifdef B2400
+	case 2400:
+		return B2400;
+#endif
+#ifdef B1800
+	case 1800:
+		return B1800;
+#endif
+#ifdef B1200
+	case 1200:
+		return B1200;
+#endif
+#ifdef B600
+	case 600:
+		return B600;
+#endif
+#ifdef B300
+	case 300:
+		return B300;
+#endif
+#ifdef B200
+	case 200:
+		return B200;
+#endif
+#ifdef B150
+	case 150:
+		return B150;
+#endif
+#ifdef B134
+	case 134:
+		return B134;
+#endif
+#ifdef B110
+	case 110:
+		return B110;
+#endif
+#ifdef B75
+	case 75:
+		return B75;
+#endif
+#ifdef B50
+	case 50:
+		return B50;
+#endif
+	default:
+		return B0;
 	}
+}
+
+static int
+kwboot_tty_change_baudrate(int fd, int baudrate)
+{
+	struct termios tio;
+	speed_t speed;
+	int rc;
+
+	rc = tcgetattr(fd, &tio);
+	if (rc)
+		return rc;
 
-	return -1;
+	speed = kwboot_tty_baudrate_to_speed(baudrate);
+	if (speed == B0) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	rc = cfsetospeed(&tio, speed);
+	if (rc)
+		return rc;
+
+	rc = cfsetispeed(&tio, speed);
+	if (rc)
+		return rc;
+
+	rc = tcsetattr(fd, TCSANOW, &tio);
+	if (rc)
+		return rc;
+
+	return 0;
 }
 
 static int
-kwboot_open_tty(const char *path, speed_t speed)
+kwboot_open_tty(const char *path, int baudrate)
 {
 	int rc, fd;
 	struct termios tio;
@@ -271,13 +609,14 @@ kwboot_open_tty(const char *path, speed_t speed)
 	tio.c_cc[VMIN] = 1;
 	tio.c_cc[VTIME] = 10;
 
-	cfsetospeed(&tio, speed);
-	cfsetispeed(&tio, speed);
-
 	rc = tcsetattr(fd, TCSANOW, &tio);
 	if (rc)
 		goto out;
 
+	rc = kwboot_tty_change_baudrate(fd, baudrate);
+	if (rc)
+		goto out;
+
 	rc = fd;
 out:
 	if (rc < 0) {
@@ -424,13 +763,40 @@ _now(void)
 	return ts.tv_sec * 1000 + (ts.tv_nsec + 500000) / 1000000;
 }
 
+static int
+kwboot_baud_magic_handle(int fd, char c, int baudrate)
+{
+	static size_t rcv_len;
+
+	if (rcv_len < sizeof(kwb_baud_magic)) {
+		/* try to recognize whole magic word */
+		if (c == kwb_baud_magic[rcv_len]) {
+			rcv_len++;
+		} else {
+			printf("%.*s%c", (int)rcv_len, kwb_baud_magic, c);
+			fflush(stdout);
+			rcv_len = 0;
+		}
+	}
+
+	if (rcv_len == sizeof(kwb_baud_magic)) {
+		/* magic word received */
+		kwboot_printv("\nChanging baudrate to %d Bd\n", baudrate);
+
+		return kwboot_tty_change_baudrate(fd, baudrate) ? : 1;
+	} else {
+		return 0;
+	}
+}
+
 static int
 kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm,
-		    int *done_print)
+		    int *done_print, int baudrate)
 {
 	int recv_timeo = allow_non_xm ? KWBOOT_HDR_RSP_TIMEO : blk_rsp_timeo;
 	int non_xm_print = 0;
 	uint64_t timeout = 0;
+	int baud_changed = 0;
 	int rc, retries;
 	int err;
 	char c;
@@ -454,21 +820,41 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm,
 			rc = kwboot_tty_recv(fd, &c, 1, recv_timeo);
 			if (rc) {
 				if (errno != ETIMEDOUT) {
-					return rc;
+					goto can;
 				} else if (timeout && timeout < _now()) {
 					errno = ETIMEDOUT;
-					return -1;
+					goto can;
 				} else {
 					err = errno;
 					c = NAK;
 				}
 			}
 
+			/*
+			 * After whole header is sent, but before ACK of the last
+			 * block is received, we receive possible UART output
+			 * from header, which we want to print, unless it is the
+			 * magic string informing us about baudrate change (as
+			 * was requested via -B option). So we need to search for
+			 * this magic string, and when found, not print it, but
+			 * instead change the baudrate.
+			 */
 			if (!_is_xm_reply(c) && allow_non_xm) {
 				timeout = _now() + recv_timeo;
-				putchar(c);
-				fflush(stdout);
-				non_xm_print = 1;
+				if (baudrate && !baud_changed) {
+					rc = kwboot_baud_magic_handle(fd, c,
+								      baudrate);
+					if (rc == 1)
+						baud_changed = 1;
+					else if (!rc)
+						non_xm_print = 1;
+					else
+						goto can;
+				} else if (!baudrate || !baud_changed) {
+					putchar(c);
+					fflush(stdout);
+					non_xm_print = 1;
+				}
 			}
 		} while (!_is_xm_reply(c));
 
@@ -484,7 +870,20 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm,
 		return -1;
 	}
 
+	if (allow_non_xm && baudrate && !baud_changed) {
+		fprintf(stderr, "Baudrate was not changed\n");
+		rc = -1;
+		errno = EPROTO;
+		goto can;
+	}
+
 	return _xm_reply_to_error(c);
+can:
+	err = errno;
+	kwboot_tty_send_char(fd, CAN);
+	kwboot_printv("\n");
+	errno = err;
+	return rc;
 }
 
 static int
@@ -514,7 +913,7 @@ kwboot_xm_finish(int tty)
 
 static int
 kwboot_xmodem_one(int tty, int *pnum, int header, const uint8_t *data,
-		  size_t size)
+		  size_t size, int baudrate)
 {
 	int done_print = 0;
 	size_t sent, left;
@@ -537,7 +936,7 @@ kwboot_xmodem_one(int tty, int *pnum, int header, const uint8_t *data,
 		last_block = (left <= blksz);
 
 		rc = kwboot_xm_sendblock(tty, &block, header && last_block,
-					 &done_print);
+					 &done_print, baudrate);
 		if (rc)
 			goto out;
 
@@ -558,7 +957,7 @@ out:
 }
 
 static int
-kwboot_xmodem(int tty, const void *_img, size_t size)
+kwboot_xmodem(int tty, const void *_img, size_t size, int baudrate)
 {
 	const uint8_t *img = _img;
 	int rc, pnum;
@@ -572,19 +971,42 @@ kwboot_xmodem(int tty, const void *_img, size_t size)
 
 	pnum = 1;
 
-	rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz);
+	rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz, baudrate);
 	if (rc)
 		return rc;
 
 	img += hdrsz;
 	size -= hdrsz;
 
-	rc = kwboot_xmodem_one(tty, &pnum, 0, img, size);
+	rc = kwboot_xmodem_one(tty, &pnum, 0, img, size, 0);
 	if (rc)
 		return rc;
 
 	kwboot_printv("Finishing transfer\n");
-	return kwboot_xm_finish(tty);
+	rc = kwboot_xm_finish(tty);
+	if (rc)
+		return rc;
+
+	if (baudrate) {
+		char buf[sizeof(kwb_baud_magic)];
+
+		/* Wait 1s for baudrate change magic */
+		rc = kwboot_tty_recv(tty, buf, sizeof(buf), 1000);
+		if (rc)
+			return rc;
+
+		if (memcmp(buf, kwb_baud_magic, sizeof(buf))) {
+			errno = EPROTO;
+			return -1;
+		}
+
+		kwboot_printv("\nChanging baudrate back to 115200 Bd\n\n");
+		rc = kwboot_tty_change_baudrate(tty, 115200);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
 }
 
 static int
@@ -765,6 +1187,37 @@ kwboot_img_is_secure(void *img)
 	return 0;
 }
 
+static void *
+kwboot_img_grow_data_left(void *img, size_t *size, size_t grow)
+{
+	uint32_t hdrsz, datasz, srcaddr;
+	struct main_hdr_v1 *hdr = img;
+	uint8_t *data;
+
+	srcaddr = le32_to_cpu(hdr->srcaddr);
+
+	hdrsz = kwbheader_size(hdr);
+	data = (uint8_t *)img + srcaddr;
+	datasz = *size - srcaddr;
+
+	/* only move data if there is not enough space */
+	if (hdrsz + grow > srcaddr) {
+		size_t need = hdrsz + grow - srcaddr;
+
+		/* move data by enough bytes */
+		memmove(data + need, data, datasz);
+		*size += need;
+		srcaddr += need;
+	}
+
+	srcaddr -= grow;
+	hdr->srcaddr = cpu_to_le32(srcaddr);
+	hdr->destaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) - grow);
+	hdr->execaddr = cpu_to_le32(le32_to_cpu(hdr->execaddr) - grow);
+
+	return (uint8_t *)img + srcaddr;
+}
+
 static void
 kwboot_img_grow_hdr(void *img, size_t *size, size_t grow)
 {
@@ -798,8 +1251,62 @@ kwboot_img_grow_hdr(void *img, size_t *size, size_t grow)
 	}
 }
 
+static void *
+kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
+{
+	struct main_hdr_v1 *hdr = img;
+	struct opt_hdr_v1 *ohdr;
+	uint32_t ohdrsz;
+
+	ohdrsz = binsz + 8 + sizeof(*ohdr);
+	kwboot_img_grow_hdr(img, size, ohdrsz);
+
+	if (hdr->ext & 0x1) {
+		for_each_opt_hdr_v1 (ohdr, img)
+			if (opt_hdr_v1_next(ohdr) == NULL)
+				break;
+
+		*opt_hdr_v1_ext(ohdr) |= 1;
+		ohdr = opt_hdr_v1_next(ohdr);
+	} else {
+		hdr->ext |= 1;
+		ohdr = (void *)(hdr + 1);
+	}
+
+	ohdr->headertype = OPT_HDR_V1_BINARY_TYPE;
+	ohdr->headersz_msb = ohdrsz >> 16;
+	ohdr->headersz_lsb = cpu_to_le16(ohdrsz & 0xffff);
+
+	memset(&ohdr->data[0], 0, ohdrsz - sizeof(*ohdr));
+
+	return &ohdr->data[4];
+}
+
+static void
+_copy_baudrate_change_code(void *dst, int pre, int old_baud, int new_baud)
+{
+	size_t codesz = sizeof(kwboot_baud_code);
+	uint8_t *code = dst;
+
+	if (pre) {
+		size_t presz = sizeof(kwboot_pre_baud_code);
+
+		/*
+		 * We need to prepend code that loads lr register with value
+		 * just after the baudrate change code so that when baud rate
+		 * changing function returns, it returns to original payload.
+		 */
+		memcpy(code, kwboot_pre_baud_code, presz);
+		code += presz;
+	}
+
+	memcpy(code, kwboot_baud_code, codesz - 8);
+	*(uint32_t *)(code + codesz - 8) = cpu_to_le32(old_baud);
+	*(uint32_t *)(code + codesz - 4) = cpu_to_le32(new_baud);
+}
+
 static int
-kwboot_img_patch_hdr(void *img, size_t *size)
+kwboot_img_patch(void *img, size_t *size, int baudrate)
 {
 	int rc;
 	struct main_hdr_v1 *hdr;
@@ -895,6 +1402,51 @@ kwboot_img_patch_hdr(void *img, size_t *size)
 
 	hdr->blockid = IBR_HDR_UART_ID;
 
+	if (baudrate) {
+		uint32_t codesz = sizeof(kwboot_baud_code);
+		void *code;
+
+		if (image_ver == 0) {
+			fprintf(stderr,
+				"Cannot inject code for changing baudrate into v0 image header\n");
+			errno = EINVAL;
+			goto out;
+		}
+
+		if (is_secure) {
+			fprintf(stderr,
+				"Cannot inject code for changing baudrate into image with secure header\n");
+			errno = EINVAL;
+			goto out;
+		}
+
+		/*
+		 * First inject code that changes the baudrate from the default
+		 * value of 115200 Bd to requested value. This code is inserted
+		 * as a new opt hdr, so it is executed by BootROM after the
+		 * header part is received.
+		 */
+		kwboot_printv("Injecting binary header code for changing baudrate to %d Bd\n",
+			      baudrate);
+
+		code = kwboot_add_bin_ohdr_v1(img, size, codesz);
+		_copy_baudrate_change_code(code, 0, 115200, baudrate);
+
+		/*
+		 * Now inject code that changes the baudrate back to 115200 Bd.
+		 * This code is prepended to the data part of the image, so it
+		 * is executed before U-Boot proper.
+		 */
+		kwboot_printv("Injecting code for changing baudrate back\n");
+
+		codesz += sizeof(kwboot_pre_baud_code);
+		code = kwboot_img_grow_data_left(img, size, codesz);
+		_copy_baudrate_change_code(code, 1, baudrate, 115200);
+
+		/* recompute header size */
+		hdrsz = kwbheader_size(hdr);
+	}
+
 	if (hdrsz % KWBOOT_XM_BLKSZ) {
 		size_t offset = (KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ) %
 				KWBOOT_XM_BLKSZ;
@@ -951,7 +1503,8 @@ main(int argc, char **argv)
 	void *debugmsg;
 	void *img;
 	size_t size;
-	speed_t speed;
+	size_t after_img_rsv;
+	int baudrate;
 
 	rv = 1;
 	tty = -1;
@@ -961,7 +1514,8 @@ main(int argc, char **argv)
 	img = NULL;
 	term = 0;
 	size = 0;
-	speed = B115200;
+	after_img_rsv = KWBOOT_XM_BLKSZ;
+	baudrate = 115200;
 
 	kwboot_verbose = isatty(STDOUT_FILENO);
 
@@ -1011,9 +1565,7 @@ main(int argc, char **argv)
 			break;
 
 		case 'B':
-			speed = kwboot_tty_speed(atoi(optarg));
-			if (speed == -1)
-				goto usage;
+			baudrate = atoi(optarg);
 			break;
 
 		case 'h':
@@ -1031,20 +1583,29 @@ main(int argc, char **argv)
 
 	ttypath = argv[optind++];
 
-	tty = kwboot_open_tty(ttypath, speed);
+	tty = kwboot_open_tty(ttypath, imgpath ? 115200 : baudrate);
 	if (tty < 0) {
 		perror(ttypath);
 		goto out;
 	}
 
+	if (baudrate == 115200)
+		/* do not change baudrate during Xmodem to the same value */
+		baudrate = 0;
+	else
+		/* ensure we have enough space for baudrate change code */
+		after_img_rsv += KWBOOT_BAUDRATE_BIN_HEADER_SZ +
+				 sizeof(kwboot_pre_baud_code) +
+				 sizeof(kwboot_baud_code);
+
 	if (imgpath) {
-		img = kwboot_read_image(imgpath, &size, KWBOOT_XM_BLKSZ);
+		img = kwboot_read_image(imgpath, &size, after_img_rsv);
 		if (!img) {
 			perror(imgpath);
 			goto out;
 		}
 
-		rc = kwboot_img_patch_hdr(img, &size);
+		rc = kwboot_img_patch(img, &size, baudrate);
 		if (rc) {
 			fprintf(stderr, "%s: Invalid image.\n", imgpath);
 			goto out;
@@ -1066,7 +1627,7 @@ main(int argc, char **argv)
 	}
 
 	if (img) {
-		rc = kwboot_xmodem(tty, img, size);
+		rc = kwboot_xmodem(tty, img, size, baudrate);
 		if (rc) {
 			perror("xmodem");
 			goto out;
-- 
2.31.1



More information about the U-Boot mailing list