The `in` variable is set to -1 in kwboot_terminal() if stdin is not a tty. In this case we should not look whether -1 is set in fd_set, for it can lead to a buffer overflow, which can be reproduced with echo "xyz" | ./tools/kwboot -t /dev/ttyUSB0

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 7feeaa45a2..e6e99849a7 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -552,7 +552,7 @@ kwboot_terminal(int tty) break; }

- if (FD_ISSET(in, &rfds)) { + if (in >= 0 && FD_ISSET(in, &rfds)) { rc = kwboot_term_pipe(in, tty, quit, &s); if (rc) break;

There are 3 instances in kwboot.c where we need to write() a given buffer whole (iteratively writing until all data are written), and 2 of those instances are wrong, for they do not increment the buffer pointer.

Refactor the code into a new function kwboot_write() where it is fixed.

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 55 ++++++++++++++++++++++++-------------------------- 1 file changed, 26 insertions(+), 29 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index f18f6d2134..22cdd137ad 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -72,6 +72,23 @@ static int msg_req_delay = KWBOOT_MSG_REQ_DELAY; static int msg_rsp_timeo = KWBOOT_MSG_RSP_TIMEO; static int blk_rsp_timeo = KWBOOT_BLK_RSP_TIMEO;

+static ssize_t +kwboot_write(int fd, const char *buf, size_t len) +{ + size_t tot = 0; + + while (tot < len) { + ssize_t wr = write(fd, buf + tot, len - tot); + + if (wr < 0) + return -1; + + tot += wr; + } + + return tot; +} + static void kwboot_printv(const char *fmt, ...) { @@ -191,26 +208,13 @@ out: static int kwboot_tty_send(int fd, const void *buf, size_t len) { - int rc; - ssize_t n; - if (!buf) return 0;

- rc = -1; - - do { - n = write(fd, buf, len); - if (n < 0) - goto out; - - buf = (char *)buf + n; - len -= n; - } while (len > 0); + if (kwboot_write(fd, buf, len) < 0) + return -1;

- rc = tcdrain(fd); -out: - return rc; + return tcdrain(fd); }

static int @@ -462,7 +466,7 @@ can: static int kwboot_term_pipe(int in, int out, const char *quit, int *s) { - ssize_t nin, nout; + ssize_t nin; char _buf[128], *buf = _buf;

nin = read(in, buf, sizeof(_buf)); @@ -480,22 +484,15 @@ kwboot_term_pipe(int in, int out, const char *quit, int *s) buf++; nin--; } else { - while (*s > 0) { - nout = write(out, quit, *s); - if (nout <= 0) - return -1; - (*s) -= nout; - } + if (kwboot_write(out, quit, *s) < 0) + return -1; + *s = 0; } } }

- while (nin > 0) { - nout = write(out, buf, nin); - if (nout <= 0) - return -1; - nin -= nout; - } + if (kwboot_write(out, buf, nin) < 0) + return -1;

return 0; }

From: Pali Rohár pali@kernel.org

When kwboot_tty_recv() fails or times out, it does not set the `c` variable to NAK. The variable is then compared, while it holds either an undefined value or a value from previous iteration. Set `c` to NAK so that the other side will try to resend current block, and remove the now unnecessary break.

In other failure cases return immediately.

Signed-off-by: Pali Rohár pali@kernel.org Reviewed-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 454339db14..b9a402ca91 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -380,12 +380,15 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block) do { rc = kwboot_tty_send(fd, block, sizeof(*block)); if (rc) - break; + return rc;

do { rc = kwboot_tty_recv(fd, &c, 1, blk_rsp_timeo); - if (rc) - break; + if (rc) { + if (errno != ETIMEDOUT) + return rc; + c = NAK; + }

if (c != ACK && c != NAK && c != CAN) printf("%c", c);

The compiler complains that we are comparing int with size_t when compiled with -W.

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 88353d19c0..3d9f73e697 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -352,8 +352,7 @@ kwboot_xm_makeblock(struct kwboot_block *block, const void *data, size_t size, int pnum) { const size_t blksz = sizeof(block->data); - size_t n; - int i; + size_t i, n;

block->soh = SOH; block->pnum = pnum;

From: Pali Rohár pali@kernel.org

When progress was not completed, current terminal position is in progress bar. So print newline before printing error message to make error message more readable.

Signed-off-by: Pali Rohár pali@kernel.org Reviewed-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 1 + 1 file changed, 1 insertion(+)

diff --git a/tools/kwboot.c b/tools/kwboot.c index eb4b3fe230..0e533e3698 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -459,6 +459,7 @@ kwboot_xmodem(int tty, const void *_data, size_t size) rc = kwboot_tty_send_char(tty, EOT);

out: + kwboot_printv("\n"); return rc;

can:

This is a non-functional change that should make the code more readable.

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 7f231c0823..2e5684b91c 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -375,6 +375,12 @@ kwboot_xm_makeblock(struct kwboot_block *block, const void *data, return n; }

+static int +_is_xm_reply(char c) +{ + return c == ACK || c == NAK || c == CAN; +} + static int kwboot_xm_sendblock(int fd, struct kwboot_block *block) { @@ -395,10 +401,10 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block) c = NAK; }

- if (c != ACK && c != NAK && c != CAN) + if (!_is_xm_reply(c)) printf("%c", c);

- } while (c != ACK && c != NAK && c != CAN); + } while (!_is_xm_reply(c));

