/**

 * @file unicast_service.c

 * @brief Unicast service

 * @note Copyright (C) 2018 Richard Cochran <richardcochran@gmail.com>

 *

 * This program is free software; you can redistribute it and/or modify

 * it under the terms of the GNU General Public License as published by

 * the Free Software Foundation; either version 2 of the License, or

 * (at your option) any later version.

 *

 * This program is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 * GNU General Public License for more details.

 *

 * You should have received a copy of the GNU General Public License along

 * with this program; if not, write to the Free Software Foundation, Inc.,

 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335 USA.

 */

#include <stdlib.h>

#include <sys/queue.h>

#include <time.h>

#include "address.h"

#include "clock.h"

#include "missing.h"

#include "port.h"

#include "port_private.h"

#include "pqueue.h"

#include "print.h"

#include "unicast_service.h"

#include "util.h"

#define MIN_LOG_INTER_MESSAGE_PERIOD -7

#define MAX_LOG_INTER_MESSAGE_PERIOD 16

#define MAX_DURATION 3600

#define QUEUE_LEN 16

struct unicast_client_address {

	LIST_ENTRY(unicast_client_address) list;

	struct PortIdentity portIdentity;

	unsigned int message_types;

	struct address addr;

	time_t grant_tmo;

};

struct unicast_service_interval {

	LIST_HEAD(uca, unicast_client_address) clients;

	LIST_ENTRY(unicast_service_interval) list;

	struct timespec incr;

	struct timespec tmo;

	int log_period;

};

struct unicast_service {

	LIST_HEAD(usi, unicast_service_interval) intervals;

	struct pqueue *queue;

};

static struct timespec log_to_timespec(int log_seconds);

static int timespec_compare(struct timespec *a, struct timespec *b);

static void timespec_normalize(struct timespec *ts);

static int attach_grant(struct ptp_message *msg,

			struct request_unicast_xmit_tlv *req,

			int duration)

{

	struct grant_unicast_xmit_tlv *g;

	struct tlv_extra *extra;

	extra = msg_tlv_append(msg, sizeof(*g));

	if (!extra) {

		return -1;

	}

	g = (struct grant_unicast_xmit_tlv *) extra->tlv;

	g->type = TLV_GRANT_UNICAST_TRANSMISSION;

	g->length = sizeof(*g) - sizeof(g->type) - sizeof(g->length);

	g->message_type = req->message_type;

	g->logInterMessagePeriod = req->logInterMessagePeriod;

	g->durationField = duration;

	g->flags = GRANT_UNICAST_RENEWAL_INVITED;

	return 0;

}

static int compare_timeout(void *ain, void *bin)

{

	struct unicast_service_interval *a, *b;

	a = (struct unicast_service_interval *) ain;

	b = (struct unicast_service_interval *) bin;

	return timespec_compare(&a->tmo, &b->tmo);

}

static void initialize_interval(struct unicast_service_interval *interval,

				int log_period)

{

	LIST_INIT(&interval->clients);

	interval->incr = log_to_timespec(log_period);

	clock_gettime(CLOCK_MONOTONIC, &interval->tmo);

	interval->tmo.tv_nsec += 10000000;

	timespec_normalize(&interval->tmo);

	interval->log_period = log_period;

}

static void interval_increment(struct unicast_service_interval *i)

{

	i->tmo.tv_sec += i->incr.tv_sec;

	i->tmo.tv_nsec += i->incr.tv_nsec;

	timespec_normalize(&i->tmo);

}

static struct timespec log_to_timespec(int log_seconds)

{

	struct timespec ts = {0, 0};

	uint64_t ns;

	int i;

	if (log_seconds < 0) {

		log_seconds *= -1;

		for (i = 1, ns = 500000000ULL; i < log_seconds; i++) {

			ns >>= 1;

		}

		ts.tv_nsec = ns;

	} else {

		ts.tv_sec = 1 << log_seconds;

	}

	return ts;

}

/*

 * Returns:

 *    1  if 'a' is before 'b'

 *   -1  if 'a' is after  'b'

 *    0  otherwise

 */

