transport.c

wimax BS simulation code,implemented under linux.

/*
 * This piece of code is totally free. If any pitfalls found, 
 * please feel free to contact me at [e-mail]
 * THANKS A LOT!
 */

#include <time.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <arpa/inet.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>

#include "types.h"
#include "transport.h"
#include "task.h"
#include "bitops.h"
#include "wxtimer.h"
#include "prov.h"
#include "util.h"
#include "wxsched.h"
#include "wxbuff.h"


const unsigned char BROADCAST_MAC[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};


int get_macaddr(const char* dev_name, int* ifindex, unsigned char mac_addr[])
{
	struct ifreq ifr;
	int s = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	int i;

	if (s == -1) return -1;

	strncpy(ifr.ifr_name, dev_name, IFNAMSIZ);
	if (ioctl(s, SIOCGIFINDEX, &ifr) == -1) return errno;
	*ifindex = ifr.ifr_ifindex;

	if (ioctl(s, SIOCGIFHWADDR, &ifr) == -1) return errno;
	for (i = 0; i < 6; ++i) mac_addr[i] = ifr.ifr_hwaddr.sa_data[i];

	return 0;
}
		
int eth_create(TRANSPORT* tp, int ifindex, const unsigned char src_mac[])
{
	assert(tp);

	tp->fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	tp->ifindex = ifindex;
	memcpy(tp->mac, src_mac, 6);

	return (tp->fd > 0) ? 0 : errno;
}

int eth_build_header(TRANSPORT* tp, const unsigned char* dst_mac, unsigned short type, unsigned char* buf)
{
	const unsigned char* p_dst_mac = dst_mac;

	if (!p_dst_mac) p_dst_mac = BROADCAST_MAC;

	/* prepare for data */
	memcpy(buf + 6, tp->mac, 6);
	memcpy(buf, p_dst_mac, 6);
	buf[12] = (type >> 8) & 0xff;
	buf[13] = type & 0xff;

	return 14;
}

int eth_send(TRANSPORT* tp, const unsigned char* dst_mac, unsigned char* buf, unsigned short len)
{
	struct sockaddr_ll sa;
	const unsigned char* p_dst_mac = dst_mac;

	if (!p_dst_mac) p_dst_mac = BROADCAST_MAC;

	sa.sll_family  = PF_PACKET;
	sa.sll_protocol = htons(ETH_P_IP);
	sa.sll_ifindex  = tp->ifindex;
	sa.sll_hatype  = 1; //ARPHRD_ETHER;
	sa.sll_pkttype  = PACKET_OTHERHOST;
	sa.sll_halen    = ETH_ALEN;

	sa.sll_addr[0]  = p_dst_mac[0];
	sa.sll_addr[1]  = p_dst_mac[1];
	sa.sll_addr[2]  = p_dst_mac[2];
	sa.sll_addr[3]  = p_dst_mac[3];
	sa.sll_addr[4]  = p_dst_mac[4];
	sa.sll_addr[5]  = p_dst_mac[5];
	sa.sll_addr[6]  = 0x00;
	sa.sll_addr[7]  = 0x00;

	if (sendto(tp->fd, buf, len, 0, (struct sockaddr*)&sa, sizeof(sa)) < 0) return errno;
	return 0;
}

int eth_recv(TRANSPORT* tp, ETH_FRM_FILTER* filter, unsigned char* data, unsigned short len)
{
	int bytes;
	unsigned char* buf = data;

	while (1)
	{
		if ((bytes = recvfrom(tp->fd, buf, len, 0, 0, 0)) == -1)
		{
			if (!filter) return -1;
			continue;
		}

		if (filter(tp, buf + 6, buf, (*(buf + 12) << 8) | *(buf + 13))) break;
	}

	return bytes;
}

int eth_filter(const TRANSPORT* tp, const unsigned char* src, const unsigned char* dst, unsigned short type)
{
	if (memcmp(tp->mac, dst, 6) == 0) return 1;
	if (memcmp(BROADCAST_MAC, dst, 6) == 0) return 1;
	return 0;
}

static TRANSPORT tx_tp;
static TRANSPORT rx_tp;


