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 <stdio.h>
  26 #include <stdlib.h>
  27 #include <string.h>
  28 #include <unistd.h>
  29 #include <stdint.h>
  30 #include <fcntl.h>
  31 #include <errno.h>
  32 #include <termios.h>
  33 #include <sys/ioctl.h>
  34 #include <sys/types.h>
  35 #include <sys/socket.h>
  36 #include <sys/stat.h>
  37 #include <sys/un.h>
  38
  39 #include <sercomm.h>
  40
  41 #include <osmocore/linuxlist.h>
  42 #include <osmocore/select.h>
  43 #include <osmocore/talloc.h>
  44 #include <osmocore/timer.h>
  45
  46 #include <arpa/inet.h>
  47
  48 #define MODEM_BAUDRATE          B115200
  49 #define MAX_DNLOAD_SIZE         0xFFFF
  50 #define MAX_HDR_SIZE            128
  51 #define MAGIC_OFFSET            0x3be2
  52
  53 #define BEACON_INTERVAL         50000
  54 #define ROMLOAD_INIT_BAUDRATE   B19200
  55 #define ROMLOAD_DL_BAUDRATE     B115200
  56 #define ROMLOAD_BLOCK_HDR_LEN   10
  57 #define ROMLOAD_ADDRESS         0x820000
  58
  59 struct tool_server *tool_server_for_dlci[256];
  60
  61 /**
  62  * a connection from some other tool
  63  */
  64 struct tool_connection {
  65         struct tool_server *server;
  66         struct llist_head entry;
  67         struct bsc_fd fd;
  68 };
  69
  70 /**
  71  * server for a tool
  72  */
  73 struct tool_server {
  74         struct bsc_fd bfd;
  75         uint8_t dlci;
  76         struct llist_head connections;
  77 };
  78
  79
  80 enum dnload_state {
  81         WAITING_PROMPT1,
  82         WAITING_PROMPT2,
  83         DOWNLOADING,
  84 };
  85
  86 enum romload_state {
  87         WAITING_IDENTIFICATION,
  88         WAITING_PARAM_ACK,
  89         SENDING_BLOCKS,
  90         SENDING_LAST_BLOCK,
  91         LAST_BLOCK_SENT,
  92         WAITING_BLOCK_ACK,
  93         WAITING_CHECKSUM_ACK,
  94         WAITING_BRANCH_ACK,
  95         FINISHED,
  96 };
  97
  98 enum dnload_mode {
  99         MODE_C123,
 100         MODE_C123xor,
 101         MODE_C140,
 102         MODE_C140xor,
 103         MODE_C155,
 104         MODE_ROMLOAD,
 105 };
 106
 107 struct dnload {
 108         enum dnload_state state;
 109         enum romload_state romload_state;
 110         enum dnload_mode mode;
 111         struct bsc_fd serial_fd;
 112         char *filename;
 113
 114         int print_hdlc;
 115
 116         /* data to be downloaded */
 117         uint8_t *data;
 118         int data_len;
 119
 120         uint8_t *write_ptr;
 121
 122         /* romload: block to be downloaded */
 123         uint8_t *block;
 124         int block_len;
 125         uint8_t block_number;
 126         uint16_t block_payload_size;
 127         int romload_dl_checksum;
 128         uint8_t *block_ptr;
 129         uint8_t load_address[4];
 130
 131         struct tool_server layer2_server;
 132         struct tool_server loader_server;
 133 };
 134
 135
 136 static struct dnload dnload;
 137 static struct timer_list tick_timer;
 138
 139 /* Compal ramloader specific */
 140 static const uint8_t phone_prompt1[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x01, 0x40 };
 141 static const uint8_t dnload_cmd[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x52, 0x01, 0x53 };
 142 static const uint8_t phone_prompt2[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x02, 0x43 };
 143 static const uint8_t phone_ack[]     = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x42 };
 144 static const uint8_t phone_nack_magic[]= { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x57 };
 145 static const uint8_t phone_nack[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x45, 0x53, 0x16 };
 146 static const uint8_t ftmtool[] = { 0x66, 0x74, 0x6d, 0x74, 0x6f, 0x6f, 0x6c };
 147 static const uint8_t phone_magic[] = { 0x31, 0x30, 0x30, 0x33 }; /* "1003" */
 148
 149 /* The C123 has a hard-coded check inside the ramloader that requires the
 150  * following bytes to be always the first four bytes of the image */
 151 static const uint8_t data_hdr_c123[]    = { 0xee, 0x4c, 0x9f, 0x63 };
 152
 153 /* The C155 doesn't have some strange restriction on what the first four bytes
 154  *  have to be, but it starts the ramloader in THUMB mode. We use the following
 155  * four bytes to switch back to ARM mode:
 156   800100:       4778            bx      pc
 157   800102:       46c0            nop                     ; (mov r8, r8)
 158  */
 159 static const uint8_t data_hdr_c155[]    = { 0x78, 0x47, 0xc0, 0x46 };
 160
 161 /* romloader specific */
 162 static const uint8_t romload_ident_cmd[] =      { 0x3c, 0x69 }; /* <i */
 163 static const uint8_t romload_abort_cmd[] =      { 0x3c, 0x61 }; /* <a */
 164 static const uint8_t romload_write_cmd[] =      { 0x3c, 0x77 }; /* <w */
 165 static const uint8_t romload_checksum_cmd[] =   { 0x3c, 0x63 }; /* <c */
 166 static const uint8_t romload_branch_cmd[] =     { 0x3c, 0x62 }; /* <b */
 167 static const uint8_t romload_ident_ack[] =      { 0x3e, 0x69 }; /* >i */
 168 static const uint8_t romload_param_ack[] =      { 0x3e, 0x70 }; /* >p */
 169 static const uint8_t romload_param_nack[] =     { 0x3e, 0x50 }; /* >P */
 170 static const uint8_t romload_block_ack[] =      { 0x3e, 0x77 }; /* >w */
 171 static const uint8_t romload_block_nack[] =     { 0x3e, 0x57 }; /* >W */
 172 static const uint8_t romload_checksum_ack[] =   { 0x3e, 0x63 }; /* >c */
 173 static const uint8_t romload_checksum_nack[] =  { 0x3e, 0x43 }; /* >C */
 174 static const uint8_t romload_branch_ack[] =     { 0x3e, 0x62 }; /* >b */
 175 static const uint8_t romload_branch_nack[] =    { 0x3e, 0x42 }; /* >B */
 176
 177 /* romload_param: {"<p", uint8_t baudrate, uint8_t dpll, uint16_t memory_config,
 178  * uint8_t strobe_af, uint32_t uart_timeout} */
 179
 180 static const uint8_t romload_param[] = { 0x3c, 0x70, 0x00, 0x00, 0x00, 0x04,
 181                                          0x00, 0x00, 0x00, 0x00, 0x00 };
 182
 183 /* FIXME: this routine is more or less what openbsc/src/rs232:rs232_setup()
 184  * does, we should move it to libosmocore at some point */
 185 static int serial_init(const char *serial_port)
 186 {
 187         int rc, serial_fd, v24;
 188         struct termios tio;
 189
 190         serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY);
 191         if (serial_fd < 0) {
 192                 perror("cannot open serial port");
 193                 return serial_fd;
 194         }
 195
 196         //fcntl(serial_fd, F_SETFL, 0);
 197
 198         /* Configure serial interface */
 199         rc = tcgetattr(serial_fd, &tio);
 200         if (rc < 0) {
 201                 perror("tcgetattr()");
 202                 return rc;
 203         }
 204         cfsetispeed(&tio, MODEM_BAUDRATE);
 205         cfsetospeed(&tio, MODEM_BAUDRATE);
 206         tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
 207         tio.c_cflag |=  (CREAD | CLOCAL | CS8);
 208         tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
 209         tio.c_iflag |=  (INPCK | ISTRIP);
 210         tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR);
 211         tio.c_oflag &= ~(OPOST | ONLCR);
 212         rc = tcsetattr(serial_fd, TCSANOW, &tio);
 213         if (rc < 0) {
 214                 perror("tcsetattr()");
 215                 return rc;
 216         }
 217
 218         /* set ready to read/write */
 219         v24 = TIOCM_DTR | TIOCM_RTS;
 220         rc = ioctl(serial_fd, TIOCMBIS, &v24);
 221         if (rc < 0) {
 222                 perror("ioctl(TIOCMBIS)");
 223                 return rc;
 224         }
 225
 226         return serial_fd;
 227 }
 228
 229 static int serial_set_baudrate(speed_t baudrate)
 230 {
 231         int rc;
 232         struct termios tio;
 233
 234         rc = tcgetattr(dnload.serial_fd.fd, &tio);
 235         if (rc < 0) {
 236                 perror("tcgetattr()");
 237                 return rc;
 238         }
 239         cfsetispeed(&tio, baudrate);
 240         cfsetospeed(&tio, baudrate);
 241
 242         rc = tcsetattr(dnload.serial_fd.fd, TCSANOW, &tio);
 243         return rc;
 244 }
 245
 246 static void beacon_timer_cb(void *p)
 247 {
 248         int rc;
 249
 250         if (dnload.romload_state == WAITING_IDENTIFICATION) {
 251                 printf("Sending beacon...\n");
 252                 rc = write(dnload.serial_fd.fd, romload_ident_cmd,
 253                            sizeof(romload_ident_cmd));
 254
 255                 if (!(rc == sizeof(romload_ident_cmd)))
 256                         printf("Error sending identification beacon\n");
 257
 258                 bsc_schedule_timer(p, 0, BEACON_INTERVAL);
 259         }
 260 }
 261
 262 /* Read the to-be-downloaded file, prepend header and length, append XOR sum */
 263 int read_file(const char *filename)
 264 {
 265         int fd, rc, i;
 266         struct stat st;
 267         const uint8_t *hdr = NULL;
 268         int payload_size;
 269         int hdr_len = 0;
 270         uint8_t *file_data;
 271         uint16_t tot_len;
 272         uint8_t nibble;
 273         uint8_t running_xor = 0x02;
 274
 275         fd = open(filename, O_RDONLY);
 276         if (fd < 0) {
 277                 perror("opening file");
 278                 exit(1);
 279         }
 280
 281         rc = fstat(fd, &st);
 282         if (st.st_size > MAX_DNLOAD_SIZE) {
 283                 fprintf(stderr, "The maximum file size is 64kBytes (%u bytes)\n",
 284                         MAX_DNLOAD_SIZE);
 285                 return -EFBIG;
 286         }
 287
 288         if (dnload.data) {
 289                 free(dnload.data);
 290                 dnload.data = NULL;
 291         }
 292
 293         if (dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) {
 294                 if (st.st_size < (MAGIC_OFFSET + sizeof(phone_magic)))
 295                         payload_size = MAGIC_OFFSET + sizeof(phone_magic);
 296                 else {
 297                         printf("\nThe filesize is larger than 15kb, code on "
 298                                 "the magic address will be overwritten!\nUse "
 299                                 "loader.bin and upload the application with "
 300                                 "osmoload instead!\n\n");
 301                         payload_size = st.st_size;
 302                 }
 303         } else
 304                 payload_size = st.st_size;
 305
 306         dnload.data = malloc(MAX_HDR_SIZE + payload_size);
 307
 308         if (!dnload.data) {
 309                 close(fd);
 310                 fprintf(stderr, "No memory\n");
 311                 return -ENOMEM;
 312         }
 313
 314         /* copy in the header, if any */
 315         switch (dnload.mode) {
 316         case MODE_C155:
 317                 hdr = data_hdr_c155;
 318                 hdr_len = sizeof(data_hdr_c155);
 319                 break;
 320         case MODE_C140:
 321         case MODE_C140xor:
 322         case MODE_C123:
 323         case MODE_C123xor:
 324                 hdr = data_hdr_c123;
 325                 hdr_len = sizeof(data_hdr_c123);
 326                 break;
 327         case MODE_ROMLOAD:
 328                 break;
 329         default:
 330                 break;
 331         }
 332
 333         if (hdr && hdr_len)
 334                 memcpy(dnload.data, hdr, hdr_len);
 335
 336         /* 2 bytes for length + header */
 337         file_data = dnload.data + 2 + hdr_len;
 338
 339         /* write the length, keep running XOR */
 340         tot_len = hdr_len + payload_size;
 341         nibble = tot_len >> 8;
 342         dnload.data[0] = nibble;
 343         running_xor ^= nibble;
 344         nibble = tot_len & 0xff;
 345         dnload.data[1] = nibble;
 346         running_xor ^= nibble;
 347
 348         if (hdr_len && hdr) {
 349                 memcpy(dnload.data+2, hdr, hdr_len);
 350
 351                 for (i = 0; i < hdr_len; i++)
 352                         running_xor ^= hdr[i];
 353         }
 354
 355         rc = read(fd, file_data, st.st_size);
 356         if (rc < 0) {
 357                 perror("error reading file\n");
 358                 free(dnload.data);
 359                 dnload.data = NULL;
 360                 close(fd);
 361                 return -EIO;
 362         }
 363         if (rc < st.st_size) {
 364                 free(dnload.data);
 365                 dnload.data = NULL;
 366                 close(fd);
 367                 fprintf(stderr, "Short read of file (%d < %d)\n",
 368                         rc, (int)st.st_size);
 369                 return -EIO;
 370         }
 371
 372         close(fd);
 373
 374         dnload.data_len = (file_data+payload_size) - dnload.data;
 375
 376         /* fill memory between data end and magic, add magic */
 377         if(dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) {
 378                 if (st.st_size < MAGIC_OFFSET)
 379                         memset(file_data + st.st_size, 0x00,
 380                                 payload_size - st.st_size);
 381                 memcpy(dnload.data + MAGIC_OFFSET, phone_magic,
 382                         sizeof(phone_magic));
 383         }
 384
 385         /* calculate XOR sum */
 386         for (i = 0; i < payload_size; i++)
 387                 running_xor ^= file_data[i];
 388
 389         dnload.data[dnload.data_len++] = running_xor;
 390
 391         /* initialize write pointer to start of data */
 392         dnload.write_ptr = dnload.data;
 393
 394         printf("read_file(%s): file_size=%u, hdr_len=%u, dnload_len=%u\n",
 395                 filename, (int)st.st_size, hdr_len, dnload.data_len);
 396
 397         return 0;
 398 }
 399
 400 static void hexdump(const uint8_t *data, unsigned int len)
 401 {
 402         const uint8_t *bufptr = data;
 403         int n;
 404
 405         for (n=0; n < len; n++, bufptr++)
 406                 printf("%02x ", *bufptr);
 407         printf("\n");
 408 }
 409
 410 static int romload_prepare_block(void)
 411 {
 412         int rc, i;
 413
 414         int block_checksum = 5;
 415         int remaining_bytes;
 416         int fill_bytes;
 417         uint8_t *block_data;
 418         uint32_t block_address;
 419
 420         dnload.block_len = ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size;
 421
 422         /* if first block, allocate memory */
 423         if (!dnload.block_number) {
 424                 dnload.block = malloc(dnload.block_len);
 425                 if (!dnload.block) {
 426                         fprintf(stderr, "No memory\n");
 427                         return -ENOMEM;
 428                 }
 429                 dnload.romload_dl_checksum = 0;
 430                 /* initialize write pointer to start of data */
 431                 dnload.write_ptr = dnload.data;
 432         }
 433
 434         block_address = ROMLOAD_ADDRESS +
 435                         (dnload.block_number * dnload.block_payload_size);
 436
 437         /* prepare our block header (10 bytes) */
 438         memcpy(dnload.block, romload_write_cmd, sizeof(romload_write_cmd));
 439         dnload.block[2] = 0x01; /* block index */
 440         /* should normally be the block number, but hangs when sending !0x01 */
 441         dnload.block[3] = 0x01; /* dnload.block_number+1 */
 442         dnload.block[4] = (dnload.block_payload_size >> 8) & 0xff;
 443         dnload.block[5] = dnload.block_payload_size & 0xff;
 444         dnload.block[6] = (block_address >> 24) & 0xff;
 445         dnload.block[7] = (block_address >> 16) & 0xff;
 446         dnload.block[8] = (block_address >> 8) & 0xff;
 447         dnload.block[9] = block_address & 0xff;
 448
 449         block_data = dnload.block + ROMLOAD_BLOCK_HDR_LEN;
 450         dnload.write_ptr = dnload.data + 2 +
 451                         (dnload.block_payload_size * dnload.block_number);
 452
 453         remaining_bytes = dnload.data_len - 3 -
 454                         (dnload.block_payload_size * dnload.block_number);
 455
 456         memcpy(block_data, dnload.write_ptr, dnload.block_payload_size);
 457
 458         if (remaining_bytes <= dnload.block_payload_size) {
 459                 fill_bytes = (dnload.block_payload_size - remaining_bytes);
 460                 printf("Preparing the last block, filling %i bytes,",
 461                         fill_bytes);
 462                 memset(block_data + remaining_bytes, 0x00, fill_bytes);
 463                 dnload.romload_state = SENDING_LAST_BLOCK;
 464         } else {
 465                         dnload.romload_state = SENDING_BLOCKS;
 466                         printf("Preparing block %i,", dnload.block_number+1);
 467         }
 468
 469         /* block checksum is lsb of ~(5 + block_size_lsb +  all bytes of
 470          * block_address + all data bytes) */
 471         for (i = 5; i < ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size; i++)
 472                 block_checksum += dnload.block[i];
 473
 474         /* checksum is lsb of ~(sum of LSBs of all block checksums) */
 475         printf(" block checksum is 0x%02x \n", ~(block_checksum) & 0xff);
 476         dnload.romload_dl_checksum += ~(block_checksum) & 0xff;
 477
 478         /* initialize block pointer to start of block */
 479         dnload.block_ptr = dnload.block;
 480
 481         dnload.block_number++;
 482         dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 483         return rc;
 484 }
 485
 486 static int handle_write_block(void)
 487 {
 488         int bytes_left, write_len, rc;
 489
 490         printf("handle_write_block(): ");
 491
 492         if (dnload.block_ptr >= dnload.block + dnload.block_len) {
 493                 printf("Block %i finished\n", dnload.block_number);
 494                 dnload.write_ptr = dnload.data;
 495                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 496                 if (dnload.romload_state == SENDING_LAST_BLOCK) {
 497                         dnload.romload_state = LAST_BLOCK_SENT;
 498                         printf("Finished, sent %i blocks in total\n",
 499                                 dnload.block_number);
 500                 } else {
 501                         dnload.romload_state = WAITING_BLOCK_ACK;
 502                 }
 503
 504                 return 0;
 505         }
 506
 507         /* try to write a maximum of block_len bytes */
 508         bytes_left = (dnload.block + dnload.block_len) - dnload.block_ptr;
 509         write_len = dnload.block_len;
 510         if (bytes_left < dnload.block_len)
 511                 write_len = bytes_left;
 512
 513         rc = write(dnload.serial_fd.fd, dnload.block_ptr, write_len);
 514         if (rc < 0) {
 515                 perror("Error during write");
 516                 return rc;
 517         }
 518
 519         dnload.block_ptr += rc;
 520
 521         printf("%u bytes (%tu/%u)\n", rc, dnload.block_ptr - dnload.block,
 522                 dnload.block_len);
 523
 524         return 0;
 525 }
 526
 527 #define WRITE_BLOCK     4096
 528
 529 static int handle_write_dnload(void)
 530 {
 531         int bytes_left, write_len, rc;
 532         uint8_t xor_init = 0x02;
 533
 534         printf("handle_write(): ");
 535         if (dnload.write_ptr == dnload.data) {
 536                 /* no bytes have been transferred yet */
 537                 switch (dnload.mode) {
 538                 case MODE_C155:
 539                 case MODE_C140xor:
 540                 case MODE_C123xor:
 541                         rc = write(dnload.serial_fd.fd, &xor_init, 1);
 542                         break;
 543                 default:
 544                         break;
 545                 }
 546         } else if (dnload.write_ptr >= dnload.data + dnload.data_len) {
 547                 printf("finished\n");
 548                 dnload.write_ptr = dnload.data;
 549                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 550                 return 1;
 551         }
 552
 553         /* try to write a maximum of WRITE_BLOCK bytes */
 554         bytes_left = (dnload.data + dnload.data_len) - dnload.write_ptr;
 555         write_len = WRITE_BLOCK;
 556         if (bytes_left < WRITE_BLOCK)
 557                 write_len = bytes_left;
 558
 559         rc = write(dnload.serial_fd.fd, dnload.write_ptr, write_len);
 560         if (rc < 0) {
 561                 perror("Error during write");
 562                 return rc;
 563         }
 564
 565         dnload.write_ptr += rc;
 566
 567         printf("%u bytes (%tu/%u)\n", rc, dnload.write_ptr - dnload.data,
 568                 dnload.data_len);
 569
 570         return 0;
 571 }
 572
 573 static int handle_sercomm_write(void)
 574 {
 575         uint8_t c;
 576
 577         if (sercomm_drv_pull(&c) != 0) {
 578                 if (write(dnload.serial_fd.fd, &c, 1) != 1)
 579                         perror("short write");
 580                 } else
 581                         dnload.serial_fd.when &= ~BSC_FD_WRITE;
 582
 583         return 0;
 584 }
 585
 586 static int handle_write(void)
 587 {
 588         if (dnload.mode == MODE_ROMLOAD) {
 589                 switch (dnload.romload_state) {
 590                 case SENDING_BLOCKS:
 591                 case SENDING_LAST_BLOCK:
 592                         return handle_write_block();
 593                 default:
 594                         return handle_sercomm_write();
 595                 }
 596         } else {
 597                 switch (dnload.state) {
 598                 case DOWNLOADING:
 599                         return handle_write_dnload();
 600                 default:
 601                         return handle_sercomm_write();
 602                 }
 603         }
 604
 605         return 0;
 606 }
 607
 608 static uint8_t buffer[sizeof(phone_prompt1)];
 609 static uint8_t *bufptr = buffer;
 610
 611 static void hdlc_send_to_phone(uint8_t dlci, uint8_t *data, int len)
 612 {
 613         struct msgb *msg;
 614         uint8_t *dest;
 615
 616         printf("hdlc_send_to_phone(dlci=%u): ", dlci);
 617         hexdump(data, len);
 618
 619         if (len > 512) {
 620                 fprintf(stderr, "Too much data to send. %u\n", len);
 621                 return;
 622         }
 623
 624         /* push the message into the stack */
 625         msg = sercomm_alloc_msgb(512);
 626         if (!msg) {
 627                 fprintf(stderr, "Failed to create data for the frame.\n");
 628                 return;
 629         }
 630
 631         /* copy the data */
 632         dest = msgb_put(msg, len);
 633         memcpy(dest, data, len);
 634
 635         sercomm_sendmsg(dlci, msg);
 636
 637         dnload.serial_fd.when |= BSC_FD_WRITE;
 638 }
 639
 640 static void hdlc_console_cb(uint8_t dlci, struct msgb *msg)
 641 {
 642         int rc;
 643
 644         rc = write(1, msg->data, msg->len);
 645         msgb_free(msg);
 646 }
 647
 648 static void hdlc_tool_cb(uint8_t dlci, struct msgb *msg)
 649 {
 650         struct tool_server *srv = tool_server_for_dlci[dlci];
 651
 652         if(srv) {
 653                 struct tool_connection *con;
 654                 u_int16_t *len;
 655
 656                 len = (u_int16_t *) msgb_push(msg, 2);
 657                 *len = htons(msg->len - sizeof(*len));
 658
 659                 llist_for_each_entry(con, &srv->connections, entry) {
 660                         if (write(con->fd.fd, msg->data, msg->len) != msg->len) {
 661                                 fprintf(stderr,
 662                                         "Failed to write msg to the socket..\n");
 663                                 continue;
 664                         }
 665                 }
 666         }
 667
 668         msgb_free(msg);
 669 }
 670
 671 static void print_hdlc(uint8_t *buffer, int length)
 672 {
 673         int i;
 674
 675         for (i = 0; i < length; ++i)
 676                 if (sercomm_drv_rx_char(buffer[i]) == 0)
 677                         printf("Dropping sample '%c'\n", buffer[i]);
 678 }
 679
 680 static int handle_buffer(int buf_used_len)
 681 {
 682         int nbytes, buf_left;
 683
 684         buf_left = buf_used_len - (bufptr - buffer);
 685         if (buf_left <= 0) {
 686                 memmove(buffer, buffer+1, buf_used_len-1);
 687                 bufptr -= 1;
 688                 buf_left = 1;
 689         }
 690
 691         nbytes = read(dnload.serial_fd.fd, bufptr, buf_left);
 692         if (nbytes <= 0)
 693                 return nbytes;
 694
 695         if (!dnload.print_hdlc) {
 696                 printf("got %i bytes from modem, ", nbytes);
 697                 printf("data looks like: ");
 698                 hexdump(bufptr, nbytes);
 699         } else {
 700                 print_hdlc(bufptr, nbytes);
 701         }
 702
 703         return nbytes;
 704 }
 705
 706 /* Compal ramloader */
 707 static int handle_read(void)
 708 {
 709         int rc, nbytes;
 710
 711         nbytes = handle_buffer(sizeof(buffer));
 712         if (nbytes <= 0)
 713                 return nbytes;
 714
 715         if (!memcmp(buffer, phone_prompt1, sizeof(phone_prompt1))) {
 716                 printf("Received PROMPT1 from phone, responding with CMD\n");
 717                 dnload.print_hdlc = 0;
 718                 dnload.state = WAITING_PROMPT2;
 719                 rc = write(dnload.serial_fd.fd, dnload_cmd, sizeof(dnload_cmd));
 720
 721                 /* re-read file */
 722                 rc = read_file(dnload.filename);
 723                 if (rc < 0) {
 724                         fprintf(stderr, "read_file(%s) failed with %d\n",
 725                                 dnload.filename, rc);
 726                         exit(1);
 727                 }
 728         } else if (!memcmp(buffer, phone_prompt2, sizeof(phone_prompt2))) {
 729                 printf("Received PROMPT2 from phone, starting download\n");
 730                 dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 731                 dnload.state = DOWNLOADING;
 732         } else if (!memcmp(buffer, phone_ack, sizeof(phone_ack))) {
 733                 printf("Received DOWNLOAD ACK from phone, your code is"
 734                         " running now!\n");
 735                 dnload.serial_fd.when = BSC_FD_READ;
 736                 dnload.state = WAITING_PROMPT1;
 737                 dnload.write_ptr = dnload.data;
 738                 dnload.print_hdlc = 1;
 739         } else if (!memcmp(buffer, phone_nack, sizeof(phone_nack))) {
 740                 printf("Received DOWNLOAD NACK from phone, something went"
 741                         " wrong :(\n");
 742                 dnload.serial_fd.when = BSC_FD_READ;
 743                 dnload.state = WAITING_PROMPT1;
 744                 dnload.write_ptr = dnload.data;
 745         } else if (!memcmp(buffer, phone_nack_magic, sizeof(phone_nack_magic))) {
 746                 printf("Received MAGIC NACK from phone, you need to"
 747                         " have \"1003\" at 0x803ce0\n");
 748                 dnload.serial_fd.when = BSC_FD_READ;
 749                 dnload.state = WAITING_PROMPT1;
 750                 dnload.write_ptr = dnload.data;
 751         } else if (!memcmp(buffer, ftmtool, sizeof(ftmtool))) {
 752                 printf("Received FTMTOOL from phone, ramloader has aborted\n");
 753                 dnload.serial_fd.when = BSC_FD_READ;
 754                 dnload.state = WAITING_PROMPT1;
 755                 dnload.write_ptr = dnload.data;
 756         }
 757         bufptr += nbytes;
 758
 759         return nbytes;
 760 }
 761
 762 /* "Calypso non-secure romloader" */
 763 static int handle_read_romload(void)
 764 {
 765         int rc, nbytes, buf_used_len;
 766
 767         /* virtually limit buffer length for romloader, since responses
 768          * are shorter and vary in length */
 769
 770         switch (dnload.romload_state) {
 771         case WAITING_PARAM_ACK:
 772                 buf_used_len = 4;       /* ">p" + uint16_t len */
 773                 break;
 774         case WAITING_CHECKSUM_ACK:
 775                 buf_used_len = 3;       /* ">c" + uint8_t checksum */
 776                 break;
 777         case FINISHED:
 778                 buf_used_len = sizeof(buffer);
 779                 break;
 780         default:
 781                 buf_used_len = 2;       /* ">*" */
 782         }
 783
 784         nbytes = handle_buffer(buf_used_len);
 785         if (nbytes <= 0)
 786                 return nbytes;
 787
 788         switch (dnload.romload_state) {
 789         case WAITING_IDENTIFICATION:
 790                 if (memcmp(buffer, romload_ident_ack,
 791                             sizeof(romload_ident_ack)))
 792                         break;
 793
 794                 printf("Received ident ack from phone, sending "
 795                         "parameter sequence\n");
 796                 dnload.print_hdlc = 1;
 797                 dnload.romload_state = WAITING_PARAM_ACK;
 798                 rc = write(dnload.serial_fd.fd, romload_param,
 799                            sizeof(romload_param));
 800                 /* re-read file */
 801                 rc = read_file(dnload.filename);
 802                 if (rc < 0) {
 803                         fprintf(stderr, "read_file(%s) failed with %d\n",
 804                                 dnload.filename, rc);
 805                         exit(1);
 806                 }
 807                 break;
 808         case WAITING_PARAM_ACK:
 809                 if (memcmp(buffer, romload_param_ack,
 810                             sizeof(romload_param_ack)))
 811                         break;
 812
 813                 printf("Received parameter ack from phone, "
 814                         "starting download\n");
 815                 serial_set_baudrate(ROMLOAD_DL_BAUDRATE);
 816
 817                 /* using the max blocksize the phone tells us */
 818                 dnload.block_payload_size = ((buffer[3] << 8) + buffer[2]);
 819                 printf("Used blocksize for download is %i bytes\n",
 820                         dnload.block_payload_size);
 821                 dnload.block_payload_size -= ROMLOAD_BLOCK_HDR_LEN;
 822                 dnload.romload_state = SENDING_BLOCKS;
 823                 dnload.block_number = 0;
 824                 romload_prepare_block();
 825                 bufptr -= 2;
 826                 break;
 827         case WAITING_BLOCK_ACK:
 828         case LAST_BLOCK_SENT:
 829                 if (!memcmp(buffer, romload_block_ack,
 830                             sizeof(romload_block_ack))) {
 831                         printf("Received block ack from phone\n");
 832                         if (dnload.romload_state == LAST_BLOCK_SENT) {
 833                                 /* send the checksum */
 834                                 uint8_t final_checksum =
 835                                         (~(dnload.romload_dl_checksum) & 0xff);
 836                                 printf("Sending checksum: 0x%02x \n",
 837                                         final_checksum);
 838                                 rc = write(dnload.serial_fd.fd,
 839                                            romload_checksum_cmd,
 840                                            sizeof(romload_checksum_cmd));
 841                                 rc = write(dnload.serial_fd.fd,
 842                                            &final_checksum, 1);
 843                                 dnload.romload_state = WAITING_CHECKSUM_ACK;
 844                         } else
 845                                 romload_prepare_block();
 846                 } else if (!memcmp(buffer, romload_block_nack,
 847                                    sizeof(romload_block_nack))) {
 848                         printf("Received block nack from phone, "
 849                                 "something went wrong, aborting\n");
 850                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
 851                         dnload.romload_state = WAITING_IDENTIFICATION;
 852                         bsc_schedule_timer(&tick_timer, 0, BEACON_INTERVAL);
 853                 }
 854                 break;
 855         case WAITING_CHECKSUM_ACK:
 856                 if (!memcmp(buffer, romload_checksum_ack,
 857                             sizeof(romload_checksum_ack))) {
 858                         printf("Checksum on phone side matches, "
 859                                 "let's branch to your code\n");
 860                         printf("Branching to 0x%08x\n", ROMLOAD_ADDRESS);
 861
 862                         rc = write(dnload.serial_fd.fd, romload_branch_cmd,
 863                                    sizeof(romload_branch_cmd));
 864                         rc = write(dnload.serial_fd.fd, &dnload.load_address,
 865                                    sizeof(dnload.load_address));
 866                         dnload.romload_state = WAITING_BRANCH_ACK;
 867                         bufptr -= 1;
 868                 } else if (!memcmp(buffer, romload_checksum_nack,
 869                                    sizeof(romload_checksum_nack))) {
 870                         printf("Checksum on phone side (0x%02x) doesn't "
 871                                 "match ours, aborting\n", ~buffer[2]);
 872                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
 873                         dnload.romload_state = WAITING_IDENTIFICATION;
 874                         bsc_schedule_timer(&tick_timer, 0, BEACON_INTERVAL);
 875                         bufptr -= 1;
 876                 }
 877                 break;
 878         case WAITING_BRANCH_ACK:
 879                 if (!memcmp(buffer, romload_branch_ack,
 880                             sizeof(romload_branch_ack))) {
 881                         printf("Received branch ack, your code is running now!\n");
 882                         dnload.serial_fd.when = BSC_FD_READ;
 883                         dnload.romload_state = FINISHED;
 884                         dnload.write_ptr = dnload.data;
 885                         dnload.print_hdlc = 1;
 886                 } else if (!memcmp(buffer, romload_branch_nack,
 887                            sizeof(romload_branch_nack))) {
 888                         printf("Received branch nack, aborting\n");
 889                         serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
 890                         dnload.romload_state = WAITING_IDENTIFICATION;
 891                         bsc_schedule_timer(&tick_timer, 0, BEACON_INTERVAL);
 892                 }
 893                 break;
 894         default:
 895                 break;
 896         }
 897
 898         bufptr += nbytes;
 899         return nbytes;
 900 }
 901
 902 static int serial_read(struct bsc_fd *fd, unsigned int flags)
 903 {
 904         int rc;
 905         if (flags & BSC_FD_READ) {
 906                 if (dnload.mode == MODE_ROMLOAD)
 907                         rc = handle_read_romload();
 908                 else
 909                         rc = handle_read();
 910
 911                 if (rc == 0)
 912                         exit(2);
 913         }
 914
 915         if (flags & BSC_FD_WRITE) {
 916                 rc = handle_write();
 917                 if (rc == 1)
 918                         dnload.state = WAITING_PROMPT1;
 919         }
 920         return 0;
 921 }
 922
 923 static int parse_mode(const char *arg)
 924 {
 925         if (!strcasecmp(arg, "c123"))
 926                 return MODE_C123;
 927         else if (!strcasecmp(arg, "c123xor"))
 928                 return MODE_C123xor;
 929         else if (!strcasecmp(arg, "c140"))
 930                 return MODE_C140;
 931         else if (!strcasecmp(arg, "c140xor"))
 932                 return MODE_C140xor;
 933         else if (!strcasecmp(arg, "c155"))
 934                 return MODE_C155;
 935         else if (!strcasecmp(arg, "romload"))
 936                 return MODE_ROMLOAD;
 937
 938         return -1;
 939 }
 940
 941 #define HELP_TEXT \
 942         "[ -v | -h ] [ -p /dev/ttyXXXX ] [ -s /tmp/osmocom_l2 ]\n" \
 943         "\t\t[ -l /tmp/osmocom_loader ]\n" \
 944         "\t\t[ -m {c123,c123xor,c140,c140xor,c155,romload} ]\n" \
 945         "\t\t file.bin\n\n" \
 946         "* Open serial port /dev/ttyXXXX (connected to your phone)\n" \
 947         "* Perform handshaking with the ramloader in the phone\n" \
 948         "* Download file.bin to the attached phone (base address 0x00800100)\n"
 949
 950 static int usage(const char *name)
 951 {
 952         printf("Usage: %s ", name);
 953         printf(HELP_TEXT);
 954         exit(2);
 955 }
 956
 957 static int version(const char *name)
 958 {
 959         printf("%s version %s\n", name, PACKAGE_VERSION);
 960         exit(2);
 961 }
 962
 963 static int un_tool_read(struct bsc_fd *fd, unsigned int flags)
 964 {
 965         int rc, c;
 966         u_int16_t length = 0xffff;
 967         u_int8_t buf[4096];
 968         struct tool_connection *con = (struct tool_connection *)fd->data;
 969
 970         c = 0;
 971         while(c < 2) {
 972                 rc = read(fd->fd, &buf + c, 2 - c);
 973                 if(rc == 0) {
 974                         // disconnect
 975                         goto close;
 976                 }
 977                 if(rc < 0) {
 978                         if(errno == EAGAIN) {
 979                                 continue;
 980                         }
 981                         fprintf(stderr, "Err from socket: %s\n", strerror(errno));
 982                         goto close;
 983                 }
 984                 c += rc;
 985         }
 986
 987         length = ntohs(*(u_int16_t*)buf);
 988
 989         c = 0;
 990         while(c < length) {
 991                 rc = read(fd->fd, &buf + c, length - c);
 992                 if(rc == 0) {
 993                         // disconnect
 994                         goto close;
 995                 }
 996                 if(rc < 0) {
 997                         if(errno == EAGAIN) {
 998                                 continue;
 999                         }
1000                         fprintf(stderr, "Err from socket: %s\n", strerror(errno));
1001                         goto close;
1002                 }
1003                 c += rc;
1004         }
1005
1006         hdlc_send_to_phone(con->server->dlci, buf, length);
1007
1008         return 0;
1009 close:
1010
1011         close(fd->fd);
1012         bsc_unregister_fd(fd);
1013         llist_del(&con->entry);
1014         talloc_free(con);
1015         return -1;
1016 }
1017
1018 /* accept a new connection */
1019 static int tool_accept(struct bsc_fd *fd, unsigned int flags)
1020 {
1021         struct tool_server *srv = (struct tool_server *)fd->data;
1022         struct tool_connection *con;
1023         struct sockaddr_un un_addr;
1024         socklen_t len;
1025         int rc;
1026
1027         len = sizeof(un_addr);
1028         rc = accept(fd->fd, (struct sockaddr *) &un_addr, &len);
1029         if (rc < 0) {
1030                 fprintf(stderr, "Failed to accept a new connection.\n");
1031                 return -1;
1032         }
1033
1034         con = talloc_zero(NULL, struct tool_connection);
1035         if (!con) {
1036                 fprintf(stderr, "Failed to create tool connection.\n");
1037                 return -1;
1038         }
1039
1040         con->server = srv;
1041
1042         con->fd.fd = rc;
1043         con->fd.when = BSC_FD_READ;
1044         con->fd.cb = un_tool_read;
1045         con->fd.data = con;
1046         if (bsc_register_fd(&con->fd) != 0) {
1047                 fprintf(stderr, "Failed to register the fd.\n");
1048                 return -1;
1049         }
1050
1051         llist_add(&con->entry, &srv->connections);
1052         return 0;
1053 }
1054
1055 /*
1056  * Register and start a tool server
1057  */
1058 static int register_tool_server(struct tool_server *ts,
1059                                 const char *path,
1060                                 uint8_t dlci)
1061 {
1062         struct bsc_fd *bfd = &ts->bfd;
1063         struct sockaddr_un local;
1064         unsigned int namelen;
1065         int rc;
1066
1067         bfd->fd = socket(AF_UNIX, SOCK_STREAM, 0);
1068
1069         if (bfd->fd < 0) {
1070                 fprintf(stderr, "Failed to create Unix Domain Socket.\n");
1071                 return -1;
1072         }
1073
1074         local.sun_family = AF_UNIX;
1075         strncpy(local.sun_path, path, sizeof(local.sun_path));
1076         local.sun_path[sizeof(local.sun_path) - 1] = '\0';
1077         unlink(local.sun_path);
1078
1079         /* we use the same magic that X11 uses in Xtranssock.c for
1080          * calculating the proper length of the sockaddr */
1081 #if defined(BSD44SOCKETS) || defined(__UNIXWARE__)
1082         local.sun_len = strlen(local.sun_path);
1083 #endif
1084 #if defined(BSD44SOCKETS) || defined(SUN_LEN)
1085         namelen = SUN_LEN(&local);
1086 #else
1087         namelen = strlen(local.sun_path) +
1088                   offsetof(struct sockaddr_un, sun_path);
1089 #endif
1090
1091         rc = bind(bfd->fd, (struct sockaddr *) &local, namelen);
1092         if (rc != 0) {
1093                 fprintf(stderr, "Failed to bind the unix domain socket. '%s'\n",
1094                         local.sun_path);
1095                 return -1;
1096         }
1097
1098         if (listen(bfd->fd, 0) != 0) {
1099                 fprintf(stderr, "Failed to listen.\n");
1100                 return -1;
1101         }
1102
1103         bfd->when = BSC_FD_READ;
1104         bfd->cb = tool_accept;
1105         bfd->data = ts;
1106
1107         ts->dlci = dlci;
1108         INIT_LLIST_HEAD(&ts->connections);
1109
1110         tool_server_for_dlci[dlci] = ts;
1111
1112         sercomm_register_rx_cb(dlci, hdlc_tool_cb);
1113
1114         if (bsc_register_fd(bfd) != 0) {
1115                 fprintf(stderr, "Failed to register the bfd.\n");
1116                 return -1;
1117         }
1118
1119         return 0;
1120 }
1121
1122 extern void hdlc_tpudbg_cb(uint8_t dlci, struct msgb *msg);
1123
1124 int main(int argc, char **argv)
1125 {
1126         int opt, flags;
1127         uint32_t tmp_load_address = 0;
1128         const char *serial_dev = "/dev/ttyUSB1";
1129         const char *layer2_un_path = "/tmp/osmocom_l2";
1130         const char *loader_un_path = "/tmp/osmocom_loader";
1131
1132         dnload.mode = MODE_C123;
1133
1134         while ((opt = getopt(argc, argv, "hl:p:m:s:v")) != -1) {
1135                 switch (opt) {
1136                 case 'p':
1137                         serial_dev = optarg;
1138                         break;
1139                 case 'm':
1140                         dnload.mode = parse_mode(optarg);
1141                         if (dnload.mode < 0)
1142                                 usage(argv[0]);
1143                         break;
1144                 case 's':
1145                         layer2_un_path = optarg;
1146                         break;
1147                 case 'l':
1148                         loader_un_path = optarg;
1149                         break;
1150                 case 'v':
1151                         version(argv[0]);
1152                         break;
1153                 case 'h':
1154                 default:
1155                         usage(argv[0]);
1156                         break;
1157                 }
1158         }
1159
1160         if (argc <= optind) {
1161                 fprintf(stderr, "You have to specify the filename\n");
1162                 usage(argv[0]);
1163         }
1164
1165         dnload.filename = argv[optind];
1166
1167         dnload.serial_fd.fd = serial_init(serial_dev);
1168         if (dnload.serial_fd.fd < 0) {
1169                 fprintf(stderr, "Cannot open serial device %s\n", serial_dev);
1170                 exit(1);
1171         }
1172
1173         if (bsc_register_fd(&dnload.serial_fd) != 0) {
1174                 fprintf(stderr, "Failed to register the serial.\n");
1175                 exit(1);
1176         }
1177
1178         /* Set serial socket to non-blocking mode of operation */
1179         flags = fcntl(dnload.serial_fd.fd, F_GETFL);
1180         flags |= O_NONBLOCK;
1181         fcntl(dnload.serial_fd.fd, F_SETFL, flags);
1182
1183         dnload.serial_fd.when = BSC_FD_READ;
1184         dnload.serial_fd.cb = serial_read;
1185
1186         /* initialize the HDLC layer */
1187         sercomm_init();
1188         sercomm_register_rx_cb(SC_DLCI_CONSOLE, hdlc_console_cb);
1189         sercomm_register_rx_cb(SC_DLCI_DEBUG, hdlc_tpudbg_cb);
1190
1191         /* unix domain socket handling */
1192         if (register_tool_server(&dnload.layer2_server, layer2_un_path,
1193                                  SC_DLCI_L1A_L23) != 0)
1194                 exit(1);
1195
1196         if (register_tool_server(&dnload.loader_server, loader_un_path,
1197                                  SC_DLCI_LOADER) != 0)
1198                 exit(1);
1199
1200         /* if in romload mode, start our beacon timer */
1201         if (dnload.mode == MODE_ROMLOAD) {
1202                 tmp_load_address = ROMLOAD_ADDRESS;
1203                 serial_set_baudrate(ROMLOAD_INIT_BAUDRATE);
1204                 tick_timer.cb = &beacon_timer_cb;
1205                 tick_timer.data = &tick_timer;
1206                 bsc_schedule_timer(&tick_timer, 0, BEACON_INTERVAL);
1207         }
1208
1209         dnload.load_address[0] = (tmp_load_address >> 24) & 0xff;
1210         dnload.load_address[1] = (tmp_load_address >> 16) & 0xff;
1211         dnload.load_address[2] = (tmp_load_address >> 8) & 0xff;
1212         dnload.load_address[3] = tmp_load_address & 0xff;
1213
1214         while (1)
1215                 bsc_select_main(0);
1216
1217         close(dnload.serial_fd.fd);
1218
1219         exit(0);
1220 }