/* 
 * Real-Time and Multimedia Systems Laboratory
 * Copyright (c) 2000 Carnegie Mellon University
 * All Rights Reserved.
 * 
 * Permission to use, copy, modify and distribute this software and its
 * documentation is hereby granted, provided that both the copyright
 * notice and this permission notice appear in all copies of the
 * software, derivative works or modified versions, and any portions
 * thereof, and that both notices appear in supporting documentation.
 * 
 * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS"
 * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND FOR
 * ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
 * 
 * Carnegie Mellon requests users of this software to return to
 * 
 *  Real-Time and Multimedia Systems Laboratory
 *  Attn: Prof. Raj Rajkumar
 *  Electrical and Computer Engineering, and Computer Science
 *  Carnegie Mellon University
 *  Pittsburgh PA 15213-3890
 *
 *  or via email to raj@ece.cmu.edu
 * 
 * any improvements or extensions that they make and grant Carnegie Mellon
 * the rights to redistribute these changes.
 */
/*
 * Portable QoS manager, implementing the comcepts described in
 * ``Adaptive Bandwidth Reservation for Multimedia Computing''
 * by L. Abeni and G. Buttazzo
 * IEEE Real Time Computing Systems and Applications 1999
 *
 *			program developed by Luca Abeni
 *					luca@sssup.it
 *					http://hartik.sssup.it/~luca
 */

/************************************************************************/
/*	This file contains the OS dependent layer of the portable	*/
/*	QoS manager							*/
/************************************************************************/

#include <arch/local.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <errno.h>
#include <stdarg.h>

#include <rk/rk.h>

#define CSERV  1000000
#define TSERV 10000000

/*
#define QOSDBG
*/

/************************************************************************/
/*	These parameters define the reserve time for MM tasks...	*/
/*	PPS Tasks always use HARD enforcement				*/
/************************************************************************/
#define ENF_MODE        RSV_HARD
#define ENF_MODE	RSV_SOFT
#define SCH_MODE	RSV_HARD
#define REP_MODE	RSV_HARD

#define MAX_RSV 100

static struct qtask_local localdescr;

/*
 * This is the printf function used for debugging...
 * Writes on standard error, no buffering.
 */
int cprintf(char *s, ...)
{
    char buff[1024];
    va_list parms;
    int result;

    va_start(parms, s);
    result = vsprintf(buff, s, parms);
    va_end(parms);
    write(2, buff, strlen(buff));

    return (result);
}

/*
 * Register/Unregister QoS task
 * Called by the QoS manager task for allocating/deallocationg
 * a qtask structure
 */
struct qtask_global *manager_registertask(void)
{
    struct qtask_global *p;

    p = malloc(sizeof(struct qtask_global));

    return p;
}
void manager_unregistertask(struct qtask_global *p)
{
    free(p);
}

/*
 * Exits the program printing an error message...
 */
void qos_abort(int code)
{
    cprintf("Aborting... code = %d\n", code);
    exit(-code);
}

/*
 * Computes the system utilization and return the free bandwidth
 * (multiplied by 1000000)
 */
DWORD sys_utilization(float rmu)
{
    DWORD tot, res;
    DWORD t, q, used;
    int nrs, i;
    rk_reserve_t rsvs[MAX_RSV];
    rk_reserve_t rsv;
    cpu_reserve_attr_data_t param;

    if (rk_cpu_reserves_get_scheduling_policy() == 0) {
	if ((rmu <= 0) || (rmu > 1)) {
	    tot = 66000;
	} else {
	    tot = rmu * 100000;
	}
    } else {
	tot = 100000;
    }
    nrs = rk_cpu_reserves_get_list(rsvs, MAX_RSV * 4);
    if (nrs <= 0) {
	cprintf("Error calculating free BW\n");
	return 0;
    }
    cprintf("%d reserves\n", nrs);
    used = 0;
    for (i = 0; i < nrs; i++) {
	rsv = rsvs[i];
	cprintf("RS #%d: %p   %p\n", i, rsvs[i], rsv);
	rk_cpu_reserve_get_attr(rsv, &param);
	q =
	    param.compute_time.tv_sec * 1000000 +
	    param.compute_time.tv_nsec / 1000;
	t = param.period.tv_sec * 1000000 + param.period.tv_nsec / 1000;
	cprintf("Addin' (%lu %lu)\n", q, t);
	used += (q * 1000 / (t / 100));

    }

    cprintf("Used %lu\n", used);
    //res = (100000 - tot) + (CSERV / (TSERV / 10000));
    res = 100000 - tot + used;
    return res;
}

/*
 * Allocates a local qtask structure
 */
struct qtask_local *getfreedescriptor(void)
{
    return &localdescr;
}

/*
 * Returns the local qtask structure
 * associated with the current task
 */
struct qtask_local *getdescr(void)
{
    return &localdescr;
}

ID currenttask(void)
{
    struct qtask_local *p;

    p = getdescr();
    return p->id;
}


/*
 * Create and initialize a MM or a PPS task
 */
