/******************************************************************************* OpenAirInterface Copyright(c) 1999 - 2014 Eurecom OpenAirInterface is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. OpenAirInterface is distributed in the hope that 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 OpenAirInterface.The full GNU General Public License is included in this distribution in the file called "COPYING". If not, see <http://www.gnu.org/licenses/>. Contact Information OpenAirInterface Admin: openair_admin@eurecom.fr OpenAirInterface Tech : openair_tech@eurecom.fr OpenAirInterface Dev : openair4g-devel@eurecom.fr Address : Eurecom, Compus SophiaTech 450, route des chappes, 06451 Biot, France. *******************************************************************************/ /*! \file udp_primitives_server.c * \brief * \author Sebastien ROUX, Lionel Gauthier * \company Eurecom * \email: lionel.gauthier@eurecom.fr */ #include <stdio.h> #include <stdlib.h> #include <string.h> #include <unistd.h> #include <errno.h> #include <sys/time.h> #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <fcntl.h> #include <pthread.h> #include "intertask_interface.h" #include "udp_primitives_server.h" #include "assertions.h" #include "conversions.h" #include "msc.h" #define UDP_DEBUG(x, args...) do { fprintf(stdout, "[UDP] [D]"x, ##args); } while(0) #define UDP_ERROR(x, args...) do { fprintf(stderr, "[UDP] [E]"x, ##args); } while(0) //void *udp_receiver_thread(void *args_p); struct udp_socket_desc_s { uint8_t buffer[4096]; int sd; /* Socket descriptor to use */ pthread_t listener_thread; /* Thread affected to recv */ char *local_address; /* Local ipv4 address to use */ uint16_t local_port; /* Local port to use */ task_id_t task_id; /* Task who has requested the new endpoint */ STAILQ_ENTRY(udp_socket_desc_s) entries; }; static STAILQ_HEAD(udp_socket_list_s, udp_socket_desc_s) udp_socket_list; static pthread_mutex_t udp_socket_list_mutex = PTHREAD_MUTEX_INITIALIZER; //----------------------------------------------------------------------------- void udp_print_hex_octets(unsigned char* dataP, unsigned long sizeP) //----------------------------------------------------------------------------- { unsigned long octet_index = 0; unsigned long buffer_marker = 0; unsigned char aindex; #define UDP_2_PRINT_BUFFER_LEN 8000 char udp_2_print_buffer[UDP_2_PRINT_BUFFER_LEN]; struct timeval tv; struct timezone tz; char timeofday[64]; unsigned int h,m,s; if (dataP == NULL) { return; } gettimeofday(&tv, &tz); h = tv.tv_sec/3600/24; m = (tv.tv_sec / 60) % 60; s = tv.tv_sec % 60; snprintf(timeofday, 64, "%02d:%02d:%02d.%06d", h,m,s,tv.tv_usec); UDP_DEBUG("%s------+-------------------------------------------------|\n",timeofday); UDP_DEBUG("%s | 0 1 2 3 4 5 6 7 8 9 a b c d e f |\n",timeofday); UDP_DEBUG("%s------+-------------------------------------------------|\n",timeofday); for (octet_index = 0; octet_index < sizeP; octet_index++) { if (UDP_2_PRINT_BUFFER_LEN < (buffer_marker + 32)) { buffer_marker+=snprintf(&udp_2_print_buffer[buffer_marker], UDP_2_PRINT_BUFFER_LEN - buffer_marker, "... (print buffer overflow)"); UDP_DEBUG("%s%s",timeofday,udp_2_print_buffer); return; } if ((octet_index % 16) == 0) { if (octet_index != 0) { buffer_marker+=snprintf(&udp_2_print_buffer[buffer_marker], UDP_2_PRINT_BUFFER_LEN - buffer_marker, " |\n"); UDP_DEBUG("%s%s",timeofday, udp_2_print_buffer); buffer_marker = 0; } buffer_marker+=snprintf(&udp_2_print_buffer[buffer_marker], UDP_2_PRINT_BUFFER_LEN - buffer_marker, " %04ld |", octet_index); } /* * Print every single octet in hexadecimal form */ buffer_marker+=snprintf(&udp_2_print_buffer[buffer_marker], UDP_2_PRINT_BUFFER_LEN - buffer_marker, " %02x", dataP[octet_index]); /* * Align newline and pipes according to the octets in groups of 2 */ } /* * Append enough spaces and put final pipe */ for (aindex = octet_index; aindex < 16; ++aindex) buffer_marker+=snprintf(&udp_2_print_buffer[buffer_marker], UDP_2_PRINT_BUFFER_LEN - buffer_marker, " "); //GTPU_DEBUG(" "); buffer_marker+=snprintf(&udp_2_print_buffer[buffer_marker], UDP_2_PRINT_BUFFER_LEN - buffer_marker, " |\n"); UDP_DEBUG("%s%s",timeofday,udp_2_print_buffer); } static void udp_server_receive_and_process(struct udp_socket_desc_s *udp_sock_pP); /* @brief Retrieve the descriptor associated with the task_id */ static struct udp_socket_desc_s *udp_server_get_socket_desc(task_id_t task_id) { struct udp_socket_desc_s *udp_sock_p = NULL; UDP_DEBUG("Looking for task %d\n", task_id); STAILQ_FOREACH(udp_sock_p, &udp_socket_list, entries) { if (udp_sock_p->task_id == task_id) { UDP_DEBUG("Found matching task desc\n"); break; } } return udp_sock_p; } static struct udp_socket_desc_s *udp_server_get_socket_desc_by_sd(int sdP) { struct udp_socket_desc_s *udp_sock_p = NULL; UDP_DEBUG("Looking for sd %d\n", sdP); STAILQ_FOREACH(udp_sock_p, &udp_socket_list, entries) { if (udp_sock_p->sd == sdP) { UDP_DEBUG("Found matching task desc\n"); break; } } return udp_sock_p; } static int udp_server_create_socket(int port, char *address, task_id_t task_id) { struct sockaddr_in addr; int sd; struct udp_socket_desc_s *socket_desc_p = NULL; UDP_DEBUG("Creating new listen socket on address "IPV4_ADDR" and port %u\n", IPV4_ADDR_FORMAT(inet_addr(address)), port); /* Create UDP socket */ if ((sd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) { /* Socket creation has failed... */ UDP_ERROR("Socket creation failed (%s)\n", strerror(errno)); return sd; } memset(&addr, 0, sizeof(struct sockaddr_in)); addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = inet_addr(address); if (bind(sd, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)) < 0) { /* Bind failed */ UDP_ERROR("Socket bind failed (%s) for address "IPV4_ADDR" and port %u\n", strerror(errno), IPV4_ADDR_FORMAT(inet_addr(address)), port); close(sd); return -1; } /* Add the socket to list of fd monitored by ITTI */ /* Mark the socket as non-blocking */ if (fcntl(sd, F_SETFL, O_NONBLOCK) < 0) { UDP_ERROR("fcntl F_SETFL O_NONBLOCK failed: %s\n", strerror(errno)); close(sd); return -1; } socket_desc_p = calloc(1, sizeof(struct udp_socket_desc_s)); DevAssert(socket_desc_p != NULL); socket_desc_p->sd = sd; socket_desc_p->local_address = address; socket_desc_p->local_port = port; socket_desc_p->task_id = task_id; UDP_DEBUG("Inserting new descriptor for task %d, sd %d\n", socket_desc_p->task_id, socket_desc_p->sd); pthread_mutex_lock(&udp_socket_list_mutex); STAILQ_INSERT_TAIL(&udp_socket_list, socket_desc_p, entries); pthread_mutex_unlock(&udp_socket_list_mutex); itti_subscribe_event_fd(TASK_UDP, sd); return sd; } static void udp_server_flush_sockets(struct epoll_event *events, int nb_events) { int event; struct udp_socket_desc_s *udp_sock_p = NULL; UDP_DEBUG("Received %d events\n", nb_events); for (event = 0; event < nb_events; event++) { if (events[event].events != 0) { /* If the event has not been yet been processed (not an itti message) */ pthread_mutex_lock(&udp_socket_list_mutex); udp_sock_p = udp_server_get_socket_desc_by_sd(events[event].data.fd); if (udp_sock_p != NULL) { udp_server_receive_and_process(udp_sock_p); } else { UDP_ERROR("Failed to retrieve the udp socket descriptor %d", events[event].data.fd); } pthread_mutex_unlock(&udp_socket_list_mutex); } } } static void udp_server_receive_and_process(struct udp_socket_desc_s *udp_sock_pP) { UDP_DEBUG("Inserting new descriptor for task %d, sd %d\n", udp_sock_pP->task_id, udp_sock_pP->sd); { int bytes_received = 0; socklen_t from_len; struct sockaddr_in addr; from_len = (socklen_t)sizeof(struct sockaddr_in); if ((bytes_received = recvfrom(udp_sock_pP->sd, udp_sock_pP->buffer, sizeof(udp_sock_pP->buffer), 0, (struct sockaddr *)&addr, &from_len)) <= 0) { UDP_ERROR("Recvfrom failed %s\n", strerror(errno)); //break; } else { MessageDef *message_p = NULL; udp_data_ind_t *udp_data_ind_p; uint8_t *forwarded_buffer = NULL; AssertFatal(sizeof(udp_sock_pP->buffer) >= bytes_received, "UDP BUFFER OVERFLOW"); forwarded_buffer = itti_malloc(TASK_UDP, udp_sock_pP->task_id, bytes_received); DevAssert(forwarded_buffer != NULL); memcpy(forwarded_buffer, udp_sock_pP->buffer, bytes_received); message_p = itti_alloc_new_message(TASK_UDP, UDP_DATA_IND); DevAssert(message_p != NULL); udp_data_ind_p = &message_p->ittiMsg.udp_data_ind; udp_data_ind_p->buffer = forwarded_buffer; udp_data_ind_p->buffer_length = bytes_received; udp_data_ind_p->peer_port = htons(addr.sin_port); udp_data_ind_p->peer_address = addr.sin_addr.s_addr; UDP_DEBUG("Msg of length %d received from %s:%u\n", bytes_received, inet_ntoa(addr.sin_addr), ntohs(addr.sin_port)); if (itti_send_msg_to_task(udp_sock_pP->task_id, INSTANCE_DEFAULT, message_p) < 0) { UDP_DEBUG("Failed to send message %d to task %d\n", UDP_DATA_IND, udp_sock_pP->task_id); //break; } } } //close(udp_sock_pP->sd); //udp_sock_pP->sd = -1; //pthread_mutex_lock(&udp_socket_list_mutex); //STAILQ_REMOVE(&udp_socket_list, udp_sock_pP, udp_socket_desc_s, entries); //pthread_mutex_unlock(&udp_socket_list_mutex); //return NULL; } static void *udp_intertask_interface(void *args_p) { int rc = 0; int nb_events = 0; struct epoll_event *events = NULL; itti_mark_task_ready(TASK_UDP); MSC_START_USE(); while(1) { MessageDef *received_message_p = NULL; itti_receive_msg(TASK_UDP, &received_message_p); if (received_message_p != NULL) { switch (ITTI_MSG_ID(received_message_p)) { case UDP_INIT: { udp_init_t *udp_init_p; udp_init_p = &received_message_p->ittiMsg.udp_init; rc = udp_server_create_socket( udp_init_p->port, udp_init_p->address, ITTI_MSG_ORIGIN_ID(received_message_p)); } break; case UDP_DATA_REQ: { int udp_sd = -1; ssize_t bytes_written; struct udp_socket_desc_s *udp_sock_p = NULL; udp_data_req_t *udp_data_req_p; struct sockaddr_in peer_addr; udp_data_req_p = &received_message_p->ittiMsg.udp_data_req; //UDP_DEBUG("-- UDP_DATA_REQ -----------------------------------------------------\n%s :\n", // __FUNCTION__); //udp_print_hex_octets(&udp_data_req_p->buffer[udp_data_req_p->buffer_offset], // udp_data_req_p->buffer_length); memset(&peer_addr, 0, sizeof(struct sockaddr_in)); peer_addr.sin_family = AF_INET; peer_addr.sin_port = htons(udp_data_req_p->peer_port); peer_addr.sin_addr.s_addr = udp_data_req_p->peer_address; pthread_mutex_lock(&udp_socket_list_mutex); udp_sock_p = udp_server_get_socket_desc(ITTI_MSG_ORIGIN_ID(received_message_p)); if (udp_sock_p == NULL) { UDP_ERROR("Failed to retrieve the udp socket descriptor " "associated with task %d\n", ITTI_MSG_ORIGIN_ID(received_message_p)); pthread_mutex_unlock(&udp_socket_list_mutex); if (udp_data_req_p->buffer) { itti_free(ITTI_MSG_ORIGIN_ID(received_message_p),udp_data_req_p->buffer); } goto on_error; } udp_sd = udp_sock_p->sd; pthread_mutex_unlock(&udp_socket_list_mutex); UDP_DEBUG("[%d] Sending message of size %u to "IPV4_ADDR" and port %u\n", udp_sd, udp_data_req_p->buffer_length, IPV4_ADDR_FORMAT(udp_data_req_p->peer_address), udp_data_req_p->peer_port); bytes_written = sendto(udp_sd, &udp_data_req_p->buffer[udp_data_req_p->buffer_offset], udp_data_req_p->buffer_length, 0, (struct sockaddr *)&peer_addr, sizeof(struct sockaddr_in)); itti_free(ITTI_MSG_ORIGIN_ID(received_message_p),udp_data_req_p->buffer); if (bytes_written != udp_data_req_p->buffer_length) { UDP_ERROR("There was an error while writing to socket " "(%d:%s)\n", errno, strerror(errno)); } } break; case TERMINATE_MESSAGE: { itti_exit_task(); } break; case MESSAGE_TEST: { } break; default: { UDP_DEBUG("Unkwnon message ID %d:%s\n", ITTI_MSG_ID(received_message_p), ITTI_MSG_NAME(received_message_p)); } break; } on_error: rc = itti_free(ITTI_MSG_ORIGIN_ID(received_message_p), received_message_p); AssertFatal(rc == EXIT_SUCCESS, "Failed to free memory (%d)!\n", rc); received_message_p = NULL; } nb_events = itti_get_events(TASK_UDP, &events); if ((nb_events > 0) && (events != NULL)) { /* Now handle notifications for other sockets */ udp_server_flush_sockets(events, nb_events); } } return NULL; } int udp_init(const mme_config_t *mme_config_p) { UDP_DEBUG("Initializing UDP task interface\n"); STAILQ_INIT(&udp_socket_list); if (itti_create_task(TASK_UDP, &udp_intertask_interface, NULL) < 0) { UDP_ERROR("udp pthread_create (%s)\n", strerror(errno)); return -1; } UDP_DEBUG("Initializing UDP task interface: DONE\n"); return 0; }