#include <string.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>

#include "grspw_router_lib.h"

struct router_lib_priv {
	int fd;
};

void *router_init(char *path)
{
	struct router_lib_priv *priv;

	priv = (struct router_lib_priv *)calloc(1, sizeof(*priv));
	if ( !priv )
		return NULL;
	priv->fd = open(path, O_RDWR);
	if ( priv->fd < 0 ) {
		printf("ROUTER_LIB: Failed to open device: %s (%d)\n",
			strerror(errno), errno);
		free(priv);
		return NULL;
	}
	printf("Opened SpaceWire Router at %s, FD=%d\n", path, priv->fd);

	return priv;
}

/* Get Hardware information about router */
int router_hwinfo_get(void *p, struct router_hw_info *hwinfo)
{
	struct router_lib_priv *priv = p;

	if ( ioctl(priv->fd, GRSPWR_IOCTL_HWINFO, hwinfo) ) {
		printf("ROUTER_LIB(%d): Failed to get HW info: %d\n", priv->fd, errno);
		return -1;
	}

	return 0;
}

/* Configure Router. Leave field NULL in order to skip configuration
 */
int router_set_config(
	void *p,
	struct router_config *cfg,
	struct router_ps *ps,
	struct router_routes *routes)
{
	struct router_lib_priv *priv = p;

	if ( cfg && (ioctl(priv->fd, GRSPWR_IOCTL_CFG_SET, cfg) < 0) ) {
		printf("ROUTER_LIB: Failed to set Configuration (%d)\n", errno);
		return -1;
	}

	if ( ps && (ioctl(priv->fd, GRSPWR_IOCTL_PS_SET, ps) < 0) ) {
		printf("ROUTER_LIB: Failed to set Port Setup (%d)\n", errno);
		return -1;
	}

	if ( routes && (ioctl(priv->fd, GRSPWR_IOCTL_ROUTES_SET, routes) < 0) ) {
		printf("ROUTER_LIB: Failed to set Routes (%d)\n", errno);
		return -1;
	}

	return 0;
}

/* Get current Router Configuration. Leave field NULL in order to skip
 * configuration read.
 */
int router_get_config(
	void *p,
	struct router_config *cfg,
	struct router_ps *ps,
	struct router_routes *routes)
{
	struct router_lib_priv *priv = p;

	if ( cfg && (ioctl(priv->fd, GRSPWR_IOCTL_CFG_GET, cfg) < 0) ) {
		printf("ROUTER_LIB: Failed to get Configuration (%d)\n", errno);
		return -1;
	}

	if ( ps && (ioctl(priv->fd, GRSPWR_IOCTL_PS_GET, ps) < 0) ) {
		printf("ROUTER_LIB: Failed to get Port Setup (%d)\n", errno);
		return -1;
	}

	if ( routes && (ioctl(priv->fd, GRSPWR_IOCTL_ROUTES_GET, routes) < 0) ) {
		printf("ROUTER_LIB: Failed to get Routes (%d)\n", errno);
		return -1;
	}

	return 0;
}

/* Set Configuration Write Enable */
int router_set_we(void *p, int we)
{
	struct router_lib_priv *priv = p;

	if ( ioctl(priv->fd, GRSPWR_IOCTL_WE_SET, we) ) {
		printf("ROUTER_LIB: Failed to set WE: %d\n", errno);
		return -1;
	}

	return 0;
}

/* Set/Get Port Control/Status */
int router_port_ioc(void *p, struct router_port *opts)
{
	struct router_lib_priv *priv = p;

	if ( ioctl(priv->fd, GRSPWR_IOCTL_PORT, opts) ) {
		printf("ROUTER_LIB: Fail set/get Port Ctrl/Sts: %d\n", errno);
		return -1;
	}

	return 0;
}

/* Read Port Control and/or Port Status register */
int router_port_get(void *p, int port, unsigned int *ctrl, unsigned int *sts)
{
	struct router_port opts;

	opts.flag = 0;
	opts.port = port;
	if ( ctrl )
		opts.flag |= ROUTER_PORTFLG_GET_CTRL;
	if ( sts )
		opts.flag |= ROUTER_PORTFLG_GET_STS;
	if ( opts.flag == 0 )
		return 0;

	if ( router_port_ioc(p, &opts) < 0 ) {
		return -1;
	}

	if ( ctrl )
		*ctrl = opts.ctrl;
	if ( sts )
		*sts = opts.sts;
	return 0;
}

