mobipass.c 5.72 KB
Newer Older
Cedric Roux's avatar
Cedric Roux committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <net/if.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/ioctl.h>
#include <linux/if_packet.h>
#include <netinet/ether.h>
#include <unistd.h>
#include <pthread.h>

#include "queues.h"
#include "mobipass.h"

/******************************************************************/
/* time begin                                                     */
/******************************************************************/

#include <time.h>

static void init_time(mobipass_state_t *mobi)
{
  struct timespec now;
  if (clock_gettime(CLOCK_MONOTONIC_RAW, &now)) abort();
  mobi->t0 = (uint64_t)now.tv_sec * (uint64_t)1000000000 + (uint64_t)now.tv_nsec;
}

/* called before sending data to mobipass
 * waits if called too early with respect to system clock
 * does not wait more than 1ms in any case
 */
static void synch_time(mobipass_state_t *mobi, uint32_t ts)
{
  if (ts < mobi->synch_time_last_ts) mobi->synch_time_mega_ts++;
  mobi->synch_time_last_ts = ts;

  struct timespec now;
  if (clock_gettime(CLOCK_MONOTONIC_RAW, &now)) abort();

  uint64_t tnow;
  tnow = (uint64_t)now.tv_sec * (uint64_t)1000000000 + (uint64_t)now.tv_nsec;

  uint64_t cur = tnow - mobi->t0;

  /* 15360000 samples/second, in nanoseconds:
   *  = 15360000 / 1000000000 = 1536 / 100000 = 48 / 3125*/

  uint64_t ts_ns = ((uint64_t)ts + mobi->synch_time_mega_ts * (uint64_t)mobi->samples_per_1024_frames) * (uint64_t)3125 / (uint64_t)48;

  /* TODO: if cur is way higher than ts_ns, we are very late, log something? */
  if (cur >= ts_ns) return;

  uint64_t delta = ts_ns - cur;
  /* don't sleep more than 1 ms */
  if (delta > 1000*1000) delta = 1000*1000;
  delta = delta/1000;
  if (delta) usleep(delta);
}

/******************************************************************/
/* time end                                                       */
/******************************************************************/

struct ethernet_header {
  unsigned char dst[6];
  unsigned char src[6];
  uint16_t packet_type;
} __attribute__((__packed__));

struct mobipass_header {
  uint16_t flags;
  uint16_t fifo_status;
  unsigned char seqno;
  unsigned char ack;
  uint32_t word0;
  uint32_t timestamp;
} __attribute__((__packed__));

static void do_receive(mobipass_state_t *mobi, unsigned char *b)
{
  if (recv(mobi->sock, b, 14+14+1280, 0) != 14+14+1280) { perror("recv"); exit(1); }
  struct mobipass_header *mh = (struct mobipass_header *)(b+14);
  mh->timestamp = htonl((ntohl(mh->timestamp)-45378/*40120*/) % mobi->samples_per_1024_frames);
//printf("recv timestamp %u\n", ntohl(mh->timestamp));
}

/* receiver thread */
static void *receiver(void *_mobi)
{
  mobipass_state_t *mobi = _mobi;
  unsigned char receive_packet[14 + 14 + 1280];
  while (1) {
    do_receive(mobi, receive_packet);
    enqueue_from_mobipass(mobi->qstate, receive_packet);
  }
  return 0;
}

static void do_send(mobipass_state_t *mobi, int seqno, uint32_t ts,
    unsigned char *packet)
{
  struct ethernet_header *eh = (struct ethernet_header *)packet;
  struct mobipass_header *mh = (struct mobipass_header *)(packet+14);

  ts %= mobi->samples_per_1024_frames;
//printf("SEND seqno %d ts %d\n", seqno, ts);

  memcpy(eh->dst, mobi->eth_remote, 6);
  memcpy(eh->src, mobi->eth_local, 6);

  eh->packet_type = htons(0xbffe);

  mh->flags = 0;
  mh->fifo_status = 0;
  mh->seqno = seqno;
  mh->ack = 0;
  mh->word0 = 0;
  mh->timestamp = htonl(ts);

  synch_time(mobi, ts);

  if (send(mobi->sock, packet, 14+14+1280, 0) != 14+14+1280) { perror("send"); exit(1); }
}

/* sender thread */
static void *sender(void *_mobi)
{
  mobipass_state_t *mobi = _mobi;
  unsigned char packet[14 + 14 + 1280];
  uint32_t ts = 0;
  unsigned char seqno = 0;
  while (1) {
    dequeue_to_mobipass(mobi->qstate, ts, packet);
    do_send(mobi, seqno, ts, packet);
    seqno++;
    ts += 640;
    ts %= mobi->samples_per_1024_frames;
  }
  return 0;
}

static void new_thread(void *(*f)(void *), void *data)
{
  pthread_t t;
  pthread_attr_t att;

  if (pthread_attr_init(&att))
    { fprintf(stderr, "pthread_attr_init err\n"); exit(1); }
  if (pthread_attr_setdetachstate(&att, PTHREAD_CREATE_DETACHED))
    { fprintf(stderr, "pthread_attr_setdetachstate err\n"); exit(1); }
  if (pthread_attr_setstacksize(&att, 10000000))
    { fprintf(stderr, "pthread_attr_setstacksize err\n"); exit(1); }
  if (pthread_create(&t, &att, f, data))
    { fprintf(stderr, "pthread_create err\n"); exit(1); }
  if (pthread_attr_destroy(&att))
    { fprintf(stderr, "pthread_attr_destroy err\n"); exit(1); }
}

void init_mobipass(mobipass_state_t *mobi)
{
  int i;
  unsigned char data[14+14+640];
  memset(data, 0, 14+14+640);

  init_time(mobi);

  mobi->qstate = init_queues(mobi->samples_per_1024_frames);

  for (i = 0; i < 24*4; i++) {
    uint32_t timestamp = i*640;
    unsigned char seqno = i;
    struct mobipass_header *mh = (struct mobipass_header *)(data+14);
    mh->seqno = seqno;
    mh->timestamp = htonl(timestamp);
    enqueue_to_mobipass(mobi->qstate, data);
  }

  mobi->sock = socket(AF_PACKET, SOCK_RAW, IPPROTO_RAW);
  if (mobi->sock == -1) { perror("socket"); exit(1); }

  /* get if index */
  struct ifreq if_index;
  memset(&if_index, 0, sizeof(struct ifreq));
  strcpy(if_index.ifr_name, mobi->eth.if_name);
  if (ioctl(mobi->sock, SIOCGIFINDEX, &if_index)<0) {perror("SIOCGIFINDEX");exit(1);}

  struct sockaddr_ll local_addr;
  local_addr.sll_family   = AF_PACKET;
  local_addr.sll_ifindex  = if_index.ifr_ifindex;
  local_addr.sll_protocol = htons(0xbffe);
  local_addr.sll_halen    = ETH_ALEN;
  local_addr.sll_pkttype  = PACKET_OTHERHOST;

  if (bind(mobi->sock, (struct sockaddr *)&local_addr, sizeof(struct sockaddr_ll))<0)
    { perror("bind"); exit(1); }

  new_thread(receiver, mobi);
  new_thread(sender, mobi);
}