static int timespec_compare(struct timespec *a, struct timespec *b)

{

	if (a->tv_sec < b->tv_sec) {

		return 1;

	}

	if (b->tv_sec < a->tv_sec) {

		return -1;

	}

	if (a->tv_nsec < b->tv_nsec) {

		return 1;

	}

	if (b->tv_nsec < a->tv_nsec) {

		return -1;

	}

	return 0;

}

static void timespec_normalize(struct timespec *ts)

{

	while (ts->tv_nsec >= NS_PER_SEC) {

		ts->tv_nsec -= NS_PER_SEC;

		ts->tv_sec++;

	}

}

static int unicast_service_clients(struct port *p,

				   struct unicast_service_interval *interval)

{

	struct unicast_client_address *client, *next;

	struct timespec now;

	int err = 0;

	err = clock_gettime(CLOCK_MONOTONIC, &now;;

	if (err) {

		pr_err("clock_gettime failed: %m");

		return err;

	}

	LIST_FOREACH_SAFE(client, &interval->clients, list, next) {

		pr_debug("%s wants 0x%x", pid2str(&client->portIdentity),

			 client->message_types);

		if (now.tv_sec > client->grant_tmo) {

			pr_debug("%s service of 0x%x expired",

				 pid2str(&client->portIdentity),

				 client->message_types);

			LIST_REMOVE(client, list);

			free(client);

			continue;

		}

		if (client->message_types & (1 << ANNOUNCE)) {

			if (port_tx_announce(p, &client->addr)) {

				err = -1;

			}

		}

		if (client->message_types & (1 << SYNC)) {

			if (port_tx_sync(p, &client->addr)) {

				err = -1;

			}

		}

	}

	return err;

}

static void unicast_service_extend(struct unicast_client_address *client,

				   struct request_unicast_xmit_tlv *req)

{

	struct timespec now;

	time_t tmo;

	int err;

	err = clock_gettime(CLOCK_MONOTONIC, &now;;

	if (err) {

		pr_err("clock_gettime failed: %m");

		return;

	}

	tmo = now.tv_sec + req->durationField;

	if (tmo > client->grant_tmo) {

		client->grant_tmo = tmo;

		pr_debug("%s grant of 0x%x extended to %ld",

			 pid2str(&client->portIdentity),

			 client->message_types, tmo);

	}

}

static int unicast_service_rearm_timer(struct port *p)

{

	struct unicast_service_interval *interval;

	struct itimerspec tmo;

	int fd;

	fd = p->fda.fd[FD_UNICAST_SRV_TIMER];

	memset(&tmo, 0, sizeof(tmo));

	interval = pqueue_peek(p->unicast_service->queue);

	if (interval) {

		tmo.it_value = interval->tmo;

		pr_debug("arming timer tmo={%ld,%ld}",

			 interval->tmo.tv_sec, interval->tmo.tv_nsec);

	} else {

		pr_debug("stopping unicast service timer");

	}

	return timerfd_settime(fd, TFD_TIMER_ABSTIME, &tmo, NULL);

}

static int unicast_service_reply(struct port *p, struct ptp_message *dst,

				 struct request_unicast_xmit_tlv *req,

				 int duration)

{

	struct ptp_message *msg;

	int err;

	msg = port_signaling_construct(p, &dst->address,

				       &dst->header.sourcePortIdentity);

	if (!msg) {

		return -1;

	}

	err = attach_grant(msg, req, duration);

	if (err) {

		goto out;

	}

	err = port_prepare_and_send(p, msg, TRANS_GENERAL);

	if (err) {

		pr_err("port %hu: signaling message failed", portnum(p));

	}

out:

	msg_put(msg);

	return err;

}

/* public methods */

int unicast_service_add(struct port *p, struct ptp_message *m,

			struct tlv_extra *extra)

{

	struct unicast_client_address *client = NULL, *ctmp, *next;

	struct unicast_service_interval *interval = NULL, *itmp;

	struct request_unicast_xmit_tlv *req;

