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

#include <satlab/polaris_store.h>

#include <errno.h>

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

#include <satlab/polaris.h>

static inline size_t array_weight(const uint8_t *array, size_t length)
{
	size_t i, c;
	uint8_t v;
	int weight = 0;

	for (i = 0; i < length; i++) {
		v = array[i];
		for (c = 0; v; c++)
			v &= v - 1;
		weight += c;
	}

	return weight;
}

int sl_polaris_store_mirror(uint8_t node, uint32_t timeout, uint8_t dest_node, uint8_t dest_port, uint32_t time_ms)
{
	sl_polaris_store_mirror_req_t *request;
	sl_polaris_store_mirror_rep_t *reply;
	csp_packet_t *packet;
	csp_conn_t *conn;
	packet = csp_buffer_get(sizeof(*request));
	if (!packet)
		return -ENOMEM;

	request = (void *)packet->data;
	request->type = SL_POLARIS_STORE_MIRROR_REQ;
	request->dest_node = dest_node;
	request->dest_port = dest_port;
	request->timeout_ms = csp_hton32(time_ms);
	packet->length = sizeof(*request);

	conn = csp_connect(CSP_PRIO_NORM, node, SL_POLARIS_PORT_STORE, timeout, CSP_O_NONE);
	if (!conn) {
		csp_buffer_free(packet);
		return -ECONNREFUSED;
	}

	if (!csp_send(conn, packet, timeout)) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -EIO;
	}

	packet = csp_read(conn, timeout);
	if (!packet) {
		csp_close(conn);
		return -ETIMEDOUT;
	}

	reply = (void *)packet->data;
	if (reply->type != SL_POLARIS_STORE_MIRROR_REP) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -EINVAL;
	}

	if (reply->error != SL_POLARIS_ERR_NONE) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -reply->error;
	}

	csp_buffer_free(packet);
	csp_close(conn);

	return 0;
}

int sl_polaris_store_mirror_recv(uint8_t node, uint8_t port, uint32_t timeout, sl_polaris_frame_cb_t cb, void *arg)
{
	int ret = 0;
	bool done = false;
	csp_conn_t *conn;
	csp_packet_t *packet;
	sl_polaris_store_frame_t *frame;

	/*
	 * CSP does not have a way of "unbinding" a socket so
	 * it has to be static for now.
	 * Setup listen port
	 */
	static csp_socket_t *sock = NULL;
	if (!sock) {
		sock = csp_socket(CSP_SO_NONE);
		if (!sock)
			return CSP_ERR_NOBUFS;

		csp_bind(sock, port);
		csp_listen(sock, 1);
	}

	while (!done) {
		/* Check for ^C */
		ret = cb(NULL, arg);
		if (ret < 0)
			break;

		/* Send mirror frames request */
		ret = sl_polaris_store_mirror(node, timeout, csp_get_address(), port, 60000);
		if (ret < 0)
			break;

		if ((conn = csp_accept(sock, 1000)) == NULL)
			continue;

		packet = csp_read(conn, 0);
		if (packet) {
			if (packet->length >= SL_POLARIS_STORE_FRAME_OVERHEAD) {
				frame = (void *)packet->data;

				if (frame->type == SL_POLARIS_STORE_FRAME &&
					frame->error == SL_POLARIS_ERR_NONE) {
					frame->seq = csp_ntoh32(frame->seq);
					frame->time = csp_ntoh64(frame->time);
					frame->scale = csp_ntoh16(frame->scale);
					frame->mean = csp_ntoh16(frame->mean);
					ret = cb(frame, arg);
					done = (ret < 0);
					ret = 0;
				}
			}
			csp_buffer_free(packet);
		}
		csp_close(conn);
	}
	/* Ask the receiver to stop forwarding packets */
	sl_polaris_store_mirror(node, timeout, csp_get_address(), port, 0);

	return ret;
}

int sl_polaris_store_read(uint8_t node, uint32_t timeout, uint32_t start, uint8_t *bitmap, sl_polaris_frame_cb_t cb,
			  void *arg)
{
	int i, count;
	sl_polaris_store_read_req_t *request;
	sl_polaris_store_read_rep_t *reply;
	sl_polaris_store_frame_t *frame;
	csp_packet_t *packet;
	csp_conn_t *conn;

	/* Build request */
	packet = csp_buffer_get(sizeof(*request));
	if (!packet)
		return -ENOMEM;

	request = (void *)packet->data;
	request->type = SL_POLARIS_STORE_READ_REQ;

	request->start = csp_hton32(start);
	memcpy(request->bitmap, bitmap, sizeof(request->bitmap));

	packet->length = sizeof(*request);

	/* Calculated expected reply frames */
	count = array_weight(bitmap, sizeof(request->bitmap));

	/* Send request */
	conn = csp_connect(CSP_PRIO_NORM, node, SL_POLARIS_PORT_STORE, timeout, CSP_O_NONE);
	if (!conn) {
		csp_buffer_free(packet);
		return -ECONNREFUSED;
	}

	if (!csp_send(conn, packet, timeout)) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -EIO;
	}

	/* Wait for valid reply */
	packet = csp_read(conn, timeout);
	if (!packet) {
		csp_close(conn);
		return -ETIMEDOUT;
	}

	reply = (void *)packet->data;
	if (reply->type != SL_POLARIS_STORE_READ_REP ||
	    reply->error != SL_POLARIS_ERR_NONE) {
		csp_buffer_free(packet);
		csp_close(conn);
		return -EINVAL;
	}

	csp_buffer_free(packet);

	/* Wait for frames */
	for (i = 0; i < count; i++) {
		packet = csp_read(conn, timeout);
		if (!packet) {
			csp_close(conn);
			return -ETIMEDOUT;
		}

		frame = (void *)packet->data;

		if (frame->type != SL_POLARIS_STORE_FRAME ||
		    frame->error != SL_POLARIS_ERR_NONE) {
			csp_buffer_free(packet);
			csp_close(conn);
			return -EINVAL;
		}

		frame->seq = csp_ntoh32(frame->seq);
		frame->time = csp_ntoh64(frame->time);
		frame->scale = csp_ntoh16(frame->scale);
		frame->mean = csp_ntoh16(frame->mean);

		cb(frame, arg);
		csp_buffer_free(packet);
	}

	csp_close(conn);

	return 0;
}
