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