	unsigned int mask;

	uint8_t mtype;

	if (!p->unicast_service) {

		return SERVICE_DISABLED;

	}

	req = (struct request_unicast_xmit_tlv *) extra->tlv;

	mtype = req->message_type >> 4;

	mask = 1 << mtype;

	switch (mtype) {

	case ANNOUNCE:

	case SYNC:

		break;

	case DELAY_RESP:

	case PDELAY_RESP:

		return SERVICE_GRANTED;

	default:

		return SERVICE_DENIED;

	}

	if (req->logInterMessagePeriod < MIN_LOG_INTER_MESSAGE_PERIOD ||

	    req->logInterMessagePeriod > MAX_LOG_INTER_MESSAGE_PERIOD) {

		return SERVICE_DENIED;

	}

	if (req->durationField > MAX_DURATION) {

		req->durationField = MAX_DURATION;

	}

	LIST_FOREACH(itmp, &p->unicast_service->intervals, list) {

		/*

		 * Remember the interval of interest.

		 */

		if (itmp->log_period == req->logInterMessagePeriod) {

			interval = itmp;

		}

		/*

		 * Find any client records, and remove any stale contract.

		 */

		LIST_FOREACH_SAFE(ctmp, &itmp->clients, list, next) {

			if (!addreq(transport_type(p->trp),

				    &ctmp->addr, &m->address)) {

				continue;

			}

			if (interval == itmp) {

				if (ctmp->message_types & mask) {

					/* Contract is unchanged. */

					unicast_service_extend(ctmp, req);

					return SERVICE_GRANTED;

				}

				/* This is the one to use. */

				client = ctmp;

				continue;

			}

			/* Clear any stale contracts. */

			ctmp->message_types &= ~mask;

			if (!ctmp->message_types) {

				LIST_REMOVE(ctmp, list);

				free(ctmp);

			}

		}

	}

	if (client) {

		client->message_types |= mask;

		unicast_service_extend(client, req);

		return SERVICE_GRANTED;

	}

	client = calloc(1, sizeof(*client));

	if (!client) {

		return SERVICE_DENIED;

	}

	client->portIdentity = m->header.sourcePortIdentity;

	client->message_types = mask;

	client->addr = m->address;

	unicast_service_extend(client, req);

	if (!interval) {

		interval = calloc(1, sizeof(*interval));

		if (!interval) {

			free(client);

			return SERVICE_DENIED;

		}

		initialize_interval(interval, req->logInterMessagePeriod);

		LIST_INSERT_HEAD(&p->unicast_service->intervals, interval, list);

		if (pqueue_insert(p->unicast_service->queue, interval)) {

			LIST_REMOVE(interval, list);

			free(interval);

			free(client);

			return SERVICE_DENIED;

		}

		unicast_service_rearm_timer(p);

	}

	LIST_INSERT_HEAD(&interval->clients, client, list);

	return SERVICE_GRANTED;

}

void unicast_service_cleanup(struct port *p)

{

	struct unicast_service_interval *itmp, *inext;

	struct unicast_client_address *ctmp, *cnext;

	if (!p->unicast_service) {

		return;

	}

	LIST_FOREACH_SAFE(itmp, &p->unicast_service->intervals, list, inext) {

		LIST_FOREACH_SAFE(ctmp, &itmp->clients, list, cnext) {

			LIST_REMOVE(ctmp, list);

			free(ctmp);

		}

		LIST_REMOVE(itmp, list);

		free(itmp);

	}

	pqueue_destroy(p->unicast_service->queue);

	free(p->unicast_service);

}

int unicast_service_deny(struct port *p, struct ptp_message *m,

			 struct tlv_extra *extra)

{

	struct request_unicast_xmit_tlv *req =

		(struct request_unicast_xmit_tlv *) extra->tlv;

	return unicast_service_reply(p, m, req, 0);

}

int unicast_service_grant(struct port *p, struct ptp_message *m,

			  struct tlv_extra *extra)

{

	struct request_unicast_xmit_tlv *req =

