/*
Copyright (C) 2012  Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv

    This program 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.

    This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ipmodule.h"
#include "bacint.h"

#ifdef TEST_PACKET
	uint8_t test_packet[]={0x81, 0x0a, 0x00, 0x16,		// BVLC header
			0x01, 0x24, 0x00, 0x01, 0x01, 0x0b, 0xff,	// NPDU
			0x00, 0x03, 0x01, 0x0c, 0x0c, 0x00, 0x00, 0x00, 0x02, 0x19, 0x55};	// APDU
#endif

extern int get_local_address_ioctl(
    char *ifname,
    struct in_addr *addr,
    int request);

void* dl_ip_thread(void *pArgs) {
	MSGBOX_ID msgboxid;
	BACMSG msg_storage, *bacmsg = NULL;
	MSG_DATA *msg_data;
	ROUTER_PORT *port = (ROUTER_PORT*)pArgs;
	IP_DATA ip_data;		// port specific parameters
	BACNET_ADDRESS address = { 0 };
	int status;
	uint8_t shutdown = 0;

	// initialize router port
	if (!dl_ip_init(port, &ip_data)) {
		port->state = INIT_FAILED;
		return NULL;
	}

	// allocate buffer
	ip_data.max_buff = MAX_BIP_MPDU;
	ip_data.buff = (uint8_t*)malloc(ip_data.max_buff);

	if (ip_data.buff == NULL) {
		port->state = INIT_FAILED;
		return NULL;
	}

	msgboxid = create_msgbox();
	if (msgboxid == INVALID_MSGBOX_ID) {
		PRINT(ERROR, "Error: Failed to create message box");
		port->state = INIT_FAILED;
		return NULL;
	}

	port->port_id = msgboxid;
	port->state = RUNNING;

	while (!shutdown) {

		// check for incoming messages
		bacmsg = recv_from_msgbox(port->port_id, &msg_storage);

		if (bacmsg) {
			switch(bacmsg->type) {
			case DATA: {
				msg_data = (MSG_DATA*)bacmsg->data;
				memmove(&address.net, &msg_data->dest.net, 2);
				memmove(&address.mac_len, &msg_data->dest.len, 1);
				memmove(&address.mac[0], &msg_data->dest.adr[0], MAX_MAC_LEN);

				dl_ip_send(&ip_data, &address, msg_data->pdu, msg_data->pdu_len);

				check_data(msg_data);

				break;
			}

			case SERVICE: {
				switch(bacmsg->subtype) {
				case SHUTDOWN:
					del_msgbox(port->port_id);
					shutdown = 1;
					break;
				default:
					break;
				}
				break;
			}

			default:
				break;
			}
		} else {
			status = dl_ip_recv(&ip_data, &msg_data, &address, 1000);
			if (status > 0) {
				memmove(&msg_data->src.len, &address.mac_len, 1);
				memmove(&msg_data->src.adr[0], &address.mac[0], MAX_MAC_LEN);
				msg_storage.origin = port->port_id;
				msg_storage.type = DATA;
				msg_storage.data = msg_data;

				if (!send_to_msgbox(port->main_id, &msg_storage)) {
					free_data(msg_data);
				}
			}
		}
	}

	// cleanup procedure
	dl_ip_cleanup(&ip_data);
	port->state = FINISHED;
	return NULL;
}

bool dl_ip_init(ROUTER_PORT *port,
		IP_DATA *ip_data) {
	struct sockaddr_in sin;
	int socket_opt = 0;
	int status = 0;	// for error checking

	// setup port for later use
	ip_data->port = htons(port->params.bip_params.port);

	// get local address
	status = get_local_address_ioctl(port->iface, &ip_data->local_addr, SIOCGIFADDR);
	if (status < 0) {
		return false;
	}
	// get broadcast address
	status = get_local_address_ioctl(port->iface, &ip_data->broadcast_addr, SIOCGIFBRDADDR);
	if (status < 0) {
		return false;
	}

	ip_data->socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if (ip_data->socket < 0)
		return false;

	// setup socket options

	socket_opt = 1;
	status = setsockopt(ip_data->socket, SOL_SOCKET, SO_REUSEADDR, &socket_opt, sizeof(socket_opt));
	if (status < 0) {
		close(ip_data->socket);
		return false;
	}

	status = setsockopt(ip_data->socket, SOL_SOCKET, SO_BROADCAST, &socket_opt, sizeof(socket_opt));
	if (status < 0) {
		close(ip_data->socket);
		return false;
	}

	// bind the socket to the local port number
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = htonl(INADDR_ANY);
	sin.sin_port = ip_data->port;

	memset(&sin.sin_zero, '\0', sizeof(sin.sin_zero));

	status = bind(ip_data->socket, (const struct sockaddr *) &sin, sizeof(struct sockaddr));
	if (status < 0) {
		close(ip_data->socket);
		return false;
	}

	// add BIP address to router port structure
	memcpy(&port->route_info.mac[0], &ip_data->local_addr.s_addr, 4);
	memcpy(&port->route_info.mac[4], &port->params.bip_params.port, 2);
	port->route_info.mac_len = 6;

	PRINT(INFO, "Interface: %s\n", port->iface);
	PRINT(INFO, "IP Address: %s\n", inet_ntoa(ip_data->local_addr));
	PRINT(INFO, "IP Broadcast Address: %s\n", inet_ntoa(ip_data->broadcast_addr));
	PRINT(INFO, "UDP Port: 0x%04X [%hu]\n", (port->params.bip_params.port),
			(port->params.bip_params.port));

	return true;
}

int dl_ip_send(IP_DATA *data,
		BACNET_ADDRESS *dest,
		uint8_t *pdu,
		unsigned pdu_len)
{
	struct sockaddr_in bip_dest = { 0 };
	int buff_len = 0;
	int bytes_sent = 0;

	if (data->socket < 0)
		return -1;

	data->buff[0] = BVLL_TYPE_BACNET_IP;
	bip_dest.sin_family = AF_INET;
	if (dest->net == BACNET_BROADCAST_NETWORK) {
		// broadcast
		bip_dest.sin_addr.s_addr = data->broadcast_addr.s_addr;
		bip_dest.sin_port = data->port;
		data->buff[1] = BVLC_ORIGINAL_BROADCAST_NPDU;
	} else if (dest->mac_len == 6) {
		memcpy(&bip_dest.sin_addr.s_addr, &dest->mac[0], 4);
		memcpy(&bip_dest.sin_port, &dest->mac[4], 2);
		data->buff[1] = BVLC_ORIGINAL_UNICAST_NPDU;
	} else {
		// invalid address
		return -1;
	}

	buff_len = 2;
	buff_len +=
		encode_unsigned16(&data->buff[buff_len],
		(uint16_t) (pdu_len + 4 /*inclusive*/ ));
	memcpy(&data->buff[buff_len], pdu, pdu_len);
	buff_len += pdu_len;

	// send the packet
	bytes_sent =
		sendto(data->socket, (char *) data->buff, buff_len, 0,
		(struct sockaddr *) &bip_dest, sizeof(struct sockaddr));

	PRINT(DEBUG, "send to %s\n", inet_ntoa(bip_dest.sin_addr));

	return bytes_sent;
}

