Cedric Roux
e1d2667467
- Fix build issue of EPC - Enable event fd for EPC instead of pthread_cond - Added some new S1AP messages defintions git-svn-id: http://svn.eurecom.fr/openair4G/trunk@4506 818b1a75-f10b-46b9-bf7c-635c3b92a50f
314 lines
11 KiB
C
Executable File
314 lines
11 KiB
C
Executable File
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include <netinet/in.h>
|
|
#include <linux/netfilter.h> /* Defines verdicts (NF_ACCEPT, etc) */
|
|
#include <libnfnetlink/libnfnetlink.h>
|
|
#include <libnetfilter_queue/libnetfilter_queue.h>
|
|
#include <libnetfilter_queue/linux_nfnetlink_queue.h>
|
|
|
|
#include "mme_config.h"
|
|
#include "intertask_interface.h"
|
|
#include "sgi.h"
|
|
//-----------------------------------------------------------------------------
|
|
struct nfq_handle
|
|
{
|
|
struct nfnl_handle *nfnlh;
|
|
struct nfnl_subsys_handle *nfnlssh;
|
|
struct nfq_q_handle *qh_list;
|
|
};
|
|
|
|
struct nfq_q_handle
|
|
{
|
|
struct nfq_q_handle *next;
|
|
struct nfq_handle *h;
|
|
u_int16_t id;
|
|
|
|
nfq_callback *cb;
|
|
void *data;
|
|
};
|
|
|
|
struct nfq_data {
|
|
struct nfattr **data;
|
|
};
|
|
|
|
|
|
enum {
|
|
NFQ_XML_HW = (1 << 0), NFQ_XML_MARK = (1 << 1), NFQ_XML_DEV = (1 << 2), NFQ_XML_PHYSDEV = (1 << 3),
|
|
NFQ_XML_PAYLOAD = (1 << 4), NFQ_XML_TIME = (1 << 5), NFQ_XML_ALL = ~0U
|
|
};
|
|
//------------------------------------------------------
|
|
#define SNPRINTF_FAILURE(ret, rem, offset, len) \
|
|
do { \
|
|
if (ret < 0) \
|
|
return ret; \
|
|
len += ret; \
|
|
if (ret > rem) \
|
|
ret = rem; \
|
|
offset += ret; \
|
|
rem -= ret; \
|
|
} while (0)
|
|
|
|
//------------------------------------------------------
|
|
static int nfq_snprintf(char *buf, size_t rem, struct nfq_data *tb, int flags)
|
|
//------------------------------------------------------
|
|
{
|
|
struct nfqnl_msg_packet_hdr *ph;
|
|
struct nfqnl_msg_packet_hw *hwph;
|
|
u_int32_t mark, ifi;
|
|
int size, offset = 0, len = 0, ret;
|
|
unsigned char *data;
|
|
|
|
size = snprintf(buf + offset, rem, "<pkt>");
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
|
|
|
|
|
|
ph = nfq_get_msg_packet_hdr(tb);
|
|
if (ph) {
|
|
size = snprintf(buf + offset, rem,
|
|
"<hook> %u <id>%u",
|
|
ph->hook, ntohl(ph->packet_id));
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
|
|
hwph = nfq_get_packet_hw(tb);
|
|
if (hwph && (flags & NFQ_XML_HW)) {
|
|
int i, hlen = ntohs(hwph->hw_addrlen);
|
|
|
|
size = snprintf(buf + offset, rem, "<hw><proto>%04x"
|
|
"</proto>",
|
|
ntohs(ph->hw_protocol));
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
|
|
size = snprintf(buf + offset, rem, "<src>");
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
|
|
for (i=0; i<hlen; i++) {
|
|
size = snprintf(buf + offset, rem, "%02x",
|
|
hwph->hw_addr[i]);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
size = snprintf(buf + offset, rem, "</src></hw>");
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
} else if (flags & NFQ_XML_HW) {
|
|
size = snprintf(buf + offset, rem, "<hw><proto>%04x"
|
|
"</proto></hw>",
|
|
ntohs(ph->hw_protocol));
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
}
|
|
|
|
mark = nfq_get_nfmark(tb);
|
|
if (mark && (flags & NFQ_XML_MARK)) {
|
|
size = snprintf(buf + offset, rem, "<mark>%u</mark>", mark);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
ifi = nfq_get_indev(tb);
|
|
if (ifi && (flags & NFQ_XML_DEV)) {
|
|
size = snprintf(buf + offset, rem, "<indev>%u</indev>", ifi);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
ifi = nfq_get_outdev(tb);
|
|
if (ifi && (flags & NFQ_XML_DEV)) {
|
|
size = snprintf(buf + offset, rem, "<outdev>%u</outdev>", ifi);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
ifi = nfq_get_physindev(tb);
|
|
if (ifi && (flags & NFQ_XML_PHYSDEV)) {
|
|
size = snprintf(buf + offset, rem,
|
|
"<physindev>%u</physindev>", ifi);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
ifi = nfq_get_physoutdev(tb);
|
|
if (ifi && (flags & NFQ_XML_PHYSDEV)) {
|
|
size = snprintf(buf + offset, rem,
|
|
"<physoutdev>%u</physoutdev>", ifi);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
ret = nfq_get_payload(tb, &data);
|
|
if (ret >= 0 && (flags & NFQ_XML_PAYLOAD)) {
|
|
int i;
|
|
|
|
size = snprintf(buf + offset, rem, "<payload>");
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
|
|
for (i=0; i<ret; i++) {
|
|
size = snprintf(buf + offset, rem, "%02x",
|
|
data[i] & 0xff);
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
size = snprintf(buf + offset, rem, "</payload>");
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
}
|
|
|
|
size = snprintf(buf + offset, rem, "</pkt>");
|
|
SNPRINTF_FAILURE(size, rem, offset, len);
|
|
|
|
return len;
|
|
}
|
|
//------------------------------------------------------
|
|
static int sgi_nfqueue_callback(struct nfq_q_handle *myQueue, struct nfgenmsg *msg,
|
|
struct nfq_data *pkt, void *cbData)
|
|
//-----------------------------------------------------------------------------
|
|
{
|
|
uint32_t id = 0;
|
|
struct nfqnl_msg_packet_hdr *header;
|
|
struct nfqnl_msg_packet_hw *macAddr;
|
|
MessageDef *message_p;
|
|
Gtpv1uTunnelDataReq *data_req_p;
|
|
Teid_t local_teid;
|
|
u_int32_t verdict;
|
|
|
|
if ((header = nfq_get_msg_packet_hdr(pkt))) {
|
|
id = ntohl(header->packet_id);
|
|
SGI_IF_DEBUG("packet_id %u hw_protocol %u hook %u\n", ntohl(header->packet_id), ntohs(header->hw_protocol), header->hook);
|
|
}
|
|
|
|
// The HW address is only fetchable at certain hook points
|
|
// LG Warning HW address is src address
|
|
macAddr = nfq_get_packet_hw(pkt);
|
|
if (macAddr) {
|
|
SGI_IF_DEBUG("MAC len %u addr %02X:%02X:%02X:%02X:%02X:%02X\n",
|
|
ntohs(macAddr->hw_addrlen),
|
|
macAddr->hw_addr[0],
|
|
macAddr->hw_addr[1],
|
|
macAddr->hw_addr[2],
|
|
macAddr->hw_addr[3],
|
|
macAddr->hw_addr[4],
|
|
macAddr->hw_addr[5]);
|
|
} else {
|
|
SGI_IF_DEBUG("No MAC addr\n");
|
|
}
|
|
|
|
/*timeval tv;
|
|
if (!nfq_get_timestamp(pkt, &tv)) {
|
|
cout << "; tstamp " << tv.tv_sec << "." << tv.tv_usec;
|
|
} else {
|
|
cout << "; no tstamp";
|
|
}*/
|
|
|
|
//cout << "; mark " << nfq_get_nfmark(pkt);
|
|
// Print the payload; in copy meta mode, only headers will be included;
|
|
// in copy packet mode, whole packet will be returned.
|
|
char *pktData;
|
|
int len = nfq_get_payload(pkt, &pktData);
|
|
|
|
// Note that you can also get the physical devices
|
|
SGI_IF_DEBUG("pkt id %d recvd: indev %d outdev %d mark %d len %d\n",
|
|
id,
|
|
nfq_get_indev(pkt),
|
|
nfq_get_outdev(pkt),
|
|
nfq_get_nfmark(pkt),
|
|
len);
|
|
|
|
|
|
//sgi_print_hex_octets((unsigned char *)pktData, len);
|
|
|
|
message_p = itti_alloc_new_message(TASK_FW_IP, GTPV1U_TUNNEL_DATA_REQ);
|
|
if (message_p == NULL) {
|
|
return -1;
|
|
}
|
|
data_req_p = &message_p->ittiMsg.gtpv1uTunnelDataReq;
|
|
data_req_p->buffer = malloc(sizeof(uint8_t) * len);
|
|
if (data_req_p->buffer == NULL) {
|
|
SGI_IF_ERROR("Failed to allocate new buffer\n");
|
|
free(message_p);
|
|
return -1;
|
|
} else {
|
|
// MAY BE TO BE CHANGED
|
|
local_teid = nfq_get_nfmark(pkt);
|
|
|
|
// if packet is marked, it is a packet going into a EPS bearer
|
|
//if (local_teid != 0) {
|
|
if (1) {
|
|
data_req_p->local_S1u_teid = local_teid;
|
|
|
|
memcpy(data_req_p->buffer, pktData, len);
|
|
data_req_p->length = len;
|
|
|
|
if (itti_send_msg_to_task(TASK_GTPV1_U, INSTANCE_DEFAULT, message_p) < 0) {
|
|
SGI_IF_ERROR("Failed to send message to task\n");
|
|
free(message_p);
|
|
}
|
|
verdict = NF_STOLEN;
|
|
return nfq_set_verdict(myQueue, id, verdict, 0, NULL);
|
|
} else {
|
|
verdict = NF_ACCEPT;
|
|
return nfq_set_verdict(myQueue, id, verdict, len, pktData);
|
|
}
|
|
}
|
|
return 0;
|
|
// end sgi_nfqueue_callback
|
|
}
|
|
//-----------------------------------------------------------------------------
|
|
void* sgi_nf_fw_2_gtpv1u_thread(void *args_p)
|
|
//-----------------------------------------------------------------------------
|
|
{
|
|
struct nfq_handle *nfqHandle;
|
|
struct nfq_q_handle *myQueue;
|
|
struct nfnl_handle *netlinkHandle;
|
|
int fd, res;
|
|
volatile sgi_data_t *sgi_data_p;
|
|
|
|
sgi_data_p = (sgi_data_t*)args_p;
|
|
|
|
SGI_IF_DEBUG("RX thread on netfilter queue started, operating on Device %s iif %d\n", sgi_data_p->interface_name, sgi_data_p->interface_index);
|
|
|
|
// Get a queue connection handle from the module
|
|
if (!(nfqHandle = nfq_open())) {
|
|
SGI_IF_ERROR("Error in nfq_open()");
|
|
exit(-1);
|
|
}
|
|
|
|
// Unbind the handler from processing any IP packets
|
|
// Not totally sure why this is done, or if it's necessary...
|
|
if (nfq_unbind_pf(nfqHandle, PF_INET) < 0) {
|
|
SGI_IF_ERROR("Error in nfq_unbind_pf()");
|
|
exit(-1);
|
|
}
|
|
|
|
// Bind this handler to process IP packets...
|
|
if (nfq_bind_pf(nfqHandle, PF_INET) < 0) {
|
|
SGI_IF_ERROR("Error in nfq_bind_pf()");
|
|
exit(-1);
|
|
}
|
|
|
|
// Install a callback on queue 1
|
|
if (!(myQueue = nfq_create_queue(nfqHandle, 1, &sgi_nfqueue_callback, NULL))) {
|
|
SGI_IF_ERROR("Error in nfq_create_queue()");
|
|
exit(-1);
|
|
}
|
|
|
|
// Turn on packet copy mode
|
|
if (nfq_set_mode(myQueue, NFQNL_COPY_PACKET, 0xffff) < 0) {
|
|
SGI_IF_ERROR("Could not set packet copy mode");
|
|
exit(-1);
|
|
}
|
|
|
|
netlinkHandle = nfq_nfnlh(nfqHandle);
|
|
fd = nfnl_fd(netlinkHandle);
|
|
|
|
SGI_IF_DEBUG("Waiting for egress packets\n");
|
|
sgi_data_p->thread_started = 1;
|
|
while ((res = recv(fd, &sgi_data_p->recv_buffer[sizeof(struct ether_header)], SGI_BUFFER_RECV_LEN, 0)) && res >= 0) {
|
|
// a callback mechanism is used
|
|
// rather than just handling it directly here, but that
|
|
// seems to be the convention...
|
|
nfq_handle_packet(nfqHandle, &sgi_data_p->recv_buffer[sizeof(struct ether_header)], res);
|
|
// end while receiving traffic
|
|
}
|
|
|
|
nfq_destroy_queue(myQueue);
|
|
|
|
nfq_close(nfqHandle);
|
|
|
|
return NULL;
|
|
}
|