ID inittask(char *name, void (*body) (WORD),
	    WORD arg, int w, DWORD q, DWORD period, DWORD drel, BYTE model)
{
    pid_t pid;
    rk_resource_set_t rs;
    rk_reserve_t reserve;
    rk_reserve_param_data_t p;
    struct timespec rq, t, z;
    cpu_reserve_attr_data_t attr;
    int res;
    struct qtask_local *myself;

    void qtask_register(ID task, float b, int w, DWORD p, DWORD dl,
			DWORD q);
    void pstask_unregister(ID task);

#ifdef QOSDBG
    cprintf("Creatin' task with period...%d\n", period);
#endif

    rs = rk_resource_set_create(name);
    if (rs == NULL) {
	cprintf("Cannot create Resource Set...\n");
	return NULLID;
    }
    memset(&z, 0, sizeof(struct timespec));
    rq.tv_sec = q / 1000000;
    rq.tv_nsec = (q % 1000000) * 1000;
    t.tv_sec = period / 1000000;
    t.tv_nsec = (period % 1000000) * 1000;
    p.enf_mode = ENF_MODE;
    p.sch_mode = SCH_MODE;
    p.rep_mode = REP_MODE;
    attr.compute_time = rq;
    attr.period = t;
    attr.deadline = t;
    attr.blocking_time = z;
    attr.start_time = z;
    attr.reserve_type = p;
    reserve = rk_cpu_reserve_create(rs, &attr);
    if (reserve == 0) {
	cprintf("Cannot create the CPU reserve for task %s!!!\n", name);
	return NULLID;
    }
    cprintf("Reserve = %p\n", reserve);

    pid = fork();
    if (pid < 0) {
	perror("Cannot fork task!!!\n");
	return NULLID;
    }
    if (pid == 0) {
/* Remember: My ID is my resource set + my rsv... */
	qtask_register(reserve, USEC2BAND(q, period), w, period, drel, q);

	/* Make periodic... */
	qostask_init(period);
	while (rt_make_periodic(&t, &z) < 0) {
	}
	myself = getdescr();
	myself->id = reserve;

	cprintf("In child...");

	body(arg);
	/* We should not arrive here... */
	cprintf("Task finished...\n");
	rk_resource_set_destroy(rs);
	qtask_unregister(reserve);
	exit(0);
    }

    res = rk_resource_set_attach_process(rs, pid);
    return reserve;
}

ID initpstask(char *name, TASK(*body) (WORD), WORD arg, float weight,
	      DWORD q)
{
    pid_t pid;
    rk_resource_set_t rs;
    rk_reserve_t reserve;
    rk_reserve_param_data_t p;
    struct timespec rq, t, z;
    cpu_reserve_attr_data_t attr;
    int res;
    struct qtask_local *myself;

    void pstask_register(ID task, int w, DWORD q);
    void pstask_unregister(ID task);

#ifdef QOSDBG
    cprintf("Creatin' PS task\n");
#endif

    rs = rk_resource_set_create(name);
    memset(&z, 0, sizeof(struct timespec));
    rq.tv_sec = q / 1000000;
    rq.tv_nsec = (q % 1000000) * 1000;
    /* Create the task with a long period (like a bkg task...) */
    t.tv_sec = 1;
    t.tv_nsec = 0;
    p.enf_mode = /*ENF_MODE */ RSV_HARD;
    p.sch_mode = SCH_MODE;
    p.rep_mode = REP_MODE;
    attr.compute_time = rq;
    attr.period = t;
    attr.deadline = t;
    attr.blocking_time = z;
    attr.start_time = z;
    attr.reserve_type = p;
    reserve = rk_cpu_reserve_create(rs, &attr);
    if (reserve == 0) {
	cprintf("Cannot create the CPU reserve for task %s!!!\n", name);
	return NULLID;
    }

    pid = fork();
    if (pid < 0) {
	perror("Cannot fork task!!!\n");
	return NULLID;
    }
    if (pid == 0) {
	myself = getdescr();
	myself->id = reserve;
	pstask_register(reserve, weight, q);

	body(arg);
	/* We should not arrive here... */
	cprintf("PS task finished...\n");
	rk_resource_set_destroy(rs);
	pstask_unregister(reserve);
	exit(0);
    }

    res = rk_resource_set_attach_process(rs, pid);
    return reserve;
}

int endinstance(void)
{
    return rt_wait_for_next_period();
}

/*
 * Returns the fraction of CPU bandwidth currently allocated to a task
 */
float glob_getband(ID task)
{
    rk_reserve_t rsv;
    cpu_reserve_attr_data_t param;
    DWORD q, t;
    float band;

    rsv = currenttask();

    rk_cpu_reserve_get_attr(rsv, &param);
    q =
	param.compute_time.tv_sec * 1000000 +
	param.compute_time.tv_nsec / 1000;
    t = param.deadline.tv_sec * 1000000 + param.deadline.tv_nsec / 1000;

    band = USEC2BAND(q, t);

    return band;
}

/*
 * Creates the QoS Manager task
 */