int dl_ip_recv(
		IP_DATA *data,
		MSG_DATA **msg_data,
		BACNET_ADDRESS *src,
		unsigned timeout)
{
	int received_bytes = 0;
	uint16_t buff_len = 0;	// return value
	fd_set read_fds;
	struct timeval select_timeout;
	struct sockaddr_in sin = { 0 };
	socklen_t sin_len = sizeof(sin);

	// make sure the socket is open
	if (data->socket < 0)
		return 0;

	if (timeout >= 1000) {
		select_timeout.tv_sec = timeout / 1000;
		select_timeout.tv_usec = 1000 * (timeout - select_timeout.tv_sec * 1000);
	} else {
		select_timeout.tv_sec = 0;
		select_timeout.tv_usec = 1000 * timeout;
	}

	FD_ZERO(&read_fds);
	FD_SET(data->socket, &read_fds);

#ifdef TEST_PACKET
	received_bytes = sizeof(test_packet);
	memmove(data->buff, &test_packet, received_bytes);
	sin.sin_addr.s_addr = 0x7E1D40A; sin.sin_port = 0xC0BA;
#else
	int ret = select(data->socket + 1, &read_fds, NULL, NULL, &select_timeout);
	// see if there is a packet for us
	if (ret > 0)
		received_bytes =
			recvfrom(data->socket, (char *) &data->buff[0], data->max_buff, 0,
				(struct sockaddr *) &sin, &sin_len);
	else
		return 0;
#endif
	PRINT(DEBUG, "received from %s\n", inet_ntoa(sin.sin_addr));

	// check for errors
	if (received_bytes <= 0) {
		return 0;
	}

	// the signature of a BACnet/IP packet
	if (data->buff[0] != BVLL_TYPE_BACNET_IP)
		return 0;

	switch (data->buff[1]) {
	case BVLC_ORIGINAL_UNICAST_NPDU:
	case BVLC_ORIGINAL_BROADCAST_NPDU: {
		if ((sin.sin_addr.s_addr == data->local_addr.s_addr) &&
			(sin.sin_port == data->port)) {
			buff_len = 0;

			PRINT(DEBUG, "BIP: src is me. Discarded!\n");

		} else {
			src->mac_len = 6;
			memcpy(&src->mac[0], &sin.sin_addr.s_addr, 4);
			memcpy(&src->mac[4], &sin.sin_port, 2);

			(void) decode_unsigned16(&data->buff[2], &buff_len);
			// subtract off the BVLC header
			buff_len -= 4;
			if (buff_len < data->max_buff) {
				// allocate data message stucture
				(*msg_data) = (MSG_DATA*)malloc(sizeof(MSG_DATA));
				(*msg_data)->pdu_len = buff_len;
				(*msg_data)->pdu = (uint8_t*)malloc((*msg_data)->pdu_len);
				// fill up data message structure
				memmove(&(*msg_data)->pdu[0], &data->buff[4], (*msg_data)->pdu_len);
				memmove(&(*msg_data)->src, src, sizeof(BACNET_ADDRESS));
			}
			// ignore packets that are too large
			else {
				buff_len = 0;

				PRINT(ERROR, "BIP: PDU too large. Discarded!.\n");

			}
		}
	}
	break;

	case BVLC_FORWARDED_NPDU: {
		memcpy(&sin.sin_addr.s_addr, &data->buff[4], 4);
		memcpy(&sin.sin_port, &data->buff[8], 2);
		if ((sin.sin_addr.s_addr == data->local_addr.s_addr) &&
			(sin.sin_port == data->port)) {
			buff_len = 0;
		} else {
			src->mac_len = 6;
			memcpy(&src->mac[0], &sin.sin_addr.s_addr, 4);
			memcpy(&src->mac[4], &sin.sin_port, 2);

			(void) decode_unsigned16(&data->buff[2], &buff_len);
			// subtract off the BVLC header
			buff_len -= 10;
			if (buff_len < data->max_buff) {
				// allocate data message stucture
				(*msg_data) = (MSG_DATA*)malloc(sizeof(MSG_DATA));
				(*msg_data)->pdu_len = buff_len;
				(*msg_data)->pdu = (uint8_t*)malloc((*msg_data)->pdu_len);
				// fill up data message structure
				memmove(&(*msg_data)->pdu, &data->buff[4+6], (*msg_data)->pdu_len);
				memmove(&(*msg_data)->src, src, sizeof(BACNET_ADDRESS));
			} else {
				// ignore packets that are too large
				buff_len = 0;
			}
		}
	}
	break;
	default:

		PRINT(ERROR, "BIP: BVLC discarded!\n");

	break;
	}
	return buff_len;
}

void dl_ip_cleanup(
		IP_DATA *ip_data) {
	// free buffer
	if (ip_data->buff)
		free(ip_data->buff);
	// close socket
	if (ip_data->socket > 0)
		close(ip_data->socket);
	return;
}