/*
 * Copyright (C) 2015 Gilles Chanteperdrix <gch@xenomai.org>
 *
 * SPDX-License-Identifier: MIT
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>

#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <arpa/inet.h>
#include <netinet/ether.h>
#include <netpacket/packet.h>

#include <sys/cobalt.h>
#include <smokey/smokey.h>

#include <rtnet_chrdev.h>
#include <rtcfg_chrdev.h>
#include "smokey_net.h"
#include "smokey_net_server.h"


struct module {
	int option;
	const char *name;
};

#define TIMEOUT 10

static struct rtnet_core_cmd cmd;
static int fd;
static pthread_t loopback_server_tid;
static bool loopback_thread_created, ifup;
static struct module modules[] = {
	{
		.name = "rtnet",
	},
	{
		.name = "rtipv4",
	},
	{
		.name = "rtcfg",
	},
	{
		.option = _CC_COBALT_NET_UDP,
		.name = "rtudp",
	},
	{
		.option = _CC_COBALT_NET_AF_PACKET,
		.name = "rtpacket",
	},
	{
		.name = NULL,	/* driver */
	},
};

#define MODID_RTNET  0
#define MODID_IPV4   1
#define MODID_CFG    2
#define MODID_UDP    3
#define MODID_PACKET 4
#define MODID_DRIVER 5

static int option_to_modid(int option)
{
	unsigned i;

	for (i = 0; i < sizeof(modules)/sizeof(modules[0]); i++) {
		if (modules[i].option != option)
			continue;

		return i;
	}

	return -1;
}

static int get_info(const char *intf)
{
	int err;

	err = smokey_check_errno(
		snprintf(cmd.head.if_name, sizeof(cmd.head.if_name),
			"%s", intf));
	if (err < 0)
		return err;

	cmd.args.info.ifindex = 0;

	err = smokey_check_errno(ioctl(fd, IOC_RT_IFINFO, &cmd));
	if (err < 0)
		return err;

	return 0;
}

static int do_up(const char *intf)
{
	int err;

	snprintf(cmd.head.if_name, sizeof(cmd.head.if_name), "%s", intf);
	cmd.args.info.ifindex = 0;
	if (strcmp(intf, "rtlo")) {
		cmd.args.up.ip_addr = 0xffffffff;
		cmd.args.up.broadcast_ip = cmd.args.up.ip_addr;
	} else {
		cmd.args.up.ip_addr = htonl(0x7f000001); /* 127.0.0.1 */
		cmd.args.up.broadcast_ip = cmd.args.up.ip_addr | ~0x000000ff;
	}
	cmd.args.up.set_dev_flags = 0;
	cmd.args.up.clear_dev_flags = 0;
	cmd.args.up.dev_addr_type = 0xffff;

	err = smokey_check_errno(ioctl(fd, IOC_RT_IFUP, &cmd));
	if (err < 0)
		return err;

	return 0;
}

static int do_down(const char *intf)
{
	int err;

	snprintf(cmd.head.if_name, sizeof(cmd.head.if_name), "%s", intf);
	cmd.args.info.ifindex = 0;

	err = smokey_check_errno(ioctl(fd, IOC_RT_IFDOWN, &cmd));
	if (err < 0)
		return err;

	return 0;
}

static int smokey_net_modprobe(int modid, bool silent)
{
	struct module *m = modules + modid;

	if (modid < 0)
		return -EINVAL;

	return smokey_modprobe(m->name, silent);
}

static int smokey_net_rmmod(int modid)
{
	struct module *m = modules + modid;

	return smokey_rmmod(m->name);
}

static int smokey_net_setup_rtcfg_client(const char *intf, int net_config)
{
	struct rtcfg_cmd cmd;
	int err;

	if ((net_config & _CC_COBALT_NET_CFG) == 0)
		return -ENOSYS;

	err = smokey_net_modprobe(MODID_CFG, false);
	if (err < 0)
		return err;

	memset(&cmd, 0, sizeof(cmd));
	err = smokey_check_errno(
		snprintf(cmd.head.if_name, sizeof(cmd.head.if_name),
			intf, IFNAMSIZ));
	if (err < 0)
		return err;

	cmd.args.client.timeout      = 10000;
	cmd.args.client.max_stations = 32;
	cmd.args.client.buffer_size = 0;

	err = smokey_check_errno(ioctl(fd, RTCFG_IOC_CLIENT, &cmd));
	if (err < 0)
		return err;

	cmd.args.announce.timeout     = 5000;
	cmd.args.announce.buffer_size = 0;
	cmd.args.announce.flags       = 0;
	cmd.args.announce.burstrate   = 4;

	err = smokey_check_errno(ioctl(fd, RTCFG_IOC_ANNOUNCE, &cmd));
	if (err < 0)
		return err;

	return 0;
}

