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