if (c != ACK) kwboot_progress(-1, '+');

There is no separation between output from the code from binary header (U-Boot SPL in most cases) and subsequent kwboot output.

Print '\n' to make distinguishing these two easier.

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 5 +++++ 1 file changed, 5 insertions(+)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 2829b74a2e..2e16db83fa 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -385,6 +385,7 @@ static int kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, int *done_print) { + int non_xm_print = 0; int rc, retries; char c;

@@ -413,6 +414,7 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, if (!_is_xm_reply(c) && allow_non_xm) { putchar(c); fflush(stdout); + non_xm_print = 1; } } while (!_is_xm_reply(c));

@@ -420,6 +422,9 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, kwboot_progress(-1, '+'); } while (c == NAK && retries-- > 0);

+ if (non_xm_print) + kwboot_printv("\n"); + rc = -1;

switch (c) {

From: Pali Rohár pali@kernel.org

After kwboot sends EOT, BootROM sends back ACK. Add code for handling this and retry sending EOT on error.

Signed-off-by: Pali Rohár pali@kernel.org [ refactored ] Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 65 +++++++++++++++++++++++++++++++++++++------------- 1 file changed, 48 insertions(+), 17 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 0e60cc8a70..6674a253ff 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -383,6 +383,28 @@ _is_xm_reply(char c) return c == ACK || c == NAK || c == CAN; }

+static int +_xm_reply_to_error(int c) { + int rc = -1; + + switch (c) { + case ACK: + rc = 0; + break; + case NAK: + errno = EBADMSG; + break; + case CAN: + errno = ECANCELED; + break; + default: + errno = EPROTO; + break; + } + + return rc; +} + static uint64_t _now(void) { @@ -463,24 +485,32 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, return -1; }

- rc = -1; + return _xm_reply_to_error(c); +}

- switch (c) { - case ACK: - rc = 0; - break; - case NAK: - errno = EBADMSG; - break; - case CAN: - errno = ECANCELED; - break; - default: - errno = EPROTO; - break; - } +static int +kwboot_xm_finish(int tty) +{ + int rc, retries; + char c;

- return rc; + retries = 16; + do { + rc = kwboot_tty_send_char(tty, EOT); + if (rc) + return rc; + + do { + rc = kwboot_tty_recv(tty, &c, 1, blk_rsp_timeo); + if (rc) { + if (errno != ETIMEDOUT) + return rc; + c = NAK; + } + } while (!_is_xm_reply(c) && retries-- > 0); + } while (c == NAK && retries-- > 0); + + return _xm_reply_to_error(c); }

static int @@ -557,7 +587,8 @@ kwboot_xmodem(int tty, const void *_img, size_t size) if (rc) return rc;

- return kwboot_tty_send_char(tty, EOT); + kwboot_printv("Finishing transfer\n"); + return kwboot_xm_finish(tty); }

static int

From: Pali Rohár pali@kernel.org

It is not possible to modify image with secure header due to cryptographic signature.

Signed-off-by: Pali Rohár pali@kernel.org [ refactored ] Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 031cea7550..c23357f5bc 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -739,6 +739,18 @@ kwboot_img_csum8(void *_data, size_t size) return csum; }

+static int +kwboot_img_is_secure(void *img) +{ + struct opt_hdr_v1 *ohdr; + + for_each_opt_hdr_v1 (ohdr, img) + if (ohdr->headertype == OPT_HDR_V1_SECURE_TYPE) + return 1; + + return 0; +} + static int kwboot_img_patch_hdr(void *img, size_t size) { @@ -747,7 +759,9 @@ kwboot_img_patch_hdr(void *img, size_t size) uint8_t csum; size_t hdrsz = sizeof(*hdr); int image_ver; + int is_secure;

+ is_secure = 0; rc = -1; hdr = img;

@@ -779,9 +793,16 @@ kwboot_img_patch_hdr(void *img, size_t size) goto out; }

