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         /*
1077          * If header size is not aligned to xmodem block size (which applies
1078          * for all images in kwbimage v0 format) then we have to ensure that
1079          * the last xmodem block of header contains beginning of the data
1080          * followed by the header. So align header size to xmodem block size.
1081          */
1082         hdrsz += (KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ) % KWBOOT_XM_BLKSZ;
1083
1084         kwboot_printv("Waiting 2s and flushing tty\n");
1085         sleep(2); /* flush isn't effective without it */
1086         tcflush(tty, TCIOFLUSH);
1087
1088         pnum = 1;
1089
1090         rc = kwboot_xmodem_one(tty, &pnum, 1, img, hdrsz, baudrate);
1091         if (rc)
1092                 return rc;
1093
1094         /*
1095          * If we have already sent image data as a part of the last
1096          * xmodem header block then we have nothing more to send.
1097          */
1098         if (hdrsz < size) {
1099                 img += hdrsz;
1100                 size -= hdrsz;
1101                 rc = kwboot_xmodem_one(tty, &pnum, 0, img, size, 0);
1102                 if (rc)
1103                         return rc;
1104         }
1105
1106         rc = kwboot_xm_finish(tty);
1107         if (rc)
1108                 return rc;
1109
1110         if (baudrate) {
1111                 kwboot_printv("\nChanging baudrate back to 115200 Bd\n\n");
1112                 rc = kwboot_tty_change_baudrate(tty, 115200);
1113                 if (rc)
1114                         return rc;
1115         }
1116
1117         return 0;
1118 }
1119
1120 static int
1121 kwboot_term_pipe(int in, int out, const char *quit, int *s)
1122 {
1123         ssize_t nin;
1124         char _buf[128], *buf = _buf;
1125
1126         nin = read(in, buf, sizeof(_buf));
1127         if (nin <= 0)
1128                 return -1;
1129
1130         if (quit) {
1131                 int i;
1132
1133                 for (i = 0; i < nin; i++) {
1134                         if (*buf == quit[*s]) {
1135                                 (*s)++;
1136                                 if (!quit[*s])
1137                                         return 0;
1138                                 buf++;
1139                                 nin--;
1140                         } else {
1141                                 if (kwboot_write(out, quit, *s) < 0)
1142                                         return -1;
1143                                 *s = 0;
1144                         }
1145                 }
1146         }
1147
1148         if (kwboot_write(out, buf, nin) < 0)
1149                 return -1;
1150
1151         return 0;
1152 }
1153
1154 static int
1155 kwboot_terminal(int tty)
1156 {
1157         int rc, in, s;
1158         const char *quit = "\34c";
1159         struct termios otio, tio;
1160
1161         rc = -1;
1162
1163         in = STDIN_FILENO;
1164         if (isatty(in)) {
1165                 rc = tcgetattr(in, &otio);
1166                 if (!rc) {
1167                         tio = otio;
1168                         cfmakeraw(&tio);
1169                         rc = tcsetattr(in, TCSANOW, &tio);
1170                 }
1171                 if (rc) {
1172                         perror("tcsetattr");
1173                         goto out;
1174                 }
1175
1176                 kwboot_printv("[Type Ctrl-%c + %c to quit]\r\n",
1177                               quit[0] | 0100, quit[1]);
1178         } else
1179                 in = -1;
1180
1181         rc = 0;
1182         s = 0;
1183
1184         do {
1185                 fd_set rfds;
1186                 int nfds = 0;
1187
1188                 FD_ZERO(&rfds);
1189                 FD_SET(tty, &rfds);
1190                 nfds = nfds < tty ? tty : nfds;
1191
1192                 if (in >= 0) {
1193                         FD_SET(in, &rfds);
1194                         nfds = nfds < in ? in : nfds;
1195                 }
1196
1197                 nfds = select(nfds + 1, &rfds, NULL, NULL, NULL);
1198                 if (nfds < 0)
1199                         break;
1200
1201                 if (FD_ISSET(tty, &rfds)) {
1202                         rc = kwboot_term_pipe(tty, STDOUT_FILENO, NULL, NULL);
1203                         if (rc)
1204                                 break;
1205                 }
1206
1207                 if (in >= 0 && FD_ISSET(in, &rfds)) {
1208                         rc = kwboot_term_pipe(in, tty, quit, &s);
1209                         if (rc)
1210                                 break;
1211                 }
1212         } while (quit[s] != 0);
1213
1214         if (in >= 0)
1215                 tcsetattr(in, TCSANOW, &otio);
1216         printf("\n");
1217 out:
1218         return rc;
1219 }
1220
1221 static void *
1222 kwboot_read_image(const char *path, size_t *size, size_t reserve)
1223 {
1224         int rc, fd;
1225         struct stat st;
1226         void *img;
1227         off_t tot;
1228
1229         rc = -1;
1230         img = NULL;
1231
1232         fd = open(path, O_RDONLY);
1233         if (fd < 0)
1234                 goto out;
1235
1236         rc = fstat(fd, &st);
1237         if (rc)
1238                 goto out;
1239
1240         img = malloc(st.st_size + reserve);
1241         if (!img)
1242                 goto out;
1243
1244         tot = 0;
1245         while (tot < st.st_size) {
1246                 ssize_t rd = read(fd, img + tot, st.st_size - tot);
1247
1248                 if (rd < 0)
1249                         goto out;
1250
1251                 tot += rd;
1252
1253                 if (!rd && tot < st.st_size) {
1254                         errno = EIO;
1255                         goto out;
1256                 }
1257         }
1258
1259         rc = 0;
1260         *size = st.st_size;
1261 out:
1262         if (rc && img) {
1263                 free(img);
1264                 img = NULL;
1265         }
1266         if (fd >= 0)
1267                 close(fd);
1268
1269         return img;
1270 }
1271
1272 static uint8_t
1273 kwboot_hdr_csum8(const void *hdr)
1274 {
1275         const uint8_t *data = hdr;
1276         uint8_t csum;
1277         size_t size;
1278
1279         size = kwbheader_size_for_csum(hdr);
1280
1281         for (csum = 0; size-- > 0; data++)
1282                 csum += *data;
1283
1284         return csum;
1285 }
1286
1287 static uint32_t *
1288 kwboot_img_csum32_ptr(void *img)
1289 {
1290         struct main_hdr_v1 *hdr = img;
1291         uint32_t datasz;
1292
1293         datasz = le32_to_cpu(hdr->blocksize) - sizeof(uint32_t);
1294
1295         return img + le32_to_cpu(hdr->srcaddr) + datasz;
1296 }
1297
1298 static uint32_t
1299 kwboot_img_csum32(const void *img)
1300 {
1301         const struct main_hdr_v1 *hdr = img;
1302         uint32_t datasz, csum = 0;
1303         const uint32_t *data;
1304
1305         datasz = le32_to_cpu(hdr->blocksize) - sizeof(csum);
1306         if (datasz % sizeof(uint32_t))
1307                 return 0;
1308
1309         data = img + le32_to_cpu(hdr->srcaddr);
1310         while (datasz > 0) {
1311                 csum += le32_to_cpu(*data++);
1312                 datasz -= 4;
1313         }
1314
1315         return cpu_to_le32(csum);
1316 }
1317
1318 static int
1319 kwboot_img_is_secure(void *img)
1320 {
1321         struct opt_hdr_v1 *ohdr;
1322
1323         for_each_opt_hdr_v1 (ohdr, img)
1324                 if (ohdr->headertype == OPT_HDR_V1_SECURE_TYPE)
1325                         return 1;
1326
1327         return 0;
1328 }
1329
1330 static void *
1331 kwboot_img_grow_data_right(void *img, size_t *size, size_t grow)
1332 {
1333         struct main_hdr_v1 *hdr = img;
1334         void *result;
1335
1336         /*
1337          * 32-bit checksum comes after end of image code, so we will be putting
1338          * new code there. So we get this pointer and then increase data size
1339          * (since increasing data size changes kwboot_img_csum32_ptr() return
1340          *  value).
1341          */
1342         result = kwboot_img_csum32_ptr(img);
1343         hdr->blocksize = cpu_to_le32(le32_to_cpu(hdr->blocksize) + grow);
1344         *size += grow;
1345
1346         return result;
1347 }
1348
1349 static void
1350 kwboot_img_grow_hdr(void *img, size_t *size, size_t grow)
1351 {
1352         uint32_t hdrsz, datasz, srcaddr;
1353         struct main_hdr_v1 *hdr = img;
1354         struct opt_hdr_v1 *ohdr;
1355         uint8_t *data;
1356
1357         srcaddr = le32_to_cpu(hdr->srcaddr);
1358
1359         /* calculate real used space in kwbimage header */
1360         if (kwbimage_version(img) == 0) {
1361                 hdrsz = kwbheader_size(img);
1362         } else {
1363                 hdrsz = sizeof(*hdr);
1364                 for_each_opt_hdr_v1 (ohdr, hdr)
1365                         hdrsz += opt_hdr_v1_size(ohdr);
1366         }
1367
1368         data = (uint8_t *)img + srcaddr;
1369         datasz = *size - srcaddr;
1370
1371         /* only move data if there is not enough space */
1372         if (hdrsz + grow > srcaddr) {
1373                 size_t need = hdrsz + grow - srcaddr;
1374
1375                 /* move data by enough bytes */
1376                 memmove(data + need, data, datasz);
1377
1378                 hdr->srcaddr = cpu_to_le32(srcaddr + need);
1379                 *size += need;
1380         }
1381
1382         if (kwbimage_version(img) == 1) {
1383                 hdrsz += grow;
1384                 if (hdrsz > kwbheader_size(img)) {
1385                         hdr->headersz_msb = hdrsz >> 16;
1386                         hdr->headersz_lsb = cpu_to_le16(hdrsz & 0xffff);
1387                 }
1388         }
1389 }
1390
1391 static void *
1392 kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
1393 {
1394         struct main_hdr_v1 *hdr = img;
1395         struct opt_hdr_v1 *ohdr;
1396         uint32_t num_args;
1397         uint32_t offset;
1398         uint32_t ohdrsz;
1399         uint8_t *prev_ext;
1400
1401         if (hdr->ext & 0x1) {
1402                 for_each_opt_hdr_v1 (ohdr, img)
1403                         if (opt_hdr_v1_next(ohdr) == NULL)
1404                                 break;
1405
1406                 prev_ext = opt_hdr_v1_ext(ohdr);
1407                 ohdr = _opt_hdr_v1_next(ohdr);
1408         } else {
1409                 ohdr = (void *)(hdr + 1);
1410                 prev_ext = &hdr->ext;
1411         }
1412
1413         /*
1414          * ARM executable code inside the BIN header on some mvebu platforms
1415          * (e.g. A370, AXP) must always be aligned with the 128-bit boundary.
1416          * This requirement can be met by inserting dummy arguments into
1417          * BIN header, if needed.
1418          */
1419         offset = &ohdr->data[4] - (char *)img;
1420         num_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
1421
1422         ohdrsz = sizeof(*ohdr) + 4 + 4 * num_args + binsz + 4;
1423         kwboot_img_grow_hdr(hdr, size, ohdrsz);
1424
1425         *prev_ext |= 1;
1426
1427         ohdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1428         ohdr->headersz_msb = ohdrsz >> 16;
1429         ohdr->headersz_lsb = cpu_to_le16(ohdrsz & 0xffff);
1430
1431         memset(&ohdr->data[0], 0, ohdrsz - sizeof(*ohdr));
1432         *(uint32_t *)&ohdr->data[0] = cpu_to_le32(num_args);
1433
1434         return &ohdr->data[4 + 4 * num_args];
1435 }
1436
1437 static void
1438 _inject_baudrate_change_code(void *img, size_t *size, int for_data,
1439                              int old_baud, int new_baud)
1440 {
1441         struct main_hdr_v1 *hdr = img;
1442         uint32_t orig_datasz;
1443         uint32_t codesz;
1444         uint8_t *code;
1445
1446         if (for_data) {
1447                 orig_datasz = le32_to_cpu(hdr->blocksize) - sizeof(uint32_t);
1448
1449                 codesz = sizeof(kwboot_baud_code) +
1450                          sizeof(kwboot_baud_code_data_jump);
1451                 code = kwboot_img_grow_data_right(img, size, codesz);
1452         } else {
1453                 codesz = sizeof(kwboot_baud_code_binhdr_pre) +
1454                          sizeof(kwboot_baud_code) +
1455                          sizeof(kwboot_baud_code_binhdr_post);
1456                 code = kwboot_add_bin_ohdr_v1(img, size, codesz);
1457
1458                 codesz = sizeof(kwboot_baud_code_binhdr_pre);
1459                 memcpy(code, kwboot_baud_code_binhdr_pre, codesz);
1460                 code += codesz;
1461         }
1462
1463         codesz = sizeof(kwboot_baud_code) - 2 * sizeof(uint32_t);
1464         memcpy(code, kwboot_baud_code, codesz);
1465         code += codesz;
1466         *(uint32_t *)code = cpu_to_le32(old_baud);
1467         code += sizeof(uint32_t);
1468         *(uint32_t *)code = cpu_to_le32(new_baud);
1469         code += sizeof(uint32_t);
1470
1471         if (for_data) {
1472                 codesz = sizeof(kwboot_baud_code_data_jump) - sizeof(uint32_t);
1473                 memcpy(code, kwboot_baud_code_data_jump, codesz);
1474                 code += codesz;
1475                 *(uint32_t *)code = hdr->execaddr;
1476                 code += sizeof(uint32_t);
1477                 hdr->execaddr = cpu_to_le32(le32_to_cpu(hdr->destaddr) + orig_datasz);
1478         } else {
1479                 codesz = sizeof(kwboot_baud_code_binhdr_post);
1480                 memcpy(code, kwboot_baud_code_binhdr_post, codesz);
1481                 code += codesz;
1482         }
1483 }
1484
1485 static int
1486 kwboot_img_patch(void *img, size_t *size, int baudrate)
1487 {
1488         struct main_hdr_v1 *hdr;
1489         uint32_t srcaddr;
1490         uint8_t csum;
1491         size_t hdrsz;
1492         int image_ver;
1493         int is_secure;
1494
1495         hdr = img;
1496
1497         if (*size < sizeof(struct main_hdr_v1))
1498                 goto err;
1499
1500         image_ver = kwbimage_version(img);
1501         if (image_ver != 0 && image_ver != 1) {
1502                 fprintf(stderr, "Invalid image header version\n");
1503                 goto err;
1504         }
1505
1506         hdrsz = kwbheader_size(hdr);
1507
1508         if (*size < hdrsz)
1509                 goto err;
1510
1511         csum = kwboot_hdr_csum8(hdr) - hdr->checksum;
1512         if (csum != hdr->checksum)
1513                 goto err;
1514
1515         srcaddr = le32_to_cpu(hdr->srcaddr);
1516
1517         switch (hdr->blockid) {
1518         case IBR_HDR_SATA_ID:
1519                 if (srcaddr < 1)
1520                         goto err;
1521
1522                 hdr->srcaddr = cpu_to_le32((srcaddr - 1) * 512);
1523                 break;
1524
1525         case IBR_HDR_SDIO_ID:
1526                 hdr->srcaddr = cpu_to_le32(srcaddr * 512);
1527                 break;
1528
1529         case IBR_HDR_PEX_ID:
1530                 if (srcaddr == 0xFFFFFFFF)
1531                         hdr->srcaddr = cpu_to_le32(hdrsz);
1532                 break;
1533
1534         case IBR_HDR_SPI_ID:
1535                 if (hdr->destaddr == cpu_to_le32(0xFFFFFFFF)) {
1536                         kwboot_printv("Patching destination and execution addresses from SPI/NOR XIP area to DDR area 0x00800000\n");
1537                         hdr->destaddr = cpu_to_le32(0x00800000);
1538                         hdr->execaddr = cpu_to_le32(0x00800000);
1539                 }
1540                 break;
1541         }
1542
1543         if (hdrsz > le32_to_cpu(hdr->srcaddr) ||
1544             *size < le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize))
1545                 goto err;
1546
1547         if (kwboot_img_csum32(img) != *kwboot_img_csum32_ptr(img))
1548                 goto err;
1549
1550         is_secure = kwboot_img_is_secure(img);
1551
1552         if (hdr->blockid != IBR_HDR_UART_ID) {
1553                 if (is_secure) {
1554                         fprintf(stderr,
1555                                 "Image has secure header with signature for non-UART booting\n");
1556                         goto err;
1557                 }
1558
1559                 kwboot_printv("Patching image boot signature to UART\n");
1560                 hdr->blockid = IBR_HDR_UART_ID;
1561         }
1562
1563         if (!is_secure) {
1564                 if (image_ver == 1) {
1565                         /*
1566                          * Tell BootROM to send BootROM messages to UART port
1567                          * number 0 (used also for UART booting) with default
1568                          * baudrate (which should be 115200) and do not touch
1569                          * UART MPP configuration.
1570                          */
1571                         hdr->options &= ~0x1F;
1572                         hdr->options |= MAIN_HDR_V1_OPT_BAUD_DEFAULT;
1573                         hdr->options |= 0 << 3;
1574                 }
1575                 if (image_ver == 0)
1576                         ((struct main_hdr_v0 *)img)->nandeccmode = IBR_HDR_ECC_DISABLED;
1577                 hdr->nandpagesize = 0;
1578         }
1579
1580         if (baudrate) {
1581                 if (image_ver == 0) {
1582                         fprintf(stderr,
1583                                 "Cannot inject code for changing baudrate into v0 image header\n");
1584                         goto err;
1585                 }
1586
1587                 if (is_secure) {
1588                         fprintf(stderr,
1589                                 "Cannot inject code for changing baudrate into image with secure header\n");
1590                         goto err;
1591                 }
1592
1593                 /*
1594                  * First inject code that changes the baudrate from the default
1595                  * value of 115200 Bd to requested value. This code is inserted
1596                  * as a new opt hdr, so it is executed by BootROM after the
1597                  * header part is received.
1598                  */
1599                 kwboot_printv("Injecting binary header code for changing baudrate to %d Bd\n",
1600                               baudrate);
1601                 _inject_baudrate_change_code(img, size, 0, 115200, baudrate);
1602
1603                 /*
1604                  * Now inject code that changes the baudrate back to 115200 Bd.
1605                  * This code is appended after the data part of the image, and
1606                  * execaddr is changed so that it is executed before U-Boot
1607                  * proper.
1608                  */
1609                 kwboot_printv("Injecting code for changing baudrate back\n");
1610                 _inject_baudrate_change_code(img, size, 1, baudrate, 115200);
1611
1612                 /* Update the 32-bit data checksum */
1613                 *kwboot_img_csum32_ptr(img) = kwboot_img_csum32(img);
1614
1615                 /* recompute header size */
1616                 hdrsz = kwbheader_size(hdr);
1617         }
1618
1619         if (hdrsz % KWBOOT_XM_BLKSZ) {
1620                 size_t grow = KWBOOT_XM_BLKSZ - hdrsz % KWBOOT_XM_BLKSZ;
1621
1622                 if (is_secure) {
1623                         fprintf(stderr, "Cannot align image with secure header\n");
1624                         goto err;
1625                 }
1626
1627                 kwboot_printv("Aligning image header to Xmodem block size\n");
1628                 kwboot_img_grow_hdr(img, size, grow);
1629         }
1630
1631         hdr->checksum = kwboot_hdr_csum8(hdr) - csum;
1632
1633         *size = le32_to_cpu(hdr->srcaddr) + le32_to_cpu(hdr->blocksize);
1634         return 0;
1635 err:
1636         errno = EINVAL;
1637         return -1;
1638 }
1639
1640 static void
1641 kwboot_usage(FILE *stream, char *progname)
1642 {
1643         fprintf(stream,
1644                 "Usage: %s [OPTIONS] [-b <image> | -D <image> ] [-B <baud> ] <TTY>\n",
1645                 progname);
1646         fprintf(stream, "\n");
1647         fprintf(stream,
1648                 "  -b <image>: boot <image> with preamble (Kirkwood, Armada 370/XP)\n");
1649         fprintf(stream,
1650                 "  -D <image>: boot <image> without preamble (Dove)\n");
1651         fprintf(stream, "  -d: enter debug mode\n");
1652         fprintf(stream, "  -a: use timings for Armada XP\n");
1653         fprintf(stream, "  -q <req-delay>:  use specific request-delay\n");
1654         fprintf(stream, "  -s <resp-timeo>: use specific response-timeout\n");
1655         fprintf(stream,
1656                 "  -o <block-timeo>: use specific xmodem block timeout\n");
1657         fprintf(stream, "\n");
1658         fprintf(stream, "  -t: mini terminal\n");
1659         fprintf(stream, "\n");
1660         fprintf(stream, "  -B <baud>: set baud rate\n");
1661         fprintf(stream, "\n");
1662 }
1663
1664 int
1665 main(int argc, char **argv)
1666 {
1667         const char *ttypath, *imgpath;
1668         int rv, rc, tty, term;
1669         void *bootmsg;
1670         void *debugmsg;
1671         void *img;
1672         size_t size;
1673         size_t after_img_rsv;
1674         int baudrate;
1675
1676         rv = 1;
1677         tty = -1;
1678         bootmsg = NULL;
1679         debugmsg = NULL;
1680         imgpath = NULL;
1681         img = NULL;
1682         term = 0;
1683         size = 0;
1684         after_img_rsv = KWBOOT_XM_BLKSZ;
1685         baudrate = 115200;
1686
1687         printf("kwboot version %s\n", PLAIN_VERSION);
1688
1689         kwboot_verbose = isatty(STDOUT_FILENO);
1690
1691         do {
1692                 int c = getopt(argc, argv, "hb:ptaB:dD:q:s:o:");
1693                 if (c < 0)
1694                         break;
1695
1696                 switch (c) {
1697                 case 'b':
1698                         bootmsg = kwboot_msg_boot;
1699                         imgpath = optarg;
1700                         break;
1701
1702                 case 'D':
1703                         bootmsg = NULL;
1704                         imgpath = optarg;
1705                         break;
1706
1707                 case 'd':
1708                         debugmsg = kwboot_msg_debug;
1709                         break;
1710
1711                 case 'p':
1712                         /* nop, for backward compatibility */
1713                         break;
1714
1715                 case 't':
1716                         term = 1;
1717                         break;
1718
1719                 case 'a':
1720                         msg_req_delay = KWBOOT_MSG_REQ_DELAY_AXP;
1721                         msg_rsp_timeo = KWBOOT_MSG_RSP_TIMEO_AXP;
1722                         break;
1723
1724                 case 'q':
1725                         msg_req_delay = atoi(optarg);
1726                         break;
1727
1728                 case 's':
1729                         msg_rsp_timeo = atoi(optarg);
1730                         break;
1731
1732                 case 'o':
1733                         blk_rsp_timeo = atoi(optarg);
1734                         break;
1735
1736                 case 'B':
1737                         baudrate = atoi(optarg);
1738                         break;
1739
1740                 case 'h':
1741                         rv = 0;
1742                 default:
1743                         goto usage;
1744                 }
1745         } while (1);
1746
1747         if (!bootmsg && !term && !debugmsg)
1748                 goto usage;
1749
1750         if (argc - optind < 1)
1751                 goto usage;
1752
1753         ttypath = argv[optind++];
1754
1755         tty = kwboot_open_tty(ttypath, imgpath ? 115200 : baudrate);
1756         if (tty < 0) {
1757                 perror(ttypath);
1758                 goto out;
1759         }
1760
1761         if (baudrate == 115200)
1762                 /* do not change baudrate during Xmodem to the same value */
1763                 baudrate = 0;
1764         else
1765                 /* ensure we have enough space for baudrate change code */
1766                 after_img_rsv += sizeof(struct opt_hdr_v1) + 8 + 16 +
1767                                  sizeof(kwboot_baud_code_binhdr_pre) +
1768                                  sizeof(kwboot_baud_code) +
1769                                  sizeof(kwboot_baud_code_binhdr_post) +
1770                                  KWBOOT_XM_BLKSZ +
1771                                  sizeof(kwboot_baud_code) +
1772                                  sizeof(kwboot_baud_code_data_jump) +
1773                                  KWBOOT_XM_BLKSZ;
1774
1775         if (imgpath) {
1776                 img = kwboot_read_image(imgpath, &size, after_img_rsv);
1777                 if (!img) {
1778                         perror(imgpath);
1779                         goto out;
1780                 }
1781
1782                 rc = kwboot_img_patch(img, &size, baudrate);
1783                 if (rc) {
1784                         fprintf(stderr, "%s: Invalid image.\n", imgpath);
1785                         goto out;
1786                 }
1787         }
1788
1789         if (debugmsg) {
1790                 rc = kwboot_debugmsg(tty, debugmsg);
1791                 if (rc) {
1792                         perror("debugmsg");
1793                         goto out;
1794                 }
1795         } else if (bootmsg) {
1796                 rc = kwboot_bootmsg(tty, bootmsg);
1797                 if (rc) {
1798                         perror("bootmsg");
1799                         goto out;
1800                 }
1801         }
1802
1803         if (img) {
1804                 rc = kwboot_xmodem(tty, img, size, baudrate);
1805                 if (rc) {
1806                         perror("xmodem");
1807                         goto out;
1808                 }
1809         }
1810
1811         if (term) {
1812                 rc = kwboot_terminal(tty);
1813                 if (rc && !(errno == EINTR)) {
1814                         perror("terminal");
1815                         goto out;
1816                 }
1817         }
1818
1819         rv = 0;
1820 out:
1821         if (tty >= 0)
1822                 close(tty);
1823
1824         if (img)
1825                 free(img);
1826
1827         return rv;
1828
1829 usage:
1830         kwboot_usage(rv ? stderr : stdout, basename(argv[0]));
1831         goto out;
1832 }