tools/kwboot.c

   1 /*
   2  * Boot a Marvell SoC, with Xmodem over UART0.
   3  *  supports Kirkwood, Dove, Armada 370, Armada XP, Armada 375, Armada 38x and
   4  *           Armada 39x
   5  *
   6  * (c) 2012 Daniel Stodden <daniel.stodden@gmail.com>
   7  *
   8  * References: marvell.com, "88F6180, 88F6190, 88F6192, and 88F6281
   9  *   Integrated Controller: Functional Specifications" December 2,
  10  *   2008. Chapter 24.2 "BootROM Firmware".
  11  */
  12
  13 #include "kwbimage.h"
  14 #include "mkimage.h"
  15 #include "version.h"
  16
  17 #include <stdlib.h>
  18 #include <stdio.h>
  19 #include <string.h>
  20 #include <stdarg.h>
  21 #include <image.h>
  22 #include <libgen.h>
  23 #include <fcntl.h>
  24 #include <errno.h>
  25 #include <unistd.h>
  26 #include <stdint.h>
  27 #include <time.h>
  28 #include <sys/stat.h>
  29
  30 #ifdef __linux__
  31 #include "termios_linux.h"
  32 #else
  33 #include <termios.h>
  34 #endif
  35
  36 /*
  37  * Marvell BootROM UART Sensing
  38  */
  39
  40 static unsigned char kwboot_msg_boot[] = {
  41         0xBB, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77
  42 };
  43
  44 static unsigned char kwboot_msg_debug[] = {
  45         0xDD, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77
  46 };
  47
  48 /* Defines known to work on Kirkwood */
  49 #define KWBOOT_MSG_REQ_DELAY    10 /* ms */
  50 #define KWBOOT_MSG_RSP_TIMEO    50 /* ms */
  51
  52 /* Defines known to work on Armada XP */
  53 #define KWBOOT_MSG_REQ_DELAY_AXP        1000 /* ms */
  54 #define KWBOOT_MSG_RSP_TIMEO_AXP        1000 /* ms */
  55
  56 /*
  57  * Xmodem Transfers
  58  */
  59
  60 #define SOH     1       /* sender start of block header */
  61 #define EOT     4       /* sender end of block transfer */
  62 #define ACK     6       /* target block ack */
  63 #define NAK     21      /* target block negative ack */
  64 #define CAN     24      /* target/sender transfer cancellation */
  65
  66 #define KWBOOT_XM_BLKSZ 128 /* xmodem block size */
  67
  68 struct kwboot_block {
  69         uint8_t soh;
  70         uint8_t pnum;
  71         uint8_t _pnum;
  72         uint8_t data[KWBOOT_XM_BLKSZ];
  73         uint8_t csum;
  74 } __packed;
  75
  76 #define KWBOOT_BLK_RSP_TIMEO 1000 /* ms */
  77 #define KWBOOT_HDR_RSP_TIMEO 10000 /* ms */
  78
  79 /* ARM code making baudrate changing function return to original exec address */
  80 static unsigned char kwboot_pre_baud_code[] = {
  81                                 /* exec_addr:                                 */
  82         0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
  83         0x0c, 0xe0, 0x1f, 0xe5, /* ldr lr, exec_addr                          */
  84 };
  85
  86 /* ARM code for binary header injection to change baudrate */
  87 static unsigned char kwboot_baud_code[] = {
  88                                 /* ; #define UART_BASE 0xd0012000             */
  89                                 /* ; #define THR       0x00                   */
  90                                 /* ; #define DLL       0x00                   */
  91                                 /* ; #define DLH       0x04                   */
  92                                 /* ; #define LCR       0x0c                   */
  93                                 /* ; #define   DLAB    0x80                   */
  94                                 /* ; #define LSR       0x14                   */
  95                                 /* ; #define   THRE    0x20                   */
  96                                 /* ; #define   TEMT    0x40                   */
  97                                 /* ; #define DIV_ROUND(a, b) ((a + b/2) / b)  */
  98                                 /* ;                                          */
  99                                 /* ; u32 set_baudrate(u32 old_b, u32 new_b) { */
 100                                 /* ;   const u8 *str = "$baudratechange";     */
 101                                 /* ;   u8 c;                                  */
 102                                 /* ;   do {                                   */
 103                                 /* ;       c = *str++;                        */
 104                                 /* ;       writel(UART_BASE + THR, c);        */
 105                                 /* ;   } while (c);                           */
 106                                 /* ;   while                                  */
 107                                 /* ;      (!(readl(UART_BASE + LSR) & TEMT)); */
 108                                 /* ;   u32 lcr = readl(UART_BASE + LCR);      */
 109                                 /* ;   writel(UART_BASE + LCR, lcr | DLAB);   */
 110                                 /* ;   u8 old_dll = readl(UART_BASE + DLL);   */
 111                                 /* ;   u8 old_dlh = readl(UART_BASE + DLH);   */
 112                                 /* ;   u16 old_dl = old_dll | (old_dlh << 8); */
 113                                 /* ;   u32 clk = old_b * old_dl;              */
 114                                 /* ;   u16 new_dl = DIV_ROUND(clk, new_b);    */
 115                                 /* ;   u8 new_dll = new_dl & 0xff;            */
 116                                 /* ;   u8 new_dlh = (new_dl >> 8) & 0xff;     */
 117                                 /* ;   writel(UART_BASE + DLL, new_dll);      */
 118                                 /* ;   writel(UART_BASE + DLH, new_dlh);      */
 119                                 /* ;   writel(UART_BASE + LCR, lcr & ~DLAB);  */
 120                                 /* ;   msleep(1);                             */
 121                                 /* ;   return 0;                              */
 122                                 /* ; }                                        */
 123
 124         0xfe, 0x5f, 0x2d, 0xe9, /* push  { r1 - r12, lr }                     */
 125
 126                                 /*  ; r0 = UART_BASE                          */
 127         0x02, 0x0a, 0xa0, 0xe3, /* mov   r0, #0x2000                          */
 128         0x01, 0x00, 0x4d, 0xe3, /* movt  r0, #0xd001                          */
 129
 130                                 /*  ; r2 = address of preamble string         */
 131         0xd0, 0x20, 0x8f, 0xe2, /* adr   r2, preamble                         */
 132
 133                                 /*  ; Send preamble string over UART          */
 134                                 /* .Lloop_preamble:                           */
 135                                 /*                                            */
 136                                 /*  ; Wait until Transmitter Holding is Empty */
 137                                 /* .Lloop_thre:                               */
 138                                 /*  ; r1 = UART_BASE[LSR] & THRE              */
 139         0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
 140         0x20, 0x00, 0x11, 0xe3, /* tst   r1, #0x20                            */
 141         0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_thre                          */
 142
 143                                 /*  ; Put character into Transmitter FIFO     */
 144                                 /*  ; r1 = *r2++                              */
 145         0x01, 0x10, 0xd2, 0xe4, /* ldrb  r1, [r2], #1                         */
 146                                 /*  ; UART_BASE[THR] = r1                     */
 147         0x00, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0]                       */
 148
 149                                 /*  ; Loop until end of preamble string       */
 150         0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
 151         0xf8, 0xff, 0xff, 0x1a, /* bne   .Lloop_preamble                      */
 152
 153                                 /*  ; Wait until Transmitter FIFO is Empty    */
 154                                 /* .Lloop_txempty:                            */
 155                                 /*  ; r1 = UART_BASE[LSR] & TEMT              */
 156         0x14, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x14]                      */
 157         0x40, 0x00, 0x11, 0xe3, /* tst   r1, #0x40                            */
 158         0xfc, 0xff, 0xff, 0x0a, /* beq   .Lloop_txempty                       */
 159
 160                                 /*  ; Set Divisor Latch Access Bit            */
 161                                 /*  ; UART_BASE[LCR] |= DLAB                  */
 162         0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
 163         0x80, 0x10, 0x81, 0xe3, /* orr   r1, r1, #0x80                        */
 164         0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
 165
 166                                 /*  ; Read current Divisor Latch              */
 167                                 /*  ; r1 = UART_BASE[DLH]<<8 | UART_BASE[DLL] */
 168         0x00, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x00]                      */
 169         0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
 170         0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
 171         0x04, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x04]                      */
 172         0xff, 0x10, 0x01, 0xe2, /* and   r1, r1, #0xff                        */
 173         0x41, 0x14, 0xa0, 0xe1, /* asr   r1, r1, #8                           */
 174         0x02, 0x10, 0x81, 0xe1, /* orr   r1, r1, r2                           */
 175
 176                                 /*  ; Read old baudrate value                 */
 177                                 /*  ; r2 = old_baudrate                       */
 178         0x8c, 0x20, 0x9f, 0xe5, /* ldr   r2, old_baudrate                     */
 179
 180                                 /*  ; Calculate base clock                    */
 181                                 /*  ; r1 = r2 * r1                            */
 182         0x92, 0x01, 0x01, 0xe0, /* mul   r1, r2, r1                           */
 183
 184                                 /*  ; Read new baudrate value                 */
 185                                 /*  ; r2 = baudrate                           */
 186         0x88, 0x20, 0x9f, 0xe5, /* ldr   r2, baudrate                         */
 187
 188                                 /*  ; Calculate new Divisor Latch             */
 189                                 /*  ; r1 = DIV_ROUND(r1, r2) =                */
 190                                 /*  ;    = (r1 + r2/2) / r2                   */
 191         0xa2, 0x10, 0x81, 0xe0, /* add   r1, r1, r2, lsr #1                   */
 192         0x02, 0x40, 0xa0, 0xe1, /* mov   r4, r2                               */
 193         0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
 194                                 /* .Lloop_div1:                               */
 195         0x84, 0x40, 0xa0, 0x91, /* movls r4, r4, lsl #1                       */
 196         0xa1, 0x00, 0x54, 0xe1, /* cmp   r4, r1, lsr #1                       */
 197         0xfc, 0xff, 0xff, 0x9a, /* bls   .Lloop_div1                          */
 198         0x00, 0x30, 0xa0, 0xe3, /* mov   r3, #0                               */
 199                                 /* .Lloop_div2:                               */
 200         0x04, 0x00, 0x51, 0xe1, /* cmp   r1, r4                               */
 201         0x04, 0x10, 0x41, 0x20, /* subhs r1, r1, r4                           */
 202         0x03, 0x30, 0xa3, 0xe0, /* adc   r3, r3, r3                           */
 203         0xa4, 0x40, 0xa0, 0xe1, /* mov   r4, r4, lsr #1                       */
 204         0x02, 0x00, 0x54, 0xe1, /* cmp   r4, r2                               */
 205         0xf9, 0xff, 0xff, 0x2a, /* bhs   .Lloop_div2                          */
 206         0x03, 0x10, 0xa0, 0xe1, /* mov   r1, r3                               */
 207
 208                                 /*  ; Set new Divisor Latch Low               */
 209                                 /*  ; UART_BASE[DLL] = r1 & 0xff              */
 210         0x01, 0x20, 0xa0, 0xe1, /* mov   r2, r1                               */
 211         0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
 212         0x00, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x00]                      */
 213
 214                                 /*  ; Set new Divisor Latch High              */
 215                                 /*  ; UART_BASE[DLH] = r1>>8 & 0xff           */
 216         0x41, 0x24, 0xa0, 0xe1, /* asr   r2, r1, #8                           */
 217         0xff, 0x20, 0x02, 0xe2, /* and   r2, r2, #0xff                        */
 218         0x04, 0x20, 0x80, 0xe5, /* str   r2, [r0, #0x04]                      */
 219
 220                                 /*  ; Clear Divisor Latch Access Bit          */
 221                                 /*  ; UART_BASE[LCR] &= ~DLAB                 */
 222         0x0c, 0x10, 0x90, 0xe5, /* ldr   r1, [r0, #0x0c]                      */
 223         0x80, 0x10, 0xc1, 0xe3, /* bic   r1, r1, #0x80                        */
 224         0x0c, 0x10, 0x80, 0xe5, /* str   r1, [r0, #0x0c]                      */
 225
 226                                 /*  ; Sleep 1ms ~~ 600000 cycles at 1200 MHz  */
 227                                 /*  ; r1 = 600000                             */
 228         0x9f, 0x1d, 0xa0, 0xe3, /* mov   r1, #0x27c0                          */
 229         0x09, 0x10, 0x40, 0xe3, /* movt  r1, #0x0009                          */
 230                                 /* .Lloop_sleep:                              */
 231         0x01, 0x10, 0x41, 0xe2, /* sub   r1, r1, #1                           */
 232         0x00, 0x00, 0x51, 0xe3, /* cmp   r1, #0                               */
 233         0xfc, 0xff, 0xff, 0x1a, /* bne   .Lloop_sleep                         */
 234
 235                                 /*  ; Return 0 - no error                     */
 236         0x00, 0x00, 0xa0, 0xe3, /* mov   r0, #0                               */
 237         0xfe, 0x9f, 0xbd, 0xe8, /* pop   { r1 - r12, pc }                     */
 238
 239                                 /*  ; Preamble string                         */
 240                                 /* preamble:                                  */
 241         0x24, 0x62, 0x61, 0x75, /* .asciz "$baudratechange"                   */
 242         0x64, 0x72, 0x61, 0x74,
 243         0x65, 0x63, 0x68, 0x61,
 244         0x6e, 0x67, 0x65, 0x00,
 245
 246                                 /*  ; Placeholder for old baudrate value      */
 247                                 /* old_baudrate:                              */
 248         0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
 249
 250                                 /*  ; Placeholder for new baudrate value      */
 251                                 /* new_baudrate:                              */
 252         0x00, 0x00, 0x00, 0x00, /* .word 0                                    */
 253 };
 254
 255 #define KWBOOT_BAUDRATE_BIN_HEADER_SZ (sizeof(kwboot_baud_code) + \
 256                                        sizeof(struct opt_hdr_v1) + 8)
 257
 258 static const char kwb_baud_magic[16] = "$baudratechange";
 259
 260 static int kwboot_verbose;
 261
 262 static int msg_req_delay = KWBOOT_MSG_REQ_DELAY;
 263 static int msg_rsp_timeo = KWBOOT_MSG_RSP_TIMEO;
 264 static int blk_rsp_timeo = KWBOOT_BLK_RSP_TIMEO;
 265
 266 static ssize_t
 267 kwboot_write(int fd, const char *buf, size_t len)
 268 {
 269         size_t tot = 0;
 270
 271         while (tot < len) {
 272                 ssize_t wr = write(fd, buf + tot, len - tot);
 273
 274                 if (wr < 0)
 275                         return -1;
 276
 277                 tot += wr;
 278         }
 279
 280         return tot;
 281 }
 282
 283 static void
 284 kwboot_printv(const char *fmt, ...)
 285 {
 286         va_list ap;
 287
 288         if (kwboot_verbose) {
 289                 va_start(ap, fmt);
 290                 vprintf(fmt, ap);
 291                 va_end(ap);
 292                 fflush(stdout);
 293         }
 294 }
 295
 296 static void
 297 __spinner(void)
 298 {
 299         const char seq[] = { '-', '\\', '|', '/' };
 300         const int div = 8;
 301         static int state, bs;
 302
 303         if (state % div == 0) {
 304                 fputc(bs, stdout);
 305                 fputc(seq[state / div % sizeof(seq)], stdout);
 306                 fflush(stdout);
 307         }
 308
 309         bs = '\b';
 310         state++;
 311 }
 312
 313 static void
 314 kwboot_spinner(void)
 315 {
 316         if (kwboot_verbose)
 317                 __spinner();
 318 }
 319
 320 static void
 321 __progress(int pct, char c)
 322 {
 323         const int width = 70;
 324         static const char *nl = "";
 325         static int pos;
 326
 327         if (pos % width == 0)
 328                 printf("%s%3d %% [", nl, pct);
 329
 330         fputc(c, stdout);
 331
 332         nl = "]\n";
 333         pos = (pos + 1) % width;
 334
 335         if (pct == 100) {
 336                 while (pos && pos++ < width)
 337                         fputc(' ', stdout);
 338                 fputs(nl, stdout);
 339                 nl = "";
 340                 pos = 0;
 341         }
 342
 343         fflush(stdout);
 344
 345 }
 346
 347 static void
 348 kwboot_progress(int _pct, char c)
 349 {
 350         static int pct;
 351
 352         if (_pct != -1)
 353                 pct = _pct;
 354
 355         if (kwboot_verbose)
 356                 __progress(pct, c);
 357
 358         if (pct == 100)
 359                 pct = 0;
 360 }
 361
 362 static int
 363 kwboot_tty_recv(int fd, void *buf, size_t len, int timeo)
 364 {
 365         int rc, nfds;
 366         fd_set rfds;
 367         struct timeval tv;
 368         ssize_t n;
 369
 370         rc = -1;
 371
 372         FD_ZERO(&rfds);
 373         FD_SET(fd, &rfds);
 374
 375         tv.tv_sec = 0;
 376         tv.tv_usec = timeo * 1000;
 377         if (tv.tv_usec > 1000000) {
 378                 tv.tv_sec += tv.tv_usec / 1000000;
 379                 tv.tv_usec %= 1000000;
 380         }
 381
 382         do {
 383                 nfds = select(fd + 1, &rfds, NULL, NULL, &tv);
 384                 if (nfds < 0)
 385                         goto out;
 386                 if (!nfds) {
 387                         errno = ETIMEDOUT;
 388                         goto out;
 389                 }
 390
 391                 n = read(fd, buf, len);
 392                 if (n <= 0)
 393                         goto out;
 394
 395                 buf = (char *)buf + n;
 396                 len -= n;
 397         } while (len > 0);
 398
 399         rc = 0;
 400 out:
 401         return rc;
 402 }
 403
 404 static int
 405 kwboot_tty_send(int fd, const void *buf, size_t len)
 406 {
 407         if (!buf)
 408                 return 0;
 409
 410         if (kwboot_write(fd, buf, len) < 0)
 411                 return -1;
 412
 413         return tcdrain(fd);
 414 }
 415
 416 static int
 417 kwboot_tty_send_char(int fd, unsigned char c)
 418 {
 419         return kwboot_tty_send(fd, &c, 1);
 420 }
 421
 422 static speed_t
 423 kwboot_tty_baudrate_to_speed(int baudrate)
 424 {
 425         switch (baudrate) {
 426 #ifdef B4000000
 427         case 4000000:
 428                 return B4000000;
 429 #endif
 430 #ifdef B3500000
 431         case 3500000:
 432                 return B3500000;
 433 #endif
 434 #ifdef B3000000
 435         case 3000000:
 436                 return B3000000;
 437 #endif
 438 #ifdef B2500000
 439         case 2500000:
 440                 return B2500000;
 441 #endif
 442 #ifdef B2000000
 443         case 2000000:
 444                 return B2000000;
 445 #endif
 446 #ifdef B1500000
 447         case 1500000:
 448                 return B1500000;
 449 #endif
 450 #ifdef B1152000
 451         case 1152000:
 452                 return B1152000;
 453 #endif
 454 #ifdef B1000000
 455         case 1000000:
 456                 return B1000000;
 457 #endif
 458 #ifdef B921600
 459         case 921600:
 460                 return B921600;
 461 #endif
 462 #ifdef B614400
 463         case 614400:
 464                 return B614400;
 465 #endif
 466 #ifdef B576000
 467         case 576000:
 468                 return B576000;
 469 #endif
 470 #ifdef B500000
 471         case 500000:
 472                 return B500000;
 473 #endif
 474 #ifdef B460800
 475         case 460800:
 476                 return B460800;
 477 #endif
 478 #ifdef B307200
 479         case 307200:
 480                 return B307200;
 481 #endif
 482 #ifdef B230400
 483         case 230400:
 484                 return B230400;
 485 #endif
 486 #ifdef B153600
 487         case 153600:
 488                 return B153600;
 489 #endif
 490 #ifdef B115200
 491         case 115200:
 492                 return B115200;
 493 #endif
 494 #ifdef B76800
 495         case 76800:
 496                 return B76800;
 497 #endif
 498 #ifdef B57600
 499         case 57600:
 500                 return B57600;
 501 #endif
 502 #ifdef B38400
 503         case 38400:
 504                 return B38400;
 505 #endif
 506 #ifdef B19200
 507         case 19200:
 508                 return B19200;
 509 #endif
 510 #ifdef B9600
 511         case 9600:
 512                 return B9600;
 513 #endif
 514 #ifdef B4800
 515         case 4800:
 516                 return B4800;
 517 #endif
 518 #ifdef B2400
 519         case 2400:
 520                 return B2400;
 521 #endif
 522 #ifdef B1800
 523         case 1800:
 524                 return B1800;
 525 #endif
 526 #ifdef B1200
 527         case 1200:
 528                 return B1200;
 529 #endif
 530 #ifdef B600
 531         case 600:
 532                 return B600;
 533 #endif
 534 #ifdef B300
 535         case 300:
 536                 return B300;
 537 #endif
 538 #ifdef B200
 539         case 200:
 540                 return B200;
 541 #endif
 542 #ifdef B150
 543         case 150:
 544                 return B150;
 545 #endif
 546 #ifdef B134
 547         case 134:
 548                 return B134;
 549 #endif
 550 #ifdef B110
 551         case 110:
 552                 return B110;
 553 #endif
 554 #ifdef B75
 555         case 75:
 556                 return B75;
 557 #endif
 558 #ifdef B50
 559         case 50:
 560                 return B50;
 561 #endif
 562         default:
 563 #ifdef BOTHER
 564                 return BOTHER;
 565 #else
 566                 return B0;
 567 #endif
 568         }
 569 }
 570
 571 static int
 572 _is_within_tolerance(int value, int reference, int tolerance)
 573 {
 574         return 100 * value >= reference * (100 - tolerance) &&
 575                100 * value <= reference * (100 + tolerance);
 576 }
 577
 578 static int
 579 kwboot_tty_change_baudrate(int fd, int baudrate)
 580 {
 581         struct termios tio;
 582         speed_t speed;
 583         int rc;
 584
 585         rc = tcgetattr(fd, &tio);
 586         if (rc)
 587                 return rc;
 588
 589         speed = kwboot_tty_baudrate_to_speed(baudrate);
 590         if (speed == B0) {
 591                 errno = EINVAL;
 592                 return -1;
 593         }
 594
 595 #ifdef BOTHER
 596         if (speed == BOTHER)
 597                 tio.c_ospeed = tio.c_ispeed = baudrate;
 598 #endif
 599
 600         rc = cfsetospeed(&tio, speed);
 601         if (rc)
 602                 return rc;
 603
 604         rc = cfsetispeed(&tio, speed);
 605         if (rc)
 606                 return rc;
 607
 608         rc = tcsetattr(fd, TCSANOW, &tio);
 609         if (rc)
 610                 return rc;
 611
 612         rc = tcgetattr(fd, &tio);
 613         if (rc)
 614                 return rc;
 615
 616         if (cfgetospeed(&tio) != speed || cfgetispeed(&tio) != speed)
 617                 goto baud_fail;
 618
 619 #ifdef BOTHER
 620         /*
 621          * Check whether set baudrate is within 3% tolerance.
 622          * If BOTHER is defined, Linux always fills out c_ospeed / c_ispeed
 623          * with real values.
 624          */
 625         if (!_is_within_tolerance(tio.c_ospeed, baudrate, 3))
 626                 goto baud_fail;
 627
 628         if (!_is_within_tolerance(tio.c_ispeed, baudrate, 3))
 629                 goto baud_fail;
 630 #endif
 631
 632         return 0;
 633
 634 baud_fail:
 635         fprintf(stderr, "Could not set baudrate to requested value\n");
 636         errno = EINVAL;
 637         return -1;
 638 }
 639
 640 static int
 641 kwboot_open_tty(const char *path, int baudrate)
 642 {
 643         int rc, fd, flags;
 644         struct termios tio;
 645
 646         rc = -1;
 647
 648         fd = open(path, O_RDWR | O_NOCTTY | O_NDELAY);
 649         if (fd < 0)
 650                 goto out;
 651
 652         rc = tcgetattr(fd, &tio);
 653         if (rc)
 654                 goto out;
 655
 656         cfmakeraw(&tio);
 657         tio.c_cflag |= CREAD | CLOCAL;
 658         tio.c_cc[VMIN] = 1;
 659         tio.c_cc[VTIME] = 0;
 660
 661         rc = tcsetattr(fd, TCSANOW, &tio);
 662         if (rc)
 663                 goto out;
 664
 665         flags = fcntl(fd, F_GETFL);
 666         if (flags < 0)
 667                 goto out;
 668
 669         rc = fcntl(fd, F_SETFL, flags & ~O_NDELAY);
 670         if (rc)
 671                 goto out;
 672
 673         rc = kwboot_tty_change_baudrate(fd, baudrate);
 674         if (rc)
 675                 goto out;
 676
 677         rc = fd;
 678 out:
 679         if (rc < 0) {
 680                 if (fd >= 0)
 681                         close(fd);
 682         }
 683
 684         return rc;
 685 }
 686
 687 static int
 688 kwboot_bootmsg(int tty, void *msg)
 689 {
 690         int rc;
 691         char c;
 692         int count;
 693
 694         if (msg == NULL)
 695                 kwboot_printv("Please reboot the target into UART boot mode...");
 696         else
 697                 kwboot_printv("Sending boot message. Please reboot the target...");
 698
 699         do {
 700                 rc = tcflush(tty, TCIOFLUSH);
 701                 if (rc)
 702                         break;
 703
 704                 for (count = 0; count < 128; count++) {
 705                         rc = kwboot_tty_send(tty, msg, 8);
 706                         if (rc) {
 707                                 usleep(msg_req_delay * 1000);
 708                                 continue;
 709                         }
 710                 }
 711
 712                 rc = kwboot_tty_recv(tty, &c, 1, msg_rsp_timeo);
 713
 714                 kwboot_spinner();
 715
 716         } while (rc || c != NAK);
 717
 718         kwboot_printv("\n");
 719
 720         return rc;
 721 }
 722
 723 static int
 724 kwboot_debugmsg(int tty, void *msg)
 725 {
 726         int rc;
 727
 728         kwboot_printv("Sending debug message. Please reboot the target...");
 729
 730         do {
 731                 char buf[16];
 732
 733                 rc = tcflush(tty, TCIOFLUSH);
 734                 if (rc)
 735                         break;
 736
 737                 rc = kwboot_tty_send(tty, msg, 8);
 738                 if (rc) {
 739                         usleep(msg_req_delay * 1000);
 740                         continue;
 741                 }
 742
 743                 rc = kwboot_tty_recv(tty, buf, 16, msg_rsp_timeo);
 744
 745                 kwboot_spinner();
 746
 747         } while (rc);
 748
 749         kwboot_printv("\n");
 750
 751         return rc;
 752 }
 753
 754 static size_t
 755 kwboot_xm_makeblock(struct kwboot_block *block, const void *data,
 756                     size_t size, int pnum)
 757 {
 758         size_t i, n;
 759
 760         block->soh = SOH;
 761         block->pnum = pnum;
 762         block->_pnum = ~block->pnum;
 763
 764         n = size < KWBOOT_XM_BLKSZ ? size : KWBOOT_XM_BLKSZ;
 765         memcpy(&block->data[0], data, n);
 766         memset(&block->data[n], 0, KWBOOT_XM_BLKSZ - n);
 767
 768         block->csum = 0;
 769         for (i = 0; i < n; i++)
 770                 block->csum += block->data[i];
 771
 772         return n;
 773 }
 774
 775 static uint64_t
 776 _now(void)
 777 {
 778         struct timespec ts;
 779
 780         if (clock_gettime(CLOCK_MONOTONIC, &ts)) {
 781                 static int err_print;
 782
 783                 if (!err_print) {
 784                         perror("clock_gettime() does not work");
 785                         err_print = 1;
 786                 }
 787
 788                 /* this will just make the timeout not work */
 789                 return -1ULL;
 790         }
 791
 792         return ts.tv_sec * 1000ULL + (ts.tv_nsec + 500000) / 1000000;
 793 }
 794
 795 static int
 796 _is_xm_reply(char c)
 797 {
 798         return c == ACK || c == NAK || c == CAN;
 799 }
 800
 801 static int
 802 _xm_reply_to_error(int c)
 803 {
 804         int rc = -1;
 805
 806         switch (c) {
 807         case ACK:
 808                 rc = 0;
 809                 break;
 810         case NAK:
 811                 errno = EBADMSG;
 812                 break;
 813         case CAN:
 814                 errno = ECANCELED;
 815                 break;
 816         default:
 817                 errno = EPROTO;
 818                 break;
 819         }
 820
 821         return rc;
 822 }
 823
 824 static int
 825 kwboot_baud_magic_handle(int fd, char c, int baudrate)
 826 {
 827         static size_t rcv_len;
 828
 829         if (rcv_len < sizeof(kwb_baud_magic)) {
 830                 /* try to recognize whole magic word */
 831                 if (c == kwb_baud_magic[rcv_len]) {
 832                         rcv_len++;
 833                 } else {
 834                         printf("%.*s%c", (int)rcv_len, kwb_baud_magic, c);
 835                         fflush(stdout);
 836                         rcv_len = 0;
 837                 }
 838         }
 839
 840         if (rcv_len == sizeof(kwb_baud_magic)) {
 841                 /* magic word received */
 842                 kwboot_printv("\nChanging baudrate to %d Bd\n", baudrate);
 843
 844                 return kwboot_tty_change_baudrate(fd, baudrate) ? : 1;
 845         } else {
 846                 return 0;
 847         }
 848 }
 849
 850 static int
 851 kwboot_xm_recv_reply(int fd, char *c, int allow_non_xm, int *non_xm_print,
 852                      int baudrate, int *baud_changed)
 853 {
 854         int timeout = allow_non_xm ? KWBOOT_HDR_RSP_TIMEO : blk_rsp_timeo;
 855         uint64_t recv_until = _now() + timeout;
 856         int rc;
 857
 858         if (non_xm_print)
 859                 *non_xm_print = 0;
 860         if (baud_changed)
 861                 *baud_changed = 0;
 862
 863         while (1) {
 864                 rc = kwboot_tty_recv(fd, c, 1, timeout);
 865                 if (rc) {
 866                         if (errno != ETIMEDOUT)
 867                                 return rc;
 868                         else if (allow_non_xm && *non_xm_print)
 869                                 return -1;
 870                         else
 871                                 *c = NAK;
 872                 }
 873
 874                 /* If received xmodem reply, end. */
 875                 if (_is_xm_reply(*c))
 876                         break;
 877
 878                 /*
 879                  * If receiving/printing non-xmodem text output is allowed and
 880                  * such a byte was received, we want to increase receiving time
 881                  * and either:
 882                  * - print the byte, if it is not part of baudrate change magic
 883                  *   sequence while baudrate change was requested (-B option)
 884                  * - change baudrate
 885                  * Otherwise decrease timeout by time elapsed.
 886                  */
 887                 if (allow_non_xm) {
 888                         recv_until = _now() + timeout;
 889
 890                         if (baudrate && !*baud_changed) {
 891                                 rc = kwboot_baud_magic_handle(fd, *c, baudrate);
 892                                 if (rc == 1)
 893                                         *baud_changed = 1;
 894                                 else if (!rc)
 895                                         *non_xm_print = 1;
 896                                 else
 897                                         return rc;
 898                         } else if (!baudrate || !*baud_changed) {
 899                                 putchar(*c);
 900                                 fflush(stdout);
 901                                 *non_xm_print = 1;
 902                         }
 903                 } else {
 904                         timeout = recv_until - _now();
 905                         if (timeout < 0) {
 906                                 errno = ETIMEDOUT;
 907                                 return -1;
 908                         }
 909                 }
 910         }
 911
 912         return 0;
 913 }
 914
 915 static int
 916 kwboot_xm_sendblock(int fd, struct kwboot_block *block, int allow_non_xm,
 917                     int *done_print, int baudrate)
 918 {
 919         int non_xm_print, baud_changed;
 920         int rc, err, retries;
 921         char c;
 922
 923         *done_print = 0;
 924
 925         retries = 16;
 926         do {
 927                 rc = kwboot_tty_send(fd, block, sizeof(*block));
 928                 if (rc)
 929                         return rc;
 930
 931                 if (allow_non_xm && !*done_print) {
 932                         kwboot_progress(100, '.');
 933                         kwboot_printv("Done\n");
 934                         *done_print = 1;
 935                 }
 936
 937                 rc = kwboot_xm_recv_reply(fd, &c, allow_non_xm, &non_xm_print,
 938                                           baudrate, &baud_changed);
 939                 if (rc)
 940                         goto can;
 941
 942                 if (!allow_non_xm && c != ACK)
 943                         kwboot_progress(-1, '+');
 944         } while (c == NAK && retries-- > 0);
 945
 946         if (non_xm_print)
 947                 kwboot_printv("\n");
 948
 949         if (allow_non_xm && baudrate && !baud_changed) {
 950                 fprintf(stderr, "Baudrate was not changed\n");
 951                 rc = -1;
 952                 errno = EPROTO;
 953                 goto can;
 954         }
 955
 956         return _xm_reply_to_error(c);
 957 can:
 958         err = errno;
 959         kwboot_tty_send_char(fd, CAN);
 960         kwboot_printv("\n");
 961         errno = err;
 962         return rc;
 963 }
 964
 965 static int
 966 kwboot_xm_finish(int fd)
 967 {
 968         int rc, retries;
 969         char c;
 970
 971         kwboot_printv("Finishing transfer\n");
 972
 973         retries = 16;
 974         do {
 975                 rc = kwboot_tty_send_char(fd, EOT);
 976                 if (rc)
 977                         return rc;
 978
 979                 rc = kwboot_xm_recv_reply(fd, &c, 0, NULL, 0, NULL);
 980                 if (rc)
 981                         return rc;
 982         } while (c == NAK && retries-- > 0);
 983
 984         return _xm_reply_to_error(c);
 985 }
 986
 987 static int
 988 kwboot_xmodem_one(int tty, int *pnum, int header, const uint8_t *data,
 989                   size_t size, int baudrate)
 990 {
 991         int done_print = 0;
 992         size_t sent, left;
 993         int rc;
 994
 995         kwboot_printv("Sending boot image %s (%zu bytes)...\n",
 996                       header ? "header" : "data", size);
 997
 998         left = size;
 999         sent = 0;
1000
1001         while (sent < size) {
1002                 struct kwboot_block block;
1003                 int last_block;
1004                 size_t blksz;
1005
1006                 blksz = kwboot_xm_makeblock(&block, data, left, (*pnum)++);
1007                 data += blksz;
1008
1009                 last_block = (left <= blksz);
1010
1011                 rc = kwboot_xm_sendblock(tty, &block, header && last_block,
1012                                          &done_print, baudrate);
1013                 if (rc)
1014                         goto out;
1015
1016                 sent += blksz;
1017                 left -= blksz;
1018
1019                 if (!done_print)
1020                         kwboot_progress(sent * 100 / size, '.');
1021         }
1022
1023         if (!done_print)
1024                 kwboot_printv("Done\n");
1025
1026         return 0;
1027 out:
1028         kwboot_printv("\n");
1029         return rc;
1030 }
1031
1032 static int
1033 kwboot_xmodem(int tty, const void *_img, size_t size, int baudrate)
1034 {
1035         const uint8_t *img = _img;
1036         int rc, pnum;
1037         size_t hdrsz;
1038
1039         hdrsz = kwbheader_size(img);
1040
1041         kwboot_printv("Waiting 2s and flushing tty\n");
1042         sleep(2); /* flush isn't effective without it */
1043         tcflush(tty, TCIOFLUSH);
1044
1045         pnum = 1;
1046
1047         rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz, baudrate);
1048         if (rc)
1049                 return rc;
1050
1051         img += hdrsz;
1052         size -= hdrsz;
1053
1054         rc = kwboot_xmodem_one(tty, &pnum, 0, img, size, 0);
1055         if (rc)
1056                 return rc;
1057
1058         rc = kwboot_xm_finish(tty);
1059         if (rc)
1060                 return rc;
1061
1062         if (baudrate) {
1063                 char buf[sizeof(kwb_baud_magic)];
1064
1065                 /* Wait 1s for baudrate change magic */
1066                 rc = kwboot_tty_recv(tty, buf, sizeof(buf), 1000);
1067                 if (rc)
1068                         return rc;
1069
1070                 if (memcmp(buf, kwb_baud_magic, sizeof(buf))) {
1071                         errno = EPROTO;
1072                         return -1;
1073                 }
1074
1075                 kwboot_printv("\nChanging baudrate back to 115200 Bd\n\n");
1076                 rc = kwboot_tty_change_baudrate(tty, 115200);
1077                 if (rc)
1078                         return rc;
1079         }
1080
1081         return 0;
1082 }
1083
1084 static int
1085 kwboot_term_pipe(int in, int out, const char *quit, int *s)
1086 {
1087         ssize_t nin;
1088         char _buf[128], *buf = _buf;
1089
1090         nin = read(in, buf, sizeof(_buf));
1091         if (nin <= 0)
1092                 return -1;
1093
1094         if (quit) {
1095                 int i;
1096
1097                 for (i = 0; i < nin; i++) {
1098                         if (*buf == quit[*s]) {
1099                                 (*s)++;
1100                                 if (!quit[*s])
1101                                         return 0;
1102                                 buf++;
1103                                 nin--;
1104                         } else {
1105                                 if (kwboot_write(out, quit, *s) < 0)
1106                                         return -1;
1107                                 *s = 0;
1108                         }
1109                 }
1110         }
1111
1112         if (kwboot_write(out, buf, nin) < 0)
1113                 return -1;
1114
1115         return 0;
1116 }
1117
1118 static int
1119 kwboot_terminal(int tty)
1120 {
1121         int rc, in, s;
1122         const char *quit = "\34c";
1123         struct termios otio, tio;
1124
1125         rc = -1;
1126
1127         in = STDIN_FILENO;
1128         if (isatty(in)) {
1129                 rc = tcgetattr(in, &otio);
1130                 if (!rc) {
1131                         tio = otio;
1132                         cfmakeraw(&tio);
1133                         rc = tcsetattr(in, TCSANOW, &tio);
1134                 }
1135                 if (rc) {
1136                         perror("tcsetattr");
1137                         goto out;
1138                 }
1139
1140                 kwboot_printv("[Type Ctrl-%c + %c to quit]\r\n",
1141                               quit[0] | 0100, quit[1]);
1142         } else
1143                 in = -1;
1144
1145         rc = 0;
1146         s = 0;
1147
1148         do {
1149                 fd_set rfds;
1150                 int nfds = 0;
1151
1152                 FD_SET(tty, &rfds);
1153                 nfds = nfds < tty ? tty : nfds;
1154
1155                 if (in >= 0) {
1156                         FD_SET(in, &rfds);
1157                         nfds = nfds < in ? in : nfds;
1158                 }
1159
1160                 nfds = select(nfds + 1, &rfds, NULL, NULL, NULL);
1161                 if (nfds < 0)
1162                         break;
1163
1164                 if (FD_ISSET(tty, &rfds)) {
1165                         rc = kwboot_term_pipe(tty, STDOUT_FILENO, NULL, NULL);
1166                         if (rc)
1167                                 break;
1168                 }
1169
1170                 if (in >= 0 && FD_ISSET(in, &rfds)) {
1171                         rc = kwboot_term_pipe(in, tty, quit, &s);
1172                         if (rc)
1173                                 break;
1174                 }
1175         } while (quit[s] != 0);
1176
1177         if (in >= 0)
1178                 tcsetattr(in, TCSANOW, &otio);
1179         printf("\n");
1180 out:
1181         return rc;
1182 }
1183
1184 static void *
1185 kwboot_read_image(const char *path, size_t *size, size_t reserve)
1186 {
1187         int rc, fd;
1188         struct stat st;
1189         void *img;
1190         off_t tot;
1191
1192         rc = -1;
1193         img = NULL;
1194
1195         fd = open(path, O_RDONLY);
1196         if (fd < 0)
1197                 goto out;
1198
1199         rc = fstat(fd, &st);
1200         if (rc)
1201                 goto out;
1202
1203         img = malloc(st.st_size + reserve);
1204         if (!img)
1205                 goto out;
1206
1207         tot = 0;
1208         while (tot < st.st_size) {
1209                 ssize_t rd = read(fd, img + tot, st.st_size - tot);
1210
1211                 if (rd < 0)
1212                         goto out;
1213
1214                 tot += rd;
1215
1216                 if (!rd && tot < st.st_size) {
1217                         errno = EIO;
1218                         goto out;
1219                 }
1220         }
1221
1222         rc = 0;
1223         *size = st.st_size;
1224 out:
1225         if (rc && img) {
1226                 free(img);
1227                 img = NULL;
1228         }
1229         if (fd >= 0)
1230                 close(fd);
1231
1232         return img;
1233 }
1234
1235 static uint8_t
1236 kwboot_hdr_csum8(const void *hdr)
1237 {
1238         const uint8_t *data = hdr;
1239         uint8_t csum;
1240         size_t size;
1241
1242         size = kwbheader_size_for_csum(hdr);
1243
1244         for (csum = 0; size-- > 0; data++)
1245                 csum += *data;
1246
1247         return csum;
1248 }
1249
1250 static int
1251 kwboot_img_is_secure(void *img)
1252 {
1253         struct opt_hdr_v1 *ohdr;
1254
1255         for_each_opt_hdr_v1 (ohdr, img)
1256                 if (ohdr->headertype == OPT_HDR_V1_SECURE_TYPE)
1257                         return 1;
1258
1259         return 0;
1260 }
1261
1262 static void *
1263 kwboot_img_grow_data_left(void *img, size_t *size, size_t grow)
1264 {
1265         uint32_t hdrsz, datasz, srcaddr;
1266         struct main_hdr_v1 *hdr = img;
1267         uint8_t *data;
1268
1269         srcaddr = le32_to_cpu(hdr->srcaddr);
1270
1271         hdrsz = kwbheader_size(hdr);
1272         data = (uint8_t *)img + srcaddr;
1273         datasz = *size - srcaddr;
1274
1275         /* only move data if there is not enough space */
1276         if (hdrsz + grow > srcaddr) {
1277                 size_t need = hdrsz + grow - srcaddr;
1278
1279                 /* move data by enough bytes */
1280                 memmove(data + need, data, datasz);
1281                 *size += need;
1282                 srcaddr += need;
1283         }
1284
1285         srcaddr -= grow;
1286         hdr->srcaddr = cpu_to_le32(srcaddr);
1287         hdr->destaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) - grow);
1288         hdr->blocksize = cpu_to_le32(le32_to_cpu(hdr->blocksize) + grow);
1289
1290         return (uint8_t *)img + srcaddr;
1291 }
1292
1293 static void
1294 kwboot_img_grow_hdr(void *img, size_t *size, size_t grow)
1295 {
1296         uint32_t hdrsz, datasz, srcaddr;
1297         struct main_hdr_v1 *hdr = img;
1298         uint8_t *data;
1299
1300         srcaddr = le32_to_cpu(hdr->srcaddr);
1301
1302         hdrsz = kwbheader_size(img);
1303         data = (uint8_t *)img + srcaddr;
1304         datasz = *size - srcaddr;
1305
1306         /* only move data if there is not enough space */
1307         if (hdrsz + grow > srcaddr) {
1308                 size_t need = hdrsz + grow - srcaddr;
1309
1310                 /* move data by enough bytes */
1311                 memmove(data + need, data, datasz);
1312
1313                 hdr->srcaddr = cpu_to_le32(srcaddr + need);
1314                 *size += need;
1315         }
1316
1317         if (kwbimage_version(img) == 1) {
1318                 hdrsz += grow;
1319                 hdr->headersz_msb = hdrsz >> 16;
1320                 hdr->headersz_lsb = cpu_to_le16(hdrsz & 0xffff);
1321         }
1322 }
1323
1324 static void *
1325 kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
1326 {
1327         struct main_hdr_v1 *hdr = img;
1328         struct opt_hdr_v1 *ohdr;
1329         uint32_t ohdrsz;
1330
1331         ohdrsz = binsz + 8 + sizeof(*ohdr);
1332         kwboot_img_grow_hdr(img, size, ohdrsz);
1333
1334         if (hdr->ext & 0x1) {
1335                 for_each_opt_hdr_v1 (ohdr, img)
1336                         if (opt_hdr_v1_next(ohdr) == NULL)
1337                                 break;
1338
1339                 *opt_hdr_v1_ext(ohdr) |= 1;
1340                 ohdr = opt_hdr_v1_next(ohdr);
1341         } else {
1342                 hdr->ext |= 1;
1343                 ohdr = (void *)(hdr + 1);
1344         }
1345
1346         ohdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1347         ohdr->headersz_msb = ohdrsz >> 16;
1348         ohdr->headersz_lsb = cpu_to_le16(ohdrsz & 0xffff);
1349
1350         memset(&ohdr->data[0], 0, ohdrsz - sizeof(*ohdr));
1351
1352         return &ohdr->data[4];
1353 }
1354
1355 static void
1356 _copy_baudrate_change_code(struct main_hdr_v1 *hdr, void *dst, int pre,
1357                            int old_baud, int new_baud)
1358 {
1359         size_t codesz = sizeof(kwboot_baud_code);
1360         uint8_t *code = dst;
1361
1362         if (pre) {
1363                 size_t presz = sizeof(kwboot_pre_baud_code);
1364
1365                 /*
1366                  * We need to prepend code that loads lr register with original
1367                  * value of hdr->execaddr. We do this by putting the original
1368                  * exec address before the code that loads it relatively from
1369                  * it's beginning.
1370                  * Afterwards we change the exec address to this code (which is
1371                  * at offset 4, because the first 4 bytes contain the original
1372                  * exec address).
1373                  */
1374                 memcpy(code, kwboot_pre_baud_code, presz);
1375                 *(uint32_t *)code = hdr->execaddr;
1376
1377                 hdr->execaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) + 4);
1378
1379                 code += presz;
1380         }
1381
1382         memcpy(code, kwboot_baud_code, codesz - 8);
1383         *(uint32_t *)(code + codesz - 8) = cpu_to_le32(old_baud);
1384         *(uint32_t *)(code + codesz - 4) = cpu_to_le32(new_baud);
1385 }
1386
1387 static int
1388 kwboot_img_patch(void *img, size_t *size, int baudrate)
1389 {
1390         struct main_hdr_v1 *hdr;
1391         uint32_t srcaddr;
1392         uint8_t csum;
1393         size_t hdrsz;
1394         int image_ver;
1395         int is_secure;
1396
1397         hdr = img;
1398
1399         if (*size < sizeof(struct main_hdr_v1))
1400                 goto err;
1401
1402         image_ver = kwbimage_version(img);
1403         if (image_ver != 0 && image_ver != 1) {
1404                 fprintf(stderr, "Invalid image header version\n");
1405                 goto err;
1406         }
1407
1408         hdrsz = kwbheader_size(hdr);
1409
1410         if (*size < hdrsz)
1411                 goto err;
1412
1413         csum = kwboot_hdr_csum8(hdr) - hdr->checksum;
1414         if (csum != hdr->checksum)
1415                 goto err;
1416
1417         if (image_ver == 0) {
1418                 struct main_hdr_v0 *hdr_v0 = img;
1419
1420                 hdr_v0->nandeccmode = IBR_HDR_ECC_DISABLED;
1421                 hdr_v0->nandpagesize = 0;
1422         }
1423
1424         srcaddr = le32_to_cpu(hdr->srcaddr);
1425
1426         switch (hdr->blockid) {
1427         case IBR_HDR_SATA_ID:
1428                 if (srcaddr < 1)
1429                         goto err;
1430
1431                 hdr->srcaddr = cpu_to_le32((srcaddr - 1) * 512);
1432                 break;
1433
1434         case IBR_HDR_SDIO_ID:
1435                 hdr->srcaddr = cpu_to_le32(srcaddr * 512);
1436                 break;
1437
1438         case IBR_HDR_PEX_ID:
1439                 if (srcaddr == 0xFFFFFFFF)
1440                         hdr->srcaddr = cpu_to_le32(hdrsz);
1441                 break;
1442
1443         case IBR_HDR_SPI_ID:
1444                 if (hdr->destaddr == cpu_to_le32(0xFFFFFFFF)) {
1445                         kwboot_printv("Patching destination and execution addresses from SPI/NOR XIP area to DDR area 0x00800000\n");
1446                         hdr->destaddr = cpu_to_le32(0x00800000);
1447                         hdr->execaddr = cpu_to_le32(0x00800000);
1448                 }
1449                 break;
1450         }
1451
1452         if (hdrsz > le32_to_cpu(hdr->srcaddr) ||
1453             *size < le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize))
1454                 goto err;
1455
1456         is_secure = kwboot_img_is_secure(img);
1457
1458         if (hdr->blockid != IBR_HDR_UART_ID) {
1459                 if (is_secure) {
1460                         fprintf(stderr,
1461                                 "Image has secure header with signature for non-UART booting\n");
1462                         goto err;
1463                 }
1464
1465                 kwboot_printv("Patching image boot signature to UART\n");
1466                 hdr->blockid = IBR_HDR_UART_ID;
1467         }
1468
1469         if (baudrate) {
1470                 uint32_t codesz = sizeof(kwboot_baud_code);
1471                 void *code;
1472
1473                 if (image_ver == 0) {
1474                         fprintf(stderr,
1475                                 "Cannot inject code for changing baudrate into v0 image header\n");
1476                         goto err;
1477                 }
1478
1479                 if (is_secure) {
1480                         fprintf(stderr,
1481                                 "Cannot inject code for changing baudrate into image with secure header\n");
1482                         goto err;
1483                 }
1484
1485                 /*
1486                  * First inject code that changes the baudrate from the default
1487                  * value of 115200 Bd to requested value. This code is inserted
1488                  * as a new opt hdr, so it is executed by BootROM after the
1489                  * header part is received.
1490                  */
1491                 kwboot_printv("Injecting binary header code for changing baudrate to %d Bd\n",
1492                               baudrate);
1493
1494                 code = kwboot_add_bin_ohdr_v1(img, size, codesz);
1495                 _copy_baudrate_change_code(hdr, code, 0, 115200, baudrate);
1496
1497                 /*
1498                  * Now inject code that changes the baudrate back to 115200 Bd.
1499                  * This code is prepended to the data part of the image, so it
1500                  * is executed before U-Boot proper.
1501                  */
1502                 kwboot_printv("Injecting code for changing baudrate back\n");
1503
1504                 codesz += sizeof(kwboot_pre_baud_code);
1505                 code = kwboot_img_grow_data_left(img, size, codesz);
1506                 _copy_baudrate_change_code(hdr, code, 1, baudrate, 115200);
1507
1508                 /* recompute header size */
1509                 hdrsz = kwbheader_size(hdr);
1510         }
1511
1512         if (hdrsz % KWBOOT_XM_BLKSZ) {
1513                 size_t offset = (KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ) %
1514                                 KWBOOT_XM_BLKSZ;
1515
1516                 if (is_secure) {
1517                         fprintf(stderr, "Cannot align image with secure header\n");
1518                         goto err;
1519                 }
1520
1521                 kwboot_printv("Aligning image header to Xmodem block size\n");
1522                 kwboot_img_grow_hdr(img, size, offset);
1523         }
1524
1525         hdr->checksum = kwboot_hdr_csum8(hdr) - csum;
1526
1527         *size = le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize);
1528         return 0;
1529 err:
1530         errno = EINVAL;
1531         return -1;
1532 }
1533
1534 static void
1535 kwboot_usage(FILE *stream, char *progname)
1536 {
1537         fprintf(stream, "kwboot version %s\n", PLAIN_VERSION);
1538         fprintf(stream,
1539                 "Usage: %s [OPTIONS] [-b <image> | -D <image> ] [-B <baud> ] <TTY>\n",
1540                 progname);
1541         fprintf(stream, "\n");
1542         fprintf(stream,
1543                 "  -b <image>: boot <image> with preamble (Kirkwood, Armada 370/XP)\n");
1544         fprintf(stream,
1545                 "  -D <image>: boot <image> without preamble (Dove)\n");
1546         fprintf(stream, "  -d: enter debug mode\n");
1547         fprintf(stream, "  -a: use timings for Armada XP\n");
1548         fprintf(stream, "  -q <req-delay>:  use specific request-delay\n");
1549         fprintf(stream, "  -s <resp-timeo>: use specific response-timeout\n");
1550         fprintf(stream,
1551                 "  -o <block-timeo>: use specific xmodem block timeout\n");
1552         fprintf(stream, "\n");
1553         fprintf(stream, "  -t: mini terminal\n");
1554         fprintf(stream, "\n");
1555         fprintf(stream, "  -B <baud>: set baud rate\n");
1556         fprintf(stream, "\n");
1557 }
1558
1559 int
1560 main(int argc, char **argv)
1561 {
1562         const char *ttypath, *imgpath;
1563         int rv, rc, tty, term;
1564         void *bootmsg;
1565         void *debugmsg;
1566         void *img;
1567         size_t size;
1568         size_t after_img_rsv;
1569         int baudrate;
1570
1571         rv = 1;
1572         tty = -1;
1573         bootmsg = NULL;
1574         debugmsg = NULL;
1575         imgpath = NULL;
1576         img = NULL;
1577         term = 0;
1578         size = 0;
1579         after_img_rsv = KWBOOT_XM_BLKSZ;
1580         baudrate = 115200;
1581
1582         kwboot_verbose = isatty(STDOUT_FILENO);
1583
1584         do {
1585                 int c = getopt(argc, argv, "hb:ptaB:dD:q:s:o:");
1586                 if (c < 0)
1587                         break;
1588
1589                 switch (c) {
1590                 case 'b':
1591                         bootmsg = kwboot_msg_boot;
1592                         imgpath = optarg;
1593                         break;
1594
1595                 case 'D':
1596                         bootmsg = NULL;
1597                         imgpath = optarg;
1598                         break;
1599
1600                 case 'd':
1601                         debugmsg = kwboot_msg_debug;
1602                         break;
1603
1604                 case 'p':
1605                         /* nop, for backward compatibility */
1606                         break;
1607
1608                 case 't':
1609                         term = 1;
1610                         break;
1611
1612                 case 'a':
1613                         msg_req_delay = KWBOOT_MSG_REQ_DELAY_AXP;
1614                         msg_rsp_timeo = KWBOOT_MSG_RSP_TIMEO_AXP;
1615                         break;
1616
1617                 case 'q':
1618                         msg_req_delay = atoi(optarg);
1619                         break;
1620
1621                 case 's':
1622                         msg_rsp_timeo = atoi(optarg);
1623                         break;
1624
1625                 case 'o':
1626                         blk_rsp_timeo = atoi(optarg);
1627                         break;
1628
1629                 case 'B':
1630                         baudrate = atoi(optarg);
1631                         break;
1632
1633                 case 'h':
1634                         rv = 0;
1635                 default:
1636                         goto usage;
1637                 }
1638         } while (1);
1639
1640         if (!bootmsg && !term && !debugmsg)
1641                 goto usage;
1642
1643         if (argc - optind < 1)
1644                 goto usage;
1645
1646         ttypath = argv[optind++];
1647
1648         tty = kwboot_open_tty(ttypath, imgpath ? 115200 : baudrate);
1649         if (tty < 0) {
1650                 perror(ttypath);
1651                 goto out;
1652         }
1653
1654         if (baudrate == 115200)
1655                 /* do not change baudrate during Xmodem to the same value */
1656                 baudrate = 0;
1657         else
1658                 /* ensure we have enough space for baudrate change code */
1659                 after_img_rsv += KWBOOT_BAUDRATE_BIN_HEADER_SZ +
1660                                  sizeof(kwboot_pre_baud_code) +
1661                                  sizeof(kwboot_baud_code);
1662
1663         if (imgpath) {
1664                 img = kwboot_read_image(imgpath, &size, after_img_rsv);
1665                 if (!img) {
1666                         perror(imgpath);
1667                         goto out;
1668                 }
1669
1670                 rc = kwboot_img_patch(img, &size, baudrate);
1671                 if (rc) {
1672                         fprintf(stderr, "%s: Invalid image.\n", imgpath);
1673                         goto out;
1674                 }
1675         }
1676
1677         if (debugmsg) {
1678                 rc = kwboot_debugmsg(tty, debugmsg);
1679                 if (rc) {
1680                         perror("debugmsg");
1681                         goto out;
1682                 }
1683         } else if (bootmsg) {
1684                 rc = kwboot_bootmsg(tty, bootmsg);
1685                 if (rc) {
1686                         perror("bootmsg");
1687                         goto out;
1688                 }
1689         }
1690
1691         if (img) {
1692                 rc = kwboot_xmodem(tty, img, size, baudrate);
1693                 if (rc) {
1694                         perror("xmodem");
1695                         goto out;
1696                 }
1697         }
1698
1699         if (term) {
1700                 rc = kwboot_terminal(tty);
1701                 if (rc && !(errno == EINTR)) {
1702                         perror("terminal");
1703                         goto out;
1704                 }
1705         }
1706
1707         rv = 0;
1708 out:
1709         if (tty >= 0)
1710                 close(tty);
1711
1712         if (img)
1713                 free(img);
1714
1715         return rv;
1716
1717 usage:
1718         kwboot_usage(rv ? stderr : stdout, basename(argv[0]));
1719         goto out;
1720 }