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