775 lines
31 KiB
C
775 lines
31 KiB
C
#include <pthread.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <assert.h>
|
|
#include <fcntl.h>
|
|
#include <stdlib.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/mman.h> //mmap
|
|
|
|
#include <asm/types.h>
|
|
#include <net/if.h>
|
|
#include <net/ethernet.h>
|
|
#include <linux/if_ether.h>
|
|
#include <net/if_arp.h>
|
|
#include <linux/if_packet.h>
|
|
#include <netinet/in.h>
|
|
#include <poll.h>
|
|
#include <arpa/inet.h>
|
|
#ifndef SGI_UE_ETHER_TRAFFIC
|
|
#include <sys/uio.h>
|
|
#endif
|
|
|
|
|
|
#include <netinet/ip6.h>
|
|
#include <netinet/ip.h>
|
|
#include "sgi.h"
|
|
#include "intertask_interface.h"
|
|
|
|
#ifdef ENABLE_USE_NETFILTER_FOR_SGI
|
|
#define SGI_SOCKET_RAW 1
|
|
#define SGI_SOCKET_BIND_TO_IF 1
|
|
#undef SGI_MARKING
|
|
#undef SGI_PACKET_RX_RING
|
|
#undef SGI_SOCKET_UDP
|
|
#else
|
|
#define SGI_SOCKET_RAW 1
|
|
#define SGI_SOCKET_BIND_TO_IF 1
|
|
#undef SGI_PACKET_RX_RING
|
|
#undef SGI_MARKING
|
|
#undef SGI_SOCKET_UDP
|
|
#endif
|
|
|
|
|
|
//static char sgi_command_buffer[256];
|
|
|
|
#define HW_ADDRESS "%02x:%02x:%02x:%02x:%02x:%02x"
|
|
#define HW_ADDRESS_FORMAT(iFR) \
|
|
(uint8_t)(iFR)[0], \
|
|
(uint8_t)(iFR)[1], \
|
|
(uint8_t)(iFR)[2], \
|
|
(uint8_t)(iFR)[3], \
|
|
(uint8_t)(iFR)[4], \
|
|
(uint8_t)(iFR)[5]
|
|
|
|
#define IPV4_ADDR "%u.%u.%u.%u"
|
|
#define IPV4_ADDR_FORMAT(aDDRESS) \
|
|
(uint8_t)((aDDRESS) & 0x000000ff), \
|
|
(uint8_t)(((aDDRESS) & 0x0000ff00) >> 8 ), \
|
|
(uint8_t)(((aDDRESS) & 0x00ff0000) >> 16), \
|
|
(uint8_t)(((aDDRESS) & 0xff000000) >> 24)
|
|
|
|
struct ipv6hdr {
|
|
__u8 priority:4,
|
|
version:4;
|
|
__u8 flow_lbl[3];
|
|
|
|
__be16 payload_len;
|
|
__u8 nexthdr;
|
|
__u8 hop_limit;
|
|
|
|
struct in6_addr saddr;
|
|
struct in6_addr daddr;
|
|
};
|
|
|
|
|
|
/// The number of frames in the ring
|
|
// This number is not set in stone. Nor are block_size, block_nr or frame_size
|
|
#define CONF_RING_FRAMES 128
|
|
|
|
/// Offset of data from start of frame
|
|
#define PKT_OFFSET (TPACKET_ALIGN(sizeof(struct tpacket_hdr)) + \
|
|
TPACKET_ALIGN(sizeof(struct sockaddr_ll)))
|
|
|
|
int sgi_create_sockets(sgi_data_t *sgi_data_p)
|
|
{
|
|
#ifdef SGI_SOCKET_BIND_TO_IF
|
|
struct sockaddr_ll socket_address;
|
|
char if_name[16];
|
|
#endif
|
|
#ifdef SGI_MARKING
|
|
int value = 0;
|
|
const int *val_p=&value;
|
|
#endif
|
|
#ifdef SGI_PACKET_RX_RING
|
|
struct tpacket_req tp;
|
|
#endif
|
|
uint32_t i = 0;
|
|
#ifndef SGI_UE_ETHER_TRAFFIC
|
|
FILE *fp = NULL;
|
|
char filename[128];
|
|
int rc = 0;
|
|
char *router_mac_addr = NULL;
|
|
int b = 0;
|
|
char addr_mac_byte[16];
|
|
#endif
|
|
|
|
if (sgi_data_p == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
SGI_IF_DEBUG("Creating sockets for sgi on interface %s\n", sgi_data_p->interface_name);
|
|
|
|
// qci = 0 is for the default bearers QOS, Non-GBR
|
|
/* Create a RAW IP socket and request for all internet IP traffic */
|
|
// work
|
|
//#define SGI_SOCKET_RAW
|
|
for (i = 0; i < SGI_MAX_EPS_BEARERS_PER_USER; i++) {
|
|
//sgi_data_p->sd[i] = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
|
|
// works also
|
|
sgi_data_p->sd[i] = socket(PF_PACKET, SOCK_RAW, htons(IPPROTO_RAW));
|
|
//sgi_data_p->sd[i] = socket(AF_INET, SOCK_RAW, ETH_P_IP);
|
|
//sgi_data_p->sd[i] = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_IP));
|
|
|
|
if (sgi_data_p->sd[i] < 0) {
|
|
SGI_IF_ERROR("Error during socket creation (%s:%d)\n",strerror(errno), errno);
|
|
goto error;
|
|
}
|
|
|
|
/* socket options, tell the kernel we provide the IP structure
|
|
int one = 1;
|
|
const int *val = &one;
|
|
if(setsockopt(sgi_data_p->sd[i], IPPROTO_IP, IP_HDRINCL, val, sizeof(one)) < 0)
|
|
{
|
|
SGI_IF_ERROR("Error during socket setsockopt IP_HDRINCL (%s:%d)\n", strerror(errno), errno);
|
|
goto error;
|
|
}*/
|
|
|
|
|
|
#ifdef SGI_MARKING
|
|
// setting socket option to use MARK value
|
|
//value = rab_id + SGI_MIN_EPS_BEARER_ID;
|
|
value = 5;
|
|
if (setsockopt (sgi_data_p->sd[i], SOL_SOCKET, SO_MARK, val_p, sizeof (value)) < 0)
|
|
{
|
|
SGI_IF_ERROR("error notifying kernel about MARK");
|
|
goto error;
|
|
}
|
|
SGI_IF_DEBUG("Created socket %d for rab_id %d (for any UE context)\n", sgi_data_p->sd[i], value);
|
|
#endif
|
|
|
|
#ifdef SGI_PACKET_RX_RING
|
|
// tell kernel to export data through mmap()ped ring
|
|
tp.tp_block_size = CONF_RING_FRAMES * getpagesize();
|
|
tp.tp_block_nr = 1;
|
|
tp.tp_frame_size = getpagesize();
|
|
tp.tp_frame_nr = CONF_RING_FRAMES;
|
|
|
|
if (setsockopt(sgi_data_p->sd[i], SOL_PACKET, PACKET_RX_RING, (void*) &tp, sizeof(tp))) {
|
|
SGI_IF_ERROR("setsockopt() ring\n");
|
|
goto error;
|
|
}
|
|
|
|
// open ring
|
|
sgi_data_p->sock_mmap_ring = mmap(0, tp.tp_block_size * tp.tp_block_nr, PROT_READ | PROT_WRITE, MAP_SHARED, sgi_data_p->sd[i], 0);
|
|
if (!sgi_data_p->sock_mmap_ring) {
|
|
SGI_IF_ERROR("Failed to mmap socket (%s:%d)\n", strerror(errno), errno);
|
|
goto error;
|
|
}
|
|
/* Setup our ringbuffer */
|
|
sgi_data_p->malloc_ring = malloc(tp.tp_frame_nr * sizeof(struct iovec));
|
|
for(i=0; i<tp.tp_frame_nr; i++) {
|
|
sgi_data_p->malloc_ring[i].iov_base=(void *)((long)sgi_data_p->sock_mmap_ring)+(i*tp.tp_frame_size);
|
|
sgi_data_p->malloc_ring[i].iov_len=tp.tp_frame_size;
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef SGI_SOCKET_BIND_TO_IF
|
|
sprintf(if_name, "%s.%d",sgi_data_p->interface_name,i+SGI_MIN_EPS_BEARER_ID);
|
|
|
|
memset(&socket_address, 0, sizeof(struct sockaddr_ll));
|
|
socket_address.sll_family = PF_PACKET;
|
|
|
|
//socket_address.sll_addr = ;// Filled when we want to tx
|
|
//socket_address.sll_halen = ;// Filled when we want to tx
|
|
//socket_address.sll_family ;// Filled when we want to tx
|
|
|
|
//socket_address.sll_hatype = ;// Filled when packet received
|
|
//socket_address.sll_pkttype = ;// Filled when packet received
|
|
socket_address.sll_ifindex = if_nametoindex(if_name);
|
|
socket_address.sll_protocol = htons(ETH_P_IP);
|
|
//socket_address.sll_protocol = htons(ETH_P_ALL);
|
|
|
|
// Now we can bind the socket to send the IP traffic
|
|
if (bind(sgi_data_p->sd[i], (struct sockaddr *)&socket_address, sizeof(struct sockaddr_ll)) < 0) {
|
|
SGI_IF_ERROR("Bind socket to %s (%s:%d)\n", if_name, strerror(errno), errno);
|
|
goto error;
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
#ifndef SGI_UE_ETHER_TRAFFIC
|
|
if (sprintf(filename, "/sys/class/net/%s/address", sgi_data_p->interface_name) <= 0 ) {
|
|
SGI_IF_ERROR("Build of file name /sys/class/net/sgi_if_name/address Failed (%s:%d)\n", strerror(errno), errno);
|
|
goto error;
|
|
}
|
|
fp = fopen(filename, "r");
|
|
if (fp) {
|
|
rc = fscanf(fp, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
|
|
&sgi_data_p->interface_hw_address[0],
|
|
&sgi_data_p->interface_hw_address[1],
|
|
&sgi_data_p->interface_hw_address[2],
|
|
&sgi_data_p->interface_hw_address[3],
|
|
&sgi_data_p->interface_hw_address[4],
|
|
&sgi_data_p->interface_hw_address[5]);
|
|
fclose(fp);
|
|
if (rc <= 0) {
|
|
SGI_IF_ERROR("ERROR reading %s\n" ,filename);
|
|
goto error;
|
|
}
|
|
} else {
|
|
SGI_IF_ERROR("Correct /sys/class/net/sgi_if_name file not found\n");
|
|
goto error;
|
|
}
|
|
memcpy((void*)(&sgi_data_p->eh.ether_shost[0]), (void*)(&sgi_data_p->interface_hw_address[0]), ETH_ALEN);
|
|
router_mac_addr = getenv("MAC_ROUTER");
|
|
if (router_mac_addr == NULL) {
|
|
SGI_IF_ERROR("ERROR getting ENV variable MAC_ROUTER, should be set by start script\n");
|
|
goto error;
|
|
}
|
|
for (b = 0 ; b < 6; b++) {
|
|
strncpy(addr_mac_byte, router_mac_addr+2*b, 2);
|
|
addr_mac_byte[2] = '\0';
|
|
// we can do (unsigned int*) because x86 processors are little-endian
|
|
// (done for suppressing a warning)
|
|
rc = sscanf ((const char*)addr_mac_byte, "%02x", (uint32_t*)&i);
|
|
if (rc < 1) {
|
|
SGI_IF_ERROR("Error in converting string MAC_ROUTER address %s in char array\n", router_mac_addr);
|
|
goto error;
|
|
}
|
|
sgi_data_p->eh.ether_dhost[b] = (i & 0x000000FF);
|
|
}
|
|
SGI_IF_DEBUG("Found MAC address for ROUTER : %02x:%02x:%02x:%02x:%02x:%02x\n",
|
|
NMACADDR(sgi_data_p->eh.ether_dhost));
|
|
|
|
#ifdef VLAN8021Q
|
|
sgi_data_p->eh.ether_vlan8021q.vlan_tpid = htons(0x8100);
|
|
sgi_data_p->eh.ether_vlan8021q.vlan_tci = htons(0x7000 | 0x0000 | 0x0000);
|
|
#endif
|
|
SGI_IF_DEBUG("Found MAC address for SGI IF %s: %02x:%02x:%02x:%02x:%02x:%02x\n",
|
|
sgi_data_p->interface_name, NMACADDR(sgi_data_p->eh.ether_shost));
|
|
|
|
#endif
|
|
return 0;
|
|
error:
|
|
SGI_IF_ERROR("ERROR (%s)\n", strerror(errno));
|
|
for (i=0; i<SGI_MAX_EPS_BEARERS_PER_USER; i++) {
|
|
if (sgi_data_p->sd[i] > 0) {
|
|
close(sgi_data_p->sd[i]);
|
|
}
|
|
sgi_data_p->sd[i] = -1;
|
|
}
|
|
return -1;
|
|
}
|
|
#ifdef SGI_UE_ETHER_TRAFFIC
|
|
int sgi_send_data(uint8_t *buffer_pP, uint32_t length, sgi_data_t *sgi_data_pP, Teid_t originating_sgw_S1u_teidP)
|
|
{
|
|
struct ether_header *eh_p = (struct ether_header *) buffer_pP;
|
|
struct iphdr *iph_p = NULL;
|
|
struct arphdr *arph_p = NULL;
|
|
struct ipv6hdr *ip6h_p = NULL;
|
|
sgi_teid_mapping_t *mapping_p = NULL;
|
|
sgi_addr_mapping_t *addr_mapping_p = NULL;
|
|
hashtbl_rc_t hash_rc;
|
|
struct in6_addr src6_addr; /* source address */
|
|
struct in6_addr *src6_addr_p; /* source address */
|
|
u_int32_t src4_addr;
|
|
|
|
if (buffer_pP == NULL) {
|
|
SGI_IF_ERROR("sgi_send_data: received bad parameter\n");
|
|
return -1;
|
|
}
|
|
|
|
// get IP version of this packet
|
|
switch (htons(eh_p->ether_type)) {
|
|
|
|
//*******************
|
|
case ETHERTYPE_IP:
|
|
//*******************
|
|
iph_p = (struct iphdr *) (buffer_pP + sizeof(struct ether_header));
|
|
// The entry should be here but all signalling tied with RRC procedures not finished so
|
|
// a data packet can arrive before the MODIFY_BEARER REQUEST
|
|
src4_addr = iph_p->saddr;
|
|
if (hashtbl_get(sgi_data_pP->addr_v4_mapping, src4_addr, (void**)&addr_mapping_p) != HASH_TABLE_OK) {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
if (hash_rc == HASH_TABLE_KEY_NOT_EXISTS) {
|
|
SGI_IF_ERROR("%s Error unknown context SGW teid %d\n", __FUNCTION__, originating_sgw_S1u_teidP);
|
|
return -1;
|
|
} else {
|
|
if (mapping_p->is_outgoing_ipv4_packet_seen == 0) {
|
|
mapping_p->hw_addrlen = ETH_ALEN; // TO DO
|
|
memcpy(mapping_p->ue_mac_addr, eh_p->ether_shost, ETH_ALEN);
|
|
mapping_p->is_outgoing_ipv4_packet_seen = 1;
|
|
mapping_p->in_add_captured.s_addr = src4_addr;
|
|
|
|
addr_mapping_p = calloc(1, sizeof(sgi_addr_mapping_t));
|
|
memcpy(addr_mapping_p->ue_mac_addr, eh_p->ether_shost, ETH_ALEN);
|
|
addr_mapping_p->is_outgoing_packet_seen = 1;
|
|
addr_mapping_p->enb_S1U_teid = mapping_p->enb_S1U_teid;
|
|
addr_mapping_p->sgw_S1U_teid = originating_sgw_S1u_teidP;
|
|
hashtbl_insert(sgi_data_pP->addr_v4_mapping, src4_addr, (void*)addr_mapping_p);
|
|
SGI_IF_DEBUG("%s ASSOCIATED %d.%d.%d.%d to MAC %02x:%02x:%02x:%02x:%02x:%02x teid %d\n",
|
|
__FUNCTION__, NIPADDR(src4_addr), NMACADDR(eh_p->ether_shost), originating_sgw_S1u_teidP);
|
|
} else {
|
|
SGI_IF_ERROR("Error IPv4 address already registered for teid %d\n", originating_sgw_S1u_teidP);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
}
|
|
break;
|
|
|
|
//*******************
|
|
case ETHERTYPE_ARP:
|
|
//*******************
|
|
arph_p = (struct arphdr *) (buffer_pP + sizeof(struct ether_header));
|
|
// The entry should be here but all signalling tied with RRC procedures not finished so
|
|
// a data packet can arrive before the MODIFY_BEARER REQUEST
|
|
memcpy(&src4_addr, &((unsigned char*)(&arph_p[1]))[ETH_ALEN], 4);
|
|
SGI_IF_ERROR("%s ARP OPCODE %s TARGET IP %d.%d.%d.%d\n", __FUNCTION__, sgi_arpopcode_2_str(ntohl(arph_p->ar_op)), NIPADDR(src4_addr));
|
|
if (hashtbl_get(sgi_data_pP->addr_v4_mapping, src4_addr, (void**)&addr_mapping_p) != HASH_TABLE_OK) {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
if (hash_rc == HASH_TABLE_KEY_NOT_EXISTS) {
|
|
SGI_IF_ERROR("%s Error unknown context SGW teid %d\n", __FUNCTION__, originating_sgw_S1u_teidP);
|
|
return -1;
|
|
} else {
|
|
if (mapping_p->is_outgoing_ipv4_packet_seen == 0) {
|
|
mapping_p->hw_addrlen = ETH_ALEN; // TO DO
|
|
memcpy(mapping_p->ue_mac_addr, eh_p->ether_shost, ETH_ALEN);
|
|
mapping_p->is_outgoing_ipv4_packet_seen = 1;
|
|
mapping_p->in_add_captured.s_addr = src4_addr;
|
|
|
|
addr_mapping_p = calloc(1, sizeof(sgi_addr_mapping_t));
|
|
memcpy(addr_mapping_p->ue_mac_addr, eh_p->ether_shost, ETH_ALEN);
|
|
addr_mapping_p->is_outgoing_packet_seen = 1;
|
|
addr_mapping_p->enb_S1U_teid = mapping_p->enb_S1U_teid;
|
|
addr_mapping_p->sgw_S1U_teid = originating_sgw_S1u_teidP;
|
|
hashtbl_insert(sgi_data_pP->addr_v4_mapping, src4_addr, (void*)addr_mapping_p);
|
|
SGI_IF_DEBUG("%s ASSOCIATED %d.%d.%d.%d to MAC %02x:%02x:%02x:%02x:%02x:%02x teid %d\n",
|
|
__FUNCTION__, NIPADDR(src4_addr), NMACADDR(eh_p->ether_shost), originating_sgw_S1u_teidP);
|
|
} else {
|
|
SGI_IF_ERROR("Error IPv4 address already registered for teid %d\n", originating_sgw_S1u_teidP);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
}
|
|
break;
|
|
|
|
|
|
//*******************
|
|
case ETHERTYPE_IPV6:
|
|
ip6h_p = (struct ipv6hdr *) (buffer_pP + sizeof(struct ether_header));
|
|
|
|
if (obj_hashtbl_get(sgi_data_pP->addr_v6_mapping, &src6_addr, sizeof(struct in6_addr), (void**)&addr_mapping_p) != HASH_TABLE_OK) {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
if (hash_rc == HASH_TABLE_KEY_NOT_EXISTS) {
|
|
SGI_IF_ERROR("%s Error unknown context SGW teid %d\n", __FUNCTION__, originating_sgw_S1u_teidP);
|
|
return -1;
|
|
} else {
|
|
|
|
if (mapping_p->is_outgoing_ipv6_packet_seen < MAX_DEFINED_IPV6_ADDRESSES_PER_UE) {
|
|
mapping_p->hw_addrlen = ETH_ALEN;
|
|
memcpy(mapping_p->ue_mac_addr, eh_p->ether_shost, ETH_ALEN);
|
|
|
|
memcpy(&mapping_p->in6_addr_captured[mapping_p->is_outgoing_ipv6_packet_seen].s6_addr, src6_addr.s6_addr, 16);
|
|
mapping_p->is_outgoing_ipv6_packet_seen +=1;
|
|
|
|
addr_mapping_p = calloc(1, sizeof(sgi_addr_mapping_t));
|
|
memcpy(addr_mapping_p->ue_mac_addr, eh_p->ether_shost, ETH_ALEN);
|
|
addr_mapping_p->is_outgoing_packet_seen = 1;
|
|
addr_mapping_p->enb_S1U_teid = mapping_p->enb_S1U_teid;
|
|
addr_mapping_p->sgw_S1U_teid = originating_sgw_S1u_teidP;
|
|
|
|
src6_addr_p = malloc(sizeof(struct in6_addr));
|
|
if (src6_addr_p == NULL) {
|
|
return -1;
|
|
}
|
|
memcpy(src6_addr_p->s6_addr, ip6h_p->saddr.s6_addr, 16);
|
|
obj_hashtbl_insert(sgi_data_pP->addr_v6_mapping, src6_addr_p,sizeof(struct in6_addr), (void*)addr_mapping_p);
|
|
} else {
|
|
SGI_IF_ERROR("Error TOO MANY IPv6 address already registered for teid %d\n", originating_sgw_S1u_teidP);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
SGI_IF_WARNING("%s UNHANDLED ETHERTYPE: %X\n", __FUNCTION__, htons(eh_p->ether_type));
|
|
break;
|
|
}
|
|
|
|
assert(mapping_p != NULL);
|
|
if (send(sgi_data_pP->sd[mapping_p->eps_bearer_id - SGI_MIN_EPS_BEARER_ID], (void *)buffer_pP, length, 0) < 0) {
|
|
SGI_IF_ERROR("Error during send to socket %d bearer id %d : (%s:%d)\n",
|
|
sgi_data_pP->sd[mapping_p->eps_bearer_id - SGI_MIN_EPS_BEARER_ID],
|
|
mapping_p->eps_bearer_id,
|
|
strerror(errno),
|
|
errno);
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
#else
|
|
int sgi_send_data(uint8_t *buffer_pP, uint32_t length, sgi_data_t *sgi_data_pP, Teid_t originating_sgw_S1u_teidP)
|
|
{
|
|
struct iphdr *iph_p = (struct iphdr *) buffer_pP;
|
|
struct ipv6hdr *ip6h_p = (struct ipv6hdr *) buffer_pP;
|
|
sgi_teid_mapping_t *mapping_p = NULL;
|
|
sgi_addr_mapping_t *addr_mapping_p = NULL;
|
|
hashtbl_rc_t hash_rc;
|
|
struct in6_addr src6_addr; /* source address */
|
|
struct in6_addr *src6_addr_p; /* source address */
|
|
u_int32_t src4_addr;
|
|
struct iovec iov[2];
|
|
|
|
if (buffer_pP == NULL) {
|
|
SGI_IF_ERROR("sgi_send_data: received bad parameter\n");
|
|
return -1;
|
|
}
|
|
|
|
// get IP version of this packet
|
|
switch (iph_p->version) {
|
|
|
|
//*******************
|
|
case 4:
|
|
//*******************
|
|
// The entry should be here but all signalling tied with RRC procedures not finished so
|
|
// a data packet can arrive before the MODIFY_BEARER REQUEST
|
|
sgi_data_pP->eh.ether_type = htons(ETHERTYPE_IP);
|
|
src4_addr = iph_p->saddr;
|
|
if (hashtbl_get(sgi_data_pP->addr_v4_mapping, src4_addr, (void**)&addr_mapping_p) != HASH_TABLE_OK) {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
if (hash_rc == HASH_TABLE_KEY_NOT_EXISTS) {
|
|
SGI_IF_ERROR("%s Error unknown context SGW teid %d\n", __FUNCTION__, originating_sgw_S1u_teidP);
|
|
return -1;
|
|
} else {
|
|
if (mapping_p->is_outgoing_ipv4_packet_seen == 0) {
|
|
mapping_p->hw_addrlen = ETH_ALEN; // TO DO
|
|
mapping_p->is_outgoing_ipv4_packet_seen = 1;
|
|
mapping_p->in_add_captured.s_addr = src4_addr;
|
|
|
|
addr_mapping_p = calloc(1, sizeof(sgi_addr_mapping_t));
|
|
addr_mapping_p->is_outgoing_packet_seen = 1;
|
|
addr_mapping_p->enb_S1U_teid = mapping_p->enb_S1U_teid;
|
|
addr_mapping_p->sgw_S1U_teid = originating_sgw_S1u_teidP;
|
|
hashtbl_insert(sgi_data_pP->addr_v4_mapping, src4_addr, (void*)addr_mapping_p);
|
|
SGI_IF_DEBUG("%s ASSOCIATED %d.%d.%d.%d to teid %d\n",
|
|
__FUNCTION__, NIPADDR(src4_addr), originating_sgw_S1u_teidP);
|
|
} else {
|
|
SGI_IF_ERROR("Error IPv4 address already registered for teid %d\n", originating_sgw_S1u_teidP);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
}
|
|
break;
|
|
|
|
|
|
|
|
//*******************
|
|
case 6:
|
|
sgi_data_pP->eh.ether_type = htons(ETHERTYPE_IPV6);
|
|
if (obj_hashtbl_get(sgi_data_pP->addr_v6_mapping, &src6_addr, sizeof(struct in6_addr), (void**)&addr_mapping_p) != HASH_TABLE_OK) {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
if (hash_rc == HASH_TABLE_KEY_NOT_EXISTS) {
|
|
SGI_IF_ERROR("%s Error unknown context SGW teid %d\n", __FUNCTION__, originating_sgw_S1u_teidP);
|
|
return -1;
|
|
} else {
|
|
|
|
if (mapping_p->is_outgoing_ipv6_packet_seen < MAX_DEFINED_IPV6_ADDRESSES_PER_UE) {
|
|
mapping_p->hw_addrlen = 0;
|
|
|
|
memcpy(&mapping_p->in6_addr_captured[mapping_p->is_outgoing_ipv6_packet_seen].s6_addr, src6_addr.s6_addr, 16);
|
|
mapping_p->is_outgoing_ipv6_packet_seen +=1;
|
|
|
|
addr_mapping_p = calloc(1, sizeof(sgi_addr_mapping_t));
|
|
addr_mapping_p->is_outgoing_packet_seen = 1;
|
|
addr_mapping_p->enb_S1U_teid = mapping_p->enb_S1U_teid;
|
|
addr_mapping_p->sgw_S1U_teid = originating_sgw_S1u_teidP;
|
|
|
|
src6_addr_p = malloc(sizeof(struct in6_addr));
|
|
if (src6_addr_p == NULL) {
|
|
return -1;
|
|
}
|
|
memcpy(src6_addr_p->s6_addr, ip6h_p->saddr.s6_addr, 16);
|
|
obj_hashtbl_insert(sgi_data_pP->addr_v6_mapping, src6_addr_p,sizeof(struct in6_addr), (void*)addr_mapping_p);
|
|
} else {
|
|
SGI_IF_ERROR("Error TOO MANY IPv6 address already registered for teid %d\n", originating_sgw_S1u_teidP);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
hash_rc = hashtbl_get(sgi_data_pP->teid_mapping, originating_sgw_S1u_teidP, (void**)&mapping_p);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
SGI_IF_WARNING("%s UNHANDLED IP VERSION: %X\n", __FUNCTION__, iph_p->version);
|
|
break;
|
|
}
|
|
|
|
assert(mapping_p != NULL);
|
|
iov[0].iov_base = &sgi_data_pP->eh;
|
|
iov[0].iov_len = sizeof(sgi_data_pP->eh);
|
|
iov[1].iov_base = (void *)buffer_pP;
|
|
iov[1].iov_len = length;
|
|
#ifdef VLAN8021Q
|
|
sgi_data_pP->eh.ether_vlan8021q.vlan_tci = htons(0x7000 | 0x0000 | mapping_p->eps_bearer_id);
|
|
#endif
|
|
//sgi_print_hex_octets(iov[0].iov_base, iov[0].iov_len);
|
|
//sgi_print_hex_octets(iov[1].iov_base, iov[1].iov_len);
|
|
if (writev(sgi_data_pP->sd[mapping_p->eps_bearer_id - SGI_MIN_EPS_BEARER_ID], (const struct iovec *)&iov, 2) < 0) {
|
|
SGI_IF_ERROR("Error during send to socket %d bearer id %d : (%s:%d)\n",
|
|
sgi_data_pP->sd[mapping_p->eps_bearer_id - SGI_MIN_EPS_BEARER_ID],
|
|
mapping_p->eps_bearer_id,
|
|
strerror(errno),
|
|
errno);
|
|
return -1;
|
|
}
|
|
|
|
/* if (send(sgi_data_pP->sd[mapping_p->eps_bearer_id - SGI_MIN_EPS_BEARER_ID], (void *)buffer_pP, length, 0) < 0) {
|
|
SGI_IF_ERROR("Error during send to socket %d bearer id %d : (%s:%d)\n",
|
|
sgi_data_pP->sd[mapping_p->eps_bearer_id - SGI_MIN_EPS_BEARER_ID],
|
|
mapping_p->eps_bearer_id,
|
|
strerror(errno),
|
|
errno);
|
|
return -1;
|
|
}
|
|
*/
|
|
return 0;
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef SGI_TEST
|
|
#include <stdio.h>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <netdb.h>
|
|
#include <netinet/in.h>
|
|
#include <netinet/in_systm.h>
|
|
#include <netinet/ether.h>
|
|
#include <netinet/ip.h>
|
|
#include <netinet/ip_icmp.h>
|
|
#include <string.h>
|
|
#include <arpa/inet.h>
|
|
#include <sys/socket.h>
|
|
|
|
#define SGI_TEST_BUFF_LEN 500
|
|
unsigned char sgi_test_tx_buffer[SGI_TEST_BUFF_LEN];
|
|
|
|
|
|
void sgi_test_send_ping(sgi_data_t *sgi_dataP, uint32_t markP, uint64_t src_mac_addrP, uint64_t dst_mac_addrP, char* src_ip_addrP, char* dst_ip_addrP)
|
|
{
|
|
#ifdef SGI_PF_PACKET
|
|
struct ether_header *eh = (struct ether_header *) sgi_test_tx_buffer;
|
|
struct iphdr *iph = (struct iphdr *) (sgi_test_tx_buffer + sizeof(struct ether_header));
|
|
struct icmphdr *icmp = (struct icmphdr *) (sgi_test_tx_buffer + sizeof(struct ether_header) + sizeof(struct iphdr));
|
|
#else
|
|
struct iphdr *iph = (struct iphdr *) sgi_test_tx_buffer;
|
|
struct icmphdr *icmp = (struct icmphdr *) (sgi_test_tx_buffer + sizeof(struct iphdr));
|
|
#endif
|
|
struct hostent *hp, *hp2;
|
|
struct in_addr ip_dst;
|
|
struct in_addr ip_src;
|
|
|
|
|
|
|
|
#ifdef SGI_PF_PACKET
|
|
eh->ether_shost[0] = (u_int8_t)((src_mac_addrP & 0x0000FF0000000000) >> 40);
|
|
eh->ether_shost[1] = (u_int8_t)((src_mac_addrP & 0x000000FF00000000) >> 32);
|
|
eh->ether_shost[2] = (u_int8_t)((src_mac_addrP & 0x00000000FF000000) >> 24);
|
|
eh->ether_shost[3] = (u_int8_t)((src_mac_addrP & 0x0000000000FF0000) >> 16);
|
|
eh->ether_shost[4] = (u_int8_t)((src_mac_addrP & 0x000000000000FF00) >> 8);
|
|
eh->ether_shost[5] = (u_int8_t) (src_mac_addrP & 0x00000000000000FF);
|
|
|
|
eh->ether_dhost[0] = (u_int8_t)((dst_mac_addrP & 0x0000FF0000000000) >> 40);
|
|
eh->ether_dhost[1] = (u_int8_t)((dst_mac_addrP & 0x000000FF00000000) >> 32);
|
|
eh->ether_dhost[2] = (u_int8_t)((dst_mac_addrP & 0x00000000FF000000) >> 24);
|
|
eh->ether_dhost[3] = (u_int8_t)((dst_mac_addrP & 0x0000000000FF0000) >> 16);
|
|
eh->ether_dhost[4] = (u_int8_t)((dst_mac_addrP & 0x000000000000FF00) >> 8);
|
|
eh->ether_dhost[5] = (u_int8_t) (dst_mac_addrP & 0x00000000000000FF);
|
|
eh->ether_type = htons(ETH_P_IP);
|
|
#endif
|
|
if ((hp = gethostbyname(dst_ip_addrP)) == NULL)
|
|
{
|
|
SGI_IF_DEBUG("gethostbyname() is OK.\n");
|
|
if ((ip_dst.s_addr = inet_addr(dst_ip_addrP)) == -1)
|
|
{
|
|
fprintf(stderr, "%s: Can't resolve, unknown host.\n", dst_ip_addrP);
|
|
exit(1);
|
|
}
|
|
}
|
|
else
|
|
bcopy(hp->h_addr_list[0], &ip_dst.s_addr, hp->h_length);
|
|
/* The following source address just redundant for target to collect */
|
|
if ((hp2 = gethostbyname(src_ip_addrP)) == NULL)
|
|
{
|
|
SGI_IF_DEBUG("gethostbyname() is OK.\n");
|
|
if ((ip_src.s_addr = inet_addr(src_ip_addrP)) == -1)
|
|
{
|
|
fprintf(stderr, "%s: Can't resolve, unknown host\n", src_ip_addrP);
|
|
exit(1);
|
|
}
|
|
}
|
|
else
|
|
bcopy(hp2->h_addr_list[0], &ip_src.s_addr, hp->h_length);
|
|
printf("Sending to %08X from spoofed %08X\n", ip_dst.s_addr, ip_src.s_addr);
|
|
/* Ip structure, check the ip.h */
|
|
iph->version = 4;
|
|
iph->ihl = sizeof *iph >> 2;
|
|
iph->tos = 0;
|
|
iph->tot_len = htons(SGI_TEST_BUFF_LEN-sizeof(struct ether_header));
|
|
iph->id = htons(4321);
|
|
iph->frag_off = htons(0);
|
|
iph->ttl = 255;
|
|
iph->protocol = IPPROTO_ICMP;
|
|
iph->check = htons(0);
|
|
iph->saddr = ip_src.s_addr;
|
|
iph->daddr = ip_dst.s_addr;
|
|
|
|
icmp->type = ICMP_ECHO;
|
|
icmp->code = 0;
|
|
icmp->un.echo.id = 1000;
|
|
icmp->un.echo.sequence = 10;
|
|
/* Header checksum */
|
|
icmp->checksum = 0;
|
|
|
|
icmp-> checksum = in_cksum((unsigned short *)icmp, sizeof(struct icmphdr));
|
|
iph->check = in_cksum((unsigned short *)iph, sizeof(struct iphdr));
|
|
/* sending time */
|
|
if (send(sgi_dataP->sd[0], sgi_test_tx_buffer, SGI_TEST_BUFF_LEN, 0) < 0)
|
|
{
|
|
SGI_IF_ERROR("sendto() error (%s:%d)\n", strerror(errno), errno);
|
|
}
|
|
else
|
|
SGI_IF_DEBUG("sendto() is OK.\n");
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef ENABLE_USE_RAW_SOCKET_FOR_SGI
|
|
|
|
void sgi_sock_raw_cleanup_handler(void *args_p)
|
|
{
|
|
sgi_data_t *sgi_data_p;
|
|
int socket_index;
|
|
|
|
sgi_data_p = ((sgi_read_thread_args_t*)args_p)->sgi_data;
|
|
socket_index = ((sgi_read_thread_args_t*)args_p)->socket_index;
|
|
|
|
SGI_IF_DEBUG("Called %s\n", __FUNCTION__);
|
|
#ifdef SGI_PACKET_RX_RING
|
|
free(sgi_data_p->malloc_ring);
|
|
sgi_data_p->malloc_ring = NULL;
|
|
if (munmap(sgi_data_p->sock_mmap_ring, CONF_RING_FRAMES * getpagesize())) {
|
|
SGI_IF_ERROR("munmap\n");
|
|
} else {
|
|
sgi_data_p->sock_mmap_ring = NULL;
|
|
}
|
|
#endif
|
|
if (sgi_data_p->sd[socket_index] > 0) {
|
|
close(sgi_data_p->sd[socket_index]);
|
|
SGI_IF_DEBUG("Closed soket %d\n", sgi_data_p->sd[socket_index]);
|
|
}
|
|
sgi_data_p->sd[socket_index] = -1;
|
|
|
|
free(args_p);
|
|
}
|
|
|
|
void* sgi_sock_raw_fw_2_gtpv1u_thread(void* args_p)
|
|
{
|
|
sgi_data_t *sgi_data_p;
|
|
int socket_index;
|
|
#ifdef SGI_PACKET_RX_RING
|
|
struct pollfd pfd;
|
|
int i;
|
|
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
|
char *names[]={
|
|
"<", /* incoming */
|
|
"B", /* broadcast */
|
|
"M", /* multicast */
|
|
"P", /* promisc */
|
|
">", /* outgoing */
|
|
};
|
|
#else
|
|
int num_bytes;
|
|
#endif
|
|
sgi_data_p = ((sgi_read_thread_args_t*)args_p)->sgi_data;
|
|
socket_index = ((sgi_read_thread_args_t*)args_p)->socket_index;
|
|
|
|
SGI_IF_DEBUG("RX thread on raw socket started, operating on Device %s.%d\n", sgi_data_p->interface_name, socket_index+SGI_MIN_EPS_BEARER_ID);
|
|
|
|
|
|
pthread_cleanup_push(sgi_sock_raw_cleanup_handler, args_p);
|
|
|
|
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
|
|
|
|
pthread_mutex_lock (&sgi_data_p->thread_started_mutex);
|
|
sgi_data_p->thread_started += 1;
|
|
pthread_mutex_unlock (&sgi_data_p->thread_started_mutex);
|
|
|
|
#ifdef SGI_PACKET_RX_RING
|
|
i = 0;
|
|
while (1) {
|
|
while(*(unsigned long*)sgi_data_p->malloc_ring[i].iov_base) {
|
|
struct tpacket_hdr *h=sgi_data_p->malloc_ring[i].iov_base;
|
|
struct sockaddr_ll *sll=(void *)h + TPACKET_ALIGN(sizeof(*h));
|
|
unsigned char *bp=(unsigned char *)h + h->tp_mac;
|
|
|
|
SGI_IF_DEBUG("%u.%.6u: if%u %s %u bytes\n",
|
|
h->tp_sec, h->tp_usec,
|
|
sll->sll_ifindex,
|
|
names[sll->sll_pkttype],
|
|
h->tp_len);
|
|
|
|
sgi_process_raw_packet(sgi_data_p, bp, h->tp_len);
|
|
pthread_mutex_lock(&mutex); /* memory barrier */
|
|
/* tell the kernel this packet is done with */
|
|
h->tp_status=0;
|
|
pthread_mutex_unlock(&mutex); /* memory barrier */
|
|
|
|
i=(i==CONF_RING_FRAMES-1) ? 0 : i+1;
|
|
}
|
|
|
|
/* Sleep when nothings happen */
|
|
pfd.fd=sgi_data_p->sd;
|
|
pfd.events=POLLIN|POLLERR;
|
|
pfd.revents=0;
|
|
poll(&pfd, 1, -1);
|
|
}
|
|
#else
|
|
while (1) {
|
|
num_bytes = recvfrom(sgi_data_p->sd[socket_index], &sgi_data_p->recv_buffer[0][socket_index], SGI_BUFFER_RECV_LEN, 0, NULL, NULL);
|
|
if (num_bytes > 0) {
|
|
sgi_process_raw_packet(sgi_data_p, &sgi_data_p->recv_buffer[0][socket_index], num_bytes);
|
|
} else {
|
|
SGI_IF_DEBUG("recvfrom bearer id %d %d (%s:%d)\n", socket_index + SGI_MIN_EPS_BEARER_ID, num_bytes, strerror(errno), errno);
|
|
}
|
|
}
|
|
#endif
|
|
pthread_cleanup_pop(0);
|
|
pthread_exit(NULL);
|
|
}
|
|
#endif
|
|
|