int server_create(void)
{
    rk_resource_set_t server_rs;
    rk_reserve_t server_reserve;
    rk_reserve_param_data_t p;
    struct timespec q, t, z;
    cpu_reserve_attr_data_t attr;
    pid_t server_pid;
    char name[256];
    int res;

    extern TASK manager(void);

    sprintf(name, "QoS Manager");
    server_rs = rk_resource_set_create(name);
    if ((server_rs == NULL) || ((int) server_rs == -1)) {
	return -1;
    }
    memset(&z, 0, sizeof(struct timespec));
    q.tv_sec = 0;
    q.tv_nsec = CSERV;		/* Computation time: 1 ms */
    t.tv_sec = 0;
    t.tv_nsec = TSERV;		/* Period: 10 ms */
    p.enf_mode = RSV_HARD;
    p.sch_mode = RSV_HARD;
    p.rep_mode = RSV_HARD;
    attr.compute_time = q;
    attr.period = t;
    attr.deadline = t;
    attr.blocking_time = z;
    attr.start_time = z;
    attr.reserve_type = p;
    server_reserve = rk_cpu_reserve_create(server_rs, &attr);
    if ((server_reserve == NULL) || ((int) server_reserve == -1)) {
	cprintf("Cannot create the CPU reserve for the QoS Manager!!!\n");
	return -1;
    }

    server_pid = fork();
    if (server_pid < 0) {
	perror("Cannot fork the QoS Manager task!!!\n");
	return -1;
    }
    if (server_pid == 0) {
	manager();
	/* We should not arrive here... */
	cprintf("Something went wrong...\n");
	qos_abort(200);
    }

    res = rk_resource_set_attach_process(server_rs, server_pid);
    return server_pid;
}

/*
 * Communications between QoS Library stubs and QoS Manager task
 */
/* Err... Probably we will need to store the message dimension in
	a general way. For now, let's make it simple...
*/
static int msgdim = 0;
PORT makeport(char *name, int dim, int mode)
{
    int res;
    char localname[256];

#ifdef QOSDBG
    cprintf("Makeport: creating port for messages with dim: %d\n", dim);
#endif

    sprintf(localname, "/tmp/%s", name);
    res = mkfifo(localname, S_IRWXU);
    if ((res < 0) && (errno != EEXIST)) {
	perror("Cannot create QoS port:");
	return -1;
    }

    res = open(localname, mode);
    if (res < 0) {
	perror("Cannot connect to QoS port:");
	return -1;
    }

    msgdim = dim;

    return res;
}

PORT connecttoport(char *name, int size, int mode)
{
    int res = -1;
    char localname[256];

    sprintf(localname, "/tmp/%s", name);

#ifdef QOSDBG
    cprintf("Waiting for the %s port\n", name);
#endif
    while (res < 0) {
	res = open(localname, mode);
#ifdef QOSDBG
	cprintf("%s --> ", localname);
	perror("Not yet:");
#endif
    }
#ifdef QOSDBG
    cprintf("Connected\n");
#endif

    msgdim = size;

    return res;			/* In Linux, this is not used... */
}

int receivefromport(PORT port, void *p)
{
    int res;

    res = read(port, p, msgdim);

    if (res == 0) {
	cprintf("Port %d: EOF?\n", port);
	perror("");
	return 0;
    }
    if (res < 0) {
	cprintf("[%d] -->", port);
	perror("Error reading from QoS port:");
	return -1;
    }

    return 1;
}

int sendtoport(PORT port, void *p)
{
    write(port, p, msgdim);

    return 1;
}

/************************************************************************/
/*	And these are the functions to compute the scheduling error!	*/
/************************************************************************/
#include <signal.h>
static int replcount;
static int period;
static unsigned int epsilon;
static int policy;

void myhandler(int signum, siginfo_t * info, void *data)
{
#ifdef __QOS_DBG__
    cprintf("Err[%d]: %ld\n", getpid(), info->si_int);
#endif
    epsilon = info->si_int;
    replcount++;
}

int qostask_init(int p)
{
    int res;
    struct sigaction act;

    replcount = 0;
    period = p;
    memset(&act, 0, sizeof(struct sigaction));
    act.sa_sigaction = myhandler;
    act.sa_flags = SA_SIGINFO;

    res = sigaction(SIGRTMIN + 1, &act, NULL);
    if (res < 0) {
	perror("SigAction failed: ");
    }

    policy = rk_cpu_reserves_get_scheduling_policy();
    return res;
}

int howlateOld(void)
{
    int res;

    res = replcount * period;
/*    res = replcount; */
    replcount = 0;
    return res;
}

DWORD howlate(ID dummy)
{
    int res;

    if (policy == 2) {		/* EDF */
	res = rt_get_scheduling_error();
    } else {
	res = (replcount - 1) * period;
	if (res < 0) {
	    res = 0;
	}
/*    res = epsilon; */
    }

    return res;
}

void resetcount(void)
{
    if (epsilon < period + period / 2) {
	epsilon = 0;
    }
    replcount = 0;
}