- if (hdr->blockid == IBR_HDR_UART_ID) { - rc = 0; - goto out; + is_secure = kwboot_img_is_secure(img); + + if (hdr->blockid != IBR_HDR_UART_ID) { + if (is_secure) { + fprintf(stderr, + "Image has secure header with signature for non-UART booting\n"); + errno = EINVAL; + goto out; + } + kwboot_printv("Patching image boot signature to UART\n"); }

hdr->blockid = IBR_HDR_UART_ID;

From: Pali Rohár pali@kernel.org

SPI/NOR kwbimage may have destination address set to 0xFFFFFFFF, which means that the image is not downloaded to DDR but rather it is executed directly from SPI/NOR. In this case execution address is set to SPI/NOR area.

When patching image to UART type, change destination and execution addresses from SPI/NOR XIP area to DDR area 0x00800000 (which is default for A38x).

Signed-off-by: Pali Rohár pali@kernel.org [ refactored ] Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 7 +++++++ 1 file changed, 7 insertions(+)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 9cc7936959..7c6ea2f75d 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -836,6 +836,13 @@ kwboot_img_patch_hdr(void *img, size_t size) if (hdr->srcaddr == 0xFFFFFFFF) hdr->srcaddr = cpu_to_le32(hdrsz); break; + + case IBR_HDR_SPI_ID: + if (hdr->destaddr == 0xFFFFFFFF) { + kwboot_printv("Patching destination and execution addresses from SPI/NOR XIP area to DDR area 0x00800000\n"); + hdr->destaddr = cpu_to_le32(0x00800000); + hdr->execaddr = cpu_to_le32(0x00800000); + } }

hdr->blockid = IBR_HDR_UART_ID;

Add functions kwbheader_size() and kwbheader_size_for_csum().

Refactor code determining header size to use these functions.

Refactor header checksum determining function.

Remove stuff that is not needed anymore.

This simplifies the code a little and fixes one instance of validating header size meant for checksum instead of whole header size.

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwbimage.c | 29 +++++++---------------------- tools/kwbimage.h | 35 +++++++++++++++++++++++------------ tools/kwboot.c | 26 +++++++++++--------------- 3 files changed, 41 insertions(+), 49 deletions(-)

diff --git a/tools/kwbimage.c b/tools/kwbimage.c index e98ef455de..d3a52df736 100644 --- a/tools/kwbimage.c +++ b/tools/kwbimage.c @@ -282,14 +282,6 @@ static uint8_t image_checksum8(void *start, uint32_t len) return csum; }

