222 lines
6.7 KiB
C
222 lines
6.7 KiB
C
/*
|
|
* Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
|
|
* contributor license agreements. See the NOTICE file distributed with
|
|
* this work for additional information regarding copyright ownership.
|
|
* The OpenAirInterface Software Alliance licenses this file to You under
|
|
* the OAI Public License, Version 1.1 (the "License"); you may not use this file
|
|
* except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.openairinterface.org/?page_id=698
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*-------------------------------------------------------------------------------
|
|
* For more information about the OpenAirInterface (OAI) Software Alliance:
|
|
* contact@openairinterface.org
|
|
*/
|
|
|
|
#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);
|
|
}
|