From: Pali Rohár pali@kernel.org

Some image types have source address in non-bytes unit; for example for SATA images, it is in 512 B units.

We need to multiply by unit size when patching image type to UART.

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

diff --git a/tools/kwboot.c b/tools/kwboot.c index 2446d0a7b5..907a574bfc 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -773,6 +773,7 @@ kwboot_img_patch_hdr(void *img, size_t size) { int rc; struct main_hdr_v1 *hdr; + uint32_t srcaddr; uint8_t csum; size_t hdrsz = sizeof(*hdr); int image_ver; @@ -809,6 +810,34 @@ kwboot_img_patch_hdr(void *img, size_t size) goto out; }

+ if (image_ver == 0) { + struct main_hdr_v0 *hdr_v0 = img; + + hdr_v0->nandeccmode = IBR_HDR_ECC_DISABLED; + hdr_v0->nandpagesize = 0; + } + + srcaddr = le32_to_cpu(hdr->srcaddr); + + switch (hdr->blockid) { + case IBR_HDR_SATA_ID: + if (srcaddr < 1) { + errno = EINVAL; + goto out; + } + hdr->srcaddr = cpu_to_le32((srcaddr - 1) * 512); + break; + + case IBR_HDR_SDIO_ID: + hdr->srcaddr = cpu_to_le32(srcaddr * 512); + break; + + case IBR_HDR_PEX_ID: + if (srcaddr == 0xFFFFFFFF) + hdr->srcaddr = cpu_to_le32(hdrsz); + break; + } + is_secure = kwboot_img_is_secure(img);

if (hdr->blockid != IBR_HDR_UART_ID) { @@ -823,17 +852,6 @@ kwboot_img_patch_hdr(void *img, size_t size) hdr->blockid = IBR_HDR_UART_ID; }

- if (image_ver == 0) { - struct main_hdr_v0 *hdr_v0 = img; - - 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->checksum = kwboot_img_csum8(hdr, hdrsz) - csum;

rc = 0;

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 | 8 ++++++++ 1 file changed, 8 insertions(+)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 907a574bfc..b1dcd3796c 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -836,6 +836,14 @@ kwboot_img_patch_hdr(void *img, size_t size) if (srcaddr == 0xFFFFFFFF) hdr->srcaddr = cpu_to_le32(hdrsz); break; + + case IBR_HDR_SPI_ID: + if (hdr->destaddr == cpu_to_le32(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); + } + break; }

is_secure = kwboot_img_is_secure(img);

From: Marek Behún marek.behun@nic.cz

Rename this function to kwbimage_version() and don't cast argument if not needed.

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

diff --git a/tools/kwbimage.c b/tools/kwbimage.c index 16b082b35f..944a108cee 100644 --- a/tools/kwbimage.c +++ b/tools/kwbimage.c @@ -282,7 +282,7 @@ static uint8_t image_checksum8(void *start, uint32_t len)