-size_t kwbimage_header_size(unsigned char *ptr) -{ - if (kwbimage_version((void *)ptr) == 0) - return sizeof(struct main_hdr_v0); - else - return KWBHEADER_V1_SIZE((struct main_hdr_v1 *)ptr); -} - /* * Verify checksum over a complete header that includes the checksum field. * Return 1 when OK, otherwise 0. @@ -300,7 +292,7 @@ static int main_hdr_checksum_ok(void *hdr) struct main_hdr_v0 *main_hdr = (struct main_hdr_v0 *)hdr; uint8_t checksum;

- checksum = image_checksum8(hdr, kwbimage_header_size(hdr)); + checksum = image_checksum8(hdr, kwbheader_size_for_csum(hdr)); /* Calculated checksum includes the header checksum field. Compensate * for that. */ @@ -1651,8 +1643,8 @@ static int kwbimage_check_image_types(uint8_t type) static int kwbimage_verify_header(unsigned char *ptr, int image_size, struct image_tool_params *params) { - uint8_t checksum; - size_t header_size = kwbimage_header_size(ptr); + size_t header_size = kwbheader_size(ptr); + uint8_t csum;

if (header_size > image_size) return -FDT_ERR_BADSTRUCTURE; @@ -1665,17 +1657,10 @@ static int kwbimage_verify_header(unsigned char *ptr, int image_size, struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;

if (mhdr->ext & 0x1) { - struct ext_hdr_v0 *ext_hdr; - - if (header_size + sizeof(*ext_hdr) > image_size) - return -FDT_ERR_BADSTRUCTURE; + struct ext_hdr_v0 *ext_hdr = (void *)(mhdr + 1);

- ext_hdr = (struct ext_hdr_v0 *) - (ptr + sizeof(struct main_hdr_v0)); - checksum = image_checksum8(ext_hdr, - sizeof(struct ext_hdr_v0) - - sizeof(uint8_t)); - if (checksum != ext_hdr->checksum) + csum = image_checksum8(ext_hdr, sizeof(*ext_hdr) - 1); + if (csum != ext_hdr->checksum) return -FDT_ERR_BADSTRUCTURE; } } else if (kwbimage_version(ptr) == 1) { @@ -1834,7 +1819,7 @@ static int kwbimage_generate(struct image_tool_params *params, static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params) { struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr; - size_t header_size = kwbimage_header_size(ptr); + size_t header_size = kwbheader_size(ptr); struct opt_hdr_v1 *ohdr; int idx = params->pflag; int cur_idx = 0; diff --git a/tools/kwbimage.h b/tools/kwbimage.h index 738034beb1..56a748d4cf 100644 --- a/tools/kwbimage.h +++ b/tools/kwbimage.h @@ -69,11 +69,6 @@ struct ext_hdr_v0 { uint8_t checksum; } __packed;

-struct kwb_header { - struct main_hdr_v0 kwb_hdr; - struct ext_hdr_v0 kwb_exthdr; -} __packed; - /* Structure of the main header, version 1 (Armada 370/38x/XP) */ struct main_hdr_v1 { uint8_t blockid; /* 0x0 */ @@ -195,13 +190,6 @@ struct register_set_hdr_v1 { #define OPT_HDR_V1_BINARY_TYPE 0x2 #define OPT_HDR_V1_REGISTER_TYPE 0x3

-#define KWBHEADER_V0_SIZE(hdr) \ - (((hdr)->ext & 0x1) ? sizeof(struct kwb_header) : \ - sizeof(struct main_hdr_v0)) - -#define KWBHEADER_V1_SIZE(hdr) \ - (((hdr)->headersz_msb << 16) | le16_to_cpu((hdr)->headersz_lsb)) - enum kwbimage_cmd { CMD_INVALID, CMD_BOOT_FROM, @@ -235,6 +223,29 @@ static inline unsigned int kwbimage_version(const void *header) return ptr[8]; }

+static inline size_t kwbheader_size(const void *header) +{ + if (kwbimage_version(header) == 0) { + const struct main_hdr_v0 *hdr = header; + + return sizeof(*hdr) + + (hdr->ext & 0x1) ? sizeof(struct ext_hdr_v0) : 0; + } else { + const struct main_hdr_v1 *hdr = header; + + return (hdr->headersz_msb << 16) | + le16_to_cpu(hdr->headersz_lsb); + } +} + +static inline size_t kwbheader_size_for_csum(const void *header) +{ + if (kwbimage_version(header) == 0) + return sizeof(struct main_hdr_v0); + else + return kwbheader_size(header); +} + static inline uint32_t opt_hdr_v1_size(const struct opt_hdr_v1 *ohdr) { return (ohdr->headersz_msb << 16) | le16_to_cpu(ohdr->headersz_lsb); diff --git a/tools/kwboot.c b/tools/kwboot.c index f3afe2d383..1481735346 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -565,10 +565,7 @@ kwboot_xmodem(int tty, const void *_img, size_t size) int rc, pnum; size_t hdrsz;

- if (kwbimage_version(img) == 0) - hdrsz = KWBHEADER_V0_SIZE((struct main_hdr_v0 *)img); - else - hdrsz = KWBHEADER_V1_SIZE((struct main_hdr_v1 *)img); + hdrsz = kwbheader_size(img);

kwboot_printv("Waiting 2s and flushing tty\n"); sleep(2); /* flush isn't effective without it */ @@ -729,9 +726,13 @@ out: }

static uint8_t -kwboot_img_csum8(void *_data, size_t size) +kwboot_hdr_csum8(const void *hdr) { - uint8_t *data = _data, csum; + const uint8_t *data = hdr; + uint8_t csum; + size_t size; + + size = kwbheader_size_for_csum(hdr);

for (csum = 0; size-- > 0; data++) csum += *data; @@ -778,17 +779,14 @@ kwboot_img_patch_hdr(void *img, size_t size) goto out; }

- if (image_ver == 0) - hdrsz = sizeof(*hdr); - else - hdrsz = KWBHEADER_V1_SIZE(hdr); + hdrsz = kwbheader_size(hdr);

if (size < hdrsz) { errno = EINVAL; goto out; }

- csum = kwboot_img_csum8(hdr, hdrsz) - hdr->checksum; + csum = kwboot_hdr_csum8(hdr) - hdr->checksum; if (csum != hdr->checksum) { errno = EINVAL; goto out; @@ -812,9 +810,7 @@ kwboot_img_patch_hdr(void *img, size_t size) hdr_v0->nandeccmode = IBR_HDR_ECC_DISABLED; hdr_v0->nandpagesize = 0;

- hdr_v0->srcaddr = hdr_v0->ext - ? sizeof(struct kwb_header) - : sizeof(*hdr_v0); + hdr_v0->srcaddr = cpu_to_le32(hdrsz); }

srcaddr = le32_to_cpu(hdr->srcaddr); @@ -847,7 +843,7 @@ kwboot_img_patch_hdr(void *img, size_t size)

hdr->blockid = IBR_HDR_UART_ID;

- hdr->checksum = kwboot_img_csum8(hdr, hdrsz) - csum; + hdr->checksum = kwboot_hdr_csum8(hdr) - csum;

rc = 0; out:

From: Pali Rohár pali@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@kernel.org [ major refactor ] Signed-off-by: Marek Behún marek.behun@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;

From: Pali Rohár pali@kernel.org

Add Pali and Marek as another authors of the kwboot utility.

Signed-off-by: Pali Rohár pali@kernel.org Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 2 ++ 1 file changed, 2 insertions(+)

diff --git a/tools/kwboot.c b/tools/kwboot.c index f60cfd130e..b183e4b35b 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -3,6 +3,8 @@ * supports Kirkwood, Dove, Armada 370, Armada XP * * (c) 2012 Daniel Stodden daniel.stodden@gmail.com + * (c) 2021 Pali Rohár pali@kernel.org + * (c) 2021 Marek Behún marek.behun@nic.cz * * References: marvell.com, "88F6180, 88F6190, 88F6192, and 88F6281 * Integrated Controller: Functional Specifications" December 2,

Create macro for_each_opt_hdr_v1 and functions opt_hdr_v1_size(), opt_hdr_v1_valid_size(), opt_hdr_v1_ext(), opt_hdr_v1_first() and opt_hdr_v1_next() to simplify iteration over version 1 optional headers.

This prevents ugly code repetition and makes it nicer to read.

Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwbimage.c | 90 +++++++++++++----------------------------------- tools/kwbimage.h | 58 +++++++++++++++++++++++++++++++ 2 files changed, 81 insertions(+), 67 deletions(-)

diff --git a/tools/kwbimage.c b/tools/kwbimage.c index ed11c835a5..6678ef15bd 100644 --- a/tools/kwbimage.c +++ b/tools/kwbimage.c @@ -1620,34 +1620,20 @@ static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd, static void kwbimage_print_header(const void *ptr) { struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr; + struct opt_hdr_v1 *ohdr;

printf("Image Type: MVEBU Boot from %s Image\n", image_boot_mode_name(mhdr->blockid)); printf("Image version:%d\n", image_version((void *)ptr)); - if (image_version((void *)ptr) == 1) { - struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;

- if (mhdr->ext & 0x1) { - struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *) - ((uint8_t *)ptr + - sizeof(*mhdr)); - - while (1) { - uint32_t ohdr_size; - - ohdr_size = (ohdr->headersz_msb << 16) | - le16_to_cpu(ohdr->headersz_lsb); - if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) { - printf("BIN Hdr Size: "); - genimg_print_size(ohdr_size - 12 - 4 * ohdr->data[0]); - } - if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1)) - break; - ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr + - ohdr_size); - } + for_each_opt_hdr_v1 (ohdr, mhdr) { + if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) { + printf("BIN Hdr Size: "); + genimg_print_size(opt_hdr_v1_size(ohdr) - 12 - + 4 * ohdr->data[0]); } } + printf("Data Size: "); genimg_print_size(mhdr->blocksize - sizeof(uint32_t)); printf("Load Address: %08x\n", mhdr->destaddr); @@ -1694,33 +1680,15 @@ static int kwbimage_verify_header(unsigned char *ptr, int image_size, } } else if (image_version((void *)ptr) == 1) { struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr; + const uint8_t *mhdr_end; + struct opt_hdr_v1 *ohdr; uint32_t offset; uint32_t size;

- if (mhdr->ext & 0x1) { - uint32_t ohdr_size; - struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *) - (ptr + sizeof(*mhdr)); - - while (1) { - if ((uint8_t *)ohdr + sizeof(*ohdr) > - (uint8_t *)mhdr + header_size) - return -FDT_ERR_BADSTRUCTURE; - - ohdr_size = (ohdr->headersz_msb << 16) | - le16_to_cpu(ohdr->headersz_lsb); - - if (ohdr_size < 8 || - (uint8_t *)ohdr + ohdr_size > - (uint8_t *)mhdr + header_size) - return -FDT_ERR_BADSTRUCTURE; - - if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1)) - break; - ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr + - ohdr_size); - } - } + mhdr_end = (uint8_t *)mhdr + header_size; + for_each_opt_hdr_v1 (ohdr, ptr) + if (!opt_hdr_v1_valid_size(ohdr, mhdr_end)) + return -FDT_ERR_BADSTRUCTURE;

