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 <osmocore/linuxlist.h>
  43 #include <osmocore/select.h>
  44 #include <osmocore/talloc.h>
  45 #include <osmocore/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 bsc_fd fd;
  73 };
  74
  75 /**
  76  * server for a tool
  77  */
  78 struct tool_server {
  79         struct bsc_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 bsc_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 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                 bsc_schedule_timer(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                 bsc_schedule_timer(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         if (dnload.data) {
 339                 free(dnload.data);
 340                 dnload.data = NULL;
 341         }
 342
 343         if (dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) {
 344                 if (st.st_size < (MAGIC_OFFSET + sizeof(phone_magic)))
 345                         payload_size = MAGIC_OFFSET + sizeof(phone_magic);
 346                 else {
 347                         printf("\nThe filesize is larger than 15kb, code on "
 348                                 "the magic address will be overwritten!\nUse "
 349                                 "loader.bin and upload the application with "
 350                                 "osmoload instead!\n\n");
 351                         payload_size = st.st_size;
 352                 }
 353         } else
 354                 payload_size = st.st_size;
 355
 356         dnload.data = malloc(MAX_HDR_SIZE + payload_size);
 357
 358         if (!dnload.data) {
 359                 close(fd);
 360                 fprintf(stderr, "No memory\n");
 361                 return -ENOMEM;
 362         }
 363
 364         /* copy in the header, if any */
 365         switch (dnload.mode) {
 366         case MODE_C155:
 367                 hdr = data_hdr_c155;
 368                 hdr_len = sizeof(data_hdr_c155);
 369                 break;
 370         case MODE_C140:
 371         case MODE_C140xor:
 372         case MODE_C123:
 373         case MODE_C123xor:
 374                 hdr = data_hdr_c123;
 375                 hdr_len = sizeof(data_hdr_c123);
 376                 break;
 377         case MODE_ROMLOAD:
 378                 break;
 379         default:
 380                 break;
 381         }
 382
 383         if (hdr && hdr_len)
 384                 memcpy(dnload.data, hdr, hdr_len);
 385
 386         /* 2 bytes for length + header */
 387         file_data = dnload.data + 2 + hdr_len;
 388
 389         /* write the length, keep running XOR */
 390         tot_len = hdr_len + payload_size;
 391         nibble = tot_len >> 8;
 392         dnload.data[0] = nibble;
 393         running_xor ^= nibble;
 394         nibble = tot_len & 0xff;
 395         dnload.data[1] = nibble;
 396         running_xor ^= nibble;
 397
 398         if (hdr_len && hdr) {
 399                 memcpy(dnload.data+2, hdr, hdr_len);
 400
 401                 for (i = 0; i < hdr_len; i++)
 402                         running_xor ^= hdr[i];
 403         }
 404
 405         rc = read(fd, file_data, st.st_size);
 406         if (rc < 0) {
 407                 perror("error reading file\n");
 408                 free(dnload.data);
 409                 dnload.data = NULL;
 410                 close(fd);
 411                 return -EIO;
 412         }
 413         if (rc < st.st_size) {
 414                 free(dnload.data);
 415                 dnload.data = NULL;
 416                 close(fd);
 417                 fprintf(stderr, "Short read of file (%d < %d)\n",
 418                         rc, (int)st.st_size);
 419                 return -EIO;
 420         }
 421
 422         close(fd);
 423
 424         dnload.data_len = (file_data+payload_size) - dnload.data;
 425
 426         /* fill memory between data end and magic, add magic */
 427         if(dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) {
 428                 if (st.st_size < MAGIC_OFFSET)
 429                         memset(file_data + st.st_size, 0x00,
 430                                 payload_size - st.st_size);
 431                 memcpy(dnload.data + MAGIC_OFFSET, phone_magic,
 432                         sizeof(phone_magic));
 433         }
 434
 435         /* calculate XOR sum */
 436         for (i = 0; i < payload_size; i++)
 437                 running_xor ^= file_data[i];
 438
 439         dnload.data[dnload.data_len++] = running_xor;
 440
 441         /* initialize write pointer to start of data */
 442         dnload.write_ptr = dnload.data;
 443
 444         printf("read_file(%s): file_size=%u, hdr_len=%u, dnload_len=%u\n",
 445                 filename, (int)st.st_size, hdr_len, dnload.data_len);
 446
 447         return 0;
 448 }
 449
 450 static void hexdump(const uint8_t *data, unsigned int len)
 451 {
 452         int n;
 453
 454         for (n=0; n < len; n++)
 455                 printf("%02x ", data[n]);
 456         printf(" ");
 457         for (n=0; n < len; n++)
 458                 if (isprint(data[n]))
 459                         putchar(data[n]);
 460                 else
 461                         putchar('.');
 462         printf("\n");
 463 }
 464
 465 static int romload_prepare_block(void)
 466 {
 467         int i;
 468
 469         int block_checksum = 5;
 470         int remaining_bytes;
 471         int fill_bytes;
 472         uint8_t *block_data;
 473         uint32_t block_address;
 474
 475         dnload.block_len = ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size;
 476
 477         /* if first block, allocate memory */
 478         if (!dnload.block_number) {
 479                 dnload.block = malloc(dnload.block_len);
 480                 if (!dnload.block) {
 481                         fprintf(stderr, "No memory\n");
 482                         return -ENOMEM;
 483                 }
 484                 dnload.romload_dl_checksum = 0;
 485                 /* initialize write pointer to start of data */
 486                 dnload.write_ptr = dnload.data;
 487         }
 488
 489         block_address = ROMLOAD_ADDRESS +
 490                         (dnload.block_number * dnload.block_payload_size);
 491
 492         /* prepare our block header (10 bytes) */
 493         memcpy(dnload.block, romload_write_cmd, sizeof(romload_write_cmd));
 494         dnload.block[2] = 0x01; /* block index */
 495         /* should normally be the block number, but hangs when sending !0x01 */
 496         dnload.block[3] = 0x01; /* dnload.block_number+1 */
 497         dnload.block[4] = (dnload.block_payload_size >> 8) & 0xff;
 498         dnload.block[5] = dnload.block_payload_size & 0xff;
 499         dnload.block[6] = (block_address >> 24) & 0xff;
 500         dnload.block[7] = (block_address >> 16) & 0xff;
 501         dnload.block[8] = (block_address >> 8) & 0xff;
 502         dnload.block[9] = block_address & 0xff;
 503
 504         block_data = dnload.block + ROMLOAD_BLOCK_HDR_LEN;
 505         dnload.write_ptr = dnload.data + 2 +
 506                         (dnload.block_payload_size * dnload.block_number);
 507
 508         remaining_bytes = dnload.data_len - 3 -
 509                         (dnload.block_payload_size * dnload.block_number);
 510
 511         memcpy(block_data, dnload.write_ptr, dnload.block_payload_size);
 512
 513         if (remaining_bytes <= dnload.block_payload_size) {
 514                 fill_bytes = (dnload.block_payload_size - remaining_bytes);
 515                 printf("Preparing the last block, filling %i bytes,",
 516                         fill_bytes);
 517                 memset(block_data + remaining_bytes, 0x00, fill_bytes);
 518                 dnload.romload_state = SENDING_LAST_BLOCK;
 519         } else {
 520                         dnload.romload_state = SENDING_BLOCKS;
 521                         printf("Preparing block %i,", dnload.block_number+1);
 522         }
 523
 524         /* block checksum is lsb of ~(5 + block_size_lsb +  all bytes of
 525          * block_address + all data bytes) */
 526         for (i = 5; i < ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size; i++)
 527                 block_checksum += dnload.block[i];
 528
 529         /* checksum is lsb of ~(sum of LSBs of all block checksums) */
 530         printf(" block checksum is 0x%02x \n", ~(block_checksum) & 0xff);
 531         dnload.romload_dl_checksum += ~(block_checksum) & 0xff;
 532
 533         /* initialize block pointer to start of block */
 534         dnload.block_ptr = dnload.block;
 535
 536         dnload.block_number++;
 537         dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 538         return 0;
 539 }
 540
 541 static int mtk_prepare_block(void)
 542 {
 543         int rc, i;
 544
 545         int remaining_bytes;
 546         int fill_bytes;
 547         uint8_t *block_data;
 548         uint8_t tmp_byteswap;
 549         uint32_t tmp_size;
 550
 551         dnload.block_len = MTK_BLOCK_SIZE;
 552         dnload.echo_bytecount = 0;
 553
 554         /* if first block, allocate memory */
 555         if (!dnload.block_number) {
 556                 dnload.block = malloc(dnload.block_len);
 557                 if (!dnload.block) {
 558                         fprintf(stderr, "No memory\n");
 559                         return -ENOMEM;
 560                 }
 561
 562                 /* calculate the number of blocks we need to send */
 563                 dnload.block_count = (dnload.data_len-3) / MTK_BLOCK_SIZE;
 564                 /* add one more block if no multiple of blocksize */
 565                 if((dnload.data_len-3) % MTK_BLOCK_SIZE)
 566                         dnload.block_count++;
 567
 568                 /* divide by 2, since we have to tell the mtk loader the size
 569                  * as count of uint16 (odd transfer sizes are not possible) */
 570                 tmp_size = (dnload.block_count * MTK_BLOCK_SIZE)/2;
 571                 dnload.mtk_send_size[0] = (tmp_size >> 24) & 0xff;
 572                 dnload.mtk_send_size[1] = (tmp_size >> 16) & 0xff;
 573                 dnload.mtk_send_size[2] = (tmp_size >> 8) & 0xff;
 574                 dnload.mtk_send_size[3] = tmp_size & 0xff;
 575
 576                 /* initialize write pointer to start of data */
 577                 dnload.write_ptr = dnload.data;
 578         }
 579
 580         block_data = dnload.block;
 581         dnload.write_ptr = dnload.data + 2 +
 582                         (dnload.block_len * dnload.block_number);
 583
 584         remaining_bytes = dnload.data_len - 3 -
 585                         (dnload.block_len * dnload.block_number);
 586
 587         memcpy(block_data, dnload.write_ptr, MTK_BLOCK_SIZE);
 588
 589         if (remaining_bytes <= MTK_BLOCK_SIZE) {
 590                 fill_bytes = (MTK_BLOCK_SIZE - remaining_bytes);
 591                 printf("Preparing the last block, filling %i bytes\n",
 592                         fill_bytes);
 593                 memset(block_data + remaining_bytes, 0x00, fill_bytes);
 594                 dnload.romload_state = SENDING_LAST_BLOCK;
 595         } else {
 596                 dnload.romload_state = SENDING_BLOCKS;
 597                 printf("Preparing block %i\n", dnload.block_number+1);
 598         }
 599
 600         /* for the mtk romloader we need to swap MSB <-> LSB */
 601         for (i = 0; i < dnload.block_len; i += 2) {
 602                 tmp_byteswap = dnload.block[i];
 603                 dnload.block[i] = dnload.block[i+1];
 604                 dnload.block[i+1] = tmp_byteswap;
 605         }
 606
 607         /* initialize block pointer to start of block */
 608         dnload.block_ptr = dnload.block;
 609
 610         dnload.block_number++;
 611         return rc;
 612 }
 613
 614 static int handle_write_block(void)
 615 {
 616         int bytes_left, write_len, rc;
 617
 618         printf("handle_write_block(): ");
 619
 620         if (dnload.block_ptr >= dnload.block + dnload.block_len) {
 621                 printf("Block %i finished\n", dnload.block_number);
 622                 dnload.write_ptr = dnload.data;
 623                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 624                 if (dnload.romload_state == SENDING_LAST_BLOCK) {
 625                         dnload.romload_state = LAST_BLOCK_SENT;
 626                         printf("Finished, sent %i blocks in total\n",
 627                                 dnload.block_number);
 628                 } else {
 629                         dnload.romload_state = WAITING_BLOCK_ACK;
 630                 }
 631
 632                 return 0;
 633         }
 634
 635         /* try to write a maximum of block_len bytes */
 636         bytes_left = (dnload.block + dnload.block_len) - dnload.block_ptr;
 637         write_len = dnload.block_len;
 638         if (bytes_left < dnload.block_len)
 639                 write_len = bytes_left;
 640
 641         rc = write(dnload.serial_fd.fd, dnload.block_ptr, write_len);
 642         if (rc < 0) {
 643                 perror("Error during write");
 644                 return rc;
 645         }
 646
 647         dnload.block_ptr += rc;
 648
 649         printf("%u bytes (%tu/%u)\n", rc, dnload.block_ptr - dnload.block,
 650                 dnload.block_len);
 651
 652         return 0;
 653 }
 654
 655 #define WRITE_BLOCK     4096
 656
 657 static int handle_write_dnload(void)
 658 {
 659         int bytes_left, write_len, rc;
 660         uint8_t xor_init = 0x02;
 661
 662         printf("handle_write(): ");
 663         if (dnload.write_ptr == dnload.data) {
 664                 /* no bytes have been transferred yet */
 665                 switch (dnload.mode) {
 666                 case MODE_C155:
 667                 case MODE_C140xor:
 668                 case MODE_C123xor:
 669                         rc = write(dnload.serial_fd.fd, &xor_init, 1);
 670                         break;
 671                 default:
 672                         break;
 673                 }
 674         } else if (dnload.write_ptr >= dnload.data + dnload.data_len) {
 675                 printf("finished\n");
 676                 dnload.write_ptr = dnload.data;
 677                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 678                 return 1;
 679         }
 680
 681         /* try to write a maximum of WRITE_BLOCK bytes */
 682         bytes_left = (dnload.data + dnload.data_len) - dnload.write_ptr;
 683         write_len = WRITE_BLOCK;
 684         if (bytes_left < WRITE_BLOCK)
 685                 write_len = bytes_left;
 686
 687         rc = write(dnload.serial_fd.fd, dnload.write_ptr, write_len);
 688         if (rc < 0) {
 689                 perror("Error during write");
 690                 return rc;
 691         }
 692
 693         dnload.write_ptr += rc;
 694
 695         printf("%u bytes (%tu/%u)\n", rc, dnload.write_ptr - dnload.data,
 696                 dnload.data_len);
 697
 698         return 0;
 699 }
 700
 701 static int handle_sercomm_write(void)
 702 {
 703         uint8_t c;
 704
 705         if (sercomm_drv_pull(&c) != 0) {
 706                 if (write(dnload.serial_fd.fd, &c, 1) != 1)
 707                         perror("short write");
 708                 } else
 709                         dnload.serial_fd.when &= ~BSC_FD_WRITE;
 710
 711         return 0;
 712 }
 713
 714 static int handle_write(void)
 715 {
 716         /* TODO: simplify this again (global state: downloading, sercomm) */
 717         switch (dnload.mode) {
 718         case MODE_ROMLOAD:
 719                 switch (dnload.romload_state) {
 720                 case SENDING_BLOCKS:
 721                 case SENDING_LAST_BLOCK:
 722                         return handle_write_block();
 723                 default:
 724                         return handle_sercomm_write();
 725                 }
 726                 break;
 727         case MODE_MTK:
 728                 switch (dnload.mtk_state) {
 729                 case MTK_SENDING_BLOCKS:
 730                         return handle_write_block();
 731                 default:
 732                         return handle_sercomm_write();
 733                 }
 734                 break;
 735         default:
 736                 switch (dnload.state) {
 737                 case DOWNLOADING:
 738                         return handle_write_dnload();
 739                 default:
 740                         return handle_sercomm_write();
 741                 }
 742         }
 743
 744         return 0;
 745 }
 746
 747 static uint8_t buffer[sizeof(phone_prompt1)];
 748 static uint8_t *bufptr = buffer;
 749
 750 static void hdlc_send_to_phone(uint8_t dlci, uint8_t *data, int len)
 751 {
 752         struct msgb *msg;
 753         uint8_t *dest;
 754
 755         if(dnload.dump_tx) {
 756                 printf("hdlc_send(dlci=%u): ", dlci);
 757                 hexdump(data, len);
 758         }
 759
 760         if (len > 512) {
 761                 fprintf(stderr, "Too much data to send. %u\n", len);
 762                 return;
 763         }
 764
 765         /* push the message into the stack */
 766         msg = sercomm_alloc_msgb(512);
 767         if (!msg) {
 768                 fprintf(stderr, "Failed to create data for the frame.\n");
 769                 return;
 770         }
 771
 772         /* copy the data */
 773         dest = msgb_put(msg, len);
 774         memcpy(dest, data, len);
 775
 776         sercomm_sendmsg(dlci, msg);
 777
 778         dnload.serial_fd.when |= BSC_FD_WRITE;
 779 }
 780
 781 static void hdlc_console_cb(uint8_t dlci, struct msgb *msg)
 782 {
 783         int rc;
 784
 785         rc = write(1, msg->data, msg->len);
 786         msgb_free(msg);
 787 }
 788
 789 static void hdlc_tool_cb(uint8_t dlci, struct msgb *msg)
 790 {
 791         struct tool_server *srv = tool_server_for_dlci[dlci];
 792
 793         if(dnload.dump_rx) {
 794                 printf("hdlc_recv(dlci=%u): ", dlci);
 795                 hexdump(msg->data, msg->len);
 796         }
 797
 798         if(srv) {
 799                 struct tool_connection *con;
 800                 uint16_t *len;
 801
 802                 len = (uint16_t *) msgb_push(msg, 2);
 803                 *len = htons(msg->len - sizeof(*len));
 804
 805                 llist_for_each_entry(con, &srv->connections, entry) {
 806                         if (write(con->fd.fd, msg->data, msg->len) != msg->len) {
 807                                 fprintf(stderr,
 808                                         "Failed to write msg to the socket..\n");
 809                                 continue;
 810                         }
 811                 }
 812         }
 813
 814         msgb_free(msg);
 815 }
 816
 817 static int handle_buffer(int buf_used_len)
 818 {
 819         int nbytes, buf_left, i;
 820
 821         buf_left = buf_used_len - (bufptr - buffer);
 822         if (buf_left <= 0) {
 823                 memmove(buffer, buffer+1, buf_used_len-1);
 824                 bufptr -= 1;
 825                 buf_left = 1;
 826         }
 827
 828         nbytes = read(dnload.serial_fd.fd, bufptr, buf_left);
 829         if (nbytes <= 0)
 830                 return nbytes;
 831
 832         if (!dnload.expect_hdlc) {
 833                 printf("got %i bytes from modem, ", nbytes);
 834                 printf("data looks like: ");
 835                 hexdump(bufptr, nbytes);
 836         } else {
 837                 for (i = 0; i < nbytes; ++i)
 838                         if (sercomm_drv_rx_char(bufptr[i]) == 0)
 839                                 printf("Dropping sample '%c'\n", bufptr[i]);
 840         }
 841
 842         return nbytes;
 843 }
 844
 845 /* Compal ramloader */
 846 static int handle_read(void)
 847 {
 848         int rc, nbytes;
 849
 850         nbytes = handle_buffer(sizeof(buffer));
 851         if (nbytes <= 0)
 852                 return nbytes;
 853
 854         if (!memcmp(buffer, phone_prompt1, sizeof(phone_prompt1))) {
 855                 printf("Received PROMPT1 from phone, responding with CMD\n");
 856                 dnload.expect_hdlc = 0;
 857                 dnload.state = WAITING_PROMPT2;
 858                 if(dnload.filename) {
 859                         rc = write(dnload.serial_fd.fd, dnload_cmd, sizeof(dnload_cmd));
 860
 861                         /* re-read file */
 862                         rc = read_file(dnload.filename);
 863                         if (rc < 0) {
 864                                 fprintf(stderr, "read_file(%s) failed with %d\n",
 865                                                 dnload.filename, rc);
 866                                 exit(1);
 867                         }
 868                 }
 869         } else if (!memcmp(buffer, phone_prompt2, sizeof(phone_prompt2))) {
 870                 printf("Received PROMPT2 from phone, starting download\n");
 871                 dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 872                 dnload.state = DOWNLOADING;
 873         } else if (!memcmp(buffer, phone_ack, sizeof(phone_ack))) {
 874                 printf("Received DOWNLOAD ACK from phone, your code is"
 875                         " running now!\n");
 876                 dnload.serial_fd.when = BSC_FD_READ;
 877                 dnload.state = WAITING_PROMPT1;
 878                 dnload.write_ptr = dnload.data;
 879                 dnload.expect_hdlc = 1;
 880
 881                 /* check for romloader chainloading mode used as a workaround
 882                  * for the magic on the C139/C140 and J100i */
 883                 if (dnload.chainload_filename != NULL) {
 884                         printf("Enabled Compal ramloader -> Calypso romloader"
 885                                 " chainloading mode\n");
 886                         bufptr = buffer;
 887                         dnload.filename = dnload.chainload_filename;
 888                         dnload.mode = MODE_ROMLOAD;
 889                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
 890                         tick_timer.cb = &beacon_timer_cb;
 891                         tick_timer.data = &tick_timer;
 892                         bsc_schedule_timer(&tick_timer, 0, dnload.beacon_interval);
 893                 }
 894         } else if (!memcmp(buffer, phone_nack, sizeof(phone_nack))) {
 895                 printf("Received DOWNLOAD NACK from phone, something went"
 896                         " wrong :(\n");
 897                 dnload.serial_fd.when = BSC_FD_READ;
 898                 dnload.state = WAITING_PROMPT1;
 899                 dnload.write_ptr = dnload.data;
 900         } else if (!memcmp(buffer, phone_nack_magic, sizeof(phone_nack_magic))) {
 901                 printf("Received MAGIC NACK from phone, you need to"
 902                         " have \"1003\" at 0x803ce0\n");
 903                 dnload.serial_fd.when = BSC_FD_READ;
 904                 dnload.state = WAITING_PROMPT1;
 905                 dnload.write_ptr = dnload.data;
 906         } else if (!memcmp(buffer, ftmtool, sizeof(ftmtool))) {
 907                 printf("Received FTMTOOL from phone, ramloader has aborted\n");
 908                 dnload.serial_fd.when = BSC_FD_READ;
 909                 dnload.state = WAITING_PROMPT1;
 910                 dnload.write_ptr = dnload.data;
 911         }
 912         bufptr += nbytes;
 913
 914         return nbytes;
 915 }
 916
 917 /* "Calypso non-secure romloader" */
 918 static int handle_read_romload(void)
 919 {
 920         int rc, nbytes, buf_used_len;
 921
 922         /* virtually limit buffer length for romloader, since responses
 923          * are shorter and vary in length */
 924
 925         switch (dnload.romload_state) {
 926         case WAITING_PARAM_ACK:
 927                 buf_used_len = 4;       /* ">p" + uint16_t len */
 928                 break;
 929         case WAITING_CHECKSUM_ACK:
 930                 buf_used_len = 3;       /* ">c" + uint8_t checksum */
 931                 break;
 932         case FINISHED:
 933                 buf_used_len = sizeof(buffer);
 934                 break;
 935         default:
 936                 buf_used_len = 2;       /* ">*" */
 937         }
 938
 939         nbytes = handle_buffer(buf_used_len);
 940         if (nbytes <= 0)
 941                 return nbytes;
 942
 943         switch (dnload.romload_state) {
 944         case WAITING_IDENTIFICATION:
 945                 if (memcmp(buffer, romload_ident_ack,
 946                             sizeof(romload_ident_ack)))
 947                         break;
 948
 949                 printf("Received ident ack from phone, sending "
 950                         "parameter sequence\n");
 951                 dnload.expect_hdlc = 1;
 952                 dnload.romload_state = WAITING_PARAM_ACK;
 953                 rc = write(dnload.serial_fd.fd, romload_param,
 954                            sizeof(romload_param));
 955                 /* re-read file */
 956                 rc = read_file(dnload.filename);
 957                 if (rc < 0) {
 958                         fprintf(stderr, "read_file(%s) failed with %d\n",
 959                                 dnload.filename, rc);
 960                         exit(1);
 961                 }
 962                 break;
 963         case WAITING_PARAM_ACK:
 964                 if (memcmp(buffer, romload_param_ack,
 965                             sizeof(romload_param_ack)))
 966                         break;
 967
 968                 printf("Received parameter ack from phone, "
 969                         "starting download\n");
 970                 serial_set_baudrate(ROMLOAD_DL_BAUDRATE);
 971
 972                 /* using the max blocksize the phone tells us */
 973                 dnload.block_payload_size = ((buffer[3] << 8) + buffer[2]);
 974                 printf("Used blocksize for download is %i bytes\n",
 975                         dnload.block_payload_size);
 976                 dnload.block_payload_size -= ROMLOAD_BLOCK_HDR_LEN;
 977                 dnload.romload_state = SENDING_BLOCKS;
 978                 dnload.block_number = 0;
 979                 romload_prepare_block();
 980                 bufptr -= 2;
 981                 break;
 982         case WAITING_BLOCK_ACK:
 983         case LAST_BLOCK_SENT:
 984                 if (!memcmp(buffer, romload_block_ack,
 985                             sizeof(romload_block_ack))) {
 986                         printf("Received block ack from phone\n");
 987                         if (dnload.romload_state == LAST_BLOCK_SENT) {
 988                                 /* send the checksum */
 989                                 uint8_t final_checksum =
 990                                         (~(dnload.romload_dl_checksum) & 0xff);
 991                                 printf("Sending checksum: 0x%02x \n",
 992                                         final_checksum);
 993                                 rc = write(dnload.serial_fd.fd,
 994                                            romload_checksum_cmd,
 995                                            sizeof(romload_checksum_cmd));
 996                                 rc = write(dnload.serial_fd.fd,
 997                                            &final_checksum, 1);
 998                                 dnload.romload_state = WAITING_CHECKSUM_ACK;
 999                         } else
1000                                 romload_prepare_block();
1001                 } else if (!memcmp(buffer, romload_block_nack,
1002                                    sizeof(romload_block_nack))) {
1003                         printf("Received block nack from phone, "
1004                                 "something went wrong, aborting\n");
1005                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1006                         dnload.romload_state = WAITING_IDENTIFICATION;
1007                         bsc_schedule_timer(&tick_timer, 0, dnload.beacon_interval);
1008                 }
1009                 break;
1010         case WAITING_CHECKSUM_ACK:
1011                 if (!memcmp(buffer, romload_checksum_ack,
1012                             sizeof(romload_checksum_ack))) {
1013                         printf("Checksum on phone side matches, "
1014                                 "let's branch to your code\n");
1015                         printf("Branching to 0x%08x\n", ROMLOAD_ADDRESS);
1016
1017                         rc = write(dnload.serial_fd.fd, romload_branch_cmd,
1018                                    sizeof(romload_branch_cmd));
1019                         rc = write(dnload.serial_fd.fd, &dnload.load_address,
1020                                    sizeof(dnload.load_address));
1021                         dnload.romload_state = WAITING_BRANCH_ACK;
1022                         bufptr -= 1;
1023                 } else if (!memcmp(buffer, romload_checksum_nack,
1024                                    sizeof(romload_checksum_nack))) {
1025                         printf("Checksum on phone side (0x%02x) doesn't "
1026                                 "match ours, aborting\n", ~buffer[2]);
1027                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1028                         dnload.romload_state = WAITING_IDENTIFICATION;
1029                         bsc_schedule_timer(&tick_timer, 0, dnload.beacon_interval);
1030                         bufptr -= 1;
1031                 }
1032                 break;
1033         case WAITING_BRANCH_ACK:
1034                 if (!memcmp(buffer, romload_branch_ack,
1035                             sizeof(romload_branch_ack))) {
1036                         printf("Received branch ack, your code is running now!\n");
1037                         dnload.serial_fd.when = BSC_FD_READ;
1038                         dnload.romload_state = FINISHED;
1039                         dnload.write_ptr = dnload.data;
1040                         dnload.expect_hdlc = 1;
1041                 } else if (!memcmp(buffer, romload_branch_nack,
1042                            sizeof(romload_branch_nack))) {
1043                         printf("Received branch nack, aborting\n");
1044                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1045                         dnload.romload_state = WAITING_IDENTIFICATION;
1046                         bsc_schedule_timer(&tick_timer, 0, dnload.beacon_interval);
1047                 }
1048                 break;
1049         default:
1050                 break;
1051         }
1052
1053         bufptr += nbytes;
1054         return nbytes;
1055 }
1056
1057 /* MTK romloader */
1058 static int handle_read_mtk(void)
1059 {
1060         int rc, nbytes, buf_used_len;
1061
1062         switch (dnload.mtk_state) {
1063         case MTK_WAIT_ADDR_ACK:
1064         case MTK_WAIT_SIZE_ACK:
1065         case MTK_WAIT_BRANCH_ADDR_ACK:
1066                 buf_used_len = 4;
1067                 break;
1068         case MTK_FINISHED:
1069                 buf_used_len = sizeof(buffer);
1070                 break;
1071         default:
1072                 buf_used_len = 1;
1073         }
1074
1075         nbytes = handle_buffer(buf_used_len);
1076         if (nbytes <= 0)
1077                 return nbytes;
1078
1079         switch (dnload.mtk_state) {
1080         case MTK_INIT_1:
1081                 if (!(buffer[0] == mtk_init_resp[0]))
1082                         break;
1083                 dnload.mtk_state = MTK_INIT_2;
1084                 printf("Received init magic byte 1\n");
1085                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[1], 1);
1086                 break;
1087         case MTK_INIT_2:
1088                 if (!(buffer[0] == mtk_init_resp[1]))
1089                         break;
1090                 dnload.mtk_state = MTK_INIT_3;
1091                 printf("Received init magic byte 2\n");
1092                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[2], 1);
1093                 break;
1094         case MTK_INIT_3:
1095                 if (!(buffer[0] == mtk_init_resp[2]))
1096                         break;
1097                 dnload.mtk_state = MTK_INIT_4;
1098                 printf("Received init magic byte 3\n");
1099                 rc = write(dnload.serial_fd.fd, &mtk_init_cmd[3], 1);
1100                 break;
1101         case MTK_INIT_4:
1102                 if (!(buffer[0] == mtk_init_resp[3]))
1103                         break;
1104                 dnload.mtk_state = MTK_WAIT_WRITE_ACK;
1105                 printf("Received init magic byte 4, requesting write\n");
1106                 rc = write(dnload.serial_fd.fd, &mtk_command[0], 1);
1107                 break;
1108         case MTK_WAIT_WRITE_ACK:
1109                 if (!(buffer[0] == mtk_command[0]))
1110                         break;
1111                 dnload.mtk_state = MTK_WAIT_ADDR_ACK;
1112                 printf("Received write ack, sending load address\n");
1113
1114                 rc = write(dnload.serial_fd.fd, &dnload.load_address,
1115                            sizeof(dnload.load_address));
1116                 break;
1117         case MTK_WAIT_ADDR_ACK:
1118                 if (memcmp(buffer, dnload.load_address,
1119                             sizeof(dnload.load_address)))
1120                         break;
1121                 printf("Received address ack from phone, sending loadsize\n");
1122                 /* re-read file */
1123                 rc = read_file(dnload.filename);
1124                 if (rc < 0) {
1125                         fprintf(stderr, "read_file(%s) failed with %d\n",
1126                                 dnload.filename, rc);
1127                         exit(1);
1128                 }
1129                 dnload.block_number = 0;
1130                 mtk_prepare_block();
1131                 dnload.mtk_state = MTK_WAIT_SIZE_ACK;
1132                 rc = write(dnload.serial_fd.fd, &dnload.mtk_send_size,
1133                            sizeof(dnload.mtk_send_size));
1134                 break;
1135         case MTK_WAIT_SIZE_ACK:
1136                 if (memcmp(buffer, dnload.mtk_send_size,
1137                             sizeof(dnload.mtk_send_size)))
1138                         break;
1139                 printf("Received size ack\n");
1140                 dnload.expect_hdlc = 1;
1141                 dnload.mtk_state = MTK_SENDING_BLOCKS;
1142                 dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
1143                 bufptr -= 3;
1144                 break;
1145         case MTK_SENDING_BLOCKS:
1146                 if (!(buffer[0] == dnload.block[dnload.echo_bytecount]))
1147                         printf("Warning: Byte %i of Block %i doesn't match,"
1148                                 " check your serial connection!\n",
1149                                 dnload.echo_bytecount, dnload.block_number);
1150                 dnload.echo_bytecount++;
1151
1152                 if ((dnload.echo_bytecount+1) > MTK_BLOCK_SIZE) {
1153                         if ( dnload.block_number == dnload.block_count) {
1154                                 rc = write(dnload.serial_fd.fd,
1155                                            &mtk_command[3], 1);
1156                                 printf("Sending branch command\n");
1157                                 dnload.expect_hdlc = 0;
1158                                 dnload.mtk_state = MTK_WAIT_BRANCH_CMD_ACK;
1159                                 break;
1160                         }
1161                         printf("Received Block %i preparing next block\n",
1162                                 dnload.block_number);
1163                         mtk_prepare_block();
1164                         dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
1165                 }
1166                 break;
1167         case MTK_WAIT_BRANCH_CMD_ACK:
1168                 if (!(buffer[0] == mtk_command[3]))
1169                         break;
1170                 dnload.mtk_state = MTK_WAIT_BRANCH_ADDR_ACK;
1171                 printf("Received branch command ack, sending address\n");
1172
1173                 rc = write(dnload.serial_fd.fd, &dnload.load_address,
1174                            sizeof(dnload.load_address));
1175                 break;
1176         case MTK_WAIT_BRANCH_ADDR_ACK:
1177                 if (memcmp(buffer, dnload.load_address,
1178                             sizeof(dnload.load_address)))
1179                         break;
1180                 printf("Received branch address ack, code should run now\n");
1181                 serial_set_baudrate(MODEM_BAUDRATE);
1182                 dnload.serial_fd.when = BSC_FD_READ;
1183                 dnload.mtk_state = MTK_FINISHED;
1184                 dnload.write_ptr = dnload.data;
1185                 dnload.expect_hdlc = 1;
1186                 break;
1187         default:
1188                 break;
1189         }
1190
1191         bufptr += nbytes;
1192         return nbytes;
1193 }
1194
1195 static int serial_read(struct bsc_fd *fd, unsigned int flags)
1196 {
1197         int rc;
1198         if (flags & BSC_FD_READ) {
1199                 switch (dnload.mode) {
1200                         case MODE_ROMLOAD:
1201                                 rc = handle_read_romload();
1202                                 break;
1203                         case MODE_MTK:
1204                                 rc = handle_read_mtk();
1205                                 break;
1206                         default:
1207                                 rc = handle_read();
1208                                 break;
1209                 }
1210                 if (rc == 0)
1211                         exit(2);
1212         }
1213
1214         if (flags & BSC_FD_WRITE) {
1215                 rc = handle_write();
1216                 if (rc == 1)
1217                         dnload.state = WAITING_PROMPT1;
1218         }
1219         return 0;
1220 }
1221
1222 static int parse_mode(const char *arg)
1223 {
1224         if (!strcasecmp(arg, "c123"))
1225                 return MODE_C123;
1226         else if (!strcasecmp(arg, "c123xor"))
1227                 return MODE_C123xor;
1228         else if (!strcasecmp(arg, "c140"))
1229                 return MODE_C140;
1230         else if (!strcasecmp(arg, "c140xor"))
1231                 return MODE_C140xor;
1232         else if (!strcasecmp(arg, "c155"))
1233                 return MODE_C155;
1234         else if (!strcasecmp(arg, "romload"))
1235                 return MODE_ROMLOAD;
1236         else if (!strcasecmp(arg, "mtk"))
1237                 return MODE_MTK;
1238
1239         return -1;
1240 }
1241
1242 #define HELP_TEXT \
1243         "[ -v | -h ] [ -d [t][r] ] [ -p /dev/ttyXXXX ]\n" \
1244         "\t\t [ -s /tmp/osmocom_l2 ]\n" \
1245         "\t\t [ -l /tmp/osmocom_loader ]\n" \
1246         "\t\t [ -m {c123,c123xor,c140,c140xor,c155,romload,mtk} ]\n" \
1247         "\t\t [ -c /to-be-chainloaded-file.bin ]\n" \
1248         "\t\t [ -i beacon-interval (mS) ]\n" \
1249         "\t\t  file.bin\n\n" \
1250         "* Open serial port /dev/ttyXXXX (connected to your phone)\n" \
1251         "* Perform handshaking with the ramloader in the phone\n" \
1252         "* Download file.bin to the attached phone (base address 0x00800100)\n"
1253
1254 static int usage(const char *name)
1255 {
1256         printf("Usage: %s ", name);
1257         printf(HELP_TEXT);
1258         exit(2);
1259 }
1260
1261 static int version(const char *name)
1262 {
1263         printf("%s version %s\n", name, PACKAGE_VERSION);
1264         exit(2);
1265 }
1266
1267 static int un_tool_read(struct bsc_fd *fd, unsigned int flags)
1268 {
1269         int rc, c;
1270         uint16_t length = 0xffff;
1271         uint8_t buf[4096];
1272         struct tool_connection *con = (struct tool_connection *)fd->data;
1273
1274         c = 0;
1275         while(c < 2) {
1276                 rc = read(fd->fd, &buf + c, 2 - c);
1277                 if(rc == 0) {
1278                         // disconnect
1279                         goto close;
1280                 }
1281                 if(rc < 0) {
1282                         if(errno == EAGAIN) {
1283                                 continue;
1284                         }
1285                         fprintf(stderr, "Err from socket: %s\n", strerror(errno));
1286                         goto close;
1287                 }
1288                 c += rc;
1289         }
1290
1291         length = ntohs(*(uint16_t*)buf);
1292
1293         c = 0;
1294         while(c < length) {
1295                 rc = read(fd->fd, &buf + c, length - c);
1296                 if(rc == 0) {
1297                         // disconnect
1298                         goto close;
1299                 }
1300                 if(rc < 0) {
1301                         if(errno == EAGAIN) {
1302                                 continue;
1303                         }
1304                         fprintf(stderr, "Err from socket: %s\n", strerror(errno));
1305                         goto close;
1306                 }
1307                 c += rc;
1308         }
1309
1310         hdlc_send_to_phone(con->server->dlci, buf, length);
1311
1312         return 0;
1313 close:
1314
1315         close(fd->fd);
1316         bsc_unregister_fd(fd);
1317         llist_del(&con->entry);
1318         talloc_free(con);
1319         return -1;
1320 }
1321
1322 /* accept a new connection */
1323 static int tool_accept(struct bsc_fd *fd, unsigned int flags)
1324 {
1325         struct tool_server *srv = (struct tool_server *)fd->data;
1326         struct tool_connection *con;
1327         struct sockaddr_un un_addr;
1328         socklen_t len;
1329         int rc;
1330
1331         len = sizeof(un_addr);
1332         rc = accept(fd->fd, (struct sockaddr *) &un_addr, &len);
1333         if (rc < 0) {
1334                 fprintf(stderr, "Failed to accept a new connection.\n");
1335                 return -1;
1336         }
1337
1338         con = talloc_zero(NULL, struct tool_connection);
1339         if (!con) {
1340                 fprintf(stderr, "Failed to create tool connection.\n");
1341                 return -1;
1342         }
1343
1344         con->server = srv;
1345
1346         con->fd.fd = rc;
1347         con->fd.when = BSC_FD_READ;
1348         con->fd.cb = un_tool_read;
1349         con->fd.data = con;
1350         if (bsc_register_fd(&con->fd) != 0) {
1351                 fprintf(stderr, "Failed to register the fd.\n");
1352                 return -1;
1353         }
1354
1355         llist_add(&con->entry, &srv->connections);
1356         return 0;
1357 }
1358
1359 /*
1360  * Register and start a tool server
1361  */
1362 static int register_tool_server(struct tool_server *ts,
1363                                 const char *path,
1364                                 uint8_t dlci)
1365 {
1366         struct bsc_fd *bfd = &ts->bfd;
1367         struct sockaddr_un local;
1368         unsigned int namelen;
1369         int rc;
1370
1371         bfd->fd = socket(AF_UNIX, SOCK_STREAM, 0);
1372
1373         if (bfd->fd < 0) {
1374                 fprintf(stderr, "Failed to create Unix Domain Socket.\n");
1375                 return -1;
1376         }
1377
1378         local.sun_family = AF_UNIX;
1379         strncpy(local.sun_path, path, sizeof(local.sun_path));
1380         local.sun_path[sizeof(local.sun_path) - 1] = '\0';
1381         unlink(local.sun_path);
1382
1383         /* we use the same magic that X11 uses in Xtranssock.c for
1384          * calculating the proper length of the sockaddr */
1385 #if defined(BSD44SOCKETS) || defined(__UNIXWARE__)
1386         local.sun_len = strlen(local.sun_path);
1387 #endif
1388 #if defined(BSD44SOCKETS) || defined(SUN_LEN)
1389         namelen = SUN_LEN(&local);
1390 #else
1391         namelen = strlen(local.sun_path) +
1392                   offsetof(struct sockaddr_un, sun_path);
1393 #endif
1394
1395         rc = bind(bfd->fd, (struct sockaddr *) &local, namelen);
1396         if (rc != 0) {
1397                 fprintf(stderr, "Failed to bind the unix domain socket. '%s'\n",
1398                         local.sun_path);
1399                 return -1;
1400         }
1401
1402         if (listen(bfd->fd, 0) != 0) {
1403                 fprintf(stderr, "Failed to listen.\n");
1404                 return -1;
1405         }
1406
1407         bfd->when = BSC_FD_READ;
1408         bfd->cb = tool_accept;
1409         bfd->data = ts;
1410
1411         ts->dlci = dlci;
1412         INIT_LLIST_HEAD(&ts->connections);
1413
1414         tool_server_for_dlci[dlci] = ts;
1415
1416         sercomm_register_rx_cb(dlci, hdlc_tool_cb);
1417
1418         if (bsc_register_fd(bfd) != 0) {
1419                 fprintf(stderr, "Failed to register the bfd.\n");
1420                 return -1;
1421         }
1422
1423         return 0;
1424 }
1425
1426 extern void hdlc_tpudbg_cb(uint8_t dlci, struct msgb *msg);
1427
1428 void parse_debug(const char *str)
1429 {
1430         while(*str) {
1431                 switch(*str) {
1432                 case 't':
1433                         dnload.dump_tx = 1;
1434                         break;
1435                 case 'r':
1436                         dnload.dump_rx = 1;
1437                         break;
1438                 default:
1439                         printf("Unknown debug flag %c\n", *str);
1440                         abort();
1441                         break;
1442                 }
1443                 str++;
1444         }
1445 }
1446
1447 int main(int argc, char **argv)
1448 {
1449         int opt, flags;
1450         uint32_t tmp_load_address = ROMLOAD_ADDRESS;
1451         const char *serial_dev = "/dev/ttyUSB1";
1452         const char *layer2_un_path = "/tmp/osmocom_l2";
1453         const char *loader_un_path = "/tmp/osmocom_loader";
1454
1455         dnload.mode = MODE_C123;
1456         dnload.chainload_filename = NULL;
1457         dnload.beacon_interval = DEFAULT_BEACON_INTERVAL;
1458
1459         while ((opt = getopt(argc, argv, "d:hl:p:m:c:s:i:v")) != -1) {
1460                 switch (opt) {
1461                 case 'p':
1462                         serial_dev = optarg;
1463                         break;
1464                 case 'm':
1465                         dnload.mode = parse_mode(optarg);
1466                         if (dnload.mode < 0)
1467                                 usage(argv[0]);
1468                         break;
1469                 case 's':
1470                         layer2_un_path = optarg;
1471                         break;
1472                 case 'l':
1473                         loader_un_path = optarg;
1474                         break;
1475                 case 'v':
1476                         version(argv[0]);
1477                         break;
1478                 case 'd':
1479                         parse_debug(optarg);
1480                         break;
1481                 case 'c':
1482                         dnload.chainload_filename = optarg;
1483                         break;
1484                 case 'i':
1485                         dnload.beacon_interval = atoi(optarg) * 1000;
1486                         break;
1487                 case 'h':
1488                 default:
1489                         usage(argv[0]);
1490                         break;
1491                 }
1492         }
1493
1494         if (argc <= optind) {
1495                 dnload.filename = NULL;
1496         } else {
1497                 dnload.filename = argv[optind];
1498         }
1499
1500         dnload.serial_fd.fd = serial_init(serial_dev);
1501         if (dnload.serial_fd.fd < 0) {
1502                 fprintf(stderr, "Cannot open serial device %s\n", serial_dev);
1503                 exit(1);
1504         }
1505
1506         if (bsc_register_fd(&dnload.serial_fd) != 0) {
1507                 fprintf(stderr, "Failed to register the serial.\n");
1508                 exit(1);
1509         }
1510
1511         /* Set serial socket to non-blocking mode of operation */
1512         flags = fcntl(dnload.serial_fd.fd, F_GETFL);
1513         flags |= O_NONBLOCK;
1514         fcntl(dnload.serial_fd.fd, F_SETFL, flags);
1515
1516         dnload.serial_fd.when = BSC_FD_READ;
1517         dnload.serial_fd.cb = serial_read;
1518
1519         /* initialize the HDLC layer */
1520         sercomm_init();
1521         sercomm_register_rx_cb(SC_DLCI_CONSOLE, hdlc_console_cb);
1522         sercomm_register_rx_cb(SC_DLCI_DEBUG, hdlc_tpudbg_cb);
1523
1524         /* unix domain socket handling */
1525         if (register_tool_server(&dnload.layer2_server, layer2_un_path,
1526                                  SC_DLCI_L1A_L23) != 0)
1527                 exit(1);
1528
1529         if (register_tool_server(&dnload.loader_server, loader_un_path,
1530                                  SC_DLCI_LOADER) != 0)
1531                 exit(1);
1532
1533         /* if in romload mode, start our beacon timer */
1534         if (dnload.mode == MODE_ROMLOAD) {
1535                 tmp_load_address = ROMLOAD_ADDRESS;
1536                 serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1537                 tick_timer.cb = &beacon_timer_cb;
1538                 tick_timer.data = &tick_timer;
1539                 bsc_schedule_timer(&tick_timer, 0, dnload.beacon_interval);
1540         }
1541         else if (dnload.mode == MODE_MTK) {
1542                 tmp_load_address = MTK_ADDRESS;
1543                 serial_set_baudrate(MTK_INIT_BAUDRATE);
1544                 tick_timer.cb = &mtk_timer_cb;
1545                 tick_timer.data = &tick_timer;
1546                 bsc_schedule_timer(&tick_timer, 0, dnload.beacon_interval);
1547         }
1548
1549         dnload.load_address[0] = (tmp_load_address >> 24) & 0xff;
1550         dnload.load_address[1] = (tmp_load_address >> 16) & 0xff;
1551         dnload.load_address[2] = (tmp_load_address >> 8) & 0xff;
1552         dnload.load_address[3] = tmp_load_address & 0xff;
1553
1554         while (1)
1555                 bsc_select_main(0);
1556
1557         close(dnload.serial_fd.fd);
1558
1559         exit(0);
1560 }