/*******************************************************************************
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_nf.c
* \brief
* \author Lionel Gauthier
* \company Eurecom
* \email: lionel.gauthier@eurecom.fr
*/
#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");
      itti_free(ITTI_MSG_ORIGIN_ID(message_p), message_p);
      message_p = NULL;
      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");
    	        itti_free(ITTI_MSG_ORIGIN_ID(message_p), message_p);
    	        message_p = NULL;
    	  }
    	  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;
}