offset = le32_to_cpu(mhdr->srcaddr);

@@ -1867,36 +1835,24 @@ static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params { struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr; size_t header_size = kwbimage_header_size(ptr); + struct opt_hdr_v1 *ohdr; int idx = params->pflag; int cur_idx = 0; uint32_t offset; ulong image; ulong size;

- if (image_version((void *)ptr) == 1 && (mhdr->ext & 0x1)) { - struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *) - ((uint8_t *)ptr + - sizeof(*mhdr)); + for_each_opt_hdr_v1 (ohdr, ptr) { + if (ohdr->headertype != OPT_HDR_V1_BINARY_TYPE) + continue;

- while (1) { - uint32_t ohdr_size = (ohdr->headersz_msb << 16) | - le16_to_cpu(ohdr->headersz_lsb); - - if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) { - if (idx == cur_idx) { - image = (ulong)&ohdr->data[4 + - 4 * ohdr->data[0]]; - size = ohdr_size - 12 - - 4 * ohdr->data[0]; - goto extract; - } - ++cur_idx; - } - if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1)) - break; - ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr + - ohdr_size); + if (idx == cur_idx) { + image = (ulong)&ohdr->data[4 + 4 * ohdr->data[0]]; + size = opt_hdr_v1_size(ohdr) - 12 - 4 * ohdr->data[0]; + goto extract; } + + ++cur_idx; }

