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