static int smokey_net_teardown_rtcfg(const char *intf)
{
	struct rtcfg_cmd cmd;
	int err;

	memset(&cmd, 0, sizeof(cmd));
	err = smokey_check_errno(
		snprintf(cmd.head.if_name, sizeof(cmd.head.if_name),
			intf, IFNAMSIZ));
	if (err < 0)
		return err;

	/*
	 * We may or may not be acting as a server; don't check the
	 * status.
	 */
	ioctl(fd, RTCFG_IOC_DETACH, &cmd);

	return smokey_net_rmmod(MODID_CFG);
}

static int find_peer(const char *intf, void *vpeer)
{
	struct sockaddr_in *in_peer = vpeer;
	struct sockaddr_ll *ll_peer = vpeer;
	struct sockaddr *peer = vpeer;
	char buf[4096];
	char hash[3];
	char dest[16];
	char mac[18];
	char dev[16];
	FILE *f;
	int err;

	f = fopen("/proc/rtnet/ipv4/host_route", "r");
	if (!f) {
		err = -errno;
		smokey_warning("open(/proc/rtnet/ipv4/host_route): %s",
			strerror(-err));
		return err;
	}

	/* Skip first line */
	if (!fgets(buf, sizeof(buf), f)) {
		err = -errno;
		smokey_warning("fgets(/proc/rtnet/ipv4/host_route): %s",
			strerror(-err));
		goto err;
	}

	for(;;) {
		err = fscanf(f, "%s\t%s\t%s\t%s\n", hash, dest, mac, dev);
		if (err == EOF) {
			smokey_warning("No peer found\n");
			err = -ENOENT;
			goto err;
		}
		if (err < 4) {
			smokey_warning("Error parsing"
				" /proc/rtnet/ipv4/host_route\n");
			err = -EINVAL;
			goto err;
		}

		if (strcmp(dev, intf))
			continue;

		if (strcmp(mac, "FF:FF:FF:FF:FF:FF") == 0)
			continue;

		if (strcmp(dest, "255.255.255.255") == 0)
			continue;

		if (strcmp(dest, "0.0.0.0") == 0)
			continue;

		break;
	}

	switch(peer->sa_family) {
	case AF_INET:
		err = smokey_check_errno(
			inet_pton(AF_INET, dest, &in_peer->sin_addr));
		if (err < 0)
			goto err;
		break;

	case AF_PACKET: {
		const unsigned eth_alen = 6;
		struct ether_addr eth;
		struct ifreq ifr;
		int sock;

		ll_peer->sll_halen = eth_alen;
		if (ether_aton_r(mac, &eth) == 0) {
			err = -errno;
			smokey_warning("ether_aton_r(%s): %m", mac);
			goto err;
		}

		memcpy(&ll_peer->sll_addr[0], eth.ether_addr_octet, eth_alen);

		snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", dev);

		err = smokey_check_errno(
			__RT(socket(PF_PACKET, SOCK_DGRAM, 0)));
		if (err < 0)
			goto err;
		sock = err;

		err = smokey_check_errno(__RT(ioctl(sock, SIOCGIFINDEX, &ifr)));
		sock = smokey_check_errno(__RT(close(sock)));
		if (err < 0)
			goto err;
		if (sock < 0) {
			err = sock;
			goto err;
		}

		ll_peer->sll_ifindex = ifr.ifr_ifindex;
	}
	}

	err = 0;
  err:
	fclose(f);
	return err;
}

int smokey_net_server_check_inner(const char *file, int line,
				     const char *msg, int status)
{
       if (status >= 0)
	       return status;

       __smokey_warning(file, line, "%s: %s", msg, strerror(-status));
       pthread_exit((void *)(long)status);
}

static void *loopback_server(void *cookie)
{
	int net_config = (long)cookie;
	smokey_net_server_loop(net_config);
	return NULL;
}