if (idx != cur_idx) { diff --git a/tools/kwbimage.h b/tools/kwbimage.h index 074bdfbe46..9a949e03c0 100644 --- a/tools/kwbimage.h +++ b/tools/kwbimage.h @@ -235,4 +235,62 @@ static inline unsigned int image_version(const void *header) return ptr[8]; }

+static inline uint32_t opt_hdr_v1_size(const struct opt_hdr_v1 *ohdr) +{ + return (ohdr->headersz_msb << 16) | le16_to_cpu(ohdr->headersz_lsb); +} + +static inline int opt_hdr_v1_valid_size(const struct opt_hdr_v1 *ohdr, + const void *mhdr_end) +{ + uint32_t ohdr_size; + + if ((void *)(ohdr + 1) > mhdr_end) + return 0; + + ohdr_size = opt_hdr_v1_size(ohdr); + if (ohdr_size < 8 || (void *)((uint8_t *)ohdr + ohdr_size) > mhdr_end) + return 0; + + return 1; +} + +static inline struct opt_hdr_v1 *opt_hdr_v1_first(void *img) { + struct main_hdr_v1 *mhdr; + + if (image_version(img) != 1) + return NULL; + + mhdr = img; + if (mhdr->ext & 0x1) + return (struct opt_hdr_v1 *)(mhdr + 1); + else + return NULL; +} + +static inline uint8_t *opt_hdr_v1_ext(struct opt_hdr_v1 *cur) +{ + uint32_t size = opt_hdr_v1_size(cur); + + return (uint8_t *)cur + size - 4; +} + +static inline struct opt_hdr_v1 *_opt_hdr_v1_next(struct opt_hdr_v1 *cur) +{ + return (struct opt_hdr_v1 *)((uint8_t *)cur + opt_hdr_v1_size(cur)); +} + +static inline struct opt_hdr_v1 *opt_hdr_v1_next(struct opt_hdr_v1 *cur) +{ + if (*opt_hdr_v1_ext(cur) & 0x1) + return _opt_hdr_v1_next(cur); + else + return NULL; +} + +#define for_each_opt_hdr_v1(ohdr, img) \ + for ((ohdr) = opt_hdr_v1_first((img)); \ + (ohdr) != NULL; \ + (ohdr) = opt_hdr_v1_next((ohdr))) + #endif /* _KWBIMAGE_H_ */