/* Write Port Control Register */
int routerl_port_ctrl(void *p, int port, unsigned int ctrl)
{
	struct router_port opts;

	opts.flag = ROUTER_PORTFLG_SET_CTRL;
	opts.port = port;
	opts.ctrl = ctrl;

	return router_port_ioc(p, &opts);
}

/* Write Port Status Register */
int router_port_sts(void *p, int port, unsigned int sts)
{
	struct router_port opts;

	opts.flag = ROUTER_PORTFLG_SET_STS;
	opts.port = port;
	opts.sts = sts;

	return router_port_ioc(p, &opts);
}

/* Set/Clear Router Configuration/Status Register */
int router_set_cfgsts(void *p, unsigned int cfgsts)
{
	struct router_lib_priv *priv = p;

	if ( ioctl(priv->fd, GRSPWR_IOCTL_CFGSTS_SET, cfgsts) ) {
		printf("ROUTER_LIB: Failed to set CFGSTS: %d\n", errno);
		return -1;
	}

	return 0;
}

/* Get Router Configuration/Status Register */
int router_get_cfgsts(void *p, unsigned int *cfgsts)
{
	struct router_lib_priv *priv = p;

	if ( ioctl(priv->fd, GRSPWR_IOCTL_CFGSTS_GET, cfgsts) ) {
		printf("ROUTER_LIB: Failed to get CFGSTS: %d\n", errno);
		return -1;
	}

	return 0;
}

/* Get Current Time-Code */
int router_get_tc(void *p, unsigned char *tc)
{
	struct router_lib_priv *priv = p;

	if ( ioctl(priv->fd, GRSPWR_IOCTL_TC_GET, tc) ) {
		printf("ROUTER_LIB: Failed to get CFGSTS: %d\n", errno);
		return -1;
	}

	return 0;
}

/******  HELP FUNCTIONS ******/

/* Set Instance ID */
int router_set_instance(void *p, unsigned char iid)
{
	struct router_config cfg;
	cfg.flags = ROUTER_FLG_IID;
	cfg.iid = iid;
	return router_set_config(p, &cfg, NULL, NULL);
}

/* Set SpaceWire Link Initialization Clock Divisor */
int router_set_idiv(void *p, unsigned char idiv)
{
	struct router_config cfg;
	cfg.flags = ROUTER_FLG_IDIV;
	cfg.idiv = idiv;
	return router_set_config(p, &cfg, NULL, NULL);
}

/* Set Timer Prescaler */
int router_set_tpresc(void *p, unsigned int prescaler)
{
	struct router_config cfg;
	cfg.flags = ROUTER_FLG_TPRES;
	cfg.timer_prescaler = prescaler;
	return router_set_config(p, &cfg, NULL, NULL);	
}

/* Set Timer Reload Value for a specific port */
int router_set_treload(void *p, int port, unsigned int reload)
{
	struct router_config cfg;
	cfg.flags = ROUTER_FLG_TRLD;
	if ( router_get_config(p, &cfg, NULL, NULL) < 0 )
		return -1;
	cfg.flags = ROUTER_FLG_TRLD;
	cfg.timer_reload[port] = reload;
	return router_set_config(p, &cfg, NULL, NULL);
}

/* Set routing table */
int router_set_routes(void *p, struct router_routes *routes)
{
	return router_set_config(p, NULL, NULL, routes);
}

/* Get routing table */
int router_get_routes(void *p, struct router_routes *routes)
{
	return router_get_config(p, NULL, NULL, routes);
}

/* Set Psort Setup */
int router_set_ps(void *p, struct router_ps *ps)
{
	return router_set_config(p, NULL, ps, NULL);
}
/* Get Port Setup */
int router_get_ps(void *p, struct router_ps *ps)
{
	return router_get_config(p, NULL, ps, NULL);
}

void router_print_hwinfo(struct router_hw_info *hwinfo)
{
	puts("Hardware Configuration of SpaceWire Router:");
	printf(" Number of SpW ports:           %d\n", hwinfo->nports_spw);
	printf(" Number of AMBA ports:          %d\n", hwinfo->nports_amba);
	printf(" Number of FIFO ports:          %d\n", hwinfo->nports_fifo);
	printf(" Timers available:              %s\n", hwinfo->timers_avail ? "YES" : "NO");
	printf(" Plug and Play available:       %s\n", hwinfo->pnp_avail ? "YES" : "NO");
	printf(" MAJOR Version:                 %d\n", hwinfo->ver_major);
	printf(" MINOR Version:                 %d\n", hwinfo->ver_minor);
	printf(" PATCH Version:                 %d\n", hwinfo->ver_patch);
	printf(" Current Instance ID:           %d\n", hwinfo->iid);
}
