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