tools/kwboot.c

   1 /*
   2  * Boot a Marvell SoC, with Xmodem over UART0.
   3  *  supports Kirkwood, Dove, Avanta, Armada 370, Armada XP, Armada 375,
   4  *           Armada 38x and Armada 39x.
   5  *
   6  * (c) 2012 Daniel Stodden <daniel.stodden@gmail.com>
   7  * (c) 2021 Pali Rohár <pali@kernel.org>
   8  * (c) 2021 Marek Behún <kabel@kernel.org>
   9  *
  10  * References:
  11  * - "88F6180, 88F6190, 88F6192, and 88F6281: Integrated Controller: Functional
  12  *   Specifications" December 2, 2008. Chapter 24.2 "BootROM Firmware".
  13  *   https://web.archive.org/web/20130730091033/https://www.marvell.com/embedded-processors/kirkwood/assets/FS_88F6180_9x_6281_OpenSource.pdf
  14  * - "88AP510: High-Performance SoC with Integrated CPU, 2D/3D Graphics
  15  *   Processor, and High-Definition Video Decoder: Functional Specifications"
  16  *   August 3, 2011. Chapter 5 "BootROM Firmware"
  17  *   https://web.archive.org/web/20120130172443/https://www.marvell.com/application-processors/armada-500/assets/Armada-510-Functional-Spec.pdf
  18  * - "88F6665, 88F6660, 88F6658, 88F6655, 88F6655F, 88F6650, 88F6650F, 88F6610,
  19  *   and 88F6610F Avanta LP Family Integrated Single/Dual CPU Ecosystem for
  20  *   Gateway (GW), Home Gateway Unit (HGU), and Single Family Unit (SFU)
  21  *   Functional Specifications" Doc. No. MV-S108952-00, Rev. A. November 7, 2013.
  22  *   Chapter 7 "Boot Flow"
  23  *   CONFIDENTIAL, no public documentation available
  24  * - "88F6710, 88F6707, and 88F6W11: ARMADA(R) 370 SoC: Functional Specifications"
  25  *   May 26, 2014. Chapter 6 "BootROM Firmware".
  26  *   https://web.archive.org/web/20140617183701/https://www.marvell.com/embedded-processors/armada-300/assets/ARMADA370-FunctionalSpec-datasheet.pdf
  27  * - "MV78230, MV78260, and MV78460: ARMADA(R) XP Family of Highly Integrated
  28  *   Multi-Core ARMv7 Based SoC Processors: Functional Specifications"
  29  *   May 29, 2014. Chapter 6 "BootROM Firmware".
  30  *   https://web.archive.org/web/20180829171131/https://www.marvell.com/embedded-processors/armada-xp/assets/ARMADA-XP-Functional-SpecDatasheet.pdf
  31  * - "BobCat2 Control and Management Subsystem Functional Specifications"
  32  *   Doc. No. MV-S109400-00, Rev. A. December 4, 2014.
  33  *   Chapter 1.6 BootROM Firmware
  34  *   CONFIDENTIAL, no public documentation available
  35  * - "AlleyCat3 and PONCat3 Highly Integrated 1/10 Gigabit Ethernet Switch
  36  *   Control and Management Subsystem: Functional Specifications"
  37  *   Doc. No. MV-S109693-00, Rev. A. May 20, 2014.
  38  *   Chapter 1.6 BootROM Firmware
  39  *   CONFIDENTIAL, no public documentation available
  40  * - "ARMADA(R) 375 Value-Performance Dual Core CPU System on Chip: Functional
  41  *   Specifications" Doc. No. MV-S109377-00, Rev. A. September 18, 2013.
  42  *   Chapter 7 "Boot Sequence"
  43  *   CONFIDENTIAL, no public documentation available
  44  * - "88F6810, 88F6811, 88F6821, 88F6W21, 88F6820, and 88F6828: ARMADA(R) 38x
  45  *   Family High-Performance Single/Dual CPU System on Chip: Functional
  46  *   Specifications" Doc. No. MV-S109094-00, Rev. C. August 2, 2015.
  47  *   Chapter 7 "Boot Flow"
  48  *   CONFIDENTIAL, no public documentation available
  49  * - "88F6920, 88F6925 and 88F6928: ARMADA(R) 39x High-Performance Dual Core CPU
  50  *   System on Chip Functional Specifications" Doc. No. MV-S109896-00, Rev. B.
  51  *   December 22, 2015. Chapter 7 "Boot Flow"
  52  *   CONFIDENTIAL, no public documentation available
  53  * - "Marvell boot image parser", Marvell U-Boot 2013.01, version 18.06. September 17, 2015.
  54  *   https://github.com/MarvellEmbeddedProcessors/u-boot-marvell/blob/u-boot-2013.01-armada-18.06/tools/marvell/doimage_mv/hdrparser.c
  55  * - "Marvell doimage Tool", Marvell U-Boot 2013.01, version 18.06. August 30, 2015.
  56  *   https://github.com/MarvellEmbeddedProcessors/u-boot-marvell/blob/u-boot-2013.01-armada-18.06/tools/marvell/doimage_mv/doimage.c
  57  *
  58  * Storage location / offset of different image types:
  59  * - IBR_HDR_SPI_ID (0x5A):
  60  *   SPI image can be stored at any 2 MB aligned offset in the first 16 MB of
  61  *   SPI-NOR or parallel-NOR. Despite the type name it really can be stored on
  62  *   parallel-NOR and cannot be stored on other SPI devices, like SPI-NAND.
  63  *   So it should have been named NOR image, not SPI image. This image type
  64  *   supports XIP - Execute In Place directly from NOR memory. Destination
  65  *   address of the XIP image is set to 0xFFFFFFFF and execute address to the
  66  *   absolute offset in bytes from the beginning of NOR memory.
  67  *
  68  * - IBR_HDR_NAND_ID (0x8B):
  69  *   NAND image can be stored either at any 2 MB aligned offset in the first
  70  *   16 MB of SPI-NAND or at any blocksize aligned offset in the first 64 MB
  71  *   of parallel-NAND.
  72  *
  73  * - IBR_HDR_PEX_ID (0x9C):
  74  *   PEX image is used for booting from PCI Express device. Source address
  75  *   stored in image is ignored by BootROM. It is not the BootROM who parses
  76  *   or loads data part of the PEX image. BootROM just configures SoC to the
  77  *   PCIe endpoint mode and let the PCIe device on the other end of the PCIe
  78  *   link (which must be in Root Complex mode) to load kwbimage into SoC's
  79  *   memory and tell BootROM physical address.
  80  *
  81  * - IBR_HDR_UART_ID (0x69):
  82  *   UART image can be transfered via xmodem protocol over first UART.
  83  *   Unlike all other image types, header size stored in the image must be
  84  *   multiply of the 128 bytes (for all other image types it can be any size)
  85  *   and data part of the image does not have to contain 32-bit checksum
  86  *   (all other image types must have valid 32-bit checksum in its data part).
  87  *   And data size stored in the image is ignored. A38x BootROM determinates
  88  *   size of the data part implicitly by the end of the xmodem transfer.
  89  *   A38x BootROM has a bug which cause that BootROM loads data part of UART
  90  *   image into RAM target address increased by one byte when source address
  91  *   and header size stored in the image header are not same. So UART image
  92  *   should be constructed in a way that there is no gap between header and
  93  *   data part.
  94  *
  95  * - IBR_HDR_I2C_ID (0x4D):
  96  *   It is unknown for what kind of storage is used this image. It is not
  97  *   specified in any document from References section.
  98  *
  99  * - IBR_HDR_SATA_ID (0x78):
 100  *   SATA image can be stored at sector 1 (after the MBR table), sector 34
 101  *   (after the GPT table) or at any next sector which is aligned to 2 MB and
 102  *   is in the first 16 MB of SATA disk. Note that source address in SATA image
 103  *   is stored in sector unit and not in bytes like for any other images.
 104  *   Unfortunately sector size is disk specific, in most cases it is 512 bytes
 105  *   but there are also Native 4K SATA disks which have 4096 bytes long sectors.
 106  *
 107  * - IBR_HDR_SDIO_ID (0xAE):
 108  *   SDIO image can be stored on different medias:
 109  *   - SD(SC) card
 110  *   - SDHC/SDXC card
 111  *   - eMMC HW boot partition
 112  *   - eMMC user data partition / MMC card
 113  *   It cannot be stored on SDIO card despite the image name.
 114  *
 115  *   For SD(SC)/SDHC/SDXC cards, image can be stored at the same locations as
 116  *   the SATA image (sector 1, sector 34 or any 2 MB aligned sector) but within
 117  *   the first 64 MB. SDHC and SDXC cards have fixed 512 bytes long sector size.
 118  *   Old SD(SC) cards unfortunately can have also different sector sizes, mostly
 119  *   1024 bytes long sector sizes and also can be changed at runtime.
 120  *
 121  *   For MMC-compatible devices, image can be stored at offset 0 or at offset
 122  *   2 MB. If MMC device supports HW boot partitions then image must be stored
 123  *   on the HW partition as is configured in the EXT_CSC register (it can be
 124  *   either boot or user data).
 125  *
 126  *   Note that source address for SDIO image is stored in byte unit, like for
 127  *   any other images (except SATA). Marvell Functional Specifications for
 128  *   A38x and A39x SoCs say that source address is in sector units, but this
 129  *   is purely incorrect information. A385 BootROM really expects source address
 130  *   for SDIO images in bytes and also Marvell tools generate SDIO image with
 131  *   source address in byte units.
 132  */
 133
 134 #include "kwbimage.h"
 135 #include "mkimage.h"
 136 #include "version.h"
 137
 138 #include <stdlib.h>
 139 #include <stdio.h>
 140 #include <string.h>
 141 #include <stdarg.h>
 142 #include <image.h>
 143 #include <libgen.h>
 144 #include <fcntl.h>
 145 #include <errno.h>
 146 #include <unistd.h>
 147 #include <stdint.h>
 148 #include <time.h>
 149 #include <sys/stat.h>
 150 #include <pthread.h>
 151
 152 #ifdef __linux__
 153 #include "termios_linux.h"
 154 #else
 155 #include <termios.h>
 156 #endif
 157
 158 /*
 159  * These functions are in <term.h> header file, but this header file conflicts
 160  * with "termios_linux.h" header file. So declare these functions manually.
 161  */
 162 extern int setupterm(const char *, int, int *);
 163 extern char *tigetstr(const char *);
 164
 165 /*
 166  * Marvell BootROM UART Sensing
 167  */
 168
 169 static unsigned char kwboot_msg_boot[] = {
 170         0xBB, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77
 171 };
 172
 173 static unsigned char kwboot_msg_debug[] = {
 174         0xDD, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77
 175 };
 176
 177 /* Defines known to work on Kirkwood */
 178 #define KWBOOT_MSG_RSP_TIMEO    50 /* ms */
 179
 180 /* Defines known to work on Armada XP */
 181 #define KWBOOT_MSG_RSP_TIMEO_AXP        10 /* ms */
 182
 183 /*
 184  * Xmodem Transfers
 185  */
 186
 187 #define SOH     1       /* sender start of block header */
 188 #define EOT     4       /* sender end of block transfer */
 189 #define ACK     6       /* target block ack */
 190 #define NAK     21      /* target block negative ack */
 191
 192 #define KWBOOT_XM_BLKSZ 128 /* xmodem block size */
 193
 194 struct kwboot_block {
 195         uint8_t soh;
 196         uint8_t pnum;
 197         uint8_t _pnum;
 198         uint8_t data[KWBOOT_XM_BLKSZ];
 199         uint8_t csum;
 200 } __packed;
 201
 202 #define KWBOOT_BLK_RSP_TIMEO 2000 /* ms */
 203 #define KWBOOT_HDR_RSP_TIMEO 10000 /* ms */
 204
 205 /* ARM code to change baudrate */
 206 static unsigned char kwboot_baud_code[] = {
 207                                 /* ; #define UART_BASE 0xd0012000             */
 208                                 /* ; #define DLL       0x00                   */
 209                                 /* ; #define DLH       0x04                   */
 210                                 /* ; #define LCR       0x0c                   */
 211                                 /* ; #define   DLAB    0x80                   */
 212                                 /* ; #define LSR       0x14                   */
 213                                 /* ; #define   TEMT    0x40                   */
 214                                 /* ; #define DIV_ROUND(a, b) ((a + b/2) / b)  */
 215                                 /* ;                                          */
 216                                 /* ; u32 set_baudrate(u32 old_b, u32 new_b) { */
 217                                 /* ;   while                                  */
 218                                 /* ;      (!(readl(UART_BASE + LSR) & TEMT)); */
 219                                 /* ;   u32 lcr = readl(UART_BASE + LCR);      */
 220                                 /* ;   writel(UART_BASE + LCR, lcr | DLAB);   */
 221                                 /* ;   u8 old_dll = readl(UART_BASE + DLL);   */
 222                                 /* ;   u8 old_dlh = readl(UART_BASE + DLH);   */
 223                                 /* ;   u16 old_dl = old_dll | (old_dlh << 8); */
 224                                 /* ;   u32 clk = old_b * old_dl;              */
 225                                 /* ;   u16 new_dl = DIV_ROUND(clk, new_b);    */
 226                                 /* ;   u8 new_dll = new_dl & 0xff;            */
 227                                 /* ;   u8 new_dlh = (new_dl >> 8) & 0xff;     */
 228                                 /* ;   writel(UART_BASE + DLL, new_dll);      */
 229                                 /* ;   writel(UART_BASE + DLH, new_dlh);      */
 230                                 /* ;   writel(UART_BASE + LCR, lcr & ~DLAB);  */
 231                                 /* ;   msleep(5);                             */
 232                                 /* ;   return 0;                              */
 233                                 /* ; }                                        */
 234
 235                                 /*  ; r0 = UART_BASE                          */
 236         0x0d, 0x02, 0xa0, 0xe3, /* mov   r0, #0xd0000000                      */
 237         0x12, 0x0a, 0x80, 0xe3, /* orr   r0, r0, #0x12000                     */
 238
 239                                 /*  ; Wait until Transmitter FIFO is Empty    */
 240                                 /* .Lloop_txempty:                            */
 241                                 /*  ; r1 = UART_BASE[LSR] & TEMT              */
 242         0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
 243         0x40, 0x00, 0x11, 0xe3, /* tst   r1, #0x40                            */
 244         0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_txempty                       */
 245
 246                                 /*  ; Set Divisor Latch Access Bit            */
 247                                 /*  ; UART_BASE[LCR] |= DLAB                  */
 248         0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
 249         0x80, 0x10, 0x81, 0xe3, /* orr   r1, r1, #0x80                        */
 250         0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
 251
 252                                 /*  ; Read current Divisor Latch              */
 253                                 /*  ; r1 = UART_BASE[DLH]<<8 | UART_BASE[DLL] */
 254         0x00, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x00]                      */
 255         0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
 256         0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
 257         0x04, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x04]                      */
 258         0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
 259         0x41, 0x14, 0xa0, 0xe1, /* asr   r1, r1, #8                           */
 260         0x02, 0x10, 0x81, 0xe1, /* orr   r1, r1, r2                           */
 261
 262                                 /*  ; Read old baudrate value                 */
 263                                 /*  ; r2 = old_baudrate                       */
 264         0x74, 0x20, 0x9f, 0xe5, /* ldr   r2, old_baudrate                     */
 265
 266                                 /*  ; Calculate base clock                    */
 267                                 /*  ; r1 = r2 * r1                            */
 268         0x92, 0x01, 0x01, 0xe0, /* mul   r1, r2, r1                           */
 269
 270                                 /*  ; Read new baudrate value                 */
 271                                 /*  ; r2 = new_baudrate                       */
 272         0x70, 0x20, 0x9f, 0xe5, /* ldr   r2, new_baudrate                     */
 273
 274                                 /*  ; Calculate new Divisor Latch             */
 275                                 /*  ; r1 = DIV_ROUND(r1, r2) =                */
 276                                 /*  ;    = (r1 + r2/2) / r2                   */
 277         0xa2, 0x10, 0x81, 0xe0, /* add   r1, r1, r2, lsr #1                   */
 278         0x02, 0x40, 0xa0, 0xe1, /* mov   r4, r2                               */
 279         0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
 280                                 /* .Lloop_div1:                               */
 281         0x84, 0x40, 0xa0, 0x91, /* movls r4, r4, lsl #1                       */
 282         0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
 283         0xfc, 0xff, 0xff, 0x9a, /* bls   .Lloop_div1                          */
 284         0x00, 0x30, 0xa0, 0xe3, /* mov   r3, #0                               */
 285                                 /* .Lloop_div2:                               */
 286         0x04, 0x00, 0x51, 0xe1, /* cmp   r1, r4                               */
 287         0x04, 0x10, 0x41, 0x20, /* subhs r1, r1, r4                           */
 288         0x03, 0x30, 0xa3, 0xe0, /* adc   r3, r3, r3                           */
 289         0xa4, 0x40, 0xa0, 0xe1, /* mov   r4, r4, lsr #1                       */
 290         0x02, 0x00, 0x54, 0xe1, /* cmp   r4, r2                               */
 291         0xf9, 0xff, 0xff, 0x2a, /* bhs   .Lloop_div2                          */
 292         0x03, 0x10, 0xa0, 0xe1, /* mov   r1, r3                               */
 293
 294                                 /*  ; Set new Divisor Latch Low               */
 295                                 /*  ; UART_BASE[DLL] = r1 & 0xff              */
 296         0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
 297         0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
 298         0x00, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x00]                      */
 299
 300                                 /*  ; Set new Divisor Latch High              */
 301                                 /*  ; UART_BASE[DLH] = r1>>8 & 0xff           */
 302         0x41, 0x24, 0xa0, 0xe1, /* asr   r2, r1, #8                           */
 303         0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
 304         0x04, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x04]                      */
 305
 306                                 /*  ; Clear Divisor Latch Access Bit          */
 307                                 /*  ; UART_BASE[LCR] &= ~DLAB                 */
 308         0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
 309         0x80, 0x10, 0xc1, 0xe3, /* bic   r1, r1, #0x80                        */
 310         0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
 311
 312                                 /*  ; Loop 0x2dc000 (2998272) cycles          */
 313                                 /*  ; which is about 5ms on 1200 MHz CPU      */
 314                                 /*  ; r1 = 0x2dc000                           */
 315         0xb7, 0x19, 0xa0, 0xe3, /* mov   r1, #0x2dc000                        */
 316                                 /* .Lloop_sleep:                              */
 317         0x01, 0x10, 0x41, 0xe2, /* sub   r1, r1, #1                           */
 318         0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
 319         0xfc, 0xff, 0xff, 0x1a, /* bne   .Lloop_sleep                         */
 320
 321                                 /*  ; Jump to the end of execution            */
 322         0x01, 0x00, 0x00, 0xea, /* b     end                                  */
 323
 324                                 /*  ; Placeholder for old baudrate value      */
 325                                 /* old_baudrate:                              */
 326         0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
 327
 328                                 /*  ; Placeholder for new baudrate value      */
 329                                 /* new_baudrate:                              */
 330         0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
 331
 332                                 /* end:                                       */
 333 };
 334
 335 /* ARM code from binary header executed by BootROM before changing baudrate */
 336 static unsigned char kwboot_baud_code_binhdr_pre[] = {
 337                                 /* ; #define UART_BASE 0xd0012000             */
 338                                 /* ; #define THR       0x00                   */
 339                                 /* ; #define LSR       0x14                   */
 340                                 /* ; #define   THRE    0x20                   */
 341                                 /* ;                                          */
 342                                 /* ; void send_preamble(void) {               */
 343                                 /* ;   const u8 *str = "$baudratechange";     */
 344                                 /* ;   u8 c;                                  */
 345                                 /* ;   do {                                   */
 346                                 /* ;       while                              */
 347                                 /* ;       ((readl(UART_BASE + LSR) & THRE)); */
 348                                 /* ;       c = *str++;                        */
 349                                 /* ;       writel(UART_BASE + THR, c);        */
 350                                 /* ;   } while (c);                           */
 351                                 /* ; }                                        */
 352
 353                                 /*  ; Preserve registers for BootROM          */
 354         0xfe, 0x5f, 0x2d, 0xe9, /* push  { r1 - r12, lr }                     */
 355
 356                                 /*  ; r0 = UART_BASE                          */
 357         0x0d, 0x02, 0xa0, 0xe3, /* mov   r0, #0xd0000000                      */
 358         0x12, 0x0a, 0x80, 0xe3, /* orr   r0, r0, #0x12000                     */
 359
 360                                 /*  ; r2 = address of preamble string         */
 361         0x00, 0x20, 0x8f, 0xe2, /* adr   r2, .Lstr_preamble                   */
 362
 363                                 /*  ; Skip preamble data section              */
 364         0x03, 0x00, 0x00, 0xea, /* b     .Lloop_preamble                      */
 365
 366                                 /*  ; Preamble string                         */
 367                                 /* .Lstr_preamble:                            */
 368         0x24, 0x62, 0x61, 0x75, /* .asciz "$baudratechange"                   */
 369         0x64, 0x72, 0x61, 0x74,
 370         0x65, 0x63, 0x68, 0x61,
 371         0x6e, 0x67, 0x65, 0x00,
 372
 373                                 /*  ; Send preamble string over UART          */
 374                                 /* .Lloop_preamble:                           */
 375                                 /*                                            */
 376                                 /*  ; Wait until Transmitter Holding is Empty */
 377                                 /* .Lloop_thre:                               */
 378                                 /*  ; r1 = UART_BASE[LSR] & THRE              */
 379         0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
 380         0x20, 0x00, 0x11, 0xe3, /* tst   r1, #0x20                            */
 381         0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_thre                          */
 382
 383                                 /*  ; Put character into Transmitter FIFO     */
 384                                 /*  ; r1 = *r2++                              */
 385         0x01, 0x10, 0xd2, 0xe4, /* ldrb  r1, [r2], #1                         */
 386                                 /*  ; UART_BASE[THR] = r1                     */
 387         0x00, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0]                       */
 388
 389                                 /*  ; Loop until end of preamble string       */
 390         0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
 391         0xf8, 0xff, 0xff, 0x1a, /* bne   .Lloop_preamble                      */
 392 };
 393
 394 /* ARM code for returning from binary header back to BootROM */
 395 static unsigned char kwboot_baud_code_binhdr_post[] = {
 396                                 /*  ; Return 0 - no error                     */
 397         0x00, 0x00, 0xa0, 0xe3, /* mov   r0, #0                               */
 398         0xfe, 0x9f, 0xbd, 0xe8, /* pop   { r1 - r12, pc }                     */
 399 };
 400
 401 /* ARM code for jumping to the original image exec_addr */
 402 static unsigned char kwboot_baud_code_data_jump[] = {
 403         0x04, 0xf0, 0x1f, 0xe5, /* ldr   pc, exec_addr                        */
 404                                 /*  ; Placeholder for exec_addr               */
 405                                 /* exec_addr:                                 */
 406         0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
 407 };
 408
 409 static const char kwb_baud_magic[16] = "$baudratechange";
 410
 411 static int kwboot_verbose;
 412
 413 static int msg_rsp_timeo = KWBOOT_MSG_RSP_TIMEO;
 414 static int blk_rsp_timeo = KWBOOT_BLK_RSP_TIMEO;
 415
 416 static ssize_t
 417 kwboot_write(int fd, const char *buf, size_t len)
 418 {
 419         ssize_t tot = 0;
 420
 421         while (tot < len) {
 422                 ssize_t wr = write(fd, buf + tot, len - tot);
 423
 424                 if (wr < 0 && errno == EINTR)
 425                         continue;
 426                 else if (wr < 0)
 427                         return wr;
 428
 429                 tot += wr;
 430         }
 431
 432         return tot;
 433 }
 434
 435 static void
 436 kwboot_printv(const char *fmt, ...)
 437 {
 438         va_list ap;
 439
 440         if (kwboot_verbose) {
 441                 va_start(ap, fmt);
 442                 vprintf(fmt, ap);
 443                 va_end(ap);
 444                 fflush(stdout);
 445         }
 446 }
 447
 448 static void
 449 __spinner(void)
 450 {
 451         const char seq[] = { '-', '\\', '|', '/' };
 452         const int div = 8;
 453         static int state, bs;
 454
 455         if (state % div == 0) {
 456                 fputc(bs, stdout);
 457                 fputc(seq[state / div % sizeof(seq)], stdout);
 458                 fflush(stdout);
 459         }
 460
 461         bs = '\b';
 462         state++;
 463 }
 464
 465 static void
 466 kwboot_spinner(void)
 467 {
 468         if (kwboot_verbose)
 469                 __spinner();
 470 }
 471
 472 static void
 473 __progress(int pct, char c)
 474 {
 475         const int width = 70;
 476         static const char *nl = "";
 477         static int pos;
 478
 479         if (pos % width == 0)
 480                 printf("%s%3d %% [", nl, pct);
 481
 482         fputc(c, stdout);
 483
 484         nl = "]\n";
 485         pos = (pos + 1) % width;
 486
 487         if (pct == 100) {
 488                 while (pos && pos++ < width)
 489                         fputc(' ', stdout);
 490                 fputs(nl, stdout);
 491                 nl = "";
 492                 pos = 0;
 493         }
 494
 495         fflush(stdout);
 496
 497 }
 498
 499 static void
 500 kwboot_progress(int _pct, char c)
 501 {
 502         static int pct;
 503
 504         if (_pct != -1)
 505                 pct = _pct;
 506
 507         if (kwboot_verbose)
 508                 __progress(pct, c);
 509
 510         if (pct == 100)
 511                 pct = 0;
 512 }
 513
 514 static int
 515 kwboot_tty_recv(int fd, void *buf, size_t len, int timeo)
 516 {
 517         int rc, nfds;
 518         fd_set rfds;
 519         struct timeval tv;
 520         ssize_t n;
 521
 522         rc = -1;
 523
 524         FD_ZERO(&rfds);
 525         FD_SET(fd, &rfds);
 526
 527         tv.tv_sec = 0;
 528         tv.tv_usec = timeo * 1000;
 529         if (tv.tv_usec > 1000000) {
 530                 tv.tv_sec += tv.tv_usec / 1000000;
 531                 tv.tv_usec %= 1000000;
 532         }
 533
 534         do {
 535                 nfds = select(fd + 1, &rfds, NULL, NULL, &tv);
 536                 if (nfds < 0 && errno == EINTR)
 537                         continue;
 538                 else if (nfds < 0)
 539                         goto out;
 540                 else if (!nfds) {
 541                         errno = ETIMEDOUT;
 542                         goto out;
 543                 }
 544
 545                 n = read(fd, buf, len);
 546                 if (n < 0 && errno == EINTR)
 547                         continue;
 548                 else if (n <= 0)
 549                         goto out;
 550
 551                 buf = (char *)buf + n;
 552                 len -= n;
 553         } while (len > 0);
 554
 555         rc = 0;
 556 out:
 557         return rc;
 558 }
 559
 560 static int
 561 kwboot_tty_send(int fd, const void *buf, size_t len, int nodrain)
 562 {
 563         if (!buf)
 564                 return 0;
 565
 566         if (kwboot_write(fd, buf, len) < 0)
 567                 return -1;
 568
 569         if (nodrain)
 570                 return 0;
 571
 572         return tcdrain(fd);
 573 }
 574
 575 static int
 576 kwboot_tty_send_char(int fd, unsigned char c)
 577 {
 578         return kwboot_tty_send(fd, &c, 1, 0);
 579 }
 580
 581 static speed_t
 582 kwboot_tty_baudrate_to_speed(int baudrate)
 583 {
 584         switch (baudrate) {
 585 #ifdef B4000000
 586         case 4000000:
 587                 return B4000000;
 588 #endif
 589 #ifdef B3500000
 590         case 3500000:
 591                 return B3500000;
 592 #endif
 593 #ifdef B3000000
 594         case 3000000:
 595                 return B3000000;
 596 #endif
 597 #ifdef B2500000
 598         case 2500000:
 599                 return B2500000;
 600 #endif
 601 #ifdef B2000000
 602         case 2000000:
 603                 return B2000000;
 604 #endif
 605 #ifdef B1500000
 606         case 1500000:
 607                 return B1500000;
 608 #endif
 609 #ifdef B1152000
 610         case 1152000:
 611                 return B1152000;
 612 #endif
 613 #ifdef B1000000
 614         case 1000000:
 615                 return B1000000;
 616 #endif
 617 #ifdef B921600
 618         case 921600:
 619                 return B921600;
 620 #endif
 621 #ifdef B614400
 622         case 614400:
 623                 return B614400;
 624 #endif
 625 #ifdef B576000
 626         case 576000:
 627                 return B576000;
 628 #endif
 629 #ifdef B500000
 630         case 500000:
 631                 return B500000;
 632 #endif
 633 #ifdef B460800
 634         case 460800:
 635                 return B460800;
 636 #endif
 637 #ifdef B307200
 638         case 307200:
 639                 return B307200;
 640 #endif
 641 #ifdef B230400
 642         case 230400:
 643                 return B230400;
 644 #endif
 645 #ifdef B153600
 646         case 153600:
 647                 return B153600;
 648 #endif
 649 #ifdef B115200
 650         case 115200:
 651                 return B115200;
 652 #endif
 653 #ifdef B76800
 654         case 76800:
 655                 return B76800;
 656 #endif
 657 #ifdef B57600
 658         case 57600:
 659                 return B57600;
 660 #endif
 661 #ifdef B38400
 662         case 38400:
 663                 return B38400;
 664 #endif
 665 #ifdef B19200
 666         case 19200:
 667                 return B19200;
 668 #endif
 669 #ifdef B9600
 670         case 9600:
 671                 return B9600;
 672 #endif
 673 #ifdef B4800
 674         case 4800:
 675                 return B4800;
 676 #endif
 677 #ifdef B2400
 678         case 2400:
 679                 return B2400;
 680 #endif
 681 #ifdef B1800
 682         case 1800:
 683                 return B1800;
 684 #endif
 685 #ifdef B1200
 686         case 1200:
 687                 return B1200;
 688 #endif
 689 #ifdef B600
 690         case 600:
 691                 return B600;
 692 #endif
 693 #ifdef B300
 694         case 300:
 695                 return B300;
 696 #endif
 697 #ifdef B200
 698         case 200:
 699                 return B200;
 700 #endif
 701 #ifdef B150
 702         case 150:
 703                 return B150;
 704 #endif
 705 #ifdef B134
 706         case 134:
 707                 return B134;
 708 #endif
 709 #ifdef B110
 710         case 110:
 711                 return B110;
 712 #endif
 713 #ifdef B75
 714         case 75:
 715                 return B75;
 716 #endif
 717 #ifdef B50
 718         case 50:
 719                 return B50;
 720 #endif
 721         default:
 722 #ifdef BOTHER
 723                 return BOTHER;
 724 #else
 725                 return B0;
 726 #endif
 727         }
 728 }
 729
 730 static int
 731 _is_within_tolerance(int value, int reference, int tolerance)
 732 {
 733         return 100 * value >= reference * (100 - tolerance) &&
 734                100 * value <= reference * (100 + tolerance);
 735 }
 736
 737 static int
 738 kwboot_tty_change_baudrate(int fd, int baudrate)
 739 {
 740         struct termios tio;
 741         speed_t speed;
 742         int rc;
 743
 744         rc = tcgetattr(fd, &tio);
 745         if (rc)
 746                 return rc;
 747
 748         speed = kwboot_tty_baudrate_to_speed(baudrate);
 749         if (speed == B0) {
 750                 errno = EINVAL;
 751                 return -1;
 752         }
 753
 754 #ifdef BOTHER
 755         if (speed == BOTHER)
 756                 tio.c_ospeed = tio.c_ispeed = baudrate;
 757 #endif
 758
 759         rc = cfsetospeed(&tio, speed);
 760         if (rc)
 761                 return rc;
 762
 763         rc = cfsetispeed(&tio, speed);
 764         if (rc)
 765                 return rc;
 766
 767         rc = tcsetattr(fd, TCSANOW, &tio);
 768         if (rc)
 769                 return rc;
 770
 771         rc = tcgetattr(fd, &tio);
 772         if (rc)
 773                 return rc;
 774
 775         if (cfgetospeed(&tio) != speed || cfgetispeed(&tio) != speed)
 776                 goto baud_fail;
 777
 778 #ifdef BOTHER
 779         /*
 780          * Check whether set baudrate is within 3% tolerance.
 781          * If BOTHER is defined, Linux always fills out c_ospeed / c_ispeed
 782          * with real values.
 783          */
 784         if (!_is_within_tolerance(tio.c_ospeed, baudrate, 3))
 785                 goto baud_fail;
 786
 787         if (!_is_within_tolerance(tio.c_ispeed, baudrate, 3))
 788                 goto baud_fail;
 789 #endif
 790
 791         return 0;
 792
 793 baud_fail:
 794         fprintf(stderr, "Could not set baudrate to requested value\n");
 795         errno = EINVAL;
 796         return -1;
 797 }
 798
 799 static int
 800 kwboot_open_tty(const char *path, int baudrate)
 801 {
 802         int rc, fd, flags;
 803         struct termios tio;
 804
 805         rc = -1;
 806
 807         fd = open(path, O_RDWR | O_NOCTTY | O_NDELAY);
 808         if (fd < 0)
 809                 goto out;
 810
 811         rc = tcgetattr(fd, &tio);
 812         if (rc)
 813                 goto out;
 814
 815         cfmakeraw(&tio);
 816         tio.c_cflag |= CREAD | CLOCAL;
 817         tio.c_cflag &= ~(CSTOPB | HUPCL | CRTSCTS);
 818         tio.c_cc[VMIN] = 1;
 819         tio.c_cc[VTIME] = 0;
 820
 821         rc = tcsetattr(fd, TCSANOW, &tio);
 822         if (rc)
 823                 goto out;
 824
 825         flags = fcntl(fd, F_GETFL);
 826         if (flags < 0)
 827                 goto out;
 828
 829         rc = fcntl(fd, F_SETFL, flags & ~O_NDELAY);
 830         if (rc)
 831                 goto out;
 832
 833         rc = kwboot_tty_change_baudrate(fd, baudrate);
 834         if (rc)
 835                 goto out;
 836
 837         rc = fd;
 838 out:
 839         if (rc < 0) {
 840                 if (fd >= 0)
 841                         close(fd);
 842         }
 843
 844         return rc;
 845 }
 846
 847 static void *
 848 kwboot_msg_write_handler(void *arg)
 849 {
 850         int tty = *(int *)((void **)arg)[0];
 851         const void *msg = ((void **)arg)[1];
 852         int rsp_timeo = msg_rsp_timeo;
 853         int i, dummy_oldtype;
 854
 855         /* allow to cancel this thread at any time */
 856         pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &dummy_oldtype);
 857
 858         while (1) {
 859                 /* write 128 samples of message pattern into the output queue without waiting */
 860                 for (i = 0; i < 128; i++) {
 861                         if (kwboot_tty_send(tty, msg, 8, 1) < 0) {
 862                                 perror("\nFailed to send message pattern");
 863                                 exit(1);
 864                         }
 865                 }
 866                 /* wait until output queue is transmitted and then make pause */
 867                 if (tcdrain(tty) < 0) {
 868                         perror("\nFailed to send message pattern");
 869                         exit(1);
 870                 }
 871                 /* BootROM requires pause on UART after it detects message pattern */
 872                 usleep(rsp_timeo * 1000);
 873         }
 874 }
 875
 876 static int
 877 kwboot_msg_start_thread(pthread_t *thread, int *tty, void *msg)
 878 {
 879         void *arg[2];
 880         int rc;
 881
 882         arg[0] = tty;
 883         arg[1] = msg;
 884         rc = pthread_create(thread, NULL, kwboot_msg_write_handler, arg);
 885         if (rc) {
 886                 errno = rc;
 887                 return -1;
 888         }
 889
 890         return 0;
 891 }
 892
 893 static int
 894 kwboot_msg_stop_thread(pthread_t thread)
 895 {
 896         int rc;
 897
 898         rc = pthread_cancel(thread);
 899         if (rc) {
 900                 errno = rc;
 901                 return -1;
 902         }
 903
 904         rc = pthread_join(thread, NULL);
 905         if (rc) {
 906                 errno = rc;
 907                 return -1;
 908         }
 909
 910         return 0;
 911 }
 912
 913 static int
 914 kwboot_bootmsg(int tty)
 915 {
 916         struct kwboot_block block;
 917         pthread_t write_thread;
 918         int rc, err;
 919         char c;
 920
 921         /* flush input and output queue */
 922         tcflush(tty, TCIOFLUSH);
 923
 924         rc = kwboot_msg_start_thread(&write_thread, &tty, kwboot_msg_boot);
 925         if (rc) {
 926                 perror("Failed to start write thread");
 927                 return rc;
 928         }
 929
 930         kwboot_printv("Sending boot message. Please reboot the target...");
 931
 932         err = 0;
 933         while (1) {
 934                 kwboot_spinner();
 935
 936                 rc = kwboot_tty_recv(tty, &c, 1, msg_rsp_timeo);
 937                 if (rc && errno == ETIMEDOUT) {
 938                         continue;
 939                 } else if (rc) {
 940                         err = errno;
 941                         break;
 942                 }
 943
 944                 if (c == NAK)
 945                         break;
 946         }
 947
 948         kwboot_printv("\n");
 949
 950         rc = kwboot_msg_stop_thread(write_thread);
 951         if (rc) {
 952                 perror("Failed to stop write thread");
 953                 return rc;
 954         }
 955
 956         if (err) {
 957                 errno = err;
 958                 perror("Failed to read response for boot message pattern");
 959                 return -1;
 960         }
 961
 962         /*
 963          * At this stage we have sent more boot message patterns and BootROM
 964          * (at least on Armada XP and 385) started interpreting sent bytes as
 965          * part of xmodem packets. If BootROM is expecting SOH byte as start of
 966          * a xmodem packet and it receives byte 0xff, then it throws it away and
 967          * sends a NAK reply to host. If BootROM does not receive any byte for
 968          * 2s when expecting some continuation of the xmodem packet, it throws
 969          * away the partially received xmodem data and sends NAK reply to host.
 970          *
 971          * Therefore for starting xmodem transfer we have two options: Either
 972          * wait 2s or send 132 0xff bytes (which is the size of xmodem packet)
 973          * to ensure that BootROM throws away any partially received data.
 974          */
 975
 976         /* flush output queue with remaining boot message patterns */
 977         rc = tcflush(tty, TCOFLUSH);
 978         if (rc) {
 979                 perror("Failed to flush output queue");
 980                 return rc;
 981         }
 982
 983         /* send one xmodem packet with 0xff bytes to force BootROM to re-sync */
 984         memset(&block, 0xff, sizeof(block));
 985         rc = kwboot_tty_send(tty, &block, sizeof(block), 0);
 986         if (rc) {
 987                 perror("Failed to send sync sequence");
 988                 return rc;
 989         }
 990
 991         /*
 992          * Sending 132 bytes via 115200B/8-N-1 takes 11.45 ms, reading 132 bytes
 993          * takes 11.45 ms, so waiting for 30 ms should be enough.
 994          */
 995         usleep(30 * 1000);
 996
 997         /* flush remaining NAK replies from input queue */
 998         rc = tcflush(tty, TCIFLUSH);
 999         if (rc) {
1000                 perror("Failed to flush input queue");
1001                 return rc;
1002         }
1003
1004         return 0;
1005 }
1006
1007 static int
1008 kwboot_debugmsg(int tty)
1009 {
1010         unsigned char buf[8192];
1011         pthread_t write_thread;
1012         int rc, err, i, pos;
1013         size_t off;
1014
1015         /* flush input and output queue */
1016         tcflush(tty, TCIOFLUSH);
1017
1018         rc = kwboot_msg_start_thread(&write_thread, &tty, kwboot_msg_debug);
1019         if (rc) {
1020                 perror("Failed to start write thread");
1021                 return rc;
1022         }
1023
1024         kwboot_printv("Sending debug message. Please reboot the target...");
1025         kwboot_spinner();
1026
1027         err = 0;
1028         off = 0;
1029         while (1) {
1030                 /* Read immediately all bytes in queue without waiting */
1031                 rc = read(tty, buf + off, sizeof(buf) - off);
1032                 if ((rc < 0 && errno == EINTR) || rc == 0) {
1033                         continue;
1034                 } else if (rc < 0) {
1035                         err = errno;
1036                         break;
1037                 }
1038                 off += rc - 1;
1039
1040                 kwboot_spinner();
1041
1042                 /*
1043                  * Check if we received at least 4 debug message patterns
1044                  * (console echo from BootROM) in cyclic buffer
1045                  */
1046
1047                 for (pos = 0; pos < sizeof(kwboot_msg_debug); pos++)
1048                         if (buf[off] == kwboot_msg_debug[(pos + off) % sizeof(kwboot_msg_debug)])
1049                                 break;
1050
1051                 for (i = off; i >= 0; i--)
1052                         if (buf[i] != kwboot_msg_debug[(pos + i) % sizeof(kwboot_msg_debug)])
1053                                 break;
1054
1055                 off -= i;
1056
1057                 if (off >= 4 * sizeof(kwboot_msg_debug))
1058                         break;
1059
1060                 /* If not move valid suffix from end of the buffer to the beginning of buffer */
1061                 memmove(buf, buf + i + 1, off);
1062         }
1063
1064         kwboot_printv("\n");
1065
1066         rc = kwboot_msg_stop_thread(write_thread);
1067         if (rc) {
1068                 perror("Failed to stop write thread");
1069                 return rc;
1070         }
1071
1072         if (err) {
1073                 errno = err;
1074                 perror("Failed to read response for debug message pattern");
1075                 return -1;
1076         }
1077
1078         /* flush output queue with remaining debug message patterns */
1079         rc = tcflush(tty, TCOFLUSH);
1080         if (rc) {
1081                 perror("Failed to flush output queue");
1082                 return rc;
1083         }
1084
1085         kwboot_printv("Clearing input buffer...\n");
1086
1087         /*
1088          * Wait until BootROM transmit all remaining echo characters.
1089          * Experimentally it was measured that for Armada 385 BootROM
1090          * it is required to wait at least 0.415s. So wait 0.5s.
1091          */
1092         usleep(500 * 1000);
1093
1094         /*
1095          * In off variable is stored number of characters received after the
1096          * successful detection of echo reply. So these characters are console
1097          * echo for other following debug message patterns. BootROM may have in
1098          * its output queue other echo characters which were being transmitting
1099          * before above sleep call. So read remaining number of echo characters
1100          * sent by the BootROM now.
1101          */
1102         while ((rc = kwboot_tty_recv(tty, &buf[0], 1, 0)) == 0)
1103                 off++;
1104         if (errno != ETIMEDOUT) {
1105                 perror("Failed to read response");
1106                 return rc;
1107         }
1108
1109         /*
1110          * Clear every echo character set by the BootROM by backspace byte.
1111          * This is required prior writing any command to the BootROM debug
1112          * because BootROM command line buffer has limited size. If length
1113          * of the command is larger than buffer size then it looks like
1114          * that Armada 385 BootROM crashes after sending ENTER. So erase it.
1115          * Experimentally it was measured that for Armada 385 BootROM it is
1116          * required to send at least 3 backspace bytes for one echo character.
1117          * This is unknown why. But lets do it.
1118          */
1119         off *= 3;
1120         memset(buf, '\x08', sizeof(buf));
1121         while (off > sizeof(buf)) {
1122                 rc = kwboot_tty_send(tty, buf, sizeof(buf), 1);
1123                 if (rc) {
1124                         perror("Failed to send clear sequence");
1125                         return rc;
1126                 }
1127                 off -= sizeof(buf);
1128         }
1129         rc = kwboot_tty_send(tty, buf, off, 0);
1130         if (rc) {
1131                 perror("Failed to send clear sequence");
1132                 return rc;
1133         }
1134
1135         usleep(msg_rsp_timeo * 1000);
1136         rc = tcflush(tty, TCIFLUSH);
1137         if (rc) {
1138                 perror("Failed to flush input queue");
1139                 return rc;
1140         }
1141
1142         return 0;
1143 }
1144
1145 static size_t
1146 kwboot_xm_makeblock(struct kwboot_block *block, const void *data,
1147                     size_t size, int pnum)
1148 {
1149         size_t i, n;
1150
1151         block->soh = SOH;
1152         block->pnum = pnum;
1153         block->_pnum = ~block->pnum;
1154
1155         n = size < KWBOOT_XM_BLKSZ ? size : KWBOOT_XM_BLKSZ;
1156         memcpy(&block->data[0], data, n);
1157         memset(&block->data[n], 0, KWBOOT_XM_BLKSZ - n);
1158
1159         block->csum = 0;
1160         for (i = 0; i < n; i++)
1161                 block->csum += block->data[i];
1162
1163         return n;
1164 }
1165
1166 static uint64_t
1167 _now(void)
1168 {
1169         struct timespec ts;
1170
1171         if (clock_gettime(CLOCK_MONOTONIC, &ts)) {
1172                 static int err_print;
1173
1174                 if (!err_print) {
1175                         perror("clock_gettime() does not work");
1176                         err_print = 1;
1177                 }
1178
1179                 /* this will just make the timeout not work */
1180                 return -1ULL;
1181         }
1182
1183         return ts.tv_sec * 1000ULL + (ts.tv_nsec + 500000) / 1000000;
1184 }
1185
1186 static int
1187 _is_xm_reply(char c)
1188 {
1189         return c == ACK || c == NAK;
1190 }
1191
1192 static int
1193 _xm_reply_to_error(int c)
1194 {
1195         int rc = -1;
1196
1197         switch (c) {
1198         case ACK:
1199                 rc = 0;
1200                 break;
1201         case NAK:
1202                 errno = EBADMSG;
1203                 break;
1204         default:
1205                 errno = EPROTO;
1206                 break;
1207         }
1208
1209         return rc;
1210 }
1211
1212 static int
1213 kwboot_baud_magic_handle(int fd, char c, int baudrate)
1214 {
1215         static size_t rcv_len;
1216
1217         if (rcv_len < sizeof(kwb_baud_magic)) {
1218                 /* try to recognize whole magic word */
1219                 if (c == kwb_baud_magic[rcv_len]) {
1220                         rcv_len++;
1221                 } else {
1222                         printf("%.*s%c", (int)rcv_len, kwb_baud_magic, c);
1223                         fflush(stdout);
1224                         rcv_len = 0;
1225                 }
1226         }
1227
1228         if (rcv_len == sizeof(kwb_baud_magic)) {
1229                 /* magic word received */
1230                 kwboot_printv("\nChanging baudrate to %d Bd\n", baudrate);
1231
1232                 return kwboot_tty_change_baudrate(fd, baudrate) ? : 1;
1233         } else {
1234                 return 0;
1235         }
1236 }
1237
1238 static int
1239 kwboot_xm_recv_reply(int fd, char *c, int stop_on_non_xm,
1240                      int ignore_nak_reply,
1241                      int allow_non_xm, int *non_xm_print,
1242                      int baudrate, int *baud_changed)
1243 {
1244         int timeout = allow_non_xm ? KWBOOT_HDR_RSP_TIMEO : blk_rsp_timeo;
1245         uint64_t recv_until = _now() + timeout;
1246         int rc;
1247
1248         while (1) {
1249                 rc = kwboot_tty_recv(fd, c, 1, timeout);
1250                 if (rc) {
1251                         if (errno != ETIMEDOUT)
1252                                 return rc;
1253                         else if (allow_non_xm && *non_xm_print)
1254                                 return -1;
1255                         else
1256                                 *c = NAK;
1257                 }
1258
1259                 /* If received xmodem reply, end. */
1260                 if (_is_xm_reply(*c)) {
1261                         if (*c == NAK && ignore_nak_reply) {
1262                                 timeout = recv_until - _now();
1263                                 if (timeout >= 0)
1264                                         continue;
1265                         }
1266                         break;
1267                 }
1268
1269                 /*
1270                  * If receiving/printing non-xmodem text output is allowed and
1271                  * such a byte was received, we want to increase receiving time
1272                  * and either:
1273                  * - print the byte, if it is not part of baudrate change magic
1274                  *   sequence while baudrate change was requested (-B option)
1275                  * - change baudrate
1276                  * Otherwise decrease timeout by time elapsed.
1277                  */
1278                 if (allow_non_xm) {
1279                         recv_until = _now() + timeout;
1280
1281                         if (baudrate && !*baud_changed) {
1282                                 rc = kwboot_baud_magic_handle(fd, *c, baudrate);
1283                                 if (rc == 1)
1284                                         *baud_changed = 1;
1285                                 else if (!rc)
1286                                         *non_xm_print = 1;
1287                                 else
1288                                         return rc;
1289                         } else if (!baudrate || !*baud_changed) {
1290                                 putchar(*c);
1291                                 fflush(stdout);
1292                                 *non_xm_print = 1;
1293                         }
1294                 } else {
1295                         if (stop_on_non_xm)
1296                                 break;
1297                         timeout = recv_until - _now();
1298                         if (timeout < 0) {
1299                                 errno = ETIMEDOUT;
1300                                 return -1;
1301                         }
1302                 }
1303         }
1304
1305         return 0;
1306 }
1307
1308 static int
1309 kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm,
1310                     int *done_print, int baudrate, int allow_retries)
1311 {
1312         int non_xm_print, baud_changed;
1313         int rc, err, retries;
1314         char c;
1315
1316         *done_print = 0;
1317         non_xm_print = 0;
1318         baud_changed = 0;
1319
1320         retries = 0;
1321         do {
1322                 rc = kwboot_tty_send(fd, block, sizeof(*block), 1);
1323                 if (rc)
1324                         goto err;
1325
1326                 if (allow_non_xm && !*done_print) {
1327                         kwboot_progress(100, '.');
1328                         kwboot_printv("Done\n");
1329                         *done_print = 1;
1330                 }
1331
1332                 rc = kwboot_xm_recv_reply(fd, &c, retries < 3,
1333                                           retries > 8,
1334                                           allow_non_xm, &non_xm_print,
1335                                           baudrate, &baud_changed);
1336                 if (rc)
1337                         goto err;
1338
1339                 if (!allow_non_xm && c != ACK) {
1340                         if (c == NAK && allow_retries && retries + 1 < 16)
1341                                 kwboot_progress(-1, '+');
1342                         else
1343                                 kwboot_progress(-1, 'E');
1344                 }
1345         } while (c == NAK && allow_retries && retries++ < 16);
1346
1347         if (non_xm_print)
1348                 kwboot_printv("\n");
1349
1350         if (allow_non_xm && baudrate && !baud_changed) {
1351                 fprintf(stderr, "Baudrate was not changed\n");
1352                 errno = EPROTO;
1353                 return -1;
1354         }
1355
1356         return _xm_reply_to_error(c);
1357 err:
1358         err = errno;
1359         kwboot_printv("\n");
1360         errno = err;
1361         return rc;
1362 }
1363
1364 static int
1365 kwboot_xm_finish(int fd)
1366 {
1367         int rc, retries;
1368         char c;
1369
1370         kwboot_printv("Finishing transfer\n");
1371
1372         retries = 0;
1373         do {
1374                 rc = kwboot_tty_send_char(fd, EOT);
1375                 if (rc)
1376                         return rc;
1377
1378                 rc = kwboot_xm_recv_reply(fd, &c, retries < 3,
1379                                           retries > 8,
1380                                           0, NULL, 0, NULL);
1381                 if (rc)
1382                         return rc;
1383         } while (c == NAK && retries++ < 16);
1384
1385         return _xm_reply_to_error(c);
1386 }
1387
1388 static int
1389 kwboot_xmodem_one(int tty, int *pnum, int header, const uint8_t *data,
1390                   size_t size, int baudrate)
1391 {
1392         int done_print = 0;
1393         size_t sent, left;
1394         int rc;
1395
1396         kwboot_printv("Sending boot image %s (%zu bytes)...\n",
1397                       header ? "header" : "data", size);
1398
1399         left = size;
1400         sent = 0;
1401
1402         while (sent < size) {
1403                 struct kwboot_block block;
1404                 int last_block;
1405                 size_t blksz;
1406
1407                 blksz = kwboot_xm_makeblock(&block, data, left, (*pnum)++);
1408                 data += blksz;
1409
1410                 last_block = (left <= blksz);
1411
1412                 /*
1413                  * Handling of repeated xmodem packets is completely broken in
1414                  * Armada 385 BootROM - it completely ignores xmodem packet
1415                  * numbers, they are only used for checksum verification.
1416                  * BootROM can handle a retry of the xmodem packet only during
1417                  * the transmission of kwbimage header and only if BootROM
1418                  * itself sent NAK response to previous attempt (it does it on
1419                  * checksum failure). During the transmission of kwbimage data
1420                  * part, BootROM always expects next xmodem packet, even if it
1421                  * sent NAK to previous attempt - there is absolutely no way to
1422                  * repair incorrectly transmitted xmodem packet during kwbimage
1423                  * data part upload. Also, if kwboot receives non-ACK/NAK
1424                  * response (meaning that original BootROM response was damaged
1425                  * on UART) there is no way to detect if BootROM accepted xmodem
1426                  * packet or not and no way to check if kwboot could repeat the
1427                  * packet or not.
1428                  *
1429                  * Stop transfer and return failure if kwboot receives unknown
1430                  * reply if non-xmodem reply is not allowed (for all xmodem
1431                  * packets except the last header packet) or when non-ACK reply
1432                  * is received during data part transfer.
1433                  */
1434                 rc = kwboot_xm_sendblock(tty, &block, header && last_block,
1435                                          &done_print, baudrate, header);
1436                 if (rc)
1437                         goto out;
1438
1439                 sent += blksz;
1440                 left -= blksz;
1441
1442                 if (!done_print)
1443                         kwboot_progress(sent * 100 / size, '.');
1444         }
1445
1446         if (!done_print)
1447                 kwboot_printv("Done\n");
1448
1449         return 0;
1450 out:
1451         kwboot_printv("\n");
1452         return rc;
1453 }
1454
1455 static int
1456 kwboot_xmodem(int tty, const void *_img, size_t size, int baudrate)
1457 {
1458         const uint8_t *img = _img;
1459         int rc, pnum;
1460         size_t hdrsz;
1461
1462         hdrsz = kwbheader_size(img);
1463
1464         /*
1465          * If header size is not aligned to xmodem block size (which applies
1466          * for all images in kwbimage v0 format) then we have to ensure that
1467          * the last xmodem block of header contains beginning of the data
1468          * followed by the header. So align header size to xmodem block size.
1469          */
1470         hdrsz += (KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ) % KWBOOT_XM_BLKSZ;
1471         if (hdrsz > size)
1472                 hdrsz = size;
1473
1474         pnum = 1;
1475
1476         rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz, baudrate);
1477         if (rc)
1478                 return rc;
1479
1480         /*
1481          * If we have already sent image data as a part of the last
1482          * xmodem header block then we have nothing more to send.
1483          */
1484         if (hdrsz < size) {
1485                 img += hdrsz;
1486                 size -= hdrsz;
1487                 rc = kwboot_xmodem_one(tty, &pnum, 0, img, size, 0);
1488                 if (rc)
1489                         return rc;
1490         }
1491
1492         rc = kwboot_xm_finish(tty);
1493         if (rc)
1494                 return rc;
1495
1496         if (baudrate) {
1497                 kwboot_printv("\nChanging baudrate back to 115200 Bd\n\n");
1498                 rc = kwboot_tty_change_baudrate(tty, 115200);
1499                 if (rc)
1500                         return rc;
1501         }
1502
1503         return 0;
1504 }
1505
1506 static int
1507 kwboot_term_pipe(int in, int out, const char *quit, int *s, const char *kbs, int *k)
1508 {
1509         char buf[128];
1510         ssize_t nin, noff;
1511
1512         nin = read(in, buf, sizeof(buf));
1513         if (nin <= 0)
1514                 return -1;
1515
1516         noff = 0;
1517
1518         if (quit || kbs) {
1519                 int i;
1520
1521                 for (i = 0; i < nin; i++) {
1522                         if ((quit || kbs) &&
1523                             (!quit || buf[i] != quit[*s]) &&
1524                             (!kbs || buf[i] != kbs[*k])) {
1525                                 const char *prefix;
1526                                 int plen;
1527
1528                                 if (quit && kbs) {
1529                                         prefix = (*s >= *k) ? quit : kbs;
1530                                         plen = (*s >= *k) ? *s : *k;
1531                                 } else if (quit) {
1532                                         prefix = quit;
1533                                         plen = *s;
1534                                 } else {
1535                                         prefix = kbs;
1536                                         plen = *k;
1537                                 }
1538
1539                                 if (plen > i && kwboot_write(out, prefix, plen - i) < 0)
1540                                         return -1;
1541                         }
1542
1543                         if (quit && buf[i] == quit[*s]) {
1544                                 (*s)++;
1545                                 if (!quit[*s]) {
1546                                         nin = (i > *s) ? (i - *s) : 0;
1547                                         break;
1548                                 }
1549                         } else if (quit) {
1550                                 *s = 0;
1551                         }
1552
1553                         if (kbs && buf[i] == kbs[*k]) {
1554                                 (*k)++;
1555                                 if (!kbs[*k]) {
1556                                         if (i > *k + noff &&
1557                                             kwboot_write(out, buf + noff, i - *k - noff) < 0)
1558                                                 return -1;
1559                                         /*
1560                                          * Replace backspace key by '\b' (0x08)
1561                                          * byte which is the only recognized
1562                                          * backspace byte by Marvell BootROM.
1563                                          */
1564                                         if (write(out, "\x08", 1) < 0)
1565                                                 return -1;
1566                                         noff = i + 1;
1567                                         *k = 0;
1568                                 }
1569                         } else if (kbs) {
1570                                 *k = 0;
1571                         }
1572                 }
1573
1574                 if (i == nin) {
1575                         i = 0;
1576                         if (quit && i < *s)
1577                                 i = *s;
1578                         if (kbs && i < *k)
1579                                 i = *k;
1580                         nin -= (nin > i) ? i : nin;
1581                 }
1582         }
1583
1584         if (nin > noff && kwboot_write(out, buf + noff, nin - noff) < 0)
1585                 return -1;
1586
1587         return 0;
1588 }
1589
1590 static int
1591 kwboot_terminal(int tty)
1592 {
1593         int rc, in, s, k;
1594         const char *kbs = NULL;
1595         const char *quit = "\34c";
1596         struct termios otio, tio;
1597
1598         rc = -1;
1599
1600         in = STDIN_FILENO;
1601         if (isatty(in)) {
1602                 rc = tcgetattr(in, &otio);
1603                 if (!rc) {
1604                         tio = otio;
1605                         cfmakeraw(&tio);
1606                         rc = tcsetattr(in, TCSANOW, &tio);
1607                 }
1608                 if (rc) {
1609                         perror("tcsetattr");
1610                         goto out;
1611                 }
1612
1613                 /*
1614                  * Get sequence for backspace key used by the current
1615                  * terminal. Every occurrence of this sequence will be
1616                  * replaced by '\b' byte which is the only recognized
1617                  * backspace byte by Marvell BootROM.
1618                  *
1619                  * Note that we cannot read this sequence from termios
1620                  * c_cc[VERASE] as VERASE is valid only when ICANON is
1621                  * set in termios c_lflag, which is not case for us.
1622                  *
1623                  * Also most terminals do not set termios c_cc[VERASE]
1624                  * as c_cc[VERASE] can specify only one-byte sequence
1625                  * and instead let applications to read (possible
1626                  * multi-byte) sequence for backspace key from "kbs"
1627                  * terminfo database based on $TERM env variable.
1628                  *
1629                  * So read "kbs" from terminfo database via tigetstr()
1630                  * call after successful setupterm(). Most terminals
1631                  * use byte 0x7F for backspace key, so replacement with
1632                  * '\b' is required.
1633                  */
1634                 if (setupterm(NULL, STDOUT_FILENO, &rc) == 0) {
1635                         kbs = tigetstr("kbs");
1636                         if (kbs == (char *)-1)
1637                                 kbs = NULL;
1638                 }
1639
1640                 kwboot_printv("[Type Ctrl-%c + %c to quit]\r\n",
1641                               quit[0] | 0100, quit[1]);
1642         } else
1643                 in = -1;
1644
1645         rc = 0;
1646         s = 0;
1647         k = 0;
1648
1649         do {
1650                 fd_set rfds;
1651                 int nfds = 0;
1652
1653                 FD_ZERO(&rfds);
1654                 FD_SET(tty, &rfds);
1655                 nfds = nfds < tty ? tty : nfds;
1656
1657                 if (in >= 0) {
1658                         FD_SET(in, &rfds);
1659                         nfds = nfds < in ? in : nfds;
1660                 }
1661
1662                 nfds = select(nfds + 1, &rfds, NULL, NULL, NULL);
1663                 if (nfds < 0)
1664                         break;
1665
1666                 if (FD_ISSET(tty, &rfds)) {
1667                         rc = kwboot_term_pipe(tty, STDOUT_FILENO, NULL, NULL, NULL, NULL);
1668                         if (rc)
1669                                 break;
1670                 }
1671
1672                 if (in >= 0 && FD_ISSET(in, &rfds)) {
1673                         rc = kwboot_term_pipe(in, tty, quit, &s, kbs, &k);
1674                         if (rc)
1675                                 break;
1676                 }
1677         } while (quit[s] != 0);
1678
1679         if (in >= 0)
1680                 tcsetattr(in, TCSANOW, &otio);
1681         printf("\n");
1682 out:
1683         return rc;
1684 }
1685
1686 static void *
1687 kwboot_read_image(const char *path, size_t *size, size_t reserve)
1688 {
1689         int rc, fd;
1690         void *img;
1691         off_t len;
1692         off_t tot;
1693
1694         rc = -1;
1695         img = NULL;
1696
1697         fd = open(path, O_RDONLY);
1698         if (fd < 0)
1699                 goto out;
1700
1701         len = lseek(fd, 0, SEEK_END);
1702         if (len == (off_t)-1)
1703                 goto out;
1704
1705         if (lseek(fd, 0, SEEK_SET) == (off_t)-1)
1706                 goto out;
1707
1708         img = malloc(len + reserve);
1709         if (!img)
1710                 goto out;
1711
1712         tot = 0;
1713         while (tot < len) {
1714                 ssize_t rd = read(fd, img + tot, len - tot);
1715
1716                 if (rd < 0)
1717                         goto out;
1718
1719                 tot += rd;
1720
1721                 if (!rd && tot < len) {
1722                         errno = EIO;
1723                         goto out;
1724                 }
1725         }
1726
1727         rc = 0;
1728         *size = len;
1729 out:
1730         if (rc && img) {
1731                 free(img);
1732                 img = NULL;
1733         }
1734         if (fd >= 0)
1735                 close(fd);
1736
1737         return img;
1738 }
1739
1740 static uint8_t
1741 kwboot_hdr_csum8(const void *hdr)
1742 {
1743         const uint8_t *data = hdr;
1744         uint8_t csum;
1745         size_t size;
1746
1747         size = kwbheader_size_for_csum(hdr);
1748
1749         for (csum = 0; size-- > 0; data++)
1750                 csum += *data;
1751
1752         return csum;
1753 }
1754
1755 static uint32_t *
1756 kwboot_img_csum32_ptr(void *img)
1757 {
1758         struct main_hdr_v1 *hdr = img;
1759         uint32_t datasz;
1760
1761         datasz = le32_to_cpu(hdr->blocksize) - sizeof(uint32_t);
1762
1763         return img + le32_to_cpu(hdr->srcaddr) + datasz;
1764 }
1765
1766 static uint32_t
1767 kwboot_img_csum32(const void *img)
1768 {
1769         const struct main_hdr_v1 *hdr = img;
1770         uint32_t datasz, csum = 0;
1771         const uint32_t *data;
1772
1773         datasz = le32_to_cpu(hdr->blocksize) - sizeof(csum);
1774         if (datasz % sizeof(uint32_t))
1775                 return 0;
1776
1777         data = img + le32_to_cpu(hdr->srcaddr);
1778         while (datasz > 0) {
1779                 csum += le32_to_cpu(*data++);
1780                 datasz -= 4;
1781         }
1782
1783         return cpu_to_le32(csum);
1784 }
1785
1786 static int
1787 kwboot_img_is_secure(void *img)
1788 {
1789         struct opt_hdr_v1 *ohdr;
1790
1791         for_each_opt_hdr_v1 (ohdr, img)
1792                 if (ohdr->headertype == OPT_HDR_V1_SECURE_TYPE)
1793                         return 1;
1794
1795         return 0;
1796 }
1797
1798 static int
1799 kwboot_img_has_ddr_init(void *img)
1800 {
1801         const struct register_set_hdr_v1 *rhdr;
1802         const struct main_hdr_v0 *hdr0;
1803         struct opt_hdr_v1 *ohdr;
1804         u32 ohdrsz;
1805         int last;
1806
1807         /*
1808          * kwbimage v0 image headers contain DDR init code either in
1809          * extension header or in binary code header.
1810          */
1811         if (kwbimage_version(img) == 0) {
1812                 hdr0 = img;
1813                 return hdr0->ext || hdr0->bin;
1814         }
1815
1816         /*
1817          * kwbimage v1 image headers contain DDR init code either in binary
1818          * code header or in a register set list header with SDRAM_SETUP.
1819          */
1820         for_each_opt_hdr_v1 (ohdr, img) {
1821                 if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE)
1822                         return 1;
1823                 if (ohdr->headertype == OPT_HDR_V1_REGISTER_TYPE) {
1824                         rhdr = (const struct register_set_hdr_v1 *)ohdr;
1825                         ohdrsz = opt_hdr_v1_size(ohdr);
1826                         if (ohdrsz >= sizeof(*ohdr) + sizeof(rhdr->data[0].last_entry)) {
1827                                 ohdrsz -= sizeof(*ohdr) + sizeof(rhdr->data[0].last_entry);
1828                                 last = ohdrsz / sizeof(rhdr->data[0].entry);
1829                                 if (rhdr->data[last].last_entry.delay ==
1830                                     REGISTER_SET_HDR_OPT_DELAY_SDRAM_SETUP)
1831                                         return 1;
1832                         }
1833                 }
1834         }
1835
1836         return 0;
1837 }
1838
1839 static void *
1840 kwboot_img_grow_data_right(void *img, size_t *size, size_t grow)
1841 {
1842         struct main_hdr_v1 *hdr = img;
1843         void *result;
1844
1845         /*
1846          * 32-bit checksum comes after end of image code, so we will be putting
1847          * new code there. So we get this pointer and then increase data size
1848          * (since increasing data size changes kwboot_img_csum32_ptr() return
1849          *  value).
1850          */
1851         result = kwboot_img_csum32_ptr(img);
1852         hdr->blocksize = cpu_to_le32(le32_to_cpu(hdr->blocksize) + grow);
1853         *size += grow;
1854
1855         return result;
1856 }
1857
1858 static void
1859 kwboot_img_grow_hdr(void *img, size_t *size, size_t grow)
1860 {
1861         uint32_t hdrsz, datasz, srcaddr;
1862         struct main_hdr_v1 *hdr = img;
1863         struct opt_hdr_v1 *ohdr;
1864         uint8_t *data;
1865
1866         srcaddr = le32_to_cpu(hdr->srcaddr);
1867
1868         /* calculate real used space in kwbimage header */
1869         if (kwbimage_version(img) == 0) {
1870                 hdrsz = kwbheader_size(img);
1871         } else {
1872                 hdrsz = sizeof(*hdr);
1873                 for_each_opt_hdr_v1 (ohdr, hdr)
1874                         hdrsz += opt_hdr_v1_size(ohdr);
1875         }
1876
1877         data = (uint8_t *)img + srcaddr;
1878         datasz = *size - srcaddr;
1879
1880         /* only move data if there is not enough space */
1881         if (hdrsz + grow > srcaddr) {
1882                 size_t need = hdrsz + grow - srcaddr;
1883
1884                 /* move data by enough bytes */
1885                 memmove(data + need, data, datasz);
1886
1887                 hdr->srcaddr = cpu_to_le32(srcaddr + need);
1888                 *size += need;
1889         }
1890
1891         if (kwbimage_version(img) == 1) {
1892                 hdrsz += grow;
1893                 if (hdrsz > kwbheader_size(img)) {
1894                         hdr->headersz_msb = hdrsz >> 16;
1895                         hdr->headersz_lsb = cpu_to_le16(hdrsz & 0xffff);
1896                 }
1897         }
1898 }
1899
1900 static void *
1901 kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
1902 {
1903         struct main_hdr_v1 *hdr = img;
1904         struct opt_hdr_v1 *ohdr;
1905         uint32_t num_args;
1906         uint32_t offset;
1907         uint32_t ohdrsz;
1908         uint8_t *prev_ext;
1909
1910         if (hdr->ext) {
1911                 for_each_opt_hdr_v1 (ohdr, img)
1912                         if (opt_hdr_v1_next(ohdr) == NULL)
1913                                 break;
1914
1915                 prev_ext = opt_hdr_v1_ext(ohdr);
1916                 ohdr = _opt_hdr_v1_next(ohdr);
1917         } else {
1918                 ohdr = (void *)(hdr + 1);
1919                 prev_ext = &hdr->ext;
1920         }
1921
1922         /*
1923          * ARM executable code inside the BIN header on some mvebu platforms
1924          * (e.g. A370, AXP) must always be aligned with the 128-bit boundary.
1925          * This requirement can be met by inserting dummy arguments into
1926          * BIN header, if needed.
1927          */
1928         offset = &ohdr->data[4] - (char *)img;
1929         num_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
1930
1931         ohdrsz = sizeof(*ohdr) + 4 + 4 * num_args + binsz + 4;
1932         kwboot_img_grow_hdr(hdr, size, ohdrsz);
1933
1934         *prev_ext = 1;
1935
1936         ohdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1937         ohdr->headersz_msb = ohdrsz >> 16;
1938         ohdr->headersz_lsb = cpu_to_le16(ohdrsz & 0xffff);
1939
1940         memset(&ohdr->data[0], 0, ohdrsz - sizeof(*ohdr));
1941         *(uint32_t *)&ohdr->data[0] = cpu_to_le32(num_args);
1942
1943         return &ohdr->data[4 + 4 * num_args];
1944 }
1945
1946 static void
1947 _inject_baudrate_change_code(void *img, size_t *size, int for_data,
1948                              int old_baud, int new_baud)
1949 {
1950         struct main_hdr_v1 *hdr = img;
1951         uint32_t orig_datasz;
1952         uint32_t codesz;
1953         uint8_t *code;
1954
1955         if (for_data) {
1956                 orig_datasz = le32_to_cpu(hdr->blocksize) - sizeof(uint32_t);
1957
1958                 codesz = sizeof(kwboot_baud_code) +
1959                          sizeof(kwboot_baud_code_data_jump);
1960                 code = kwboot_img_grow_data_right(img, size, codesz);
1961         } else {
1962                 codesz = sizeof(kwboot_baud_code_binhdr_pre) +
1963                          sizeof(kwboot_baud_code) +
1964                          sizeof(kwboot_baud_code_binhdr_post);
1965                 code = kwboot_add_bin_ohdr_v1(img, size, codesz);
1966
1967                 codesz = sizeof(kwboot_baud_code_binhdr_pre);
1968                 memcpy(code, kwboot_baud_code_binhdr_pre, codesz);
1969                 code += codesz;
1970         }
1971
1972         codesz = sizeof(kwboot_baud_code) - 2 * sizeof(uint32_t);
1973         memcpy(code, kwboot_baud_code, codesz);
1974         code += codesz;
1975         *(uint32_t *)code = cpu_to_le32(old_baud);
1976         code += sizeof(uint32_t);
1977         *(uint32_t *)code = cpu_to_le32(new_baud);
1978         code += sizeof(uint32_t);
1979
1980         if (for_data) {
1981                 codesz = sizeof(kwboot_baud_code_data_jump) - sizeof(uint32_t);
1982                 memcpy(code, kwboot_baud_code_data_jump, codesz);
1983                 code += codesz;
1984                 *(uint32_t *)code = hdr->execaddr;
1985                 code += sizeof(uint32_t);
1986                 hdr->execaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) + orig_datasz);
1987         } else {
1988                 codesz = sizeof(kwboot_baud_code_binhdr_post);
1989                 memcpy(code, kwboot_baud_code_binhdr_post, codesz);
1990                 code += codesz;
1991         }
1992 }
1993
1994 static int
1995 kwboot_img_guess_sata_blksz(void *img, uint32_t blkoff, uint32_t data_size, size_t total_size)
1996 {
1997         uint32_t sum, *ptr, *end;
1998         int blksz;
1999
2000         /*
2001          * Try all possible sector sizes which are power of two,
2002          * at least 512 bytes and up to the 32 kB.
2003          */
2004         for (blksz = 512; blksz < 0x10000; blksz *= 2) {
2005                 if (blkoff * blksz > total_size ||
2006                     blkoff * blksz + data_size > total_size ||
2007                     data_size % 4)
2008                         break;
2009
2010                 /*
2011                  * Calculate data checksum and if it matches
2012                  * then tried blksz should be correct.
2013                  */
2014                 ptr = img + blkoff * blksz;
2015                 end = (void *)ptr + data_size - 4;
2016                 for (sum = 0; ptr < end; ptr++)
2017                         sum += *ptr;
2018
2019                 if (sum == *end)
2020                         return blksz;
2021         }
2022
2023         /* Fallback to 512 bytes */
2024         return 512;
2025 }
2026
2027 static const char *
2028 kwboot_img_type(uint8_t blockid)
2029 {
2030         switch (blockid) {
2031         case IBR_HDR_I2C_ID: return "I2C";
2032         case IBR_HDR_SPI_ID: return "SPI";
2033         case IBR_HDR_NAND_ID: return "NAND";
2034         case IBR_HDR_SATA_ID: return "SATA";
2035         case IBR_HDR_PEX_ID: return "PEX";
2036         case IBR_HDR_UART_ID: return "UART";
2037         case IBR_HDR_SDIO_ID: return "SDIO";
2038         default: return "unknown";
2039         }
2040 }
2041
2042 static int
2043 kwboot_img_patch(void *img, size_t *size, int baudrate)
2044 {
2045         struct main_hdr_v1 *hdr;
2046         struct opt_hdr_v1 *ohdr;
2047         uint32_t srcaddr;
2048         uint8_t csum;
2049         size_t hdrsz;
2050         int image_ver;
2051         int is_secure;
2052
2053         hdr = img;
2054
2055         if (*size < sizeof(struct main_hdr_v1)) {
2056                 fprintf(stderr, "Invalid image header size\n");
2057                 goto err;
2058         }
2059
2060         image_ver = kwbimage_version(img);
2061         if (image_ver != 0 && image_ver != 1) {
2062                 fprintf(stderr, "Invalid image header version\n");
2063                 goto err;
2064         }
2065
2066         hdrsz = kwbheader_size(hdr);
2067
2068         if (*size < hdrsz) {
2069                 fprintf(stderr, "Invalid image header size\n");
2070                 goto err;
2071         }
2072
2073         kwboot_printv("Detected kwbimage v%d with %s boot signature\n", image_ver, kwboot_img_type(hdr->blockid));
2074
2075         csum = kwboot_hdr_csum8(hdr) - hdr->checksum;
2076         if (csum != hdr->checksum) {
2077                 fprintf(stderr, "Image has invalid header checksum stored in image header\n");
2078                 goto err;
2079         }
2080
2081         srcaddr = le32_to_cpu(hdr->srcaddr);
2082
2083         switch (hdr->blockid) {
2084         case IBR_HDR_SATA_ID:
2085                 hdr->srcaddr = cpu_to_le32(srcaddr * kwboot_img_guess_sata_blksz(img, srcaddr, le32_to_cpu(hdr->blocksize), *size));
2086                 break;
2087
2088         case IBR_HDR_PEX_ID:
2089                 if (srcaddr == 0xFFFFFFFF)
2090                         hdr->srcaddr = cpu_to_le32(hdrsz);
2091                 break;
2092
2093         case IBR_HDR_SPI_ID:
2094                 if (hdr->destaddr == cpu_to_le32(0xFFFFFFFF)) {
2095                         kwboot_printv("Patching destination and execution addresses from SPI/NOR XIP area to DDR area 0x00800000\n");
2096                         hdr->destaddr = cpu_to_le32(0x00800000 + le32_to_cpu(hdr->srcaddr));
2097                         hdr->execaddr = cpu_to_le32(0x00800000 + le32_to_cpu(hdr->execaddr));
2098                 }
2099                 break;
2100         }
2101
2102         if (hdrsz > le32_to_cpu(hdr->srcaddr)) {
2103                 fprintf(stderr, "Image has invalid data offset stored in image header\n");
2104                 goto err;
2105         }
2106
2107         if (*size < le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize)) {
2108                 fprintf(stderr, "Image has invalid data size stored in image header\n");
2109                 goto err;
2110         }
2111
2112         for_each_opt_hdr_v1 (ohdr, hdr) {
2113                 if (!opt_hdr_v1_valid_size(ohdr, (const uint8_t *)hdr + hdrsz)) {
2114                         fprintf(stderr, "Invalid optional image header\n");
2115                         goto err;
2116                 }
2117         }
2118
2119         /*
2120          * The 32-bit data checksum is optional for UART image. If it is not
2121          * present (checksum detected as invalid) then grow data part of the
2122          * image for the checksum, so it can be inserted there.
2123          */
2124         if (kwboot_img_csum32(img) != *kwboot_img_csum32_ptr(img)) {
2125                 if (hdr->blockid != IBR_HDR_UART_ID) {
2126                         fprintf(stderr, "Image has invalid data checksum\n");
2127                         goto err;
2128                 }
2129                 kwboot_img_grow_data_right(img, size, sizeof(uint32_t));
2130                 /* Update the 32-bit data checksum */
2131                 *kwboot_img_csum32_ptr(img) = kwboot_img_csum32(img);
2132         }
2133
2134         if (!kwboot_img_has_ddr_init(img) &&
2135             (le32_to_cpu(hdr->destaddr) < 0x40000000 ||
2136              le32_to_cpu(hdr->destaddr) + le32_to_cpu(hdr->blocksize) > 0x40034000)) {
2137                 fprintf(stderr, "Image does not contain DDR init code needed for UART booting\n");
2138                 goto err;
2139         }
2140
2141         is_secure = kwboot_img_is_secure(img);
2142
2143         if (hdr->blockid != IBR_HDR_UART_ID) {
2144                 if (is_secure) {
2145                         fprintf(stderr,
2146                                 "Image has secure header with signature for non-UART booting\n");
2147                         goto err;
2148                 }
2149
2150                 kwboot_printv("Patching image boot signature to UART\n");
2151                 hdr->blockid = IBR_HDR_UART_ID;
2152         }
2153
2154         if (!is_secure) {
2155                 if (image_ver == 1) {
2156                         /*
2157                          * Tell BootROM to send BootROM messages to UART port
2158                          * number 0 (used also for UART booting) with default
2159                          * baudrate (which should be 115200) and do not touch
2160                          * UART MPP configuration.
2161                          */
2162                         hdr->flags |= 0x1;
2163                         hdr->options &= ~0x1F;
2164                         hdr->options |= MAIN_HDR_V1_OPT_BAUD_DEFAULT;
2165                         hdr->options |= 0 << 3;
2166                 }
2167                 if (image_ver == 0)
2168                         ((struct main_hdr_v0 *)img)->nandeccmode = IBR_HDR_ECC_DISABLED;
2169                 hdr->nandpagesize = 0;
2170         }
2171
2172         if (baudrate) {
2173                 if (image_ver == 0) {
2174                         fprintf(stderr,
2175                                 "Cannot inject code for changing baudrate into v0 image header\n");
2176                         goto err;
2177                 }
2178
2179                 if (is_secure) {
2180                         fprintf(stderr,
2181                                 "Cannot inject code for changing baudrate into image with secure header\n");
2182                         goto err;
2183                 }
2184
2185                 /*
2186                  * First inject code that changes the baudrate from the default
2187                  * value of 115200 Bd to requested value. This code is inserted
2188                  * as a new opt hdr, so it is executed by BootROM after the
2189                  * header part is received.
2190                  */
2191                 kwboot_printv("Injecting binary header code for changing baudrate to %d Bd\n",
2192                               baudrate);
2193                 _inject_baudrate_change_code(img, size, 0, 115200, baudrate);
2194
2195                 /*
2196                  * Now inject code that changes the baudrate back to 115200 Bd.
2197                  * This code is appended after the data part of the image, and
2198                  * execaddr is changed so that it is executed before U-Boot
2199                  * proper.
2200                  */
2201                 kwboot_printv("Injecting code for changing baudrate back\n");
2202                 _inject_baudrate_change_code(img, size, 1, baudrate, 115200);
2203
2204                 /* Update the 32-bit data checksum */
2205                 *kwboot_img_csum32_ptr(img) = kwboot_img_csum32(img);
2206
2207                 /* recompute header size */
2208                 hdrsz = kwbheader_size(hdr);
2209         }
2210
2211         if (hdrsz % KWBOOT_XM_BLKSZ) {
2212                 size_t grow = KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ;
2213
2214                 if (is_secure) {
2215                         fprintf(stderr, "Cannot align image with secure header\n");
2216                         goto err;
2217                 }
2218
2219                 kwboot_printv("Aligning image header to Xmodem block size\n");
2220                 kwboot_img_grow_hdr(img, size, grow);
2221                 hdrsz += grow;
2222
2223                 /*
2224                  * kwbimage v1 contains header size field and for UART type it
2225                  * must be set to the aligned xmodem header size because BootROM
2226                  * rounds header size down to xmodem block size.
2227                  */
2228                 if (kwbimage_version(img) == 1) {
2229                         hdr->headersz_msb = hdrsz >> 16;
2230                         hdr->headersz_lsb = cpu_to_le16(hdrsz & 0xffff);
2231                 }
2232         }
2233
2234         /* Header size and source address must be same for UART type due to A38x BootROM bug */
2235         if (hdrsz != le32_to_cpu(hdr->srcaddr)) {
2236                 if (is_secure) {
2237                         fprintf(stderr, "Cannot align image with secure header\n");
2238                         goto err;
2239                 }
2240
2241                 kwboot_printv("Removing gap between image header and data\n");
2242                 memmove(img + hdrsz, img + le32_to_cpu(hdr->srcaddr), le32_to_cpu(hdr->blocksize));
2243                 hdr->srcaddr = cpu_to_le32(hdrsz);
2244         }
2245
2246         hdr->checksum = kwboot_hdr_csum8(hdr) - csum;
2247
2248         *size = le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize);
2249         return 0;
2250 err:
2251         errno = EINVAL;
2252         return -1;
2253 }
2254
2255 static void
2256 kwboot_usage(FILE *stream, char *progname)
2257 {
2258         fprintf(stream,
2259                 "Usage: %s [OPTIONS] [-b <image> | -D <image> | -b | -d ] [-B <baud> ] [-t] <TTY>\n",
2260                 progname);
2261         fprintf(stream, "\n");
2262         fprintf(stream,
2263                 "  -b <image>: boot <image> with preamble (Kirkwood, Avanta, Armada 370/XP/375/38x/39x)\n");
2264         fprintf(stream,
2265                 "  -D <image>: boot <image> without preamble (Dove)\n");
2266         fprintf(stream, "  -b: enter xmodem boot mode\n");
2267         fprintf(stream, "  -d: enter console debug mode\n");
2268         fprintf(stream, "  -a: use timings for Armada XP\n");
2269         fprintf(stream, "  -s <resp-timeo>: use specific response-timeout\n");
2270         fprintf(stream,
2271                 "  -o <block-timeo>: use specific xmodem block timeout\n");
2272         fprintf(stream, "\n");
2273         fprintf(stream, "  -t: mini terminal\n");
2274         fprintf(stream, "\n");
2275         fprintf(stream, "  -B <baud>: set baud rate\n");
2276         fprintf(stream, "\n");
2277 }
2278
2279 int
2280 main(int argc, char **argv)
2281 {
2282         const char *ttypath, *imgpath;
2283         int rv, rc, tty, term;
2284         int bootmsg;
2285         int debugmsg;
2286         void *img;
2287         size_t size;
2288         size_t after_img_rsv;
2289         int baudrate;
2290         int prev_optind;
2291         int c;
2292
2293         rv = 1;
2294         tty = -1;
2295         bootmsg = 0;
2296         debugmsg = 0;
2297         imgpath = NULL;
2298         img = NULL;
2299         term = 0;
2300         size = 0;
2301         after_img_rsv = KWBOOT_XM_BLKSZ;
2302         baudrate = 115200;
2303
2304         printf("kwboot version %s\n", PLAIN_VERSION);
2305
2306         kwboot_verbose = isatty(STDOUT_FILENO);
2307
2308         do {
2309                 prev_optind = optind;
2310                 c = getopt(argc, argv, "hbptaB:dD:q:s:o:");
2311                 if (c < 0)
2312                         break;
2313
2314                 switch (c) {
2315                 case 'b':
2316                         if (imgpath || bootmsg || debugmsg)
2317                                 goto usage;
2318                         bootmsg = 1;
2319                         if (prev_optind == optind)
2320                                 goto usage;
2321                         /* Option -b could have optional argument which specify image path */
2322                         if (optind < argc && argv[optind] && argv[optind][0] != '-')
2323                                 imgpath = argv[optind++];
2324                         break;
2325
2326                 case 'D':
2327                         if (imgpath || bootmsg || debugmsg)
2328                                 goto usage;
2329                         bootmsg = 0;
2330                         imgpath = optarg;
2331                         break;
2332
2333                 case 'd':
2334                         if (imgpath || bootmsg || debugmsg)
2335                                 goto usage;
2336                         debugmsg = 1;
2337                         break;
2338
2339                 case 'p':
2340                         /* nop, for backward compatibility */
2341                         break;
2342
2343                 case 't':
2344                         term = 1;
2345                         break;
2346
2347                 case 'a':
2348                         msg_rsp_timeo = KWBOOT_MSG_RSP_TIMEO_AXP;
2349                         break;
2350
2351                 case 'q':
2352                         /* nop, for backward compatibility */
2353                         break;
2354
2355                 case 's':
2356                         msg_rsp_timeo = atoi(optarg);
2357                         break;
2358
2359                 case 'o':
2360                         blk_rsp_timeo = atoi(optarg);
2361                         break;
2362
2363                 case 'B':
2364                         baudrate = atoi(optarg);
2365                         break;
2366
2367                 case 'h':
2368                         rv = 0;
2369                 default:
2370                         goto usage;
2371                 }
2372         } while (1);
2373
2374         if (!bootmsg && !term && !debugmsg && !imgpath)
2375                 goto usage;
2376
2377         /*
2378          * If there is no remaining argument but optional imgpath was parsed
2379          * then it means that optional imgpath was eaten by getopt parser.
2380          * Reassing imgpath to required ttypath argument.
2381          */
2382         if (optind == argc && imgpath) {
2383                 ttypath = imgpath;
2384                 imgpath = NULL;
2385         } else if (optind + 1 == argc) {
2386                 ttypath = argv[optind];
2387         } else {
2388                 goto usage;
2389         }
2390
2391         /* boot and debug message use baudrate 115200 */
2392         if (((bootmsg && !imgpath) || debugmsg) && baudrate != 115200) {
2393                 fprintf(stderr, "Baudrate other than 115200 cannot be used for this operation.\n");
2394                 goto usage;
2395         }
2396
2397         tty = kwboot_open_tty(ttypath, baudrate);
2398         if (tty < 0) {
2399                 perror(ttypath);
2400                 goto out;
2401         }
2402
2403         /*
2404          * initial baudrate for image transfer is always 115200,
2405          * the change to different baudrate is done only after the header is sent
2406          */
2407         if (imgpath && baudrate != 115200) {
2408                 rc = kwboot_tty_change_baudrate(tty, 115200);
2409                 if (rc) {
2410                         perror(ttypath);
2411                         goto out;
2412                 }
2413         }
2414
2415         if (baudrate == 115200)
2416                 /* do not change baudrate during Xmodem to the same value */
2417                 baudrate = 0;
2418         else
2419                 /* ensure we have enough space for baudrate change code */
2420                 after_img_rsv += sizeof(struct opt_hdr_v1) + 8 + 16 +
2421                                  sizeof(kwboot_baud_code_binhdr_pre) +
2422                                  sizeof(kwboot_baud_code) +
2423                                  sizeof(kwboot_baud_code_binhdr_post) +
2424                                  KWBOOT_XM_BLKSZ +
2425                                  sizeof(kwboot_baud_code) +
2426                                  sizeof(kwboot_baud_code_data_jump) +
2427                                  sizeof(uint32_t) +
2428                                  KWBOOT_XM_BLKSZ;
2429
2430         if (imgpath) {
2431                 img = kwboot_read_image(imgpath, &size, after_img_rsv);
2432                 if (!img) {
2433                         perror(imgpath);
2434                         goto out;
2435                 }
2436
2437                 rc = kwboot_img_patch(img, &size, baudrate);
2438                 if (rc) {
2439                         fprintf(stderr, "%s: Invalid image.\n", imgpath);
2440                         goto out;
2441                 }
2442         }
2443
2444         if (debugmsg) {
2445                 rc = kwboot_debugmsg(tty);
2446                 if (rc)
2447                         goto out;
2448         } else if (bootmsg) {
2449                 rc = kwboot_bootmsg(tty);
2450                 if (rc)
2451                         goto out;
2452         }
2453
2454         if (img) {
2455                 rc = kwboot_xmodem(tty, img, size, baudrate);
2456                 if (rc) {
2457                         perror("xmodem");
2458                         goto out;
2459                 }
2460         }
2461
2462         if (term) {
2463                 rc = kwboot_terminal(tty);
2464                 if (rc && !(errno == EINTR)) {
2465                         perror("terminal");
2466                         goto out;
2467                 }
2468         }
2469
2470         rv = 0;
2471 out:
2472         if (tty >= 0)
2473                 close(tty);
2474
2475         if (img)
2476                 free(img);
2477
2478         return rv;
2479
2480 usage:
2481         kwboot_usage(rv ? stderr : stdout, basename(argv[0]));
2482         goto out;
2483 }