/******************************************************************************* 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 #include #include #include #include #include #include #include #include #include #include #include //mmap #include #include #include #include #include #include #include #include #include #ifndef SGI_UE_ETHER_TRAFFIC #include #endif #include #include #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; imalloc_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; isd[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 #include #include #include #include #include #include #include #include #include #include #include #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