int smokey_net_setup(const char *driver, const char *intf, int tested_config,
		void *vpeer)
{
	int net_config, err, i, err_teardown;
	struct sockaddr_in *in_peer = vpeer;
	struct sockaddr *peer = vpeer;

	/*
	 * Main module needs to be loaded in order to use
	 * _CC_COBALT_GET_NET_CONFIG.
	 */
	smokey_net_modprobe(MODID_RTNET, true);

	err = cobalt_corectl(_CC_COBALT_GET_NET_CONFIG,
			&net_config, sizeof(net_config));
	if (err == -EINVAL)
		return -ENOSYS;
	if (err < 0)
		return err;

	if ((net_config & (_CC_COBALT_NET | _CC_COBALT_NET_IPV4))
		!= (_CC_COBALT_NET | _CC_COBALT_NET_IPV4))
		return -ENOSYS;

	if ((net_config & tested_config) == 0)
		return -ENOSYS;

	modules[MODID_DRIVER].name = driver;
	err = smokey_net_modprobe(MODID_DRIVER, false);
	if (err < 0)
		return err;

	err = smokey_net_modprobe(MODID_IPV4, false);
	if (err < 0)
		return err;

	err = smokey_net_modprobe(option_to_modid(tested_config), false);
	if (err < 0)
		return err;

	fd = smokey_check_errno(open("/dev/rtnet", O_RDWR));
	if (fd < 0)
		return fd;

	err = get_info(intf);
	if (err < 0)
		goto err;

	if ((cmd.args.info.flags & IFF_UP) == 0) {
		err = do_up(intf);
		if (err < 0)
			goto err;
		ifup = true;
	}

	smokey_trace("Waiting for interface %s to be running", intf);

	for (i = 0; i < 30; i++) {
		err = get_info(intf);
		if (err < 0)
			goto err;

		if ((cmd.args.info.flags & (IFF_UP | IFF_RUNNING))
			== (IFF_UP | IFF_RUNNING))
			goto running;

		sleep(1);
	}

	smokey_warning("Interface is not running,"
		" giving up (cable unplugged?)");
	err = -ETIMEDOUT;
	goto err;

running:
	err = get_info(intf);
	if (err < 0)
		goto err;

	if (cmd.args.info.ip_addr == 0) {
		err = smokey_net_setup_rtcfg_client(intf, net_config);
		if (err < 0)
			goto err;
	}

	if (strcmp(driver, "rt_loopback") == 0) {
		err  = smokey_check_status(
			__RT(pthread_create(&loopback_server_tid, NULL,
						loopback_server,
						(void *)(long)tested_config)));
		if (err < 0)
			goto err;
		loopback_thread_created = true;
	}

	switch (peer->sa_family) {
	case AF_INET:
		if (in_peer->sin_addr.s_addr == htonl(INADDR_ANY) &&
			strcmp(driver, "rt_loopback") == 0) {
			in_peer->sin_addr.s_addr = cmd.args.info.ip_addr;
			break;
		}

		/* Fallthrough wanted */
	case AF_PACKET:
		err = find_peer(intf, vpeer);
		if (err < 0)
			goto err;
	}

	close(fd);
	return 0;

  err:
	close(fd);

	err_teardown = smokey_net_teardown(driver, intf, tested_config);
	if (err == 0)
		err = err_teardown;

	return err;
}

int smokey_net_teardown(const char *driver, const char *intf, int tested_config)
{
	int err = 0, tmp;

	if (loopback_thread_created) {
		void *status;

		pthread_cancel(loopback_server_tid); /* May fail */
		tmp = smokey_check_errno(
			pthread_join(loopback_server_tid, &status));
		if (err == 0)
			err = tmp;
		if (err == 0 && status != PTHREAD_CANCELED)
			err = (long)status;
	}

	tmp = smokey_check_errno(open("/dev/rtnet", O_RDWR));
	if (tmp >= 0) {
		fd = tmp;

		if (strcmp(driver, "rt_loopback")) {
			tmp = smokey_net_teardown_rtcfg(intf);
			if (err == 0)
				err = tmp;
		}

		if (ifup) {
			tmp = do_down(intf);
			if (err == 0)
				err = tmp;
		}

		close(fd);
	} else
		err = tmp;

	tmp = smokey_net_rmmod(option_to_modid(tested_config));
	if (err == 0)
		err = tmp;

	tmp = smokey_net_rmmod(MODID_DRIVER);
	if (err == 0)
		err = tmp;

	tmp = smokey_net_rmmod(MODID_IPV4);
	if (err == 0)
		err = tmp;

	tmp = smokey_net_rmmod(MODID_RTNET);
	if (err == 0)
		err = tmp;

	return err;
}