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