		(struct request_unicast_xmit_tlv *) extra->tlv;

	return unicast_service_reply(p, m, req, req->durationField);

}

int unicast_service_initialize(struct port *p)

{

	struct config *cfg = clock_config(p->clock);

	if (!config_get_int(cfg, p->name, "unicast_listen")) {

		return 0;

	}

	if (config_set_section_int(cfg, p->name, "hybrid_e2e", 1)) {

		return -1;

	}

	p->unicast_service = calloc(1, sizeof(*p->unicast_service));

	if (!p->unicast_service) {

		return -1;

	}

	LIST_INIT(&p->unicast_service->intervals);

	p->unicast_service->queue = pqueue_create(QUEUE_LEN, compare_timeout);

	if (!p->unicast_service->queue) {

		free(p->unicast_service);

		return -1;

	}

	p->inhibit_multicast_service =

		config_get_int(cfg, p->name, "inhibit_multicast_service");

	return 0;

}

void unicast_service_remove(struct port *p, struct ptp_message *m,

			    struct tlv_extra *extra)

{

	struct unicast_client_address *ctmp, *next;

	struct cancel_unicast_xmit_tlv *cancel;

	struct unicast_service_interval *itmp;

	unsigned int mask;

	uint8_t mtype;

	if (!p->unicast_service) {

		return;

	}

	cancel = (struct cancel_unicast_xmit_tlv *) extra->tlv;

	if (cancel->message_type_flags & CANCEL_UNICAST_MAINTAIN_REQUEST) {

		return;

	}

	mtype = cancel->message_type_flags >> 4;

	mask = 1 << mtype;

	switch (mtype) {

	case ANNOUNCE:

	case SYNC:

		break;

	case DELAY_RESP:

	case PDELAY_RESP:

	default:

		return;

	}

	LIST_FOREACH(itmp, &p->unicast_service->intervals, list) {

		LIST_FOREACH_SAFE(ctmp, &itmp->clients, list, next) {

			if (!addreq(transport_type(p->trp),

				    &ctmp->addr, &m->address)) {

				continue;

			}

			if (ctmp->message_types & mask) {

				ctmp->message_types &= ~mask;

				if (!ctmp->message_types) {

					LIST_REMOVE(ctmp, list);

					free(ctmp);

				}

				return;

			}

		}

	}

}

int unicast_service_timer(struct port *p)

{

	struct unicast_service_interval *interval;

	int err = 0, master = 0;

	struct timespec now;

	if (!p->unicast_service) {

		return 0;

	}

	clock_gettime(CLOCK_MONOTONIC, &now;;

	switch (p->state) {

	case PS_INITIALIZING:

	case PS_FAULTY:

	case PS_DISABLED:

	case PS_LISTENING:

	case PS_PRE_MASTER:

	case PS_PASSIVE:

	case PS_UNCALIBRATED:

	case PS_SLAVE:

		break;

	case PS_MASTER:

	case PS_GRAND_MASTER:

		master = 1;

		break;

	}

	while ((interval = pqueue_peek(p->unicast_service->queue)) != NULL) {

		pr_debug("peek i={2^%d} tmo={%ld,%ld}", interval->log_period,

			 interval->tmo.tv_sec, interval->tmo.tv_nsec);

		if (timespec_compare(&now, &interval->tmo) > 0) {

			break;

		}

		interval = pqueue_extract(p->unicast_service->queue);

		if (master && unicast_service_clients(p, interval)) {

			err = -1;

		}

		if (LIST_EMPTY(&interval->clients)) {

			pr_debug("retire interval 2^%d", interval->log_period);

			LIST_REMOVE(interval, list);

			free(interval);

			continue;

		}

		interval_increment(interval);

		pr_debug("next i={2^%d} tmo={%ld,%ld}", interval->log_period,

			 interval->tmo.tv_sec, interval->tmo.tv_nsec);

		pqueue_insert(p->unicast_service->queue, interval);

	}

	if (unicast_service_rearm_timer(p)) {

		err = -1;

	}

	return err;

}