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