void do_phy_send(uint32_t arga, void *argb)
{
	struct list_head burst_hdr, *burst_ptr, *pos, *tmp;
	uint32_t flag = 0;

	pthread_mutex_lock(&g_bsinfo.mutex);
	g_bsinfo.frame = (g_bsinfo.frame + 1) & 0xffffff;
	pthread_mutex_unlock(&g_bsinfo.mutex);

	//printf("dlmap: %p, ulmap: %p\n", g_dlscheduqe.map, g_ulschedque.map);
	if ( ! test_and_clear_bit(SCHED_MAP_STANDBY, (volatile unsigned long *)&g_dlschedque.signal) ) {
		fprintf(stdout, "dlmap abscent!\n");
		flag = 1;
		goto clean;
	}
	if ( ! test_and_clear_bit(SCHED_MAP_STANDBY, (volatile unsigned long *)&g_ulschedque.signal) ) {
		fprintf(stdout, "ulmap abscnet!\n");
		flag = 1;
		goto clean;
	}

	fprintf(stdout, "dlmap size = %d\n", g_dlschedque.map->tolen);
	dumphex(g_dlschedque.map->beg, g_dlschedque.map->end - g_dlschedque.map->beg);
	g_dlschedque.map->beg -= 14;
	g_dlschedque.map->tolen += 14;
	eth_build_header(&tx_tp, 0, prov.bs_code, g_dlschedque.map->beg);
	eth_send(&tx_tp, 0, g_dlschedque.map->beg, g_dlschedque.map->tolen);

	fprintf(stdout, "ulmap size = %d\n", g_ulschedque.map->tolen);
	dumphex(g_ulschedque.map->beg, g_ulschedque.map->end - g_ulschedque.map->beg);
	g_ulschedque.map->beg -= 14;
	g_ulschedque.map->tolen += 14;
	eth_build_header(&tx_tp, 0, prov.bs_code, g_ulschedque.map->beg);
	eth_send(&tx_tp, 0, g_ulschedque.map->beg, g_ulschedque.map->tolen);

 clean:
	burst_ptr = ringq_dump(&tq_bst_tx);
	sem_post(&sem_dsched);
	sem_post(&sem_usched);

	if ( burst_ptr != NULL ) {
		list_add_tail(&burst_hdr, burst_ptr);

		list_for_each_safe(pos, tmp, &burst_hdr) {
			wxbuff_t *wxb;
			uint8_t *buffer;
			uint32_t len;
			
			wxb = list_entry(pos, wxbuff_t, senior);
			
			wxb->beg -= 14;
			wxb->len += 14;
			wxb->tolen += 14;
			eth_build_header(&tx_tp, 0, prov.bs_code, wxb->beg);
			
			if ( (buffer = copy_and_free_wxbuff(wxb, &len)) != NULL) {
				fprintf(stdout, "SEND: burst size = %d\n", len - 14);
				dumphex(buffer+14, len-14);
				
				if ( ! flag && buffer )
					eth_send(&tx_tp, 0, buffer, len);
				
				if ( buffer )
					free(buffer);
			}
		}
	}

	{
		time_t tm;
		time(&tm);
		fprintf(stdout, "do_phy_send @ jiffies:%d, %s", jiffies, ctime(&tm));
	}
	fprintf(stdout, "------ end of sending a frame ------\n\n");

	return;
}


void * do_task_phy_tx(void *arg)
{
	PROVISION* prov = (PROVISION*)arg;

	int ifindex;
	unsigned char src_mac[6];
	int ret;

	if ((ret = get_macaddr(prov->tx_dev, &ifindex, src_mac)) < 0) return (void*)ret;
	if ((ret = eth_create(&tx_tp, ifindex, src_mac)) < 0) return (void*)ret;

	start_timer(get_timer(WXTIMER_PERIODIC, prov->frm_dur, 0, NULL, do_phy_send));

	return 0;
}

int phy_eth_frame_filter(const TRANSPORT* tp, const unsigned char* src, const unsigned char* dst, unsigned short type)
{
	return type == prov.ms_code;
}


void* do_task_phy_rx(void* arg)
{
	PROVISION* prov = (PROVISION*)arg;
	wxbuff_t *wxb;
	int ifindex;
	unsigned char dst_mac[6];
	int ret;
	int bytes;

	if ((ret = get_macaddr(prov->rx_dev, &ifindex, dst_mac)) < 0) return (void*)ret;
	if ((ret = eth_create(&rx_tp, ifindex, dst_mac)) < 0) return (void*)ret;

	/* we have the assumption that each burst will only occupy one wxbuff,
	 * that is, never exceed 2k bytes.
	 */
        for ( ; ; ) {
		wxb = get_wxbuff(NULL);

		bytes = eth_recv(&rx_tp, phy_eth_frame_filter, wxb->beg, wxb->tail-wxb->beg);
		wxb->beg += 14;
		wxb->end += bytes;
		wxb->tolen = wxb->len = wxb->end - wxb->beg;
		
		fprintf(stdout, "RECV: burst size = %d\n", wxb->tolen);
		dumphex(wxb->beg, wxb->end-wxb->beg);
		
		//put_wxbuff(wxb);
		ringq_enque(&tq_bst_rx, &wxb->senior);
        }
	
	return NULL;
}