/*
 * portmirror.c
 *
 *  Created on: 2014-01-13
 *      Author: Gervais@QNX
 */

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <errno.h>
#include <sys/param.h>
#include <sys/conf.h>
#include <sys/socket.h>
#include <sys/mbuf.h>
#include <net/if.h>
#include <net/pfil.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include "sys/io-pkt.h"
#include "nw_datastruct.h"
#include <string.h>
#include <net/if_ether.h>
//#include <sys/syslog.h>

typedef struct _port {
	struct _port *next;
	struct ifnet *ifp;
	int in_bytes;
	int out_bytes;
} port_t;

typedef struct _port_list {
	int nports;
	struct _port *port;
} port_list_t;

static port_list_t *port_list;

#define port_get(a,i) _port_get(__func__, __LINE__, a, i)
static void *_port_get(const char *funcstr, const int linenr, void *arg, struct ifnet *ifp){
	void *found = NULL;
	port_list_t *port_list = arg;
	port_t *port = port_list->port;
	while (port != NULL){
		if (port->ifp == ifp){
			found = port;
			break;
		}
		port = port->next;
	}

#ifdef PM_DEBUG
	fprintf(stderr, "%s:%d %s %s in the mirror port\n", funcstr, linenr, ifp->if_xname,
			(found != NULL)? "is" : "is not");
#endif
	return found;
}

static int input_hook(void *arg, struct mbuf **m,
		struct ifnet *ifp, int dir)
{
	port_t *port = port_get(arg, ifp);
	if (port)
		port->in_bytes += (*m)->m_len;
	return 0;
}

static int output_hook(void *arg, struct mbuf **m,
		struct ifnet *ifp, int dir)
{
	int rc;
	int i, c;
	char *p;
	char buf[512], *pbuf;
	struct mbuf *m1, *m2, *pm;
	struct sockaddr edst;
	struct sockaddr_in *pedst = &edst;
	port_t *port = port_get(arg, ifp);
	m1 =  m_dup((*m), 0, (*m)->m_len, M_DONTWAIT);
	if (m1 == NULL){
		fprintf(stderr, "Unable to dup packet\n");
		return 0;
	}
	if (port){
		port->out_bytes += (*m)->m_len;
		//return 0;
	}
	memset(pedst, 0, sizeof(*pedst));
	p = mtod(m1, char*);
	/* set address family */
	pedst->sin_family = AF_INET;
	/* set destination ip so the stack set the correct ethernet header */
	memcpy(&pedst->sin_addr, p+12, sizeof(struct in_addr));
	
	port = port_list->port;
	#ifdef PM_DEBUG
	fprintf(stderr, "%s %s\n", __func__, ifp->if_xname);
	#endif
	while (port != NULL){
		if (port->ifp == ifp)
			goto next;
		m2 = m_dup(m1, 0, m1->m_len, M_DONTWAIT);
		if (m2) {
			rc = port->ifp->if_output(port->ifp, m2, &edst, 0);
			if (rc){
				fprintf(stderr, "%s failed to send on %s\n", ifp->if_xname);
			}
		}
next:
		port = port->next;
	}
	return ifp->if_output(ifp, m1, &edst, 0);
}



static int deinit_module(void);

static port_t *port_calloc(void){
	return calloc(sizeof(port_t), 1);
}

static void port_free(port_t *del){
	free(del);
}

static port_t* port_add(void *arg, struct ifnet *ifp){
	port_list_t *port_list = arg;
	port_t *cur = NULL;

	if (port_get(arg, ifp)){
		fprintf(stderr, "%s is already in the cur\n", ifp->if_xname);
		return NULL;
	}
	cur = port_list->port;
	if (cur == NULL){
		cur = port_calloc();
		if (cur == NULL){
			fprintf(stderr, "unable to calloc first interface\n");
			return NULL;
		}
		port_list->port = cur;
	} else {
		while (cur->next != NULL){
			cur = cur->next;
		}
		cur->next = calloc(sizeof(*cur), 1);
		if (cur->next == NULL){
			fprintf(stderr, "unable to calloc additional interfaces\n");
			return NULL;
		}
		cur = cur->next;
	}

	cur->ifp = ifp;
	port_list->nports++;
	return cur;
}

