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