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