src/host/osmocon/osmocon.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <unistd.h>
   5 #include <stdint.h>
   6 #include <fcntl.h>
   7 #include <errno.h>
   8 #include <termios.h>
   9 #include <sys/ioctl.h>
  10 #include <sys/types.h>
  11 #include <sys/socket.h>
  12 #include <sys/stat.h>
  13 #include <sys/un.h>
  14
  15 #include <sercomm.h>
  16
  17 #include <osmocore/linuxlist.h>
  18 #include <osmocore/select.h>
  19 #include <osmocore/talloc.h>
  20
  21 #include <arpa/inet.h>
  22
  23 //#include "version.h"
  24
  25 #define MODEM_BAUDRATE  B115200
  26 #define MAX_DNLOAD_SIZE 0xFFFF
  27 #define MAX_HDR_SIZE    128
  28
  29 enum dnload_state {
  30         WAITING_PROMPT1,
  31         WAITING_PROMPT2,
  32         DOWNLOADING,
  33 };
  34
  35 enum dnload_mode {
  36         MODE_C123,
  37         MODE_C123xor,
  38         MODE_C155,
  39 };
  40
  41 struct dnload {
  42         enum dnload_state state;
  43         enum dnload_mode mode;
  44         struct bsc_fd serial_fd;
  45         char *filename;
  46
  47         int print_hdlc;
  48
  49         /* data to be downloaded */
  50         uint8_t *data;
  51         int data_len;
  52
  53         uint8_t *write_ptr;
  54
  55         /* sockaddr in */
  56         struct bsc_fd socket;
  57 };
  58
  59 /**
  60  * a connection of the layer2
  61  */
  62 struct layer2_connection {
  63         struct llist_head entry;
  64         struct bsc_fd fd;
  65 };
  66
  67 static LLIST_HEAD(connections);
  68 static struct dnload dnload;
  69
  70 static const uint8_t phone_prompt1[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x01, 0x40 };
  71 static const uint8_t dnload_cmd[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x52, 0x01, 0x53 };
  72 static const uint8_t phone_prompt2[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x02, 0x43 };
  73 static const uint8_t phone_ack[]     = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x42 };
  74 static const uint8_t phone_nack_magic[]= { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x57 };
  75 static const uint8_t phone_nack[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x45, 0x53, 0x16 };
  76 static const uint8_t ftmtool[] = { "ftmtool" };
  77
  78 /* The C123 has a hard-coded check inside the ramloder that requires the following
  79  * bytes to be always the first four bytes of the image */
  80 static const uint8_t data_hdr_c123[]    = { 0xee, 0x4c, 0x9f, 0x63 };
  81
  82 /* The C155 doesn't have some strange restriction on what the first four bytes have
  83  * to be, but it starts the ramloader in THUMB mode.  We use the following four bytes
  84  * to switch back to ARM mode:
  85   800100:       4778            bx      pc
  86   800102:       46c0            nop                     ; (mov r8, r8)
  87  */
  88 static const uint8_t data_hdr_c155[]    = { 0x78, 0x47, 0xc0, 0x46 };
  89
  90 static const uint8_t dummy_data[]    = { 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde };
  91
  92 static int serial_init(const char *serial_dev)
  93 {
  94     struct termios options;
  95     int fd, v24;
  96
  97     fd = open(serial_dev, O_RDWR | O_NOCTTY | O_NDELAY);
  98     if (fd < 0)
  99         return fd;
 100
 101     fcntl(fd, F_SETFL, 0);
 102
 103     /* Configure serial interface */
 104     tcgetattr(fd, &options);
 105
 106     cfsetispeed(&options, MODEM_BAUDRATE);
 107     cfsetospeed(&options, MODEM_BAUDRATE);
 108
 109     /* local read */
 110     options.c_cflag &= ~PARENB;
 111     options.c_cflag &= ~CSTOPB;
 112     options.c_cflag &= ~CSIZE;
 113     options.c_cflag |= CS8;
 114
 115     /* hardware flow control off */
 116     options.c_cflag &= ~CRTSCTS;
 117
 118     /* software flow control off */
 119     options.c_iflag &= ~(IXON | IXOFF | IXANY);
 120
 121     /* we want raw i/o */
 122     options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
 123     options.c_iflag &= ~(INLCR | ICRNL | IGNCR);
 124     options.c_oflag &= ~(ONLCR);
 125
 126     options.c_cc[VMIN] = 1;
 127     options.c_cc[VTIME] = 0;
 128     options.c_cc[VINTR] = 0;
 129     options.c_cc[VQUIT] = 0;
 130     options.c_cc[VSTART] = 0;
 131     options.c_cc[VSTOP] = 0;
 132     options.c_cc[VSUSP] = 0;
 133
 134     tcsetattr(fd, TCSANOW, &options);
 135
 136     /* set ready to read/write */
 137     v24 = TIOCM_DTR | TIOCM_RTS;
 138     ioctl(fd, TIOCMBIS, &v24);
 139
 140     return fd;
 141 }
 142
 143 /* Read the to-be-downloaded file, prepend header and length, append XOR sum */
 144 int read_file(const char *filename)
 145 {
 146         int fd, rc, i;
 147         struct stat st;
 148         const uint8_t *hdr = NULL;
 149         int hdr_len = 0;
 150         uint8_t *file_data;
 151         uint16_t tot_len;
 152         uint8_t nibble;
 153         uint8_t running_xor = 0x02;
 154
 155         fd = open(filename, O_RDONLY);
 156         if (fd < 0) {
 157                 perror("opening file");
 158                 exit(1);
 159         }
 160
 161         rc = fstat(fd, &st);
 162         if (st.st_size > MAX_DNLOAD_SIZE) {
 163                 fprintf(stderr, "The maximum file size is 64kBytes (%u bytes)\n",
 164                         MAX_DNLOAD_SIZE);
 165                 return -EFBIG;
 166         }
 167
 168         if (dnload.data) {
 169                 free(dnload.data);
 170                 dnload.data = NULL;
 171         }
 172
 173         dnload.data = malloc(MAX_HDR_SIZE + st.st_size);
 174         if (!dnload.data) {
 175                 close(fd);
 176                 fprintf(stderr, "No memory\n");
 177                 return -ENOMEM;
 178         }
 179
 180         /* copy in the header, if any */
 181         switch (dnload.mode) {
 182         case MODE_C155:
 183                 hdr = data_hdr_c155;
 184                 hdr_len = sizeof(data_hdr_c155);
 185                 break;
 186         case MODE_C123:
 187         case MODE_C123xor:
 188                 hdr = data_hdr_c123;
 189                 hdr_len = sizeof(data_hdr_c123);
 190                 break;
 191         default:
 192                 break;
 193         }
 194
 195         if (hdr && hdr_len)
 196                 memcpy(dnload.data, hdr, hdr_len);
 197
 198         /* 2 bytes for length + header */
 199         file_data = dnload.data + 2 + hdr_len;
 200
 201         /* write the length, keep running XOR */
 202         tot_len = hdr_len + st.st_size;
 203         nibble = tot_len >> 8;
 204         dnload.data[0] = nibble;
 205         running_xor ^= nibble;
 206         nibble = tot_len & 0xff;
 207         dnload.data[1] = nibble;
 208         running_xor ^= nibble;
 209
 210         if (hdr_len && hdr) {
 211                 memcpy(dnload.data+2, hdr, hdr_len);
 212
 213                 for (i = 0; i < hdr_len; i++)
 214                         running_xor ^= hdr[i];
 215         }
 216
 217         rc = read(fd, file_data, st.st_size);
 218         if (rc < 0) {
 219                 perror("error reading file\n");
 220                 free(dnload.data);
 221                 dnload.data = NULL;
 222                 close(fd);
 223                 return -EIO;
 224         }
 225         if (rc < st.st_size) {
 226                 free(dnload.data);
 227                 dnload.data = NULL;
 228                 close(fd);
 229                 fprintf(stderr, "Short read of file (%d < %d)\n",
 230                         rc, (int)st.st_size);
 231                 return -EIO;
 232         }
 233
 234         close(fd);
 235
 236         dnload.data_len = (file_data+st.st_size) - dnload.data;
 237
 238         /* calculate XOR sum */
 239         for (i = 0; i < st.st_size; i++)
 240                 running_xor ^= file_data[i];
 241
 242         dnload.data[dnload.data_len++] = running_xor;
 243
 244         /* initialize write pointer to start of data */
 245         dnload.write_ptr = dnload.data;
 246
 247         printf("read_file(%s): file_size=%u, hdr_len=%u, dnload_len=%u\n",
 248                 filename, (int)st.st_size, hdr_len, dnload.data_len);
 249
 250         return 0;
 251 }
 252
 253 static void hexdump(const uint8_t *data, unsigned int len)
 254 {
 255         const uint8_t *bufptr = data;
 256         int n;
 257
 258         for (n=0; bufptr, n < len; n++, bufptr++)
 259                 printf("%02x ", *bufptr);
 260         printf("\n");
 261 }
 262
 263 #define WRITE_BLOCK     4096
 264
 265 static int handle_write(void)
 266 {
 267         int bytes_left, write_len, rc;
 268
 269         printf("handle_write(): ");
 270         if (dnload.write_ptr == dnload.data) {
 271                 /* no bytes have been transferred yet */
 272                 if (dnload.mode == MODE_C155 ||
 273                     dnload.mode == MODE_C123xor) {
 274                         uint8_t xor_init = 0x02;
 275                         write(dnload.serial_fd.fd, &xor_init, 1);
 276                 } else
 277                         usleep(1);
 278         } else if (dnload.write_ptr >= dnload.data + dnload.data_len) {
 279                 printf("finished\n");
 280                 dnload.write_ptr = dnload.data;
 281                 dnload.serial_fd.when &= ~BSC_FD_WRITE;
 282                 return 1;
 283         }
 284
 285         /* try to write a maximum of WRITE_BLOCK bytes */
 286         bytes_left = (dnload.data + dnload.data_len) - dnload.write_ptr;
 287         write_len = WRITE_BLOCK;
 288         if (bytes_left < WRITE_BLOCK)
 289                 write_len = bytes_left;
 290
 291         rc = write(dnload.serial_fd.fd, dnload.write_ptr, write_len);
 292         if (rc < 0) {
 293                 perror("Error during write");
 294                 return rc;
 295         }
 296
 297         dnload.write_ptr += rc;
 298
 299         printf("%u bytes (%tu/%u)\n", rc, dnload.write_ptr - dnload.data, dnload.data_len);
 300
 301         return 0;
 302 }
 303
 304 static uint8_t buffer[sizeof(phone_prompt1)];
 305 static uint8_t *bufptr = buffer;
 306
 307 static void hdlc_send_to_phone(uint8_t dlci, uint8_t *data, int len)
 308 {
 309         struct msgb *msg;
 310         uint8_t c, *dest;
 311
 312         if (len > 512) {
 313                 fprintf(stderr, "Too much data to send. %u\n", len);
 314                 return;
 315         }
 316
 317         /* push the message into the stack */
 318         msg = sercomm_alloc_msgb(512);
 319         if (!msg) {
 320                 fprintf(stderr, "Failed to create data for the frame.\n");
 321                 return;
 322         }
 323
 324         /* copy the data */
 325         dest = msgb_put(msg, len);
 326         memcpy(dest, data, len);
 327
 328         sercomm_sendmsg(dlci, msg);
 329
 330         /* drain the queue: TODO: do this through the select */
 331         while (sercomm_drv_pull(&c) != 0)
 332                 if (write(dnload.serial_fd.fd, &c, 1) != 1)
 333                         perror("short write");
 334 }
 335
 336 static void hdlc_console_cb(uint8_t dlci, struct msgb *msg)
 337 {
 338         write(1, msg->data, msg->len);
 339         msgb_free(msg);
 340 }
 341
 342 static void hdlc_l1a_cb(uint8_t dlci, struct msgb *msg)
 343 {
 344         struct layer2_connection *con;
 345         u_int16_t *len;
 346
 347         len = (u_int16_t *) msgb_push(msg, 2);
 348         *len = htons(msg->len - sizeof(*len));
 349
 350         llist_for_each_entry(con, &connections, entry) {
 351                 if (write(con->fd.fd, msg->data, msg->len) != msg->len) {
 352                         fprintf(stderr, "Failed to write msg to the socket..\n");
 353                         continue;
 354                 }
 355         }
 356
 357         msgb_free(msg);
 358 }
 359
 360 static void print_hdlc(uint8_t *buffer, int length)
 361 {
 362         int i;
 363
 364         for (i = 0; i < length; ++i)
 365                 if (sercomm_drv_rx_char(buffer[i]) == 0)
 366                         printf("Dropping sample '%c'\n", buffer[i]);
 367 }
 368
 369 static int handle_read(void)
 370 {
 371         int rc, nbytes, buf_left;
 372
 373         buf_left = sizeof(buffer) - (bufptr - buffer);
 374         if (buf_left <= 0) {
 375                 memmove(buffer, buffer+1, sizeof(buffer)-1);
 376                 bufptr -= 1;
 377                 buf_left = 1;
 378         }
 379
 380         nbytes = read(dnload.serial_fd.fd, bufptr, buf_left);
 381         if (nbytes <= 0)
 382                 return nbytes;
 383
 384         if (!dnload.print_hdlc) {
 385                 printf("got %i bytes from modem, ", nbytes);
 386                 printf("data looks like: ");
 387                 hexdump(bufptr, nbytes);
 388         } else {
 389                 print_hdlc(bufptr, nbytes);
 390         }
 391
 392         if (!memcmp(buffer, phone_prompt1, sizeof(phone_prompt1))) {
 393                 printf("Received PROMPT1 from phone, responding with CMD\n");
 394                 dnload.print_hdlc = 0;
 395                 dnload.state = WAITING_PROMPT2;
 396                 rc = write(dnload.serial_fd.fd, dnload_cmd, sizeof(dnload_cmd));
 397
 398                 /* re-read file */
 399                 rc = read_file(dnload.filename);
 400                 if (rc < 0) {
 401                         fprintf(stderr, "read_file(%s) failed with %d\n", dnload.filename, rc);
 402                         exit(1);
 403                 }
 404         } else if (!memcmp(buffer, phone_prompt2, sizeof(phone_prompt2))) {
 405                 printf("Received PROMPT2 from phone, starting download\n");
 406                 dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
 407                 dnload.state = DOWNLOADING;
 408         } else if (!memcmp(buffer, phone_ack, sizeof(phone_ack))) {
 409                 printf("Received DOWNLOAD ACK from phone, your code is running now!\n");
 410                 dnload.serial_fd.when = BSC_FD_READ;
 411                 dnload.state = WAITING_PROMPT1;
 412                 dnload.write_ptr = dnload.data;
 413                 dnload.print_hdlc = 1;
 414         } else if (!memcmp(buffer, phone_nack, sizeof(phone_nack))) {
 415                 printf("Received DOWNLOAD NACK from phone, something went wrong :(\n");
 416                 dnload.serial_fd.when = BSC_FD_READ;
 417                 dnload.state = WAITING_PROMPT1;
 418                 dnload.write_ptr = dnload.data;
 419         } else if (!memcmp(buffer, phone_nack_magic, sizeof(phone_nack_magic))) {
 420                 printf("Received MAGIC NACK from phone, you need to have \"1003\" at 0x803ce0\n");
 421                 dnload.serial_fd.when = BSC_FD_READ;
 422                 dnload.state = WAITING_PROMPT1;
 423                 dnload.write_ptr = dnload.data;
 424         } else if (!memcmp(buffer, ftmtool, sizeof(ftmtool))) {
 425                 printf("Received FTMTOOL from phone, ramolader has aborted\n");
 426                 dnload.serial_fd.when = BSC_FD_READ;
 427                 dnload.state = WAITING_PROMPT1;
 428                 dnload.write_ptr = dnload.data;
 429         }
 430         bufptr += nbytes;
 431
 432         return nbytes;
 433 }
 434
 435 static int serial_read(struct bsc_fd *fd, unsigned int flags)
 436 {
 437         int rc;
 438
 439         if (flags & BSC_FD_READ) {
 440                 rc = handle_read();
 441                 if (rc == 0)
 442                         exit(2);
 443         }
 444
 445         if (flags & BSC_FD_WRITE) {
 446                 rc = handle_write();
 447                 if (rc == 1)
 448                         dnload.state = WAITING_PROMPT1;
 449         }
 450         return 0;
 451 }
 452
 453 static int parse_mode(const char *arg)
 454 {
 455         if (!strcasecmp(arg, "c123") ||
 456             !strcasecmp(arg, "c140"))
 457                 return MODE_C123;
 458         else if (!strcasecmp(arg, "c123xor"))
 459                 return MODE_C123xor;
 460         else if (!strcasecmp(arg, "c155"))
 461                 return MODE_C155;
 462
 463         return -1;
 464 }
 465
 466
 467 static int usage(const char *name)
 468 {
 469         printf("\nUsage: %s [ -v | -h ] [ -p /dev/ttyXXXX ] [ -s /tmp/osmocom_l2 ] ][ -m {c123,c123xor,c155} ] file.bin\n", name);
 470         printf("\t* Open serial port /dev/ttyXXXX (connected to your phone)\n"
 471                 "\t* Perform handshaking with the ramloader in the phone\n"
 472                 "\t* Download file.bin to the attached phone (base address 0x00800100)\n");
 473         exit(2);
 474 }
 475
 476 static int version(const char *name)
 477 {
 478         //printf("\n%s version %s\n", name, VERSION);
 479         exit(2);
 480 }
 481
 482 static int un_layer2_read(struct bsc_fd *fd, unsigned int flags)
 483 {
 484         int rc;
 485         u_int16_t length = 0xffff;
 486         u_int8_t buf[4096];
 487         struct layer2_connection *con;
 488
 489
 490         rc = read(fd->fd, &length, sizeof(length));
 491         if (rc <= 0 || ntohs(length) > 512) {
 492                 fprintf(stderr, "Unexpected result from socket. rc: %d len: %d\n",
 493                         rc, ntohs(length));
 494                 goto close;
 495         }
 496
 497         rc = read(fd->fd, buf, ntohs(length));
 498         if (rc != ntohs(length)) {
 499                 fprintf(stderr, "Could not read data.\n");
 500                 goto close;
 501         }
 502
 503         hdlc_send_to_phone(SC_DLCI_L1A_L23, buf, ntohs(length));
 504
 505         return 0;
 506 close:
 507         con = (struct layer2_connection *) fd->data;
 508
 509         close(fd->fd);
 510         bsc_unregister_fd(fd);
 511         llist_del(&con->entry);
 512         talloc_free(con);
 513         return -1;
 514 }
 515
 516 /* accept a new connection */
 517 static int un_layer2_accept(struct bsc_fd *fd, unsigned int flags)
 518 {
 519         struct layer2_connection *con;
 520         struct sockaddr_un un_addr;
 521         socklen_t len;
 522         int rc;
 523
 524         len = sizeof(un_addr);
 525         rc = accept(fd->fd, (struct sockaddr *) &un_addr, &len);
 526         if (rc < 0) {
 527                 fprintf(stderr, "Failed to accept a new connection.\n");
 528                 return -1;
 529         }
 530
 531         con = talloc_zero(NULL, struct layer2_connection);
 532         if (!con) {
 533                 fprintf(stderr, "Failed to create layer2 connection.\n");
 534                 return -1;
 535         }
 536
 537         con->fd.fd = rc;
 538         con->fd.when = BSC_FD_READ;
 539         con->fd.cb = un_layer2_read;
 540         con->fd.data = con;
 541         if (bsc_register_fd(&con->fd) != 0) {
 542                 fprintf(stderr, "Failed to register the fd.\n");
 543                 return -1;
 544         }
 545
 546         llist_add(&con->entry, &connections);
 547         return 0;
 548 }
 549
 550 /*
 551  * Create a server socket for the layer2 stack
 552  */
 553 static int register_af_unix(const char *un_path)
 554 {
 555         struct sockaddr_un local;
 556         int rc;
 557
 558         dnload.socket.fd = socket(AF_UNIX, SOCK_STREAM, 0);
 559
 560         if (dnload.socket.fd < 0) {
 561                 fprintf(stderr, "Failed to create Unix Domain Socket.\n");
 562                 return -1;
 563         }
 564
 565         local.sun_family = AF_UNIX;
 566         strncpy(local.sun_path, un_path, sizeof(local.sun_path));
 567         local.sun_path[sizeof(local.sun_path) - 1] = '\0';
 568         unlink(local.sun_path);
 569         rc = bind(dnload.socket.fd, (struct sockaddr *) &local,
 570                   sizeof(local.sun_family) + strlen(local.sun_path));
 571         if (rc != 0) {
 572                 fprintf(stderr, "Failed to bind the unix domain socket. '%s'\n",
 573                         local.sun_path);
 574                 return -1;
 575         }
 576
 577         if (listen(dnload.socket.fd, 0) != 0) {
 578                 fprintf(stderr, "Failed to listen.\n");
 579                 return -1;
 580         }
 581
 582         dnload.socket.when = BSC_FD_READ;
 583         dnload.socket.cb = un_layer2_accept;
 584
 585         if (bsc_register_fd(&dnload.socket) != 0) {
 586                 fprintf(stderr, "Failed to register the bfd.\n");
 587                 return -1;
 588         }
 589
 590         return 0;
 591 }
 592
 593 extern void hdlc_tpudbg_cb(uint8_t dlci, struct msgb *msg);
 594
 595 int main(int argc, char **argv)
 596 {
 597         int opt, flags;
 598         char *serial_dev = "/dev/ttyUSB1";
 599         char *un_path = "/tmp/osmocom_l2";
 600
 601         dnload.mode = MODE_C123;
 602
 603         while ((opt = getopt(argc, argv, "hp:m:s:v")) != -1) {
 604                 switch (opt) {
 605                 case 'p':
 606                         serial_dev = optarg;
 607                         break;
 608                 case 'm':
 609                         dnload.mode = parse_mode(optarg);
 610                         if (dnload.mode < 0)
 611                                 usage(argv[0]);
 612                         break;
 613                 case 's':
 614                         un_path = optarg;
 615                         break;
 616                 case 'v':
 617                         version(argv[0]);
 618                         break;
 619                 case 'h':
 620                 default:
 621                         usage(argv[0]);
 622                         break;
 623                 }
 624         }
 625
 626         if (argc <= optind) {
 627                 fprintf(stderr, "You have to specify the filename\n");
 628                 usage(argv[0]);
 629         }
 630
 631         dnload.filename = argv[optind];
 632
 633         dnload.serial_fd.fd = serial_init(serial_dev);
 634         if (dnload.serial_fd.fd < 0) {
 635                 fprintf(stderr, "Cannot open serial device %s\n", serial_dev);
 636                 exit(1);
 637         }
 638
 639         if (bsc_register_fd(&dnload.serial_fd) != 0) {
 640                 fprintf(stderr, "Failed to register the serial.\n");
 641                 exit(1);
 642         }
 643
 644         /* Set serial socket to non-blocking mode of operation */
 645         flags = fcntl(dnload.serial_fd.fd, F_GETFL);
 646         flags |= O_NONBLOCK;
 647         fcntl(dnload.serial_fd.fd, F_SETFL, flags);
 648
 649         dnload.serial_fd.when = BSC_FD_READ;
 650         dnload.serial_fd.cb = serial_read;
 651
 652         /* unix domain socket handling */
 653         if (register_af_unix(un_path) != 0)
 654                 exit(1);
 655
 656
 657         /* initialize the HDLC layer */
 658         sercomm_init();
 659         sercomm_register_rx_cb(SC_DLCI_CONSOLE, hdlc_console_cb);
 660         sercomm_register_rx_cb(SC_DLCI_DEBUG, hdlc_tpudbg_cb);
 661         sercomm_register_rx_cb(SC_DLCI_L1A_L23, hdlc_l1a_cb);
 662         while (1)
 663                 bsc_select_main(0);
 664
 665         close(dnload.serial_fd.fd);
 666
 667         exit(0);
 668 }