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