/*
 * Copyright (c) 2017 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr.h>
#include <net/net_core.h>
#include <net/net_context.h>
#include <net/net_pkt.h>
#include <net/net_if.h>
#include <net/udp.h>
#include <string.h>
#include <errno.h>
#include <misc/printk.h>

#include "udp_cfg.h"
#include "udp.h"

static const socklen_t addrlen = sizeof(struct sockaddr_in6);

static void set_client_address(struct sockaddr *addr, struct net_pkt *rx_pkt)
{
	struct net_udp_hdr hdr, *udp_hdr;

	udp_hdr = net_udp_get_hdr(rx_pkt, &hdr);
	if (!udp_hdr) {
		printk("Invalid UDP data\n");
		return;
	}

	net_ipaddr_copy(&net_sin6(addr)->sin6_addr, &NET_IPV6_HDR(rx_pkt)->src);
	net_sin6(addr)->sin6_family = AF_INET6;
	net_sin6(addr)->sin6_port = udp_hdr->src_port;
}

static void udp_received(struct net_context *context,
			 struct net_pkt *pkt, int status, void *user_data)
{
	struct udp_context *ctx = user_data;

	ARG_UNUSED(context);
	ARG_UNUSED(status);

	ctx->rx_pkt = pkt;
	k_sem_give(&ctx->rx_sem);
}

int udp_tx(void *context, const unsigned char *buf, size_t size)
{
	struct udp_context *ctx = context;
	struct net_context *net_ctx;
	struct net_pkt *send_pkt;

	int rc, len;

	net_ctx = ctx->net_ctx;

	send_pkt = net_pkt_get_tx(net_ctx, K_FOREVER);
	if (!send_pkt) {
		printk("cannot create pkt\n");
		return -EIO;
	}

	rc = net_pkt_append_all(send_pkt, size, (u8_t *) buf, K_FOREVER);
	if (!rc) {
		printk("cannot write buf\n");
		return -EIO;
	}

	len = net_pkt_get_len(send_pkt);

	rc = net_context_sendto(send_pkt, &net_ctx->remote,
				addrlen, NULL, K_FOREVER, NULL, NULL);

	if (rc < 0) {
		printk("Cannot send data to peer (%d)\n", rc);
		net_pkt_unref(send_pkt);
		return -EIO;
	} else {
		return len;
	}
}

int udp_rx(void *context, unsigned char *buf, size_t size)
{
	struct udp_context *ctx = context;
	struct net_context *net_ctx = ctx->net_ctx;
	struct net_pkt *rx_pkt = NULL;
	struct net_buf *rx_buf;
	u16_t read_bytes;
	u8_t *ptr;
	int pos;
	int len;
	int rc;

	k_sem_take(&ctx->rx_sem, K_FOREVER);

	read_bytes = net_pkt_appdatalen(ctx->rx_pkt);
	if (read_bytes > size) {
		return -ENOMEM;
	}

	rx_pkt = ctx->rx_pkt;

	set_client_address(&net_ctx->remote, rx_pkt);

	ptr = net_pkt_appdata(rx_pkt);
	rx_buf = rx_pkt->frags;
	len = rx_buf->len - (ptr - rx_buf->data);
	pos = 0;

	while (rx_buf) {
		memcpy(buf + pos, ptr, len);
		pos += len;

		rx_buf = rx_buf->frags;
		if (!rx_buf) {
			break;
		}

		ptr = rx_buf->data;
		len = rx_buf->len;
	}

	net_pkt_unref(ctx->rx_pkt);
	ctx->rx_pkt = NULL;

	if (read_bytes != pos) {
		return -EIO;
	}

	rc = read_bytes;
	ctx->remaining = 0;

	return rc;
}

int udp_init(struct udp_context *ctx)
{
	struct net_context *udp_ctx = { 0 };
	struct net_context *mcast_ctx = { 0 };
	struct sockaddr_in6 my_addr = { 0 };
	struct sockaddr_in6 my_mcast_addr = { 0 };
	int rc;

	k_sem_init(&ctx->rx_sem, 0, UINT_MAX);

	net_ipaddr_copy(&my_mcast_addr.sin6_addr, &mcast_addr);
	my_mcast_addr.sin6_family = AF_INET6;

	net_ipaddr_copy(&my_addr.sin6_addr, &server_addr);
	my_addr.sin6_family = AF_INET6;
	my_addr.sin6_port = htons(SERVER_PORT);

	rc = net_context_get(AF_INET6, SOCK_DGRAM, IPPROTO_UDP, &udp_ctx);
	if (rc < 0) {
		printk("Cannot get network context for IPv6 UDP (%d)", rc);
		return -EIO;
	}

	rc = net_context_bind(udp_ctx, (struct sockaddr *)&my_addr,
			      sizeof(struct sockaddr_in6));
	if (rc < 0) {
		printk("Cannot bind IPv6 UDP port %d (%d)", SERVER_PORT, rc);
		goto error;
	}

	rc = net_context_get(AF_INET6, SOCK_DGRAM, IPPROTO_UDP, &mcast_ctx);
	if (rc < 0) {
		printk("Cannot get receiving IPv6 mcast (%d)", rc);
		goto error;
	}

	rc = net_context_bind(mcast_ctx, (struct sockaddr *)&my_mcast_addr,
			      sizeof(struct sockaddr_in6));
	if (rc < 0) {
		printk("Cannot get bind IPv6 mcast (%d)", rc);
		goto error;
	}

	ctx->rx_pkt = NULL;
	ctx->remaining = 0;
	ctx->net_ctx = udp_ctx;

	rc = net_context_recv(ctx->net_ctx, udp_received, K_NO_WAIT, ctx);
	if (rc != 0) {
		return -EIO;
	}

	return 0;

error:
	net_context_put(udp_ctx);
	return -EINVAL;
}