static port_t* port_remove(void *arg, struct ifnet *ifp){
	port_list_t *port_list = arg;
	port_t *cur = NULL, *del = NULL, *prev = NULL;

	cur = port_get(arg, ifp);
	while(cur != NULL){
		if (cur->ifp == ifp){
			del = cur;
			break;
		}
		prev = cur;
		cur = cur->next;
	}

	if (cur != NULL){
		if (prev == NULL){ // cur start
			port_list->port = cur->next;
		} else { // mid to end of cur
			prev->next = cur->next;
		}
		// free(del);
		port_list->nports--;
	} // else no match found
	return del;
}
static int port_hook(void *arg, struct mbuf **m,
		struct ifnet *ifp, int dir)
{
	port_list_t *port_list = arg;
	port_t *port = NULL;
	fprintf(stderr, "Iface hook called '%s' ... \n", ifp->if_xname);
	if ( (int)m == PFIL_IFNET_ATTACH) {
		port = port_add(arg, ifp);
		if (port){
			fprintf(stderr, "Interface %s attached\n", ifp->if_xname);
			fprintf(stderr, "%d bytes in, %d bytes out\n", port->in_bytes,
					port->out_bytes);
		}
	} else if ((int)m == PFIL_IFNET_DETACH) {
		port = port_remove(arg, ifp);
		if(port != NULL){
			fprintf(stderr, "Interface %s detached\n", ifp->if_xname);
			fprintf(stderr, "%d bytes in, %d bytes out\n", port->in_bytes,
					port->out_bytes);
			port_free(port);
		}
		if (port_list->nports < 1){
			fprintf(stderr, "No ports detected\n");
			deinit_module();
		}
	} else {
		fprintf(stderr, "Unknown code %d\n", (int)m);
	}
	return 0;
}

static int ifacecfg_hook(void *arg, struct mbuf **m,
		struct ifnet *ifp, int dir)
{

	fprintf(stderr, "Iface cfg hook called with 0x%08X\n", (int)(m));

	return 0;
}

static int deinit_module(void)
{
	struct pfil_head *pfh_inet;

	free(port_list);

	pfh_inet = (struct pfil_head*)pfil_head_get(PFIL_TYPE_AF, AF_INET);
	if (pfh_inet == NULL) {
		return ESRCH;
	}
	pfil_remove_hook(input_hook, port_list, PFIL_IN | PFIL_WAITOK,
			pfh_inet);
	pfil_remove_hook(output_hook, port_list, PFIL_OUT | PFIL_WAITOK,
			pfh_inet);

	pfh_inet = (struct pfil_head*)pfil_head_get(PFIL_TYPE_IFNET, 0);
	if (pfh_inet == NULL) {
		return ESRCH;
	}

	pfil_remove_hook(ifacecfg_hook, port_list, PFIL_IFNET, pfh_inet);

	pfil_remove_hook(port_hook, port_list, PFIL_IFNET | PFIL_WAITOK,
			pfh_inet);
	fprintf(stderr, "Unloaded pfil hook\n" );

	return 0;
}

int pfil_entry(void *dll_hdl, struct _iopkt_self *iopkt,
		char *options)
{
	port_t *port;
	struct ifnet *ifp;
	struct pfil_head *pfh_inet;

	port_list = calloc(sizeof(*port_list), 1);
	if (port_list == NULL){
		return ENOMEM;
	}

	pfh_inet = (struct pfil_head*)pfil_head_get(PFIL_TYPE_AF, AF_INET);
	if (pfh_inet == NULL) {
		return ESRCH;
	}
	pfil_add_hook(input_hook, port_list, PFIL_IN | PFIL_WAITOK,
			pfh_inet);
	pfil_add_hook(output_hook, port_list, PFIL_OUT | PFIL_WAITOK,
			pfh_inet);

	pfh_inet = (struct pfil_head*)pfil_head_get(PFIL_TYPE_IFNET,0);
	if (pfh_inet == NULL) {
		return ESRCH;
	}

	pfil_add_hook(port_hook, port_list, PFIL_IFNET, pfh_inet);
	pfil_add_hook(ifacecfg_hook, port_list, PFIL_IFADDR, pfh_inet);
	fprintf(stderr, "Loaded pfil hook\n" );

	return 0;
}

struct _iopkt_lsm_entry IOPKT_LSM_ENTRY_SYM(pfil) =
IOPKT_LSM_ENTRY_SYM_INIT(pfil_entry);