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