src/host/osmocon/osmocon.c

   1 /* osmocon */
   2
   3 /* (C) 2010 by Harald Welte <laforge@gnumonks.org>
   4  * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
   5  * (C) 2010 by Steve Markgraf <steve@steve-m.de>
   6  *
   7  * All Rights Reserved
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation; either version 2 of the License, or
  12  * (at your option) any later version.
  13  *
  14  * This program is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  * GNU General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public License along
  20  * with this program; if not, write to the Free Software Foundation, Inc.,
  21  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  22  *
  23  */
  24
  25 #include <ctype.h>
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <unistd.h>
  30 #include <stdint.h>
  31 #include <fcntl.h>
  32 #include <errno.h>
  33 #include <termios.h>
  34 #include <sys/ioctl.h>
  35 #include <sys/types.h>
  36 #include <sys/socket.h>
  37 #include <sys/stat.h>
  38 #include <sys/un.h>
  39
  40 #include <sercomm.h>
  41
  42 #include <osmocom/core/linuxlist.h>
  43 #include <osmocom/core/select.h>
  44 #include <osmocom/core/talloc.h>
  45 #include <osmocom/core/timer.h>
  46
  47 #include <arpa/inet.h>
  48
  49 #define MODEM_BAUDRATE          B115200
  50 #define MAX_DNLOAD_SIZE         0xFFFF
  51 #define MAX_HDR_SIZE            128
  52 #define MAGIC_OFFSET            0x3be2
  53
  54 #define DEFAULT_BEACON_INTERVAL 50000
  55 #define ROMLOAD_INIT_BAUDRATE   B19200
  56 #define ROMLOAD_DL_BAUDRATE     B115200
  57 #define ROMLOAD_BLOCK_HDR_LEN   10
  58 #define ROMLOAD_ADDRESS         0x820000
  59
  60 #define MTK_INIT_BAUDRATE       B19200
  61 #define MTK_ADDRESS             0x40001400
  62 #define MTK_BLOCK_SIZE          1024
  63
  64 struct tool_server *tool_server_for_dlci[256];
  65
  66 /**
  67  * a connection from some other tool
  68  */
  69 struct tool_connection {
  70         struct tool_server *server;
  71         struct llist_head entry;
  72         struct osmo_fd fd;
  73 };
  74
  75 /**
  76  * server for a tool
  77  */
  78 struct tool_server {
  79         struct osmo_fd bfd;
  80         uint8_t dlci;
  81         struct llist_head connections;
  82 };
  83
  84
  85 enum dnload_state {
  86         WAITING_PROMPT1,
  87         WAITING_PROMPT2,
  88         DOWNLOADING,
  89 };
  90
  91 enum romload_state {
  92         WAITING_IDENTIFICATION,
  93         WAITING_PARAM_ACK,
  94         SENDING_BLOCKS,
  95         SENDING_LAST_BLOCK,
  96         LAST_BLOCK_SENT,
  97         WAITING_BLOCK_ACK,
  98         WAITING_CHECKSUM_ACK,
  99         WAITING_BRANCH_ACK,
 100         FINISHED,
 101 };
 102
 103 enum mtk_state {
 104         MTK_INIT_1,
 105         MTK_INIT_2,
 106         MTK_INIT_3,
 107         MTK_INIT_4,
 108         MTK_WAIT_WRITE_ACK,
 109         MTK_WAIT_ADDR_ACK,
 110         MTK_WAIT_SIZE_ACK,
 111         MTK_SENDING_BLOCKS,
 112         MTK_WAIT_BRANCH_CMD_ACK,
 113         MTK_WAIT_BRANCH_ADDR_ACK,
 114         MTK_FINISHED,
 115 };
 116
 117 enum dnload_mode {
 118         MODE_C123,
 119         MODE_C123xor,
 120         MODE_C140,
 121         MODE_C140xor,
 122         MODE_C155,
 123         MODE_ROMLOAD,
 124         MODE_MTK,
 125 };
 126
 127 struct dnload {
 128         enum dnload_state state;
 129         enum romload_state romload_state;
 130         enum mtk_state mtk_state;
 131         enum dnload_mode mode;
 132         struct osmo_fd serial_fd;
 133         char *filename;
 134         char *chainload_filename;
 135
 136         int expect_hdlc;
 137
 138         int dump_rx;
 139         int dump_tx;
 140         int beacon_interval;
 141
 142         /* data to be downloaded */
 143         uint8_t *data;
 144         int data_len;
 145
 146         uint8_t *write_ptr;
 147
 148         /* romload: block to be downloaded */
 149         uint8_t *block;
 150         int block_len;
 151         uint8_t block_number;
 152         uint16_t block_payload_size;
 153         int romload_dl_checksum;
 154         uint8_t *block_ptr;
 155         uint8_t load_address[4];
 156
 157         uint8_t mtk_send_size[4];
 158         int block_count;
 159         int echo_bytecount;
 160
 161         struct tool_server layer2_server;
 162         struct tool_server loader_server;
 163 };
 164
 165
 166 static struct dnload dnload;
 167 static struct osmo_timer_list tick_timer;
 168
 169 /* Compal ramloader specific */
 170 static const uint8_t phone_prompt1[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x01, 0x40 };
 171 static const uint8_t dnload_cmd[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x52, 0x01, 0x53 };
 172 static const uint8_t phone_prompt2[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x02, 0x43 };
 173 static const uint8_t phone_ack[]     = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x42 };
 174 static const uint8_t phone_nack_magic[]= { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x57 };
 175 static const uint8_t phone_nack[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x45, 0x53, 0x16 };
 176 static const uint8_t ftmtool[] = { 0x66, 0x74, 0x6d, 0x74, 0x6f, 0x6f, 0x6c };
 177 static const uint8_t phone_magic[] = { 0x31, 0x30, 0x30, 0x33 }; /* "1003" */
 178
 179 /* The C123 has a hard-coded check inside the ramloader that requires the
 180  * following bytes to be always the first four bytes of the image */
 181 static const uint8_t data_hdr_c123[]    = { 0xee, 0x4c, 0x9f, 0x63 };
 182
 183 /* The C155 doesn't have some strange restriction on what the first four bytes
 184  *  have to be, but it starts the ramloader in THUMB mode. We use the following
 185  * four bytes to switch back to ARM mode:
 186   800100:       4778            bx      pc
 187   800102:       46c0            nop                     ; (mov r8, r8)
 188  */
 189 static const uint8_t data_hdr_c155[]    = { 0x78, 0x47, 0xc0, 0x46 };
 190
 191 /* Calypso romloader specific */
 192 static const uint8_t romload_ident_cmd[] =      { 0x3c, 0x69 }; /* <i */
 193 static const uint8_t romload_abort_cmd[] =      { 0x3c, 0x61 }; /* <a */
 194 static const uint8_t romload_write_cmd[] =      { 0x3c, 0x77 }; /* <w */
 195 static const uint8_t romload_checksum_cmd[] =   { 0x3c, 0x63 }; /* <c */
 196 static const uint8_t romload_branch_cmd[] =     { 0x3c, 0x62 }; /* <b */
 197 static const uint8_t romload_ident_ack[] =      { 0x3e, 0x69 }; /* >i */
 198 static const uint8_t romload_param_ack[] =      { 0x3e, 0x70 }; /* >p */
 199 static const uint8_t romload_param_nack[] =     { 0x3e, 0x50 }; /* >P */
 200 static const uint8_t romload_block_ack[] =      { 0x3e, 0x77 }; /* >w */
 201 static const uint8_t romload_block_nack[] =     { 0x3e, 0x57 }; /* >W */
 202 static const uint8_t romload_checksum_ack[] =   { 0x3e, 0x63 }; /* >c */
 203 static const uint8_t romload_checksum_nack[] =  { 0x3e, 0x43 }; /* >C */
 204 static const uint8_t romload_branch_ack[] =     { 0x3e, 0x62 }; /* >b */
 205 static const uint8_t romload_branch_nack[] =    { 0x3e, 0x42 }; /* >B */
 206
 207 /* romload_param: {"<p", uint8_t baudrate, uint8_t dpll, uint16_t memory_config,
 208  * uint8_t strobe_af, uint32_t uart_timeout} */
 209
 210 static const uint8_t romload_param[] = { 0x3c, 0x70, 0x00, 0x00, 0x00, 0x04,
 211                                          0x00, 0x00, 0x00, 0x00, 0x00 };
 212
 213 /* MTK romloader specific */
 214 static const uint8_t mtk_init_cmd[] =   { 0xa0, 0x0a, 0x50, 0x05 };
 215 static const uint8_t mtk_init_resp[] =  { 0x5f, 0xf5, 0xaf, 0xfa };
 216 static const uint8_t mtk_command[] =    { 0xa1, 0xa2, 0xa4, 0xa8 };
 217
 218 /* FIXME: this routine is more or less what openbsc/src/rs232:rs232_setup()
 219  * does, we should move it to libosmocore at some point */
 220 static int serial_init(const char *serial_port)
 221 {
 222         int rc, serial_fd, v24;
 223         struct termios tio;
 224
 225         serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY);
 226         if (serial_fd < 0) {
 227                 perror("cannot open serial port");
 228                 return serial_fd;
 229         }
 230
 231         //fcntl(serial_fd, F_SETFL, 0);
 232
 233         /* Configure serial interface */
 234         rc = tcgetattr(serial_fd, &tio);
 235         if (rc < 0) {
 236                 perror("tcgetattr()");
 237                 return rc;
 238         }
 239         cfsetispeed(&tio, MODEM_BAUDRATE);
 240         cfsetospeed(&tio, MODEM_BAUDRATE);
 241         tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
 242         tio.c_cflag |=  (CREAD | CLOCAL | CS8);
 243         tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
 244         tio.c_iflag |=  (INPCK | ISTRIP);
 245         tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR);
 246         tio.c_oflag &= ~(OPOST | ONLCR);
 247         rc = tcsetattr(serial_fd, TCSANOW, &tio);
 248         if (rc < 0) {
 249                 perror("tcsetattr()");
 250                 return rc;
 251         }
 252
 253         /* set ready to read/write */
 254         v24 = TIOCM_DTR | TIOCM_RTS;
 255         rc = ioctl(serial_fd, TIOCMBIS, &v24);
 256         if (rc < 0) {
 257                 perror("ioctl(TIOCMBIS)");
 258                 return rc;
 259         }
 260
 261         return serial_fd;
 262 }
 263
 264 static int serial_set_baudrate(speed_t baudrate)
 265 {
 266         int rc;
 267         struct termios tio;
 268
 269         rc = tcgetattr(dnload.serial_fd.fd, &tio);
 270         if (rc < 0) {
 271                 perror("tcgetattr()");
 272                 return rc;
 273         }
 274         cfsetispeed(&tio, baudrate);
 275         cfsetospeed(&tio, baudrate);
 276
 277         rc = tcsetattr(dnload.serial_fd.fd, TCSANOW, &tio);
 278         return rc;
 279 }
 280
 281 static void beacon_timer_cb(void *p)
 282 {
 283         int rc;
 284
 285         if (dnload.romload_state == WAITING_IDENTIFICATION) {
 286                 printf("Sending Calypso romloader beacon...\n");
 287                 rc = write(dnload.serial_fd.fd, romload_ident_cmd,
 288                            sizeof(romload_ident_cmd));
 289
 290                 if (!(rc == sizeof(romload_ident_cmd)))
 291                         printf("Error sending identification beacon\n");
 292
 293                 osmo_timer_schedule(p, 0, dnload.beacon_interval);
 294         }
 295 }
 296
 297 static void mtk_timer_cb(void *p)
 298 {
 299         int rc;
 300
 301         if (dnload.mtk_state == MTK_INIT_1) {
 302                 printf("Sending MTK romloader beacon...\n");
 303                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[0], 1);
 304
 305                 if (!(rc == 1))
 306                         printf("Error sending identification beacon\n");
 307
 308                 osmo_timer_schedule(p, 0, dnload.beacon_interval);
 309         }
 310 }
 311
 312 /* Read the to-be-downloaded file, prepend header and length, append XOR sum */
 313 int read_file(const char *filename)
 314 {
 315         int fd, rc, i;
 316         struct stat st;
 317         const uint8_t *hdr = NULL;
 318         int payload_size;
 319         int hdr_len = 0;
 320         uint8_t *file_data;
 321         uint16_t tot_len;
 322         uint8_t nibble;
 323         uint8_t running_xor = 0x02;
 324
 325         fd = open(filename, O_RDONLY);
 326         if (fd < 0) {
 327                 perror("opening file");
 328                 exit(1);
 329         }
 330
 331         rc = fstat(fd, &st);
 332         if (st.st_size > MAX_DNLOAD_SIZE) {
 333                 fprintf(stderr, "The maximum file size is 64kBytes (%u bytes)\n",
 334                         MAX_DNLOAD_SIZE);
 335                 return -EFBIG;
 336         }
 337
 338         free(dnload.data);
 339         dnload.data = NULL;
 340
 341         if (dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) {
 342                 if (st.st_size < (MAGIC_OFFSET + sizeof(phone_magic)))
 343                         payload_size = MAGIC_OFFSET + sizeof(phone_magic);
 344                 else {
 345                         printf("\nThe filesize is larger than 15kb, code on "
 346                                 "the magic address will be overwritten!\nUse "
 347                                 "loader.bin and upload the application with "
 348                                 "osmoload instead!\n\n");
 349                         payload_size = st.st_size;
 350                 }
 351         } else
 352                 payload_size = st.st_size;
 353
 354         dnload.data = malloc(MAX_HDR_SIZE + payload_size);
 355
 356         if (!dnload.data) {
 357                 close(fd);
 358                 fprintf(stderr, "No memory\n");
 359                 return -ENOMEM;
 360         }
 361
 362         /* copy in the header, if any */
 363         switch (dnload.mode) {
 364         case MODE_C155:
 365                 hdr = data_hdr_c155;
 366                 hdr_len = sizeof(data_hdr_c155);
 367                 break;
 368         case MODE_C140:
 369         case MODE_C140xor:
 370         case MODE_C123:
 371         case MODE_C123xor:
 372                 hdr = data_hdr_c123;
 373                 hdr_len = sizeof(data_hdr_c123);
 374                 break;
 375         case MODE_ROMLOAD:
 376                 break;
 377         default:
 378                 break;
 379         }
 380
 381         if (hdr && hdr_len)
 382                 memcpy(dnload.data, hdr, hdr_len);
 383
 384         /* 2 bytes for length + header */
 385         file_data = dnload.data + 2 + hdr_len;
 386
 387         /* write the length, keep running XOR */
 388         tot_len = hdr_len + payload_size;
 389         nibble = tot_len >> 8;
 390         dnload.data[0] = nibble;
 391         running_xor ^= nibble;
 392         nibble = tot_len & 0xff;
 393         dnload.data[1] = nibble;
 394         running_xor ^= nibble;
 395
 396         if (hdr_len && hdr) {
 397                 memcpy(dnload.data+2, hdr, hdr_len);
 398
 399                 for (i = 0; i < hdr_len; i++)
 400                         running_xor ^= hdr[i];
 401         }
 402
 403         rc = read(fd, file_data, st.st_size);
 404         if (rc < 0) {
 405                 perror("error reading file\n");
 406                 free(dnload.data);
 407                 dnload.data = NULL;
 408                 close(fd);
 409                 return -EIO;
 410         }
 411         if (rc < st.st_size) {
 412                 free(dnload.data);
 413                 dnload.data = NULL;
 414                 close(fd);
 415                 fprintf(stderr, "Short read of file (%d < %d)\n",
 416                         rc, (int)st.st_size);
 417                 return -EIO;
 418         }
 419
 420         close(fd);
 421
 422         dnload.data_len = (file_data+payload_size) - dnload.data;
 423
 424         /* fill memory between data end and magic, add magic */
 425         if(dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) {
 426                 if (st.st_size < MAGIC_OFFSET)
 427                         memset(file_data + st.st_size, 0x00,
 428                                 payload_size - st.st_size);
 429                 memcpy(dnload.data + MAGIC_OFFSET, phone_magic,
 430                         sizeof(phone_magic));
 431         }
 432
 433         /* calculate XOR sum */
 434         for (i = 0; i < payload_size; i++)
 435                 running_xor ^= file_data[i];
 436
 437         dnload.data[dnload.data_len++] = running_xor;
 438
 439         /* initialize write pointer to start of data */
 440         dnload.write_ptr = dnload.data;
 441
 442         printf("read_file(%s): file_size=%u, hdr_len=%u, dnload_len=%u\n",
 443                 filename, (int)st.st_size, hdr_len, dnload.data_len);
 444
 445         return 0;
 446 }
 447
 448 static void osmocon_osmo_hexdump(const uint8_t *data, unsigned int len)
 449 {
 450         int n;
 451
 452         for (n=0; n < len; n++)
 453                 printf("%02x ", data[n]);
 454         printf(" ");
 455         for (n=0; n < len; n++)
 456                 if (isprint(data[n]))
 457                         putchar(data[n]);
 458                 else
 459                         putchar('.');
 460         printf("\n");
 461 }
 462
 463 static int romload_prepare_block(void)
 464 {
 465         int i;
 466
 467         int block_checksum = 5;
 468         int remaining_bytes;
 469         int fill_bytes;
 470         uint8_t *block_data;
 471         uint32_t block_address;
 472
 473         dnload.block_len = ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size;
 474
 475         /* if first block, allocate memory */
 476         if (!dnload.block_number) {
 477                 dnload.block = malloc(dnload.block_len);
 478                 if (!dnload.block) {
 479                         fprintf(stderr, "No memory\n");
 480                         return -ENOMEM;
 481                 }
 482                 dnload.romload_dl_checksum = 0;
 483                 /* initialize write pointer to start of data */
 484                 dnload.write_ptr = dnload.data;
 485         }
 486
 487         block_address = ROMLOAD_ADDRESS +
 488                         (dnload.block_number * dnload.block_payload_size);
 489
 490         /* prepare our block header (10 bytes) */
 491         memcpy(dnload.block, romload_write_cmd, sizeof(romload_write_cmd));
 492         dnload.block[2] = 0x01; /* block index */
 493         /* should normally be the block number, but hangs when sending !0x01 */
 494         dnload.block[3] = 0x01; /* dnload.block_number+1 */
 495         dnload.block[4] = (dnload.block_payload_size >> 8) & 0xff;
 496         dnload.block[5] = dnload.block_payload_size & 0xff;
 497         dnload.block[6] = (block_address >> 24) & 0xff;
 498         dnload.block[7] = (block_address >> 16) & 0xff;
 499         dnload.block[8] = (block_address >> 8) & 0xff;
 500         dnload.block[9] = block_address & 0xff;
 501
 502         block_data = dnload.block + ROMLOAD_BLOCK_HDR_LEN;
 503         dnload.write_ptr = dnload.data + 2 +
 504                         (dnload.block_payload_size * dnload.block_number);
 505
 506         remaining_bytes = dnload.data_len - 3 -
 507                         (dnload.block_payload_size * dnload.block_number);
 508
 509         memcpy(block_data, dnload.write_ptr, dnload.block_payload_size);
 510
 511         if (remaining_bytes <= dnload.block_payload_size) {
 512                 fill_bytes = (dnload.block_payload_size - remaining_bytes);
 513                 printf("Preparing the last block, filling %i bytes,",
 514                         fill_bytes);
 515                 memset(block_data + remaining_bytes, 0x00, fill_bytes);
 516                 dnload.romload_state = SENDING_LAST_BLOCK;
 517         } else {
 518                         dnload.romload_state = SENDING_BLOCKS;
 519                         printf("Preparing block %i,", dnload.block_number+1);
 520         }
 521
 522         /* block checksum is lsb of ~(5 + block_size_lsb +  all bytes of
 523          * block_address + all data bytes) */
 524         for (i = 5; i < ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size; i++)
 525                 block_checksum += dnload.block[i];
 526
 527         /* checksum is lsb of ~(sum of LSBs of all block checksums) */
 528         printf(" block checksum is 0x%02x \n", ~(block_checksum) & 0xff);
 529         dnload.romload_dl_checksum += ~(block_checksum) & 0xff;
 530
 531         /* initialize block pointer to start of block */
 532         dnload.block_ptr = dnload.block;
 533
 534         dnload.block_number++;
 535         dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 536         return 0;
 537 }
 538
 539 static int mtk_prepare_block(void)
 540 {
 541         int rc, i;
 542
 543         int remaining_bytes;
 544         int fill_bytes;
 545         uint8_t *block_data;
 546         uint8_t tmp_byteswap;
 547         uint32_t tmp_size;
 548
 549         dnload.block_len = MTK_BLOCK_SIZE;
 550         dnload.echo_bytecount = 0;
 551
 552         /* if first block, allocate memory */
 553         if (!dnload.block_number) {
 554                 dnload.block = malloc(dnload.block_len);
 555                 if (!dnload.block) {
 556                         fprintf(stderr, "No memory\n");
 557                         return -ENOMEM;
 558                 }
 559
 560                 /* calculate the number of blocks we need to send */
 561                 dnload.block_count = (dnload.data_len-3) / MTK_BLOCK_SIZE;
 562                 /* add one more block if no multiple of blocksize */
 563                 if((dnload.data_len-3) % MTK_BLOCK_SIZE)
 564                         dnload.block_count++;
 565
 566                 /* divide by 2, since we have to tell the mtk loader the size
 567                  * as count of uint16 (odd transfer sizes are not possible) */
 568                 tmp_size = (dnload.block_count * MTK_BLOCK_SIZE)/2;
 569                 dnload.mtk_send_size[0] = (tmp_size >> 24) & 0xff;
 570                 dnload.mtk_send_size[1] = (tmp_size >> 16) & 0xff;
 571                 dnload.mtk_send_size[2] = (tmp_size >> 8) & 0xff;
 572                 dnload.mtk_send_size[3] = tmp_size & 0xff;
 573
 574                 /* initialize write pointer to start of data */
 575                 dnload.write_ptr = dnload.data;
 576         }
 577
 578         block_data = dnload.block;
 579         dnload.write_ptr = dnload.data + 2 +
 580                         (dnload.block_len * dnload.block_number);
 581
 582         remaining_bytes = dnload.data_len - 3 -
 583                         (dnload.block_len * dnload.block_number);
 584
 585         memcpy(block_data, dnload.write_ptr, MTK_BLOCK_SIZE);
 586
 587         if (remaining_bytes <= MTK_BLOCK_SIZE) {
 588                 fill_bytes = (MTK_BLOCK_SIZE - remaining_bytes);
 589                 printf("Preparing the last block, filling %i bytes\n",
 590                         fill_bytes);
 591                 memset(block_data + remaining_bytes, 0x00, fill_bytes);
 592                 dnload.romload_state = SENDING_LAST_BLOCK;
 593         } else {
 594                 dnload.romload_state = SENDING_BLOCKS;
 595                 printf("Preparing block %i\n", dnload.block_number+1);
 596         }
 597
 598         /* for the mtk romloader we need to swap MSB <-> LSB */
 599         for (i = 0; i < dnload.block_len; i += 2) {
 600                 tmp_byteswap = dnload.block[i];
 601                 dnload.block[i] = dnload.block[i+1];
 602                 dnload.block[i+1] = tmp_byteswap;
 603         }
 604
 605         /* initialize block pointer to start of block */
 606         dnload.block_ptr = dnload.block;
 607
 608         dnload.block_number++;
 609         return rc;
 610 }
 611
 612 static int handle_write_block(void)
 613 {
 614         int bytes_left, write_len, rc;
 615
 616         printf("handle_write_block(): ");
 617
 618         if (dnload.block_ptr >= dnload.block + dnload.block_len) {
 619                 printf("Block %i finished\n", dnload.block_number);
 620                 dnload.write_ptr = dnload.data;
 621                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 622                 if (dnload.romload_state == SENDING_LAST_BLOCK) {
 623                         dnload.romload_state = LAST_BLOCK_SENT;
 624                         printf("Finished, sent %i blocks in total\n",
 625                                 dnload.block_number);
 626                 } else {
 627                         dnload.romload_state = WAITING_BLOCK_ACK;
 628                 }
 629
 630                 return 0;
 631         }
 632
 633         /* try to write a maximum of block_len bytes */
 634         bytes_left = (dnload.block + dnload.block_len) - dnload.block_ptr;
 635         write_len = dnload.block_len;
 636         if (bytes_left < dnload.block_len)
 637                 write_len = bytes_left;
 638
 639         rc = write(dnload.serial_fd.fd, dnload.block_ptr, write_len);
 640         if (rc < 0) {
 641                 perror("Error during write");
 642                 return rc;
 643         }
 644
 645         dnload.block_ptr += rc;
 646
 647         printf("%u bytes (%tu/%u)\n", rc, dnload.block_ptr - dnload.block,
 648                 dnload.block_len);
 649
 650         return 0;
 651 }
 652
 653 #define WRITE_BLOCK     4096
 654
 655 static int handle_write_dnload(void)
 656 {
 657         int bytes_left, write_len, rc;
 658         uint8_t xor_init = 0x02;
 659
 660         printf("handle_write(): ");
 661         if (dnload.write_ptr == dnload.data) {
 662                 /* no bytes have been transferred yet */
 663                 switch (dnload.mode) {
 664                 case MODE_C155:
 665                 case MODE_C140xor:
 666                 case MODE_C123xor:
 667                         rc = write(dnload.serial_fd.fd, &xor_init, 1);
 668                         break;
 669                 default:
 670                         break;
 671                 }
 672         } else if (dnload.write_ptr >= dnload.data + dnload.data_len) {
 673                 printf("finished\n");
 674                 dnload.write_ptr = dnload.data;
 675                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 676                 return 1;
 677         }
 678
 679         /* try to write a maximum of WRITE_BLOCK bytes */
 680         bytes_left = (dnload.data + dnload.data_len) - dnload.write_ptr;
 681         write_len = WRITE_BLOCK;
 682         if (bytes_left < WRITE_BLOCK)
 683                 write_len = bytes_left;
 684
 685         rc = write(dnload.serial_fd.fd, dnload.write_ptr, write_len);
 686         if (rc < 0) {
 687                 perror("Error during write");
 688                 return rc;
 689         }
 690
 691         dnload.write_ptr += rc;
 692
 693         printf("%u bytes (%tu/%u)\n", rc, dnload.write_ptr - dnload.data,
 694                 dnload.data_len);
 695
 696         return 0;
 697 }
 698
 699 static int handle_sercomm_write(void)
 700 {
 701         uint8_t c;
 702
 703         if (sercomm_drv_pull(&c) != 0) {
 704                 if (write(dnload.serial_fd.fd, &c, 1) != 1)
 705                         perror("short write");
 706                 } else
 707                         dnload.serial_fd.when &= ~BSC_FD_WRITE;
 708
 709         return 0;
 710 }
 711
 712 static int handle_write(void)
 713 {
 714         /* TODO: simplify this again (global state: downloading, sercomm) */
 715         switch (dnload.mode) {
 716         case MODE_ROMLOAD:
 717                 switch (dnload.romload_state) {
 718                 case SENDING_BLOCKS:
 719                 case SENDING_LAST_BLOCK:
 720                         return handle_write_block();
 721                 default:
 722                         return handle_sercomm_write();
 723                 }
 724                 break;
 725         case MODE_MTK:
 726                 switch (dnload.mtk_state) {
 727                 case MTK_SENDING_BLOCKS:
 728                         return handle_write_block();
 729                 default:
 730                         return handle_sercomm_write();
 731                 }
 732                 break;
 733         default:
 734                 switch (dnload.state) {
 735                 case DOWNLOADING:
 736                         return handle_write_dnload();
 737                 default:
 738                         return handle_sercomm_write();
 739                 }
 740         }
 741
 742         return 0;
 743 }
 744
 745 static uint8_t buffer[sizeof(phone_prompt1)];
 746 static uint8_t *bufptr = buffer;
 747
 748 static void hdlc_send_to_phone(uint8_t dlci, uint8_t *data, int len)
 749 {
 750         struct msgb *msg;
 751         uint8_t *dest;
 752
 753         if(dnload.dump_tx) {
 754                 printf("hdlc_send(dlci=%u): ", dlci);
 755                 osmocon_osmo_hexdump(data, len);
 756         }
 757
 758         if (len > 512) {
 759                 fprintf(stderr, "Too much data to send. %u\n", len);
 760                 return;
 761         }
 762
 763         /* push the message into the stack */
 764         msg = sercomm_alloc_msgb(512);
 765         if (!msg) {
 766                 fprintf(stderr, "Failed to create data for the frame.\n");
 767                 return;
 768         }
 769
 770         /* copy the data */
 771         dest = msgb_put(msg, len);
 772         memcpy(dest, data, len);
 773
 774         sercomm_sendmsg(dlci, msg);
 775
 776         dnload.serial_fd.when |= BSC_FD_WRITE;
 777 }
 778
 779 static void hdlc_console_cb(uint8_t dlci, struct msgb *msg)
 780 {
 781         write(1, msg->data, msg->len);
 782         msgb_free(msg);
 783 }
 784
 785 static void hdlc_tool_cb(uint8_t dlci, struct msgb *msg)
 786 {
 787         struct tool_server *srv = tool_server_for_dlci[dlci];
 788
 789         if(dnload.dump_rx) {
 790                 printf("hdlc_recv(dlci=%u): ", dlci);
 791                 osmocon_osmo_hexdump(msg->data, msg->len);
 792         }
 793
 794         if(srv) {
 795                 struct tool_connection *con;
 796                 uint16_t *len;
 797
 798                 len = (uint16_t *) msgb_push(msg, 2);
 799                 *len = htons(msg->len - sizeof(*len));
 800
 801                 llist_for_each_entry(con, &srv->connections, entry) {
 802                         if (write(con->fd.fd, msg->data, msg->len) != msg->len) {
 803                                 fprintf(stderr,
 804                                         "Failed to write msg to the socket..\n");
 805                                 continue;
 806                         }
 807                 }
 808         }
 809
 810         msgb_free(msg);
 811 }
 812
 813 static int handle_buffer(int buf_used_len)
 814 {
 815         int nbytes, buf_left, i;
 816
 817         buf_left = buf_used_len - (bufptr - buffer);
 818         if (buf_left <= 0) {
 819                 memmove(buffer, buffer+1, buf_used_len-1);
 820                 bufptr -= 1;
 821                 buf_left = 1;
 822         }
 823
 824         nbytes = read(dnload.serial_fd.fd, bufptr, buf_left);
 825         if (nbytes <= 0)
 826                 return nbytes;
 827
 828         if (!dnload.expect_hdlc) {
 829                 printf("got %i bytes from modem, ", nbytes);
 830                 printf("data looks like: ");
 831                 osmocon_osmo_hexdump(bufptr, nbytes);
 832         } else {
 833                 for (i = 0; i < nbytes; ++i)
 834                         if (sercomm_drv_rx_char(bufptr[i]) == 0)
 835                                 printf("Dropping sample '%c'\n", bufptr[i]);
 836         }
 837
 838         return nbytes;
 839 }
 840
 841 /* Compal ramloader */
 842 static int handle_read(void)
 843 {
 844         int rc, nbytes;
 845
 846         nbytes = handle_buffer(sizeof(buffer));
 847         if (nbytes <= 0)
 848                 return nbytes;
 849
 850         if (!memcmp(buffer, phone_prompt1, sizeof(phone_prompt1))) {
 851                 printf("Received PROMPT1 from phone, responding with CMD\n");
 852                 dnload.expect_hdlc = 0;
 853                 dnload.state = WAITING_PROMPT2;
 854                 if(dnload.filename) {
 855                         rc = write(dnload.serial_fd.fd, dnload_cmd, sizeof(dnload_cmd));
 856
 857                         /* re-read file */
 858                         rc = read_file(dnload.filename);
 859                         if (rc < 0) {
 860                                 fprintf(stderr, "read_file(%s) failed with %d\n",
 861                                                 dnload.filename, rc);
 862                                 exit(1);
 863                         }
 864                 }
 865         } else if (!memcmp(buffer, phone_prompt2, sizeof(phone_prompt2))) {
 866                 printf("Received PROMPT2 from phone, starting download\n");
 867                 dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 868                 dnload.state = DOWNLOADING;
 869         } else if (!memcmp(buffer, phone_ack, sizeof(phone_ack))) {
 870                 printf("Received DOWNLOAD ACK from phone, your code is"
 871                         " running now!\n");
 872                 dnload.serial_fd.when = BSC_FD_READ;
 873                 dnload.state = WAITING_PROMPT1;
 874                 dnload.write_ptr = dnload.data;
 875                 dnload.expect_hdlc = 1;
 876
 877                 /* check for romloader chainloading mode used as a workaround
 878                  * for the magic on the C139/C140 and J100i */
 879                 if (dnload.chainload_filename != NULL) {
 880                         printf("Enabled Compal ramloader -> Calypso romloader"
 881                                 " chainloading mode\n");
 882                         bufptr = buffer;
 883                         dnload.filename = dnload.chainload_filename;
 884                         dnload.mode = MODE_ROMLOAD;
 885                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
 886                         tick_timer.cb = &beacon_timer_cb;
 887                         tick_timer.data = &tick_timer;
 888                         osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval);
 889                 }
 890         } else if (!memcmp(buffer, phone_nack, sizeof(phone_nack))) {
 891                 printf("Received DOWNLOAD NACK from phone, something went"
 892                         " wrong :(\n");
 893                 dnload.serial_fd.when = BSC_FD_READ;
 894                 dnload.state = WAITING_PROMPT1;
 895                 dnload.write_ptr = dnload.data;
 896         } else if (!memcmp(buffer, phone_nack_magic, sizeof(phone_nack_magic))) {
 897                 printf("Received MAGIC NACK from phone, you need to"
 898                         " have \"1003\" at 0x803ce0\n");
 899                 dnload.serial_fd.when = BSC_FD_READ;
 900                 dnload.state = WAITING_PROMPT1;
 901                 dnload.write_ptr = dnload.data;
 902         } else if (!memcmp(buffer, ftmtool, sizeof(ftmtool))) {
 903                 printf("Received FTMTOOL from phone, ramloader has aborted\n");
 904                 dnload.serial_fd.when = BSC_FD_READ;
 905                 dnload.state = WAITING_PROMPT1;
 906                 dnload.write_ptr = dnload.data;
 907         }
 908         bufptr += nbytes;
 909
 910         return nbytes;
 911 }
 912
 913 /* "Calypso non-secure romloader" */
 914 static int handle_read_romload(void)
 915 {
 916         int rc, nbytes, buf_used_len;
 917
 918         /* virtually limit buffer length for romloader, since responses
 919          * are shorter and vary in length */
 920
 921         switch (dnload.romload_state) {
 922         case WAITING_PARAM_ACK:
 923                 buf_used_len = 4;       /* ">p" + uint16_t len */
 924                 break;
 925         case WAITING_CHECKSUM_ACK:
 926                 buf_used_len = 3;       /* ">c" + uint8_t checksum */
 927                 break;
 928         case FINISHED:
 929                 buf_used_len = sizeof(buffer);
 930                 break;
 931         default:
 932                 buf_used_len = 2;       /* ">*" */
 933         }
 934
 935         nbytes = handle_buffer(buf_used_len);
 936         if (nbytes <= 0)
 937                 return nbytes;
 938
 939         switch (dnload.romload_state) {
 940         case WAITING_IDENTIFICATION:
 941                 if (memcmp(buffer, romload_ident_ack,
 942                             sizeof(romload_ident_ack)))
 943                         break;
 944
 945                 printf("Received ident ack from phone, sending "
 946                         "parameter sequence\n");
 947                 dnload.expect_hdlc = 1;
 948                 dnload.romload_state = WAITING_PARAM_ACK;
 949                 rc = write(dnload.serial_fd.fd, romload_param,
 950                            sizeof(romload_param));
 951                 /* re-read file */
 952                 rc = read_file(dnload.filename);
 953                 if (rc < 0) {
 954                         fprintf(stderr, "read_file(%s) failed with %d\n",
 955                                 dnload.filename, rc);
 956                         exit(1);
 957                 }
 958                 break;
 959         case WAITING_PARAM_ACK:
 960                 if (memcmp(buffer, romload_param_ack,
 961                             sizeof(romload_param_ack)))
 962                         break;
 963
 964                 printf("Received parameter ack from phone, "
 965                         "starting download\n");
 966                 serial_set_baudrate(ROMLOAD_DL_BAUDRATE);
 967
 968                 /* using the max blocksize the phone tells us */
 969                 dnload.block_payload_size = ((buffer[3] << 8) + buffer[2]);
 970                 printf("Used blocksize for download is %i bytes\n",
 971                         dnload.block_payload_size);
 972                 dnload.block_payload_size -= ROMLOAD_BLOCK_HDR_LEN;
 973                 dnload.romload_state = SENDING_BLOCKS;
 974                 dnload.block_number = 0;
 975                 romload_prepare_block();
 976                 bufptr -= 2;
 977                 break;
 978         case WAITING_BLOCK_ACK:
 979         case LAST_BLOCK_SENT:
 980                 if (!memcmp(buffer, romload_block_ack,
 981                             sizeof(romload_block_ack))) {
 982                         printf("Received block ack from phone\n");
 983                         if (dnload.romload_state == LAST_BLOCK_SENT) {
 984                                 /* send the checksum */
 985                                 uint8_t final_checksum =
 986                                         (~(dnload.romload_dl_checksum) & 0xff);
 987                                 printf("Sending checksum: 0x%02x \n",
 988                                         final_checksum);
 989                                 rc = write(dnload.serial_fd.fd,
 990                                            romload_checksum_cmd,
 991                                            sizeof(romload_checksum_cmd));
 992                                 rc = write(dnload.serial_fd.fd,
 993                                            &final_checksum, 1);
 994                                 dnload.romload_state = WAITING_CHECKSUM_ACK;
 995                         } else
 996                                 romload_prepare_block();
 997                 } else if (!memcmp(buffer, romload_block_nack,
 998                                    sizeof(romload_block_nack))) {
 999                         printf("Received block nack from phone, "
1000                                 "something went wrong, aborting\n");
1001                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1002                         dnload.romload_state = WAITING_IDENTIFICATION;
1003                         osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval);
1004                 }
1005                 break;
1006         case WAITING_CHECKSUM_ACK:
1007                 if (!memcmp(buffer, romload_checksum_ack,
1008                             sizeof(romload_checksum_ack))) {
1009                         printf("Checksum on phone side matches, "
1010                                 "let's branch to your code\n");
1011                         printf("Branching to 0x%08x\n", ROMLOAD_ADDRESS);
1012
1013                         rc = write(dnload.serial_fd.fd, romload_branch_cmd,
1014                                    sizeof(romload_branch_cmd));
1015                         rc = write(dnload.serial_fd.fd, &dnload.load_address,
1016                                    sizeof(dnload.load_address));
1017                         dnload.romload_state = WAITING_BRANCH_ACK;
1018                         bufptr -= 1;
1019                 } else if (!memcmp(buffer, romload_checksum_nack,
1020                                    sizeof(romload_checksum_nack))) {
1021                         printf("Checksum on phone side (0x%02x) doesn't "
1022                                 "match ours, aborting\n", ~buffer[2]);
1023                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1024                         dnload.romload_state = WAITING_IDENTIFICATION;
1025                         osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval);
1026                         bufptr -= 1;
1027                 }
1028                 break;
1029         case WAITING_BRANCH_ACK:
1030                 if (!memcmp(buffer, romload_branch_ack,
1031                             sizeof(romload_branch_ack))) {
1032                         printf("Received branch ack, your code is running now!\n");
1033                         dnload.serial_fd.when = BSC_FD_READ;
1034                         dnload.romload_state = FINISHED;
1035                         dnload.write_ptr = dnload.data;
1036                         dnload.expect_hdlc = 1;
1037                 } else if (!memcmp(buffer, romload_branch_nack,
1038                            sizeof(romload_branch_nack))) {
1039                         printf("Received branch nack, aborting\n");
1040                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1041                         dnload.romload_state = WAITING_IDENTIFICATION;
1042                         osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval);
1043                 }
1044                 break;
1045         default:
1046                 break;
1047         }
1048
1049         bufptr += nbytes;
1050         return nbytes;
1051 }
1052
1053 /* MTK romloader */
1054 static int handle_read_mtk(void)
1055 {
1056         int rc, nbytes, buf_used_len;
1057
1058         switch (dnload.mtk_state) {
1059         case MTK_WAIT_ADDR_ACK:
1060         case MTK_WAIT_SIZE_ACK:
1061         case MTK_WAIT_BRANCH_ADDR_ACK:
1062                 buf_used_len = 4;
1063                 break;
1064         case MTK_FINISHED:
1065                 buf_used_len = sizeof(buffer);
1066                 break;
1067         default:
1068                 buf_used_len = 1;
1069         }
1070
1071         nbytes = handle_buffer(buf_used_len);
1072         if (nbytes <= 0)
1073                 return nbytes;
1074
1075         switch (dnload.mtk_state) {
1076         case MTK_INIT_1:
1077                 if (!(buffer[0] == mtk_init_resp[0]))
1078                         break;
1079                 dnload.mtk_state = MTK_INIT_2;
1080                 printf("Received init magic byte 1\n");
1081                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[1], 1);
1082                 break;
1083         case MTK_INIT_2:
1084                 if (!(buffer[0] == mtk_init_resp[1]))
1085                         break;
1086                 dnload.mtk_state = MTK_INIT_3;
1087                 printf("Received init magic byte 2\n");
1088                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[2], 1);
1089                 break;
1090         case MTK_INIT_3:
1091                 if (!(buffer[0] == mtk_init_resp[2]))
1092                         break;
1093                 dnload.mtk_state = MTK_INIT_4;
1094                 printf("Received init magic byte 3\n");
1095                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[3], 1);
1096                 break;
1097         case MTK_INIT_4:
1098                 if (!(buffer[0] == mtk_init_resp[3]))
1099                         break;
1100                 dnload.mtk_state = MTK_WAIT_WRITE_ACK;
1101                 printf("Received init magic byte 4, requesting write\n");
1102                 rc = write(dnload.serial_fd.fd, &mtk_command[0], 1);
1103                 break;
1104         case MTK_WAIT_WRITE_ACK:
1105                 if (!(buffer[0] == mtk_command[0]))
1106                         break;
1107                 dnload.mtk_state = MTK_WAIT_ADDR_ACK;
1108                 printf("Received write ack, sending load address\n");
1109
1110                 rc = write(dnload.serial_fd.fd, &dnload.load_address,
1111                            sizeof(dnload.load_address));
1112                 break;
1113         case MTK_WAIT_ADDR_ACK:
1114                 if (memcmp(buffer, dnload.load_address,
1115                             sizeof(dnload.load_address)))
1116                         break;
1117                 printf("Received address ack from phone, sending loadsize\n");
1118                 /* re-read file */
1119                 rc = read_file(dnload.filename);
1120                 if (rc < 0) {
1121                         fprintf(stderr, "read_file(%s) failed with %d\n",
1122                                 dnload.filename, rc);
1123                         exit(1);
1124                 }
1125                 dnload.block_number = 0;
1126                 mtk_prepare_block();
1127                 dnload.mtk_state = MTK_WAIT_SIZE_ACK;
1128                 rc = write(dnload.serial_fd.fd, &dnload.mtk_send_size,
1129                            sizeof(dnload.mtk_send_size));
1130                 break;
1131         case MTK_WAIT_SIZE_ACK:
1132                 if (memcmp(buffer, dnload.mtk_send_size,
1133                             sizeof(dnload.mtk_send_size)))
1134                         break;
1135                 printf("Received size ack\n");
1136                 dnload.expect_hdlc = 1;
1137                 dnload.mtk_state = MTK_SENDING_BLOCKS;
1138                 dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
1139                 bufptr -= 3;
1140                 break;
1141         case MTK_SENDING_BLOCKS:
1142                 if (!(buffer[0] == dnload.block[dnload.echo_bytecount]))
1143                         printf("Warning: Byte %i of Block %i doesn't match,"
1144                                 " check your serial connection!\n",
1145                                 dnload.echo_bytecount, dnload.block_number);
1146                 dnload.echo_bytecount++;
1147
1148                 if ((dnload.echo_bytecount+1) > MTK_BLOCK_SIZE) {
1149                         if ( dnload.block_number == dnload.block_count) {
1150                                 rc = write(dnload.serial_fd.fd,
1151                                            &mtk_command[3], 1);
1152                                 printf("Sending branch command\n");
1153                                 dnload.expect_hdlc = 0;
1154                                 dnload.mtk_state = MTK_WAIT_BRANCH_CMD_ACK;
1155                                 break;
1156                         }
1157                         printf("Received Block %i preparing next block\n",
1158                                 dnload.block_number);
1159                         mtk_prepare_block();
1160                         dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
1161                 }
1162                 break;
1163         case MTK_WAIT_BRANCH_CMD_ACK:
1164                 if (!(buffer[0] == mtk_command[3]))
1165                         break;
1166                 dnload.mtk_state = MTK_WAIT_BRANCH_ADDR_ACK;
1167                 printf("Received branch command ack, sending address\n");
1168
1169                 rc = write(dnload.serial_fd.fd, &dnload.load_address,
1170                            sizeof(dnload.load_address));
1171                 break;
1172         case MTK_WAIT_BRANCH_ADDR_ACK:
1173                 if (memcmp(buffer, dnload.load_address,
1174                             sizeof(dnload.load_address)))
1175                         break;
1176                 printf("Received branch address ack, code should run now\n");
1177                 serial_set_baudrate(MODEM_BAUDRATE);
1178                 dnload.serial_fd.when = BSC_FD_READ;
1179                 dnload.mtk_state = MTK_FINISHED;
1180                 dnload.write_ptr = dnload.data;
1181                 dnload.expect_hdlc = 1;
1182                 break;
1183         default:
1184                 break;
1185         }
1186
1187         bufptr += nbytes;
1188         return nbytes;
1189 }
1190
1191 static int serial_read(struct osmo_fd *fd, unsigned int flags)
1192 {
1193         int rc;
1194         if (flags & BSC_FD_READ) {
1195                 switch (dnload.mode) {
1196                         case MODE_ROMLOAD:
1197                                 rc = handle_read_romload();
1198                                 break;
1199                         case MODE_MTK:
1200                                 rc = handle_read_mtk();
1201                                 break;
1202                         default:
1203                                 rc = handle_read();
1204                                 break;
1205                 }
1206                 if (rc == 0)
1207                         exit(2);
1208         }
1209
1210         if (flags & BSC_FD_WRITE) {
1211                 rc = handle_write();
1212                 if (rc == 1)
1213                         dnload.state = WAITING_PROMPT1;
1214         }
1215         return 0;
1216 }
1217
1218 static int parse_mode(const char *arg)
1219 {
1220         if (!strcasecmp(arg, "c123"))
1221                 return MODE_C123;
1222         else if (!strcasecmp(arg, "c123xor"))
1223                 return MODE_C123xor;
1224         else if (!strcasecmp(arg, "c140"))
1225                 return MODE_C140;
1226         else if (!strcasecmp(arg, "c140xor"))
1227                 return MODE_C140xor;
1228         else if (!strcasecmp(arg, "c155"))
1229                 return MODE_C155;
1230         else if (!strcasecmp(arg, "romload"))
1231                 return MODE_ROMLOAD;
1232         else if (!strcasecmp(arg, "mtk"))
1233                 return MODE_MTK;
1234
1235         return -1;
1236 }
1237
1238 #define HELP_TEXT \
1239         "[ -v | -h ] [ -d [t][r] ] [ -p /dev/ttyXXXX ]\n" \
1240         "\t\t [ -s /tmp/osmocom_l2 ]\n" \
1241         "\t\t [ -l /tmp/osmocom_loader ]\n" \
1242         "\t\t [ -m {c123,c123xor,c140,c140xor,c155,romload,mtk} ]\n" \
1243         "\t\t [ -c /to-be-chainloaded-file.bin ]\n" \
1244         "\t\t [ -i beacon-interval (mS) ]\n" \
1245         "\t\t  file.bin\n\n" \
1246         "* Open serial port /dev/ttyXXXX (connected to your phone)\n" \
1247         "* Perform handshaking with the ramloader in the phone\n" \
1248         "* Download file.bin to the attached phone (base address 0x00800100)\n"
1249
1250 static int usage(const char *name)
1251 {
1252         printf("Usage: %s ", name);
1253         printf(HELP_TEXT);
1254         exit(2);
1255 }
1256
1257 static int version(const char *name)
1258 {
1259         printf("%s version %s\n", name, PACKAGE_VERSION);
1260         exit(2);
1261 }
1262
1263 static int un_tool_read(struct osmo_fd *fd, unsigned int flags)
1264 {
1265         int rc, c;
1266         uint16_t length = 0xffff;
1267         uint8_t buf[4096];
1268         struct tool_connection *con = (struct tool_connection *)fd->data;
1269
1270         c = 0;
1271         while(c < 2) {
1272                 rc = read(fd->fd, &buf + c, 2 - c);
1273                 if(rc == 0) {
1274                         // disconnect
1275                         goto close;
1276                 }
1277                 if(rc < 0) {
1278                         if(errno == EAGAIN) {
1279                                 continue;
1280                         }
1281                         fprintf(stderr, "Err from socket: %s\n", strerror(errno));
1282                         goto close;
1283                 }
1284                 c += rc;
1285         }
1286
1287         length = ntohs(*(uint16_t*)buf);
1288
1289         c = 0;
1290         while(c < length) {
1291                 rc = read(fd->fd, &buf + c, length - c);
1292                 if(rc == 0) {
1293                         // disconnect
1294                         goto close;
1295                 }
1296                 if(rc < 0) {
1297                         if(errno == EAGAIN) {
1298                                 continue;
1299                         }
1300                         fprintf(stderr, "Err from socket: %s\n", strerror(errno));
1301                         goto close;
1302                 }
1303                 c += rc;
1304         }
1305
1306         hdlc_send_to_phone(con->server->dlci, buf, length);
1307
1308         return 0;
1309 close:
1310
1311         close(fd->fd);
1312         osmo_fd_unregister(fd);
1313         llist_del(&con->entry);
1314         talloc_free(con);
1315         return -1;
1316 }
1317
1318 /* accept a new connection */
1319 static int tool_accept(struct osmo_fd *fd, unsigned int flags)
1320 {
1321         struct tool_server *srv = (struct tool_server *)fd->data;
1322         struct tool_connection *con;
1323         struct sockaddr_un un_addr;
1324         socklen_t len;
1325         int rc;
1326
1327         len = sizeof(un_addr);
1328         rc = accept(fd->fd, (struct sockaddr *) &un_addr, &len);
1329         if (rc < 0) {
1330                 fprintf(stderr, "Failed to accept a new connection.\n");
1331                 return -1;
1332         }
1333
1334         con = talloc_zero(NULL, struct tool_connection);
1335         if (!con) {
1336                 fprintf(stderr, "Failed to create tool connection.\n");
1337                 return -1;
1338         }
1339
1340         con->server = srv;
1341
1342         con->fd.fd = rc;
1343         con->fd.when = BSC_FD_READ;
1344         con->fd.cb = un_tool_read;
1345         con->fd.data = con;
1346         if (osmo_fd_register(&con->fd) != 0) {
1347                 fprintf(stderr, "Failed to register the fd.\n");
1348                 return -1;
1349         }
1350
1351         llist_add(&con->entry, &srv->connections);
1352         return 0;
1353 }
1354
1355 /*
1356  * Register and start a tool server
1357  */
1358 static int register_tool_server(struct tool_server *ts,
1359                                 const char *path,
1360                                 uint8_t dlci)
1361 {
1362         struct osmo_fd *bfd = &ts->bfd;
1363         struct sockaddr_un local;
1364         unsigned int namelen;
1365         int rc;
1366
1367         bfd->fd = socket(AF_UNIX, SOCK_STREAM, 0);
1368
1369         if (bfd->fd < 0) {
1370                 fprintf(stderr, "Failed to create Unix Domain Socket.\n");
1371                 return -1;
1372         }
1373
1374         local.sun_family = AF_UNIX;
1375         strncpy(local.sun_path, path, sizeof(local.sun_path));
1376         local.sun_path[sizeof(local.sun_path) - 1] = '\0';
1377         unlink(local.sun_path);
1378
1379         /* we use the same magic that X11 uses in Xtranssock.c for
1380          * calculating the proper length of the sockaddr */
1381 #if defined(BSD44SOCKETS) || defined(__UNIXWARE__)
1382         local.sun_len = strlen(local.sun_path);
1383 #endif
1384 #if defined(BSD44SOCKETS) || defined(SUN_LEN)
1385         namelen = SUN_LEN(&local);
1386 #else
1387         namelen = strlen(local.sun_path) +
1388                   offsetof(struct sockaddr_un, sun_path);
1389 #endif
1390
1391         rc = bind(bfd->fd, (struct sockaddr *) &local, namelen);
1392         if (rc != 0) {
1393                 fprintf(stderr, "Failed to bind the unix domain socket. '%s'\n",
1394                         local.sun_path);
1395                 return -1;
1396         }
1397
1398         if (listen(bfd->fd, 0) != 0) {
1399                 fprintf(stderr, "Failed to listen.\n");
1400                 return -1;
1401         }
1402
1403         bfd->when = BSC_FD_READ;
1404         bfd->cb = tool_accept;
1405         bfd->data = ts;
1406
1407         ts->dlci = dlci;
1408         INIT_LLIST_HEAD(&ts->connections);
1409
1410         tool_server_for_dlci[dlci] = ts;
1411
1412         sercomm_register_rx_cb(dlci, hdlc_tool_cb);
1413
1414         if (osmo_fd_register(bfd) != 0) {
1415                 fprintf(stderr, "Failed to register the bfd.\n");
1416                 return -1;
1417         }
1418
1419         return 0;
1420 }
1421
1422 extern void hdlc_tpudbg_cb(uint8_t dlci, struct msgb *msg);
1423
1424 void parse_debug(const char *str)
1425 {
1426         while(*str) {
1427                 switch(*str) {
1428                 case 't':
1429                         dnload.dump_tx = 1;
1430                         break;
1431                 case 'r':
1432                         dnload.dump_rx = 1;
1433                         break;
1434                 default:
1435                         printf("Unknown debug flag %c\n", *str);
1436                         abort();
1437                         break;
1438                 }
1439                 str++;
1440         }
1441 }
1442
1443 int main(int argc, char **argv)
1444 {
1445         int opt, flags;
1446         uint32_t tmp_load_address = ROMLOAD_ADDRESS;
1447         const char *serial_dev = "/dev/ttyUSB1";
1448         const char *layer2_un_path = "/tmp/osmocom_l2";
1449         const char *loader_un_path = "/tmp/osmocom_loader";
1450
1451         dnload.mode = MODE_C123;
1452         dnload.chainload_filename = NULL;
1453         dnload.beacon_interval = DEFAULT_BEACON_INTERVAL;
1454
1455         while ((opt = getopt(argc, argv, "d:hl:p:m:c:s:i:v")) != -1) {
1456                 switch (opt) {
1457                 case 'p':
1458                         serial_dev = optarg;
1459                         break;
1460                 case 'm':
1461                         dnload.mode = parse_mode(optarg);
1462                         if (dnload.mode < 0)
1463                                 usage(argv[0]);
1464                         break;
1465                 case 's':
1466                         layer2_un_path = optarg;
1467                         break;
1468                 case 'l':
1469                         loader_un_path = optarg;
1470                         break;
1471                 case 'v':
1472                         version(argv[0]);
1473                         break;
1474                 case 'd':
1475                         parse_debug(optarg);
1476                         break;
1477                 case 'c':
1478                         dnload.chainload_filename = optarg;
1479                         break;
1480                 case 'i':
1481                         dnload.beacon_interval = atoi(optarg) * 1000;
1482                         break;
1483                 case 'h':
1484                 default:
1485                         usage(argv[0]);
1486                         break;
1487                 }
1488         }
1489
1490         if (argc <= optind) {
1491                 dnload.filename = NULL;
1492         } else {
1493                 dnload.filename = argv[optind];
1494         }
1495
1496         dnload.serial_fd.fd = serial_init(serial_dev);
1497         if (dnload.serial_fd.fd < 0) {
1498                 fprintf(stderr, "Cannot open serial device %s\n", serial_dev);
1499                 exit(1);
1500         }
1501
1502         if (osmo_fd_register(&dnload.serial_fd) != 0) {
1503                 fprintf(stderr, "Failed to register the serial.\n");
1504                 exit(1);
1505         }
1506
1507         /* Set serial socket to non-blocking mode of operation */
1508         flags = fcntl(dnload.serial_fd.fd, F_GETFL);
1509         flags |= O_NONBLOCK;
1510         fcntl(dnload.serial_fd.fd, F_SETFL, flags);
1511
1512         dnload.serial_fd.when = BSC_FD_READ;
1513         dnload.serial_fd.cb = serial_read;
1514
1515         /* initialize the HDLC layer */
1516         sercomm_init();
1517         sercomm_register_rx_cb(SC_DLCI_CONSOLE, hdlc_console_cb);
1518         sercomm_register_rx_cb(SC_DLCI_DEBUG, hdlc_tpudbg_cb);
1519
1520         /* unix domain socket handling */
1521         if (register_tool_server(&dnload.layer2_server, layer2_un_path,
1522                                  SC_DLCI_L1A_L23) != 0)
1523                 exit(1);
1524
1525         if (register_tool_server(&dnload.loader_server, loader_un_path,
1526                                  SC_DLCI_LOADER) != 0)
1527                 exit(1);
1528
1529         /* if in romload mode, start our beacon timer */
1530         if (dnload.mode == MODE_ROMLOAD) {
1531                 tmp_load_address = ROMLOAD_ADDRESS;
1532                 serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1533                 tick_timer.cb = &beacon_timer_cb;
1534                 tick_timer.data = &tick_timer;
1535                 osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval);
1536         }
1537         else if (dnload.mode == MODE_MTK) {
1538                 tmp_load_address = MTK_ADDRESS;
1539                 serial_set_baudrate(MTK_INIT_BAUDRATE);
1540                 tick_timer.cb = &mtk_timer_cb;
1541                 tick_timer.data = &tick_timer;
1542                 osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval);
1543         }
1544
1545         dnload.load_address[0] = (tmp_load_address >> 24) & 0xff;
1546         dnload.load_address[1] = (tmp_load_address >> 16) & 0xff;
1547         dnload.load_address[2] = (tmp_load_address >> 8) & 0xff;
1548         dnload.load_address[3] = tmp_load_address & 0xff;
1549
1550         while (1) {
1551                 if (osmo_select_main(0) < 0)
1552                         break;
1553         }
1554
1555         close(dnload.serial_fd.fd);
1556
1557         exit(0);
1558 }