/*
 * Copyright (c) 2016-2017 Satlab Aps <satlab@satlab.com>
 * All rights reserved. Do not distribute without permission.
 */

#include <csp_if_udp.h>

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <inttypes.h>

#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <net/if.h>

#include <pthread.h>

#include <csp/csp.h>
#include <csp/csp_interface.h>
#include <csp/csp_endian.h>

/* MTU of the UDP interface */
#define CSP_IF_UDP_MTU	256

/* Default UDP port */
#define CSP_IF_UDP_PORT	53001

static int dest_socket;
static struct sockaddr_in dest_addr;

static void *csp_udp_rx(void *arg)
{
	(void)arg;

	int nbytes;
	uint32_t id;
	uint16_t length;
	csp_packet_t *packet;
	uint8_t frame[sizeof(id) + CSP_IF_UDP_MTU];

	while (1) {
		/* Wait for frames */
		nbytes = recvfrom(dest_socket, frame, sizeof(frame), 0, NULL, NULL);
		if (nbytes < 0 || (unsigned int)nbytes < sizeof(id))
			continue;

		/* Extract CSP length */
		length = nbytes - sizeof(id);

		/* Allocate buffer for frame */
		packet = csp_buffer_get(length);
		if (!packet)
			continue;
		packet->length = length;

		/* Set packet ID */
		memcpy(&id, &frame[0], sizeof(id));
		packet->id.ext = csp_ntoh32(id);

		/* Copy received data into CSP buffer */
		memcpy(packet->data, &frame[sizeof(id)], packet->length);

		/* Pass frame to CSP stack */
		csp_new_packet(packet, &csp_if_udp, NULL);
	}

	return NULL;
}

int csp_udp_tx(csp_iface_t *interface, csp_packet_t *packet, uint32_t timeout)
{
	(void)interface;
	(void)timeout;

	int nbytes;
	uint32_t id;
	size_t length = 0;
	struct sockaddr_in dest = dest_addr;
	uint8_t destb, frame[sizeof(id) + CSP_IF_UDP_MTU];

	id = csp_hton32(packet->id.ext);

	memcpy(&frame[length], &id, sizeof(id));
	length += sizeof(id);
	memcpy(&frame[length], packet->data, packet->length);
	length += packet->length;

	/* Build destination address */
	destb = csp_rtable_find_mac(packet->id.dst);
	if (destb == CSP_NODE_MAC)
		destb = packet->id.dst;

	/* Last byte in destination IP is CSP address (or MAC if via node) */
	dest.sin_addr.s_addr = dest.sin_addr.s_addr | destb << 24;

	/* Send message */
	nbytes = sendto(dest_socket, frame, length, 0,
			(struct sockaddr *) &dest, sizeof(dest));
	if (nbytes != (int)length)
		return CSP_ERR_TIMEDOUT;

	csp_buffer_free(packet);

	return CSP_ERR_NONE;
}

int csp_udp_init(const char *ifc)
{
	int ret;
	struct sockaddr_in sa;
	struct ifreq ifr;
	pthread_t rx_thread;

	/* Create socket */
	dest_socket = socket(AF_INET, SOCK_DGRAM, 0);
	if (dest_socket < 0) {
		csp_log_error("failed to allocate UDP socket\n");
		return CSP_ERR_DRIVER;
	}

	/* Get interface IP address */
	ifr.ifr_addr.sa_family = AF_INET;
	strncpy(ifr.ifr_name, ifc, IFNAMSIZ-1);
	ret = ioctl(dest_socket, SIOCGIFADDR, &ifr);
	if (ret < 0) {
		csp_log_error("failed to get interface address\n");
		return CSP_ERR_DRIVER;
	}

	/* Bind to incoming port on interface */
	memset(&sa, 0, sizeof(struct sockaddr_in));
	sa.sin_family = AF_INET;
	sa.sin_addr = ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr;
	sa.sin_port = htons(CSP_IF_UDP_PORT);

	ret = bind(dest_socket, (const struct sockaddr *)&sa, sizeof(struct sockaddr));
	if (ret < 0) {
		csp_log_error("failed to bind receive socket\n");
		return CSP_ERR_DRIVER;
	}

	/* Create destination address */
	memset(&dest_addr, 0, sizeof(dest_addr));
	dest_addr.sin_family = AF_INET;
	dest_addr.sin_addr = ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr;
	dest_addr.sin_port = htons(CSP_IF_UDP_PORT);

	/* Last byte in destination is set in TX function */
	dest_addr.sin_addr.s_addr = dest_addr.sin_addr.s_addr & 0x00ffffff;

	/* Start receiver thread */
	ret = pthread_create(&rx_thread, NULL, csp_udp_rx, NULL);
	if (ret < 0) {
		csp_log_error("failed to start receive thread\n");
		return CSP_ERR_DRIVER;
	}

	/* Register interface */
	csp_iflist_add(&csp_if_udp);

	return CSP_ERR_NONE;
}

/* Interface definition */
csp_iface_t csp_if_udp = {
	.name = "UDP",
	.nexthop = csp_udp_tx,
	.mtu = CSP_IF_UDP_MTU,
};
