#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; 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 #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