size_t kwbimage_header_size(unsigned char *ptr) { - if (image_version((void *)ptr) == 0) + if (kwbimage_version((void *)ptr) == 0) return sizeof(struct main_hdr_v0); else return KWBHEADER_V1_SIZE((struct main_hdr_v1 *)ptr); @@ -1622,7 +1622,7 @@ static void kwbimage_print_header(const void *ptr)

printf("Image Type: MVEBU Boot from %s Image\n", image_boot_mode_name(mhdr->blockid)); - printf("Image version:%d\n", image_version((void *)ptr)); + printf("Image version:%d\n", kwbimage_version(ptr));

for_each_opt_hdr_v1 (ohdr, mhdr) { if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) { @@ -1659,7 +1659,7 @@ static int kwbimage_verify_header(unsigned char *ptr, int image_size, return -FDT_ERR_BADSTRUCTURE;

/* Only version 0 extended header has checksum */ - if (image_version((void *)ptr) == 0) { + if (kwbimage_version(ptr) == 0) { struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;

if (mhdr->ext & 0x1) { @@ -1676,7 +1676,7 @@ static int kwbimage_verify_header(unsigned char *ptr, int image_size, if (checksum != ext_hdr->checksum) return -FDT_ERR_BADSTRUCTURE; } - } else if (image_version((void *)ptr) == 1) { + } else if (kwbimage_version(ptr) == 1) { struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr; const uint8_t *mhdr_end; struct opt_hdr_v1 *ohdr; diff --git a/tools/kwbimage.h b/tools/kwbimage.h index 9a949e03c0..738034beb1 100644 --- a/tools/kwbimage.h +++ b/tools/kwbimage.h @@ -229,7 +229,7 @@ void init_kwb_image_type (void); * header, byte 8 was reserved, and always set to 0. In the v1 header, * byte 8 has been changed to a proper field, set to 1. */ -static inline unsigned int image_version(const void *header) +static inline unsigned int kwbimage_version(const void *header) { const unsigned char *ptr = header; return ptr[8]; @@ -258,7 +258,7 @@ static inline int opt_hdr_v1_valid_size(const struct opt_hdr_v1 *ohdr, static inline struct opt_hdr_v1 *opt_hdr_v1_first(void *img) { struct main_hdr_v1 *mhdr;

- if (image_version(img) != 1) + if (kwbimage_version(img) != 1) return NULL;

mhdr = img; diff --git a/tools/kwboot.c b/tools/kwboot.c index b1dcd3796c..e47bf94e89 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -583,7 +583,7 @@ kwboot_xmodem(int tty, const void *_img, size_t size) int rc, pnum; size_t hdrsz;

- if (image_version(img) == 0) + if (kwbimage_version(img) == 0) hdrsz = KWBHEADER_V0_SIZE((struct main_hdr_v0 *)img); else hdrsz = KWBHEADER_V1_SIZE((struct main_hdr_v1 *)img); @@ -787,7 +787,7 @@ kwboot_img_patch_hdr(void *img, size_t size) goto out; }

- image_ver = image_version(img); + image_ver = kwbimage_version(img); if (image_ver != 0 && image_ver != 1) { fprintf(stderr, "Invalid image header version\n"); errno = EINVAL;

From: Marek Behún marek.behun@nic.cz

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 | 22 ++++++++++------------ 3 files changed, 40 insertions(+), 46 deletions(-)

diff --git a/tools/kwbimage.c b/tools/kwbimage.c index 944a108cee..d1f4f93e0f 100644 --- a/tools/kwbimage.c +++ b/tools/kwbimage.c @@ -280,14 +280,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. @@ -298,7 +290,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. */ @@ -1649,8 +1641,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; @@ -1663,17 +1655,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) { @@ -1832,7 +1817,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 e47bf94e89..e60f7c5b7a 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -583,10 +583,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 */ @@ -746,9 +743,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; @@ -794,17 +795,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; @@ -860,7 +858,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

These structures are relevant for several other platforms, mention them all.

Signed-off-by: Pali Rohár pali@kernel.org Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwbimage.c | 3 ++- tools/kwbimage.h | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/tools/kwbimage.c b/tools/kwbimage.c index d1f4f93e0f..77bf4dd886 100644 --- a/tools/kwbimage.c +++ b/tools/kwbimage.c @@ -1,7 +1,8 @@ // SPDX-License-Identifier: GPL-2.0+ /* * Image manipulator for Marvell SoCs - * supports Kirkwood, Dove, Armada 370, Armada XP, and Armada 38x + * supports Kirkwood, Dove, Armada 370, Armada XP, Armada 375, Armada 38x and + * Armada 39x * * (C) Copyright 2013 Thomas Petazzoni * thomas.petazzoni@free-electrons.com diff --git a/tools/kwbimage.h b/tools/kwbimage.h index 56a748d4cf..679c454367 100644 --- a/tools/kwbimage.h +++ b/tools/kwbimage.h @@ -69,7 +69,7 @@ struct ext_hdr_v0 { uint8_t checksum; } __packed;

-/* Structure of the main header, version 1 (Armada 370/38x/XP) */ +/* Structure of the main header, version 1 (Armada 370/XP/375/38x/39x) */ struct main_hdr_v1 { uint8_t blockid; /* 0x0 */ uint8_t flags; /* 0x1 */ @@ -103,7 +103,7 @@ struct main_hdr_v1 { #define MAIN_HDR_V1_OPT_BAUD_115200 0x7

/* - * Header for the optional headers, version 1 (Armada 370, Armada XP) + * Header for the optional headers, version 1 (Armada 370/XP/375/38x/39x) */ struct opt_hdr_v1 { uint8_t headertype; @@ -127,7 +127,7 @@ struct sig_v1 { } __packed;

/* - * Structure of secure header (Armada 38x) + * Structure of secure header (Armada XP/375/38x/39x) */ struct secure_hdr_v1 { uint8_t headertype; /* 0x0 */

From: Pali Rohár pali@kernel.org

The beginning of image data must be sent in a separate xmodem block; the block must not contain end of header with the beginning of data.

Therefore we need to ensure that the image header size is a multiple of xmodem block size (which is 128 B).

Read the file into a malloc()ed buffer of enough size instead of mmap()ing it. (If we are going to move the data, most of the pages will be dirty anyway.) Then move the payload if header size needs to be increased.

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

diff --git a/tools/kwboot.c b/tools/kwboot.c index e60f7c5b7a..4fae44c46e 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -25,7 +25,6 @@ #include <stdint.h> #include <termios.h> #include <time.h> -#include <sys/mman.h> #include <sys/stat.h>

/* @@ -706,11 +705,12 @@ out: }

static void * -kwboot_mmap_image(const char *path, size_t *size) +kwboot_read_image(const char *path, size_t *size, size_t reserve) { int rc, fd; struct stat st; void *img; + off_t tot;

rc = -1; img = NULL; @@ -723,17 +723,30 @@ kwboot_mmap_image(const char *path, size_t *size) if (rc) goto out;

- img = mmap(NULL, st.st_size, PROT_READ | PROT_WRITE, MAP_PRIVATE, fd, 0); - if (img == MAP_FAILED) { - img = NULL; + img = malloc(st.st_size + reserve); + if (!img) goto out; + + tot = 0; + while (tot < st.st_size) { + ssize_t rd = read(fd, img + tot, st.st_size - tot); + + if (rd < 0) + goto out; + + tot += rd; + + if (!rd && tot < st.st_size) { + errno = EIO; + goto out; + } }

rc = 0; *size = st.st_size; out: if (rc && img) { - munmap(img, st.st_size); + free(img); img = NULL; } if (fd >= 0) @@ -769,8 +782,39 @@ kwboot_img_is_secure(void *img) return 0; }

+static void +kwboot_img_grow_hdr(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(img); + 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); + + hdr->srcaddr = cpu_to_le32(srcaddr + need); + *size += need; + } + + if (kwbimage_version(img) == 1) { + hdrsz += grow; + hdr->headersz_msb = hdrsz >> 16; + hdr->headersz_lsb = cpu_to_le16(hdrsz & 0xffff); + } +} + static int -kwboot_img_patch_hdr(void *img, size_t size) +kwboot_img_patch_hdr(void *img, size_t *size) { int rc; struct main_hdr_v1 *hdr; @@ -783,7 +827,7 @@ kwboot_img_patch_hdr(void *img, size_t size) rc = -1; hdr = img;

- if (size < hdrsz) { + if (*size < hdrsz) { errno = EINVAL; goto out; } @@ -797,7 +841,7 @@ kwboot_img_patch_hdr(void *img, size_t size)

hdrsz = kwbheader_size(hdr);

- if (size < hdrsz) { + if (*size < hdrsz) { errno = EINVAL; goto out; } @@ -844,6 +888,12 @@ kwboot_img_patch_hdr(void *img, size_t size) break; }

+ if (hdrsz > le32_to_cpu(hdr->srcaddr) || + *size < le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize)) { + errno = EINVAL; + goto out; + } + is_secure = kwboot_img_is_secure(img);

if (hdr->blockid != IBR_HDR_UART_ID) { @@ -858,8 +908,23 @@ kwboot_img_patch_hdr(void *img, size_t size) hdr->blockid = IBR_HDR_UART_ID; }

+ if (hdrsz % KWBOOT_XM_BLKSZ) { + size_t offset = (KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ) % + KWBOOT_XM_BLKSZ; + + if (is_secure) { + fprintf(stderr, "Cannot align image with secure header\n"); + errno = EINVAL; + goto out; + } + + kwboot_printv("Aligning image header to Xmodem block size\n"); + kwboot_img_grow_hdr(img, size, offset); + } + hdr->checksum = kwboot_hdr_csum8(hdr) - csum;

+ *size = le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize); rc = 0; out: return rc; @@ -986,13 +1051,13 @@ main(int argc, char **argv) }

if (imgpath) { - img = kwboot_mmap_image(imgpath, &size); + img = kwboot_read_image(imgpath, &size, KWBOOT_XM_BLKSZ); if (!img) { perror(imgpath); goto out; }

- rc = kwboot_img_patch_hdr(img, size); + rc = kwboot_img_patch_hdr(img, &size); if (rc) { fprintf(stderr, "%s: Invalid image.\n", imgpath); goto out; @@ -1035,7 +1100,7 @@ out: close(tty);

if (img) - munmap(img, size); + free(img);

return rv;

From: Marek Behún marek.behun@nic.cz

Explicitly check the image size against size of struct main_hdr_v1. This way the check is more readable, since the `hdrsz` variable may semantically contain another value.

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

diff --git a/tools/kwboot.c b/tools/kwboot.c index 4fae44c46e..77bf5cb80b 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -820,14 +820,14 @@ kwboot_img_patch_hdr(void *img, size_t *size) struct main_hdr_v1 *hdr; uint32_t srcaddr; uint8_t csum; - size_t hdrsz = sizeof(*hdr); + size_t hdrsz; int image_ver; int is_secure;

rc = -1; hdr = img;

- if (*size < hdrsz) { + if (*size < sizeof(struct main_hdr_v1)) { errno = EINVAL; goto 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 | 637 ++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 603 insertions(+), 34 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index 77bf5cb80b..ba2fd10ff6 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -70,6 +70,187 @@ struct kwboot_block { #define KWBOOT_BLK_RSP_TIMEO 1000 /* ms */ #define KWBOOT_HDR_RSP_TIMEO 10000 /* ms */

+/* ARM code making baudrate changing function return to original exec address */ +static unsigned char kwboot_pre_baud_code[] = { + /* exec_addr: */ + 0x00, 0x00, 0x00, 0x00, /* .word 0 */ + 0x0c, 0xe0, 0x1f, 0xe5, /* ldr lr, exec_addr */ +}; + +/* 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 */ +}; + +#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 +414,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; + + speed = kwboot_tty_baudrate_to_speed(baudrate); + if (speed == B0) { + errno = EINVAL; + return -1; }

- 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 +610,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) { @@ -426,7 +766,34 @@ _xm_reply_to_error(int c) }

static int -kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print) +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_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print, + int baudrate, int *baud_changed) { int timeout = allow_non_xm ? KWBOOT_HDR_RSP_TIMEO : blk_rsp_timeo; uint64_t recv_until = _now() + timeout; @@ -434,6 +801,8 @@ kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print)

if (non_xm_print) *non_xm_print = 0; + if (baud_changed) + *baud_changed = 0;

while (1) { rc = kwboot_tty_recv(fd, c, 1, timeout); @@ -451,15 +820,30 @@ kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print) break;

/* - * If printing non-xmodem text output is allowed and such a byte - * was received, print it and increase receiving time. + * If receiving/printing non-xmodem text output is allowed and + * such a byte was received, we want to increase receiving time + * and either: + * - print the byte, if it is not part of baudrate change magic + * sequence while baudrate change was requested (-B option) + * - change baudrate * Otherwise decrease timeout by time elapsed. */ if (allow_non_xm) { recv_until = _now() + timeout; - 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 + return rc; + } else if (!baudrate || !*baud_changed) { + putchar(*c); + fflush(stdout); + *non_xm_print = 1; + } } else { timeout = recv_until - _now(); if (timeout < 0) { @@ -474,10 +858,10 @@ kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print)

static int kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, - int *done_print) + int *done_print, int baudrate) { - int non_xm_print; - int rc, retries; + int non_xm_print, baud_changed; + int rc, err, retries; char c;

*done_print = 0; @@ -494,9 +878,10 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, *done_print = 1; }

- rc = kwboot_xm_recv_reply(fd, &c, allow_non_xm, &non_xm_print); + rc = kwboot_xm_recv_reply(fd, &c, allow_non_xm, &non_xm_print, + baudrate, &baud_changed); if (rc) - return rc; + goto can;

if (!allow_non_xm && c != ACK) kwboot_progress(-1, '+'); @@ -505,7 +890,20 @@ kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm, if (non_xm_print) kwboot_printv("\n");

+ 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 @@ -522,7 +920,7 @@ kwboot_xm_finish(int fd) if (rc) return rc;

- rc = kwboot_xm_recv_reply(fd, &c, 0, NULL); + rc = kwboot_xm_recv_reply(fd, &c, 0, NULL, 0, NULL); if (rc) return rc; } while (c == NAK && retries-- > 0); @@ -532,7 +930,7 @@ kwboot_xm_finish(int fd)

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; @@ -555,7 +953,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;

@@ -576,7 +974,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; @@ -590,18 +988,41 @@ 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; + + rc = kwboot_xm_finish(tty); if (rc) return rc;

- return kwboot_xm_finish(tty); + 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 @@ -782,6 +1203,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->blocksize = cpu_to_le32(le32_to_cpu(hdr->blocksize) + grow); + + return (uint8_t *)img + srcaddr; +} + static void kwboot_img_grow_hdr(void *img, size_t *size, size_t grow) { @@ -813,8 +1265,71 @@ 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(struct main_hdr_v1 *hdr, 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 original + * value of hdr->execaddr. We do this by putting the original + * exec address before the code that loads it relatively from + * it's beginning. + * Afterwards we change the exec address to this code (which is + * at offset 4, because the first 4 bytes contain the original + * exec address). + */ + memcpy(code, kwboot_pre_baud_code, presz); + *(uint32_t *)code = hdr->execaddr; + + hdr->execaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) + 4); + + 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; @@ -908,6 +1423,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(hdr, 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(hdr, 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; @@ -964,7 +1524,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; @@ -974,7 +1535,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);

@@ -1024,9 +1586,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': @@ -1044,20 +1604,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; @@ -1079,7 +1648,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

The A38x platform supports more baudrates than just those defined by the Bn constants, and some of them are higher than the highest Bn baudrate (the highest is 4 MBd while A38x support 5.15 MBd).

On Linux, add support for arbitrary baudrates. (Since there is no standard POSIX API to specify arbitrary baudrate for a tty device, this change is Linux-specific.)

We need to use raw TCGETS2/TCSETS2 or TCGETS/TCSETS ioctls with the BOTHER flag in struct termios2/termios, defined in Linux headers <asm/ioctls.h> (included by <sys/ioctl.h>) and <asm/termbits.h>. Since these headers conflict with glibc's header file <termios.h>, it is not possible to use libc's termios functions and we need to reimplement them via ioctl() calls.

Note that the Bnnn constants from <termios.h> need not be compatible with Bnnn constants from <asm/termbits.h>.

Signed-off-by: Pali Rohár pali@kernel.org [ termios macros rewritten to static inline functions (for type control) and moved to tools/termios_linux.h ] Signed-off-by: Marek Behún marek.behun@nic.cz --- tools/kwboot.c | 16 +++- tools/termios_linux.h | 189 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 204 insertions(+), 1 deletion(-) create mode 100644 tools/termios_linux.h

diff --git a/tools/kwboot.c b/tools/kwboot.c index ba2fd10ff6..7ccab2993f 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -23,10 +23,15 @@ #include <errno.h> #include <unistd.h> #include <stdint.h> -#include <termios.h> #include <time.h> #include <sys/stat.h>

+#ifdef __linux__ +#include "termios_linux.h" +#else +#include <termios.h> +#endif + /* * Marvell BootROM UART Sensing */ @@ -554,7 +559,11 @@ kwboot_tty_baudrate_to_speed(int baudrate) return B50; #endif default: +#ifdef BOTHER + return BOTHER; +#else return B0; +#endif } }

@@ -575,6 +584,11 @@ kwboot_tty_change_baudrate(int fd, int baudrate) return -1; }

+#ifdef BOTHER + if (speed == BOTHER) + tio.c_ospeed = tio.c_ispeed = baudrate; +#endif + rc = cfsetospeed(&tio, speed); if (rc) return rc; diff --git a/tools/termios_linux.h b/tools/termios_linux.h new file mode 100644 index 0000000000..d73989b625 --- /dev/null +++ b/tools/termios_linux.h @@ -0,0 +1,189 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * termios fuctions to support arbitrary baudrates (on Linux) + * + * Copyright (c) 2021 Pali Rohár pali@kernel.org + * Copyright (c) 2021 Marek Behún marek.behun@nic.cz + */ + +#ifndef _TERMIOS_LINUX_H_ +#define _TERMIOS_LINUX_H_ + +/* + * We need to use raw TCGETS2/TCSETS2 or TCGETS/TCSETS ioctls with the BOTHER + * flag in struct termios2/termios, defined in Linux headers <asm/ioctls.h> + * (included by <sys/ioctl.h>) and <asm/termbits.h>. Since these headers + * conflict with glibc's header file <termios.h>, it is not possible to use + * libc's termios functions and we need to reimplement them via ioctl() calls. + * + * An arbitrary baudrate is supported when the macro BOTHER is defined. The + * baudrate value itself is then stored into the c_ospeed and c_ispeed members. + * If ioctls TCGETS2/TCSETS2 are defined and supported then these fields are + * present in struct termios2, otherwise these fields are present in struct + * termios. + * + * Note that the Bnnn constants from <termios.h> need not be compatible with Bnnn + * constants from <asm/termbits.h>. + */ + +#include <errno.h> +#include <sys/ioctl.h> +#include <sys/types.h> +#include <asm/termbits.h> + +#if defined(BOTHER) && defined(TCGETS2) +#define termios termios2 +#endif + +static inline int tcgetattr(int fd, struct termios *t) +{ +#if defined(BOTHER) && defined(TCGETS2) + return ioctl(fd, TCGETS2, t); +#else + return ioctl(fd, TCGETS, t); +#endif +} + +static inline int tcsetattr(int fd, int a, const struct termios *t) +{ + int cmd; + + switch (a) { +#if defined(BOTHER) && defined(TCGETS2) + case TCSANOW: + cmd = TCSETS2; + break; + case TCSADRAIN: + cmd = TCSETSW2; + break; + case TCSAFLUSH: + cmd = TCSETSF2; + break; +#else + case TCSANOW: + cmd = TCSETS; + break; + case TCSADRAIN: + cmd = TCSETSW; + break; + case TCSAFLUSH: + cmd = TCSETSF; + break; +#endif + default: + errno = EINVAL; + return -1; + } + + return ioctl(fd, cmd, t); +} + +static inline int tcdrain(int fd) +{ + return ioctl(fd, TCSBRK, 1); +} + +static inline int tcflush(int fd, int q) +{ + return ioctl(fd, TCFLSH, q); +} + +static inline int tcsendbreak(int fd, int d) +{ + return ioctl(fd, TCSBRK, d); +} + +static inline int tcflow(int fd, int a) +{ + return ioctl(fd, TCXONC, a); +} + +static inline pid_t tcgetsid(int fd) +{ + pid_t sid; + + if (ioctl(fd, TIOCGSID, &sid) < 0) + return (pid_t)-1; + + return sid; +} + +static inline speed_t cfgetospeed(const struct termios *t) +{ + return t->c_cflag & CBAUD; +} + +static inline int cfsetospeed(struct termios *t, speed_t s) +{ + if (s & ~CBAUD) { + errno = EINVAL; + return -1; + } + + t->c_cflag &= ~CBAUD; + t->c_cflag |= s; + + return 0; +} + +#ifdef IBSHIFT +static inline speed_t cfgetispeed(const struct termios *t) +{ + speed_t s = (t->c_cflag >> IBSHIFT) & CBAUD; + + if (s == B0) + return cfgetospeed(t); + else + return s; +} + +static inline int cfsetispeed(struct termios *t, speed_t s) +{ + if (s == 0) + s = B0; + + if (s & ~CBAUD) { + errno = EINVAL; + return -1; + } + + t->c_cflag &= ~(CBAUD << IBSHIFT); + t->c_cflag |= s << IBSHIFT; + + return 0; +} +#else /* !IBSHIFT */ +static inline speed_t cfgetispeed(const struct termios *t) +{ + return cfgetospeed(t); +} + +static inline int cfsetispeed(struct termios *t, speed_t s) +{ + return cfsetospeed(t, s); +} +#endif /* !IBSHIFT */ + +static inline int cfsetspeed(struct termios *t, speed_t s) +{ + if (cfsetospeed(t, s)) + return -1; +#ifdef IBSHIFT + if (cfsetispeed(t, s)) + return -1; +#endif + + return 0; +} + +static void cfmakeraw(struct termios *t) +{ + t->c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | + ICRNL | IXON); + t->c_oflag &= ~OPOST; + t->c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + t->c_cflag &= ~(CSIZE | PARENB); + t->c_cflag |= CS8; +} + +#endif /* _TERMIOS_LINUX_H_ */

From: Marek Behún marek.behun@nic.cz

The tcsetattr() function can return 0 even if baudrate was not changed. Check whether baudrate was changed to requested value, and in case of arbitrary baudrate, check whether the set value is within 3% tolerance.

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

diff --git a/tools/kwboot.c b/tools/kwboot.c index 7ccab2993f..d8b950787b 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -567,6 +567,13 @@ kwboot_tty_baudrate_to_speed(int baudrate) } }

+static int +_is_within_tolerance(int value, int reference, int tolerance) +{ + return 100 * value >= reference * (100 - tolerance) && + 100 * value <= reference * (100 + tolerance); +} + static int kwboot_tty_change_baudrate(int fd, int baudrate) { @@ -601,7 +608,32 @@ kwboot_tty_change_baudrate(int fd, int baudrate) if (rc) return rc;

+ rc = tcgetattr(fd, &tio); + if (rc) + return rc; + + if (cfgetospeed(&tio) != speed || cfgetispeed(&tio) != speed) + goto baud_fail; + +#ifdef BOTHER + /* + * Check whether set baudrate is within 3% tolerance. + * If BOTHER is defined, Linux always fills out c_ospeed / c_ispeed + * with real values. + */ + if (!_is_within_tolerance(tio.c_ospeed, baudrate, 3)) + goto baud_fail; + + if (!_is_within_tolerance(tio.c_ispeed, baudrate, 3)) + goto baud_fail; +#endif + return 0; + +baud_fail: + fprintf(stderr, "Could not set baudrate to requested value\n"); + errno = EINVAL; + return -1; }

static int

From: Pali Rohár pali@kernel.org

Retrieve current terminal settings via tcgetattr(), set to raw mode with cfmakeraw(), enable receiver via CREAD and ignore modem control lines via CLOCAL.

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

diff --git a/tools/kwboot.c b/tools/kwboot.c index d8b950787b..fc83161d70 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -648,11 +648,12 @@ kwboot_open_tty(const char *path, int baudrate) if (fd < 0) goto out;

- memset(&tio, 0, sizeof(tio)); - - tio.c_iflag = 0; - tio.c_cflag = CREAD|CLOCAL|CS8; + rc = tcgetattr(fd, &tio); + if (rc) + goto out;

+ cfmakeraw(&tio); + tio.c_cflag |= CREAD|CLOCAL; tio.c_cc[VMIN] = 1; tio.c_cc[VTIME] = 10;

From: Pali Rohár pali@kernel.org

Function kwboot_tty_recv() has its own handling of read timeout, we don't need to do set it in tty settings.

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, 1 insertion(+), 1 deletion(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c index fc83161d70..a527c79cf3 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -655,7 +655,7 @@ kwboot_open_tty(const char *path, int baudrate) cfmakeraw(&tio); tio.c_cflag |= CREAD|CLOCAL; tio.c_cc[VMIN] = 1; - tio.c_cc[VTIME] = 10; + tio.c_cc[VTIME] = 0;

rc = tcsetattr(fd, TCSANOW, &tio); if (rc)

From: Pali Rohár pali@kernel.org

The kwboot utility does not handle EAGAIN / EBUSY errors, it expects blocking mode on tty - it uses select() to check if data is available.

Disable non-blocking mode by clearing O_NDELAY flag which was set by open().

We can't just take O_NDELAY from open(), because it is required there until the CLOCAL flag is set on the tty.

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

diff --git a/tools/kwboot.c b/tools/kwboot.c index a527c79cf3..5e491f31a4 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -639,7 +639,7 @@ baud_fail: static int kwboot_open_tty(const char *path, int baudrate) { - int rc, fd; + int rc, fd, flags; struct termios tio;

rc = -1; @@ -661,6 +661,14 @@ kwboot_open_tty(const char *path, int baudrate) if (rc) goto out;

+ flags = fcntl(fd, F_GETFL); + if (flags < 0) + goto out; + + rc = fcntl(fd, F_SETFL, flags & ~O_NDELAY); + if (rc) + goto out; + rc = kwboot_tty_change_baudrate(fd, baudrate); if (rc) goto out;

From: Marek Behún marek.behun@nic.cz

Add entry for these tools with Marek, Pali and Stefan as maintainers.

Signed-off-by: Marek Behún marek.behun@nic.cz --- MAINTAINERS | 10 ++++++++++ 1 file changed, 10 insertions(+)

diff --git a/MAINTAINERS b/MAINTAINERS index 67c96a6045..6f103230da 100644 --- a/MAINTAINERS +++ b/MAINTAINERS @@ -776,6 +776,16 @@ S: Maintained T: git https://source.denx.de/u-boot/custodians/u-boot-i2c.git F: drivers/i2c/

+KWBIMAGE / KWBOOT TOOLS +M: Pali Rohár pali@kernel.org +M: Marek Behún marek.behun@nic.cz +M: Stefan Roese sr@denx.de +S: Maintained +T: git https://source.denx.de/u-boot/custodians/u-boot-marvell.git +F: doc/README.kwbimage +F: doc/kwboot.1 +F: tools/kwb* + LOGGING M: Simon Glass sjg@chromium.org S: Maintained