maki/openairinterface5g
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5015ea2b80a895aa50f2a5bbd384d2c99caea971
openairinterface5g/openair-cn/SGI/sgi_socket.c
T
Lionel Gauthier 91e6729d46 Licence 
git-svn-id: http://svn.eurecom.fr/openair4G/trunk@5169 818b1a75-f10b-46b9-bf7c-635c3b92a50f
2014-03-14 09:49:18 +00:00

812 lines
32 KiB
C
Raw Blame History

/*******************************************************************************
Eurecom OpenAirInterface core network
Copyright(c) 1999 - 2014 Eurecom
This program is free software; you can redistribute it and/or modify it
under the terms and conditions of the GNU General Public License,
version 2, as published by the Free Software Foundation.
This program is distributed in the hope it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
more details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
The full GNU General Public License is included in this distribution in
the file called "COPYING".
Contact Information
Openair Admin: openair_admin@eurecom.fr
Openair Tech : openair_tech@eurecom.fr
Forums : http://forums.eurecom.fsr/openairinterface
Address : EURECOM,
Campus SophiaTech,
450 Route des Chappes,
CS 50193
06904 Biot Sophia Antipolis cedex,
FRANCE
*******************************************************************************/
/*! \file sgi_socket.c
* \brief
* \author Lionel Gauthier
* \company Eurecom
* \email: lionel.gauthier@eurecom.fr
*/
#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;
hashtable_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 (hashtable_get(sgi_data_pP->addr_v4_mapping, src4_addr, (void**)&addr_mapping_p) != HASH_TABLE_OK) {
hash_rc = hashtable_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;
hashtable_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 = hashtable_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 (hashtable_get(sgi_data_pP->addr_v4_mapping, src4_addr, (void**)&addr_mapping_p) != HASH_TABLE_OK) {
hash_rc = hashtable_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;
hashtable_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 = hashtable_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_hashtable_get(sgi_data_pP->addr_v6_mapping, &src6_addr, sizeof(struct in6_addr), (void**)&addr_mapping_p) != HASH_TABLE_OK) {
hash_rc = hashtable_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_hashtable_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 = hashtable_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;
hashtable_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 (hashtable_get(sgi_data_pP->addr_v4_mapping, src4_addr, (void**)&addr_mapping_p) != HASH_TABLE_OK) {
hash_rc = hashtable_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;
hashtable_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 = hashtable_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_hashtable_get(sgi_data_pP->addr_v6_mapping, &src6_addr, sizeof(struct in6_addr), (void**)&addr_mapping_p) != HASH_TABLE_OK) {
hash_rc = hashtable_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_hashtable_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 = hashtable_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
Reference in New Issue View Git Blame Copy Permalink