tools/kwboot.c

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