tools/kwboot.c

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