tools/kwboot.c

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