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

#include <csp_if_slgnd.h>

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

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

#include <pthread.h>

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

/* The MTU is actually defined by the radio configuration */
#define CSP_IF_SLGND_MTU	256

static int radio_socket;
static struct addrinfo *radio_addr;

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

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

	while (1) {
		/* Wait for frames from radio */
		nbytes = recvfrom(radio_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_slgnd, NULL);
	}

	return NULL;
}

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

	int nbytes;
	uint32_t id;
	uint8_t frame[sizeof(id) + CSP_IF_SLGND_MTU];
	size_t frame_length = 0;

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

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

	/* Send message to radio */
	nbytes = sendto(radio_socket, frame, frame_length, 0,
			radio_addr->ai_addr, radio_addr->ai_addrlen);
	if (nbytes != (int)frame_length)
		return CSP_ERR_TIMEDOUT;

	csp_buffer_free(packet);

	return CSP_ERR_NONE;
}

int csp_slgnd_init(const char *radio_host)
{
	int ret;
	struct addrinfo hints;

	memset(&hints, 0, sizeof(hints));
	hints.ai_family = AF_INET; /* GNU Radio Socket PDU only supports IPv4 */
	hints.ai_socktype = SOCK_DGRAM;

	/* Look up host info */
	ret = getaddrinfo(radio_host, "52002", &hints, &radio_addr);
	if (ret < 0) {
		csp_log_error("failed to look up host info for %s\n");
		return CSP_ERR_DRIVER;
	}

	/* Create radio socket */
	radio_socket = socket(radio_addr->ai_family, radio_addr->ai_socktype,
			      radio_addr->ai_protocol);
	if (radio_socket < 0) {
		csp_log_error("failed to allocate radio socket\n");
		return CSP_ERR_DRIVER;
	}

	/* Start receiver thread */
	pthread_t rx_thread;
	ret = pthread_create(&rx_thread, NULL, csp_slgnd_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_slgnd);

	return CSP_ERR_NONE;
}

/* Interface definition */
csp_iface_t csp_if_slgnd = {
	.name = "SLGND",
	.nexthop = csp_slgnd_tx,
	.mtu = CSP_IF_SLGND_MTU,
};
