/*
 * hwapi_linux.c
 *
 * Copyright (c) 2009 Ismael Gomez-Miguelez, UPC <ismael.gomez at tsc.upc.edu>,
 *                    Xavier Reves, UPC <xavier.reves at tsc.upc.edu>
 * All rights reserved.
 *
 *
 * This file is part of ALOE.
 *
 * ALOE 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 3 of the License, or
 * (at your option) any later version.
 *
 * ALOE 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 ALOE.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
/**
 * @file phal_hw_api.c
 * @brief ALOE Hardware Library LINUX Implementation
 *
 * This file contains the implementation of the HW Library for
 * linux platforms.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sched.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/msg.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/wait.h>
#include <sys/time.h>
#include <time.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <dirent.h>
#include <sys/resource.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/sem.h>
#include <setjmp.h>

#define __USE_GNU
#include <ucontext.h>
#include <execinfo.h>

#include "mem.h"
#include "phal_hw_api.h"
#include "hwapi_backd.h"

#define VERBOSE_MEM

//#define PRINT_CPUDAEMONS_LEN 1000

#ifdef PRINT_CPUDAEMONS_LEN
char cpusecData[50*PRINT_CPUDAEMONS_LEN];
FILE *cpusecFile;
int cpusecCnt=0;
#endif

//#define REPORT_STARTEND


/*#define PRINT_OBJ_TIME
#define DEBUG_ITF
*/
/* fraction of the time slot sw daemons must sleep */
#define DAEMON_REL_FRACTION	4

#define STRING_BUFF             (SYSTEM_STR_MAX_LEN*2)

int sigusr_set = 0;

/* general string buffers */
char aux_string[STRING_BUFF];
char filelockname[STRING_BUFF];

/** hw_api general database */
struct hw_api_db *hw_api_db;

/** pointer to proc db */
struct hwapi_proc_i *proc_db;

/** save my proc pointer */
struct hwapi_proc_i *my_proc;

struct proc_time *my_ptime;

int pidx;

struct launch_daemons *daemon_i;

/** shared vars db */
struct hwapi_var_i *var_db;

/** time slot duration pointer*/
int *ts_len;

/** pointer to internal interfaces db */
struct int_itf *int_itf;

/** init interface id counter */
int last_id = 0;

int mem_silent = 0;

int hwapi_initated = 0;

/** shared memory ids */
int api_shm_id, api_msg_id, data_msg_id;

/** structs for messages */
struct msgbuff msg;

/** internal itf database definition */
struct file_des {
    int int_itf_idx;
    int id;
    int mode;
    int pending_data;
    int pending_rpm;
    int *pending_pkt;
};

/** number of internal interfaces (for a single process) */
#define MAX_FILE_DES	100

/** database for local itf */
struct file_des file_des[MAX_FILE_DES];

/************************************************
 INTERNAL FUNCTION DECLARATIONS
 *************************************************/
char send_parent_cmd(char cmd, char *data, int sz_data, char *result, int sz_res);
void get_time(time_t * t);
void get_sem(int sem_id, int n);
int hwapi_itf_seturgent(hwitf_t itf_id, void callback(hwitf_t id), hwitf_t remote_itf_id);
int hwapi_var_writemyts(varid_t stat_id, void *value, int value_sz, int tstamp);
void force_relinquish();
void recv_urgent_signal(int value);



typedef union semun {
    int val;
    struct semid_ds *buf;
    ushort * array;
} semun_t;


#define MAX_MEM_SEGMENTS 40

struct memseg {
    void *ptr;
    int elem_sz;
};
struct memseg memseg[MAX_MEM_SEGMENTS];

void (*term_fnc)(void) = NULL;

int bt_printed=0;
int isterming=0;
void print_bt(ucontext_t *uc)
{
    int i, trace_size;
    void *trace[16];
    char **messages = (char **) NULL;

    if ((hw_api_db->printbt_atterm && my_proc && my_proc->status.cur_status<5)
            || hw_api_db->printbt_atexit) {
        trace_size = backtrace(trace, 16);
/*        trace[1]=uc->uc_mcontext.gregs[REG_EBP];
*/        messages = backtrace_symbols(trace, trace_size);
        printf("[bt] Execution path size %d:\n", trace_size);
        bt_printed=1;
        for (i = 1; i < trace_size; ++i)
            printf("[bt] %s\n", messages[i]);
    }
}

void print_xbt(void)
{
    if (!mem_silent)
        print_bt(NULL);
}
void bt_sighandler(int sig, siginfo_t *info,
        void *secret)
{
    ucontext_t *uc = (ucontext_t *) secret;
    /* Do something useful with siginfo_t */
    if (sig == SIGSEGV)
        printf("Got signal %d, faulty address is %p\n", sig, info->si_addr);
    else
        printf("Got signal %d\n", sig);

    if (sig==SIGTERM || sig==SIGKILL) {
        isterming=1;
    }
    print_bt(uc);
    fflush(NULL);
    exit(0);
}


/** @defgroup hwapifunc Hardware API Functions Implementation
 *
 * Here HW API functions are implemented for Linux Platforms
 *
 * @{
 */

/** @defgroup base Base API Functions
 *
 * This functions include some common and basic
 * functions required to use the HW API Library
 *
 * @{
 */
int find_lock(int pid, char *file_name, int max_len)
{
    DIR *dir;
    struct dirent *ent;
    FILE *f;
    int fpid;

    if (!(dir = opendir(LOCK_PATH))) {
        perror("opendir");
        return 0;
    }
    while ((ent = readdir(dir))) {
        if (strstr(ent->d_name, LOCK_PREFIX) == ent->d_name) {
            snprintf(file_name, max_len, "%s/%s", LOCK_PATH, ent->d_name);
            f = fopen(file_name, "r");
            if (f) {
                fscanf(f, "%d", &fpid);
                fclose(f);
                if (fpid == pid) {
                    closedir(dir);
                    return 1;
                }
            } else {
                printf("Error opening lock candidate %s\n", file_name);
                perror("Reason");
            }
        }
    }

    closedir(dir);
    return 0;
}


void hwapi_atterm(void (*fnc)(void))
{
    term_fnc = fnc;
}

void exit_fnc(void)
{
    if (hwapi_initated) {
        if (hw_api_db->printbt_atexit && !bt_printed) {
            print_bt(NULL);
        }
        if (term_fnc!=NULL) {
            term_fnc();
        }
    }
    exit(0);
}

int signaled=0;

void sigusr2_process(int sig, siginfo_t *info,
        void *secret) {

    if (info->si_pid!=hw_api_db->exec_pid) {
    	recv_urgent_signal(info->si_value.sival_int);
    } else {
        signaled=1;
        force_relinquish();
    }
}

void *jmpbuffptr;
static jmp_buf jump_buffer;

/** Initialize Hardware API.
 *
 * Initializes hwapi library. This function must be called before
 * calling any other hwapi function.
 *
 *
 * @return 1 if successfully registered
 * @return 0 if failed
 *
 */
int hwapi_init(void)
{
    int *ptr;
    semun_t arg;
    key_t key;
    int flags;
    int i, n;
    flags = 0;

    struct sigaction sa;
    jmpbuffptr=jump_buffer;

    sa.sa_sigaction = (void *) bt_sighandler;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = SA_RESTART | SA_SIGINFO;

    sigaction(SIGSEGV, &sa, NULL);
    sigaction(SIGBUS, &sa, NULL);
    sigaction(SIGILL, &sa, NULL);
    sigaction(SIGFPE, &sa, NULL);

    /* action to forced relinquish */
    sa.sa_sigaction = (void *) sigusr2_process;
    if (sigaction(SIGUSR2, &sa, NULL)) {
        perror("sigaction");
    }

    hwapi_mem_init();

    /* try to attach to parent process shared memory */
    if (!find_lock(getppid(), filelockname, STRING_BUFF)) {
        printf("HWAPI: Error could not find or access my parent process lock file.\n");
        exit(0);
    }

    key = ftok(filelockname, HW_API_SHM_KEY);
    api_shm_id = shmget(key, sizeof (struct hw_api_db), flags);
    if (api_shm_id < 0) {
        printf("Error shared memory for api doesn't exist. Run console first\n");
        perror("shmget");
        return 0;
    }

    ptr = (int *) shmat(api_shm_id, NULL, 0);
    if (ptr == (int *) - 1) {
        perror("shmat");
        return 0;
    }

    hw_api_db = (struct hw_api_db *) ptr;
    proc_db = hw_api_db->proc_db;
    int_itf = hw_api_db->int_itf;
    var_db = hw_api_db->var_db;
    ts_len = &hw_api_db->cpu_info.tslen_usec;

    /* get lock key for control messages */
    key = ftok(filelockname, HW_API_MSG_KEY);

    /* get id for control messages */
    api_msg_id = msgget(key, flags);
    if (api_msg_id < 0) {
        printf("Error initiating message queue. Console not initiated\n");
        perror("msgget");
        return 0;
    }

    arg.val = 1;
    if (hw_api_db->status_sem_id) {
        if (semctl(hw_api_db->status_sem_id, 0, SETVAL, arg) < 0) {
            perror("semctl failure");
            return 0;
        }
    }
    
    /* initialize internal interface database */
    memset(file_des, 0, MAX_FILE_DES * sizeof (struct file_des));

    hwapi_initated=1;

    /* check if I am an object process or a daemon */
    i = 0;
    n = getpid();
    while (i < MAX_PROCS && proc_db[i].pid != n) {
        i++;
    }
    /* we are a process, quit */
    if (i < MAX_PROCS) {
        pidx = i;
        my_proc = &proc_db[i];
        my_ptime = &hw_api_db->proc_time[i];
        /* set to relinquished */
        my_proc->relinquished = 1;
        my_proc->status.next_tstamp = get_tstamp();
        daemon_i = NULL;

        atexit(exit_fnc);

        if (hw_api_db->printbt_atterm) {
            sigaction(SIGTERM, &sa, NULL);
        }

        return 1;
    }

    /* we are a daemon, find which and return */
    i = 0;
    while (i < MAX_DAEMONS && hw_api_db->daemons[i].pid != n) {
        i++;
    }
    if (i == MAX_DAEMONS) {
        printf("CAUTION! did not find daemon in daemon's db\n");
        return 1;
    }

    daemon_i = &hw_api_db->daemons[i];
    my_proc = NULL;

    #ifdef PRINT_CPUDAEMONS_LEN
    cpusecFile = fopen(strcat(daemon_i->path,".cpuRep"),"w");
    #endif



    return 1;
}

/** Exit function.
 *
 * Clears hwapi resources, no more hwapi functions can be called after
 * this functions is called.
 *
 * The function does NOT kill process thread, just clears shared memory resources
 *
 */
void hwapi_exit()
{
    int i;
    
    for (i=0;i<MAX_PROCS;i++) {
        if (proc_db[i].exec_position > my_proc->exec_position && my_proc->core_idx == proc_db[i].core_idx) {
            proc_db[i].exec_position--;
            assert(post_sem_n(hw_api_db->semids[my_proc->core_idx],proc_db[i].exec_position+1));
        }
    }

    /* clear shared memory resources */
    shmdt(hw_api_db);
    hwapi_initated=0;
    exit(0);
}


void force_relinquish()
{
	longjmp(jump_buffer,1);
}

/** Process Relinquish.
 *
 * Function used by processes to relinquish their cpu ownership.
 * This function must be called by SERVICES API before the end of the
 * time slot, otherwise RTCFault will be flagged.
 *
 * @param slots number of slots to relinquish
 * @return >=0 number of processes
 */
void hwapi_relinquish(int slots)
{
    unsigned int actual_time, tstamp;
    int sleep_time;
    time_t tdata,tinislot;

    /* check params */
    if (!slots || slots < 0) {
        slots = 1;
    }

    if (hw_api_db->flushout) {
        fflush(NULL);
    }

    /* get actual time */
    get_time(&tdata);
    
    tstamp = time_to_tstamp(&tdata);
    tstamp_to_time(tstamp,&tinislot);

    my_proc->relinquished = 1;
    my_proc->rtstamp++;
    my_proc->rel_tstamp = tstamp;
    memcpy(&my_proc->tdata[2],&tdata,sizeof(time_t));
    get_time_interval(my_proc->tdata);

    my_proc->sys_start=my_proc->tdata[1].tv_usec-tinislot.tv_usec;
    my_proc->sys_end=my_proc->tdata[2].tv_usec-tinislot.tv_usec;
    my_proc->sys_cpu = my_proc->tdata[0].tv_usec;

    post_sem_n(hw_api_db->semids[my_proc->core_idx],my_proc->exec_position+1);
    wait_sem_n(hw_api_db->semids[my_proc->core_idx],my_proc->exec_position);
    my_proc->cur_tstamp = my_proc->rel_tstamp+1;

    /* check periodicity */
    get_time(&my_proc->tdata[2]);
    get_time_interval(my_proc->tdata);
    get_time(&my_proc->tdata[1]);
    
    my_proc->sys_period = my_proc->tdata[0].tv_usec;
    my_proc->relinquished = 0;

#ifdef PRINT_OBJ_TIME
    printf
            ("%d\tOBJECT %s:\tTS %d. CPU %d usec. TTI %d usec. Start %d:%d\n",
            my_proc->exec_position,my_proc->obj_name,
            my_proc->obj_tstamp, my_proc->sys_cpu, my_proc->sys_period, my_proc->tdata[1].tv_sec, my_proc->tdata[1].tv_usec);
#endif


}
int ts_cnt=0;

int hwapi_getsubslot_idx()
{
	return ts_cnt;
}

/** Daemon Relinquish.
 *
 * Relinquish for daemons.
 *
 */
void hwapi_relinquish_daemon()
{
    int tstamp,actual_time,sleep_time;
    time_t tdata;
    int i;

    /* get actual time */
    get_time(&tdata);
    if (daemon_i) {
        memcpy(&daemon_i->cpu[2],&tdata,sizeof(time_t));
        get_time_interval(daemon_i->cpu);
        daemon_i->cpu_usec = daemon_i->cpu[0].tv_usec;

        #ifdef PRINT_CPUDAEMONS_LEN
        sprintf(cpusecData,"%s,%d",cpusecData,daemon_i->cpu_usec);
        cpusecCnt++;
        if (cpusecCnt==PRINT_CPUDAEMONS_LEN) {
            cpusecCnt=0;
            fputs(cpusecData,cpusecFile);
        }
        #endif

    }
    get_time(&tdata);
    tstamp = time_to_tstamp(&tdata);
    actual_time = (unsigned int) tdata.tv_sec * 1000000 + tdata.tv_usec;

    if (daemon_i && strstr(daemon_i->path,"exec")) {
        if (hw_api_db->dac.ptr) {
            wait_sem_n(hw_api_db->semdaemons,daemon_i->semid);
        } else {
            sleep_time = ((tstamp) * (*ts_len)+(ts_cnt+1)*(*ts_len)/hw_api_db->exec_slot_div) - actual_time;
        	ts_cnt++;
			if (ts_cnt==hw_api_db->exec_slot_div) {
				ts_cnt=0;
			}
            if (sleep_time > 0) {
                sleep_us(sleep_time);
            }
        }
		if (!ts_cnt) {
			for (i=0;i<hw_api_db->cpu_info.nof_cores;i++) {
				post_sem_n(hw_api_db->semids[i],0);
			}
			for (i=0;i<MAX_DAEMONS;i++) {
				if (i!=daemon_i->semid)
					post_sem_n(hw_api_db->semdaemons,i);
			}
		}
    } else if (!daemon_i || strstr(daemon_i->path,"sync")) {
        sleep_time = ((tstamp + 1) * (*ts_len)) - actual_time;
        if (sleep_time > 0) {
			sleep_us(sleep_time);
        }
    } else {
        wait_sem_n(hw_api_db->semdaemons,daemon_i->semid);
    }

    get_time(&tdata);

    tstamp = time_to_tstamp(&tdata);

    #ifdef printexec
    if (daemon_i && !strcmp(daemon_i->path,"exec")) {
    	if (!ts_cnt)
        printf("exec starts at %d:%d ts %d runs %d period %d ts=%d div=%d\n",tdata.tv_sec,tdata.tv_usec,tstamp,daemon_i->cpu_usec,daemon_i->cpu[0].tv_usec,ts_cnt,hw_api_db->exec_slot_div);
    }
    #endif

    #ifdef testsync
    if (tstamp == hw_api_db->sync_ts) {

      actual_time = (unsigned int) tdata.tv_sec * 1000000 + tdata.tv_usec;
      sleep_time = (tstamp + 1) * (*ts_len) - actual_time;

//      if (!strcmp(daemon_i->path,"bridge"))
//        printf("daemon %s ha de saltar, ts=%d %d:%d dorm %d\n",daemon_i->path,tstamp,tdata.tv_sec,tdata.tv_usec,sleep_time);
    
      sleep_us(sleep_time);
    }
    #endif
    fflush(0);
}

/** @} */

/** @defgroup itf Packet Interface Management Functions
 *
 * HW API provides the user to a set of functions to create FIFO-like packet
 * oriented data interface.
 *
 * This interfaces have a read and a write side. Once it is created, the user
 * must attach a file descriptor to the read or write side, thus, communication is possible
 * and packets written in the write side will be available in the read side.
 *
 * A name and an owner Id (object Id) can be assigned to any of the sides so separate
 * processes can share an interface easier.
 *
 * @{
 */

/** Get number of available interfaces
 *
 */
int hwapi_itf_avail()
{
    int i, n;
    n = 0;
    for (i = 0; i < MAX_INT_ITF; i++) {
        if (!int_itf[i].key_id) {
            n++;
        }
    }
    return n;
}

/** Create Interface.
 *
 * Creates a packet-oriented internal data interface.
 * If id parameter is different of 0 it will be the
 * assigned id for the interface. Otherwise it will
 * be generated.
 *
 * @param id 0 for automatic, else for fixed
 * @param size maximum packet size allowed in bytes
 *
 * @return !=0 Id of the interface
 * @return =0 error creating interface
 */
hwitf_t hwapi_itf_create(hwitf_t id, int size)
{
    int i;
    int iid;
    key_t itf_key;
    struct msqid_ds info;

    /* check if id already used */
    if (id) {
        i = 0;
        while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
            i++;
        }
        if (i < MAX_INT_ITF) {
            printf("HWAPI: Error Id=0x%x already used\n", id);
            return 0;
        }
    } else {
        /* search an used one */
        do {
            id = (rand() & 0xff);
            i = 0; while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
            i++;
        }
        } while (i < MAX_INT_ITF);
    }

    /* search for a free position in db */
    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        printf("HWAPI: Error no more itf allowed (%d)\n", i);
        return 0;
    }

    iid = (int) id;
    itf_key = ftok(filelockname, iid);
    int_itf[i].key_id = id;

    if ((int_itf[i].id = msgget(itf_key, IPC_CREAT | IPC_EXCL | 0666))
            == -1) {
        printf("HWAPI: Message queue with key_id 0x%x could not be created\n", id);
        perror("msgget");
        memset(&int_itf[i], 0, sizeof (struct int_itf));
        return 0;
    }
        
    msgctl(int_itf[i].id, IPC_STAT, &info);
    info.msg_qbytes = MSG_QBYTES;
    if (msgctl(int_itf[i].id, IPC_SET, &info) == -1) {
        perror("msgctl");
        return 0;
    }

    /* return id */
    return id;
}

/** Set interface delay
 *
 * Set a delay of N time-slots
 * @param id Id of the interface
 * @param nof_tslots Number of time-slots to delay
 */
int hwapi_itf_setdelay(hwitf_t id, int nof_tslots)
{

    int i;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        printf("HWAPI: Error can't find itf id=0x%x.\n", id);
        return 0;
    }
    int_itf[i].delay = nof_tslots;
}


int hwapi_itf_delete_own(char *obj_name)
{
    int i;

    for (i = 0; i < MAX_INT_ITF; i++) {

        if (!strncmp(int_itf[i].w_obj, obj_name, SYSTEM_STR_MAX_LEN)
                || !strncmp(int_itf[i].r_obj, obj_name, SYSTEM_STR_MAX_LEN)) {
            hwapi_itf_delete(int_itf[i].key_id);
        }
    }

    return 1;
}

void hwapi_itf_setexternal(hwitf_t id)
{
    int i;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        #ifdef DEBUG_ITF
        printf("HWAPI: Error can't find itf id=0x%x.\n", id);
        #endif
        return 0;
    }
    int_itf[i].opts |= ITF_OPT_EXTERNAL;
}

/** Get if an interface is external or not
 * @param id for the interface
 */
int hwapi_itf_isexternal(hwitf_t id)
{
    int i;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        #ifdef DEBUG_ITF
        printf("HWAPI: Error can't find itf id=0x%x.\n", id);
        #endif
        return 0;
    }
    return int_itf[i].opts & ITF_OPT_EXTERNAL;
}

/** Delete Interface.
 *
 * Deletes internal data interface with id='id' (and clears resources)
 *
 * @param id Id of the interface to delete
 *
 * @return 1  Deleted successfully
 * @return 0 Error deleting interface
 */
int hwapi_itf_delete(hwitf_t id)
{
    int i;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        #ifdef DEBUG_ITF
        printf("HWAPI: Error can't find itf id=0x%x.\n", id);
        #endif
        return 0;
    }

    if (msgctl(int_itf[i].id, IPC_RMID, NULL) == -1) {
        printf("HWAPI: Error removing interface 0x%x\n", int_itf[i].id);
        perror("msgctl");
        return -1;
    }

    /* clear interface resources */
    memset(&int_itf[i], 0, sizeof (struct int_itf));

    return 1;
}

/** Attach to an Interface.
 *
 * Creates a file descriptor to use the interface. This function
 * should be called 'after' creating the interface and 'before' using it.
 *
 * It must be attached in FLOW_READ_ONLY (for reading) or FLOW_WRITE_ONLY
 * mode (for writting). A file descriptor will be returned which can be used
 * to read/write packets
 *
 * @param id Id of the interface created with hwapi_create_itf()
 * @param mode FLOW_READ_ONLY or FLOW_WRITE_ONLY constants
 *
 * @return >0 File descriptor
 * @return =-2 Interface not available
 * @return =-1 Error attaching interface
 */
int hwapi_itf_attach(hwitf_t id, int mode)
{
    int i, j;
    int iid;
    key_t itf_key;

    /* first of all, search itf in interfaces database */
    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        return -2;
    }

    /* then search a free position in file descriptors database */
    j = 3; /* first 2 are reserved */
    while (j < MAX_FILE_DES && file_des[j].mode) {
        j++;
    }
    if (j == MAX_FILE_DES) {
        printf("HWAPI: Error no more local itf allowed (%d).\n", j);
        return -1;
    }

    /* now fill local db */
    iid = (int) id;
    itf_key = ftok(filelockname, iid);
    if ((file_des[j].id = msgget(itf_key, 0)) == -1) {
        printf("HWAPI: Error attaching to itf 0x%x\n", id);
        perror("msgget");
        return -1;
    }
    file_des[j].mode = mode;
    file_des[j].int_itf_idx = i;

    return j;
}

/** Unattach from an Interface.
 *
 * Un-attaches a file descriptor from the (previously attached)
 * interface in mode 'mode'
 *
 * @param fd file descriptor of the interface
 * @param mode FLOW_READ_ONLY or FLOW_WRITE_ONLY constants
 *
 * @return >0 Successfully unattached
 * @return <0 Error unattaching
 */
int hwapi_itf_unattach(int fd, int mode)
{

    /* check correct fd */
    if (fd < 0 || fd > MAX_FILE_DES) {
        printf("HWAPI: Error invalid local fd %d.\n", fd);
        return -1;
    }

    /* clear resources */
    memset(&file_des[fd], 0, sizeof (struct file_des));

    return 1;
}

int hwapi_itf_seturgent_in(hwitf_t itf_id, void callback(hwitf_t id))
{
    return hwapi_itf_seturgent(itf_id, callback, 0);
}

int hwapi_itf_seturgent_out(hwitf_t itf_id, int remote_itf_id)
{
    return hwapi_itf_seturgent(itf_id, NULL, remote_itf_id);
}

void recv_urgent_signal(int value)
{
    int itf_id, i;

    itf_id = value;

    if (!itf_id)
        return;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != itf_id)
        i++;
    if (i == MAX_INT_ITF) {
        printf("HWAPI: Caution received a urgent signal for an unknown itf (0x%x)\n", itf_id);

        return;
    }
    int_itf[i].callback(itf_id);
}

int send_urgent_signal(struct int_itf *itf_db, hwitf_t itf_id)
{
    int i;

    if (!itf_id)
        return 0;

    i = 0;
    while (i < MAX_INT_ITF && itf_db[i].key_id != itf_id)
        i++;
    if (i == MAX_INT_ITF) {
        printf("HWAPI: Error sending signal: itf 0x%x not found\n", itf_id);
        return 0;
    }
    if (!itf_db[i].callback || !itf_db[i].callback_pid) {
        printf("HWAPI: Error sending signal: Itf 0x%x not configured as urgent\n", itf_id);
    }

    union sigval val;
    val.sival_int = itf_id;
    if (sigqueue(itf_db[i].callback_pid, SIGUSR2, val)) {
        printf("HWAPI: Error sending signal to pid %d at pos %d id 0x%x\n", itf_db[i].callback_pid, i, itf_id);
        perror("Reason");
        return 0;
    }
    return 1;
}

/** Set urgent option to interface 
 * @param itf_id Id of the interface
 * @param callback Function to call whan a new packet is received
 * @param remote_id Id of the remote itf, for output itfs
 */
int hwapi_itf_seturgent(hwitf_t itf_id, void callback(hwitf_t id), hwitf_t remote_itf_id)
{
    int i;
    struct sigaction action;

    if (callback && remote_itf_id) {
        printf("HWAPI: Error bidirectional interfaces not yet supported\n");
        return 0;
    }

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != itf_id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        return 0;
    }

    if (callback && (int_itf[i].callback_pid || int_itf[i].callback)) {
        printf("HWAPI: Error callback function is already defined by pid %d\n", int_itf[i].callback_pid);
        return 0;
    }

    if (!sigusr_set) {
        sigusr_set = 1;
    }
    
    if (!getuid() && callback) {
      struct sched_param param;
      param.sched_priority=EXEC_PRIO;
      if (sched_setscheduler(int_itf[i].network_pid,SCHED_TYPE,&param)) {
        printf("error setting scheduler\n");
        perror("setscheduler");
      }
      if (sched_setscheduler(int_itf[i].network_bpid,SCHED_TYPE,&param)) {
        printf("error setting scheduler\n");
        perror("setscheduler");
      }
    }

    if (callback && !remote_itf_id) {
        int_itf[i].opts |= ITF_OPT_URGENT_W;
        int_itf[i].callback = callback;
        int_itf[i].callback_pid = getpid();
    } else {
        int_itf[i].opts |= ITF_OPT_URGENT_R;
        int_itf[i].remote_id = remote_itf_id;
    }

    return 1;
}

/** Set interface to blocking
 * @param itf_id
 * @return 1 if ok, 0 if error
 */
int hwapi_itf_setblocking(hwitf_t itf_id)
{
    int i;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != itf_id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        return 0;
    }

    int_itf[i].opts |= ITF_OPT_BLOCKING;

    return 1;

}

/** Send a packet.
 *
 * Sends a packet of data through a data interface. The user needs
 * to provide a file descriptor, get after calling the attaching function.
 *
 * @param fd file descriptor
 * @param buffer pointer to data to send
 * @param size size of the packet in bytes (8-bit)
 *
 * @return >0 Successfully sended. Returns amount of bytes sended.
 * @return =0 Packet could not be send (full queue)
 * @return <0 Error sending packet
 */
int hwapi_itf_snd(int fd, void *buffer, int size)
{
    int n;
    int tstamp;
    int id;
    int delay = 0;
    int flags;
    int int_pos;
    struct msqid_ds msgstatus;
    
    assert(buffer);
    assert(size >= 0);

    /* check for valid fd */
    if (fd < 0 || fd > MAX_FILE_DES) {
        printf("HWAPI: Error invalid fd=%d. 0<=fd<=%d.\n", fd,
                MAX_FILE_DES);
        return -1;
    }

    /* check if fd is write enabled */
    if (!(file_des[fd].mode & FLOW_WRITE_ONLY)) {
        printf
                ("HWAPI: Error flow is not write enable (fd=%d id=0x%x mode=%d pid=%d)\n",
                fd, file_des[fd].id, file_des[fd].mode, getpid());
        return -1;
    }

    /* get interface id */
    id = file_des[fd].id;

    /* get interface position */
    int_pos = file_des[fd].int_itf_idx;

    /* check if size exceeds internal limit */
    if (size > MSG_BUFF_SZ) {
        printf("HWAPI: Error packet too long (%d/%d)\n", size, MSG_BUFF_SZ);
        return -1;
    }

    if (my_proc) {
        delay = int_itf[int_pos].delay;

        if (!delay) {
            delay=hw_api_db->default_delay;
        }
    } else {
        delay=0;
    }
    
    /* calculate packet time stamp and destination */
    if (delay) {
        /* write future tstamp. Packet is only available on next tstamp */
        tstamp = get_tstamp();
        msg.head.dst = (long) (tstamp + delay);
    } else {
        msg.head.dst = 1;
    }

    if (int_itf[int_pos].opts&ITF_OPT_BLOCKING) {
        flags = 0;
    } else {
        flags = IPC_NOWAIT;
    }

    /* fill message header */
    msg.head.src = 0;
    msg.head.opts = int_itf[int_pos].opts;
    msg.head.tstamp = msg.head.dst & TSTAMP_PKT_MASK;
    memcpy(msg.body, buffer, size);

    flags = IPC_NOWAIT;
    n = msgsnd(id, &msg, size + MSG_HEAD_SZ, flags);

    if (n == -1 && errno == EAGAIN) {
        /* full queue, print only for objects interfaces */
        if (my_proc) {
            msgctl(id, IPC_STAT, &msgstatus);
            time_t t;
            get_time(&t);
            printf("\nCAUTION: Full msg queue at %d:%d (ts=%d,objts=%d), caller object %s. Current status: %d/%d bytes, %d msg\n\n",
                    (int) t.tv_sec, (int) t.tv_usec, get_tstamp(), (int) my_proc->obj_tstamp,
                    my_proc->obj_name, (int) msgstatus.msg_cbytes, (int) msgstatus.msg_qbytes,
                    (int) msgstatus.msg_qnum);
        }
        return 0;
    } else if (n == -1) {
        printf("HWAPI: Error while sending through itf id %d key 0x%x at pos %d type %d size %d pid %d errno %d\n", id, int_itf[int_pos].key_id, int_pos, (long) msg.head.dst, size, getpid(),errno);
        perror("msgsnd");
        return -1;
    }
    /* success */

    /* call function if urgent interface */
    if (int_itf[int_pos].opts & ITF_OPT_URGENT_W) {
        if (!send_urgent_signal(int_itf, int_itf[int_pos].remote_id)) {
            printf("HWAPI: Error sending signal to itf 0x%x\n", int_itf[int_pos].remote_id);
        }
    }

#ifdef DEBUG_ITF
    if (my_proc) {
        time_t t;
        get_time(&t);
        printf("========= %s(%d) send packet size %d at ts=%d time %d:%d\n", my_proc->obj_name, my_proc->exec_position,
                size, my_proc->cur_tstamp,t.tv_sec,t.tv_usec);
    }
#endif

    return size;
}

int hwapi_itf_status_id(hwitf_t id)
{
    struct msqid_ds msgstatus;
    int i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != id)
        i++;
    if (i == MAX_INT_ITF) {
        #ifdef DEBUG_ITF
        printf("HWAPI: Itf %d not found\n", id);
        #endif
        return -1;
    }
    if (!msgctl(int_itf[i].id, IPC_STAT, &msgstatus)) {
        return (int) msgstatus.msg_cbytes-msgstatus.msg_qbytes*MSG_HEAD_SZ;
    } else {
        return -1;
    }
}

int hwapi_itf_status(int fd)
{
    struct msqid_ds info;

    /* check valid file descriptor */
    if (fd < 0 || fd > MAX_FILE_DES) {
        printf("HWAPI: Error invalid fd=%d. 0<=fd<=%d.\n", fd,
                MAX_FILE_DES);
        return -1;
    }

    if (!msgctl(file_des[fd].id, IPC_STAT, &info)) {
        return info.msg_cbytes-info.msg_qbytes*MSG_HEAD_SZ;
    } else {
        return 0;
    }
}

/** Receive a packet.
 *
 * Recevies a packet through a file descriptor. User must
 * indicate provided buffer's length (in bytes).
 *
 * @param fd file descriptor
 * @param buffer pointer to buffer where receive data
 * @param size size of the buffer in bytes (8-bit)
 *
 * @return >0 Successfully received. Returns amount of bytes.
 * @return =0 No packets available (empty queue)
 * @return <0 Error receiving packet
 */
int hwapi_itf_rcv(int fd, void *buffer, int size)
{
    int n;
    int type;
    int tstamp;
    int c;
    int id;
    int delay;
    int int_pos;
    int flags;

    assert(buffer);
    assert(size >= 0);

    /* check valid file descriptor */
    if (fd < 0 || fd > MAX_FILE_DES) {
        printf("HWAPI: Error invalid fd=%d. 0<=fd<=%d.\n", fd,
                MAX_FILE_DES);
        return -1;
    }

    /* check if reading enabled */
    if (!(file_des[fd].mode & FLOW_READ_ONLY)) {
        printf
                ("HWAPI: Error flow is not read enable (fd=%d id=0x%x mode=%d pid=%d)\n",
                fd, file_des[fd].id, file_des[fd].mode, getpid());

        return -1;
    }

    /* get interface id */
    id = file_des[fd].id;

    /* get interface db position */
    int_pos = file_des[fd].int_itf_idx;

    /* obtain current timestamp */
    tstamp = get_tstamp();

    if (int_itf[int_pos].opts&ITF_OPT_BLOCKING) {
        flags = 0;
    } else {
        flags = IPC_NOWAIT;
    }


    if (int_itf[int_pos].opts&ITF_OPT_URGENT_R || int_itf[int_pos].opts&ITF_OPT_URGENT_W
            || !my_proc)
      type=0;
    else
      type = -tstamp;

    n = msgrcv(id, &msg, MSG_SZ, type, flags);

    /* any message available */
    if (n == -1 && errno == ENOMSG) {
        return 0;
    }

    /* check error */
    if (n < 0) {
        printf("HWAPI: Error reading itf id 0x%x key 0x%x at pos %d (%s) size %d\n", id, int_itf[int_pos].key_id, 
                                int_pos, daemon_i ? daemon_i->path : my_proc->obj_name,MSG_SZ);
        perror("msgrcv");
        return n;
    }

    if (n<MSG_HEAD_SZ) {
        return 0;
    }

    n-=(MSG_HEAD_SZ);

    /* check packet lenght does not exceed user buffer length */
    if (n > size) {
        n = size;
    }

    /* copy packet to user buffer */
    memcpy(buffer, msg.body, n);

    return n;
}

/** Link Physical Interface to a logical Interface
 *
 *
 *
 * Links a physical interface (external) to an internal packet
 * oriented interface. After the link is successfully realized,
 * all packets written to the internal interface will be bridged
 * (by RUNPH_NET) towards the physical interface. Conversely, all
 * packets received to the phy. itf. will be bridged to the internal
 * interface.
 *
 * Interface must be created before calling this function. Then, in
 * order to use it, it must be attached.
 *
 * @param itf_id Local interface Id
 * @param phy_itf_id External Physical Interface Id
 * @param mode FLOW_READ_ONLY or FLOW_WRITE_ONLY constant
 *
 * @return >0 Successfully linked
 * @return <0 Error linking
 */
int hwapi_itf_link_phy(hwitf_t itf_id, xitfid_t phy_itf_id, char mode)
{
    int n, i;
    char cmd[3];

    /* check correct params */
    if (!itf_id) {
        printf("HWAPI: Error invalid parameter itf_id must be != 0\n");
        return -1;
    }
    if (!phy_itf_id) {
        printf("HWAPI: Error invalid parameter xitf_id must be !=0\n");
        return -1;
    }

    /* search internal interface */
    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != itf_id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        printf("HWAPI: Can't find itf id 0x%x.\n", itf_id);
        return -1;
    }

    /* send message to RUNPH process */
    cmd[0] = phy_itf_id;
    cmd[1] = itf_id;
    cmd[2] = mode;
    n = send_parent_cmd(HWD_SETUP_XITF, cmd, 3, NULL, 0);
    if (n <= 0) {
        /* error */
        printf("HWAPI: Error while setting xitf (%d)\n", n);
    }
    
    int_itf[i].opts |= ITF_OPT_EXTERNAL;

    return n;
}

/** Match Interface.
 *
 * Assign an object id and interface name for the read and write side
 *
 * @return 1 if ok 0 if error
 */
int hwapi_itf_match(hwitf_t itf_id, char *w_obj, char *w_itf, char *r_obj,
        char *r_itf)
{
    assert(w_itf && w_obj && r_itf && r_obj);
    int i;

    i = 0;
    while (i < MAX_INT_ITF && int_itf[i].key_id != itf_id) {
        i++;
    }
    if (i == MAX_INT_ITF) {
        printf("HWAPI: Interface 0x%x not found\n", itf_id);
        return 0;
    }
    strncpy(int_itf[i].w_obj, w_obj, SYSTEM_STR_MAX_LEN);
    strncpy(int_itf[i].w_itf, w_itf, SYSTEM_STR_MAX_LEN);
    strncpy(int_itf[i].r_obj, r_obj, SYSTEM_STR_MAX_LEN);
    strncpy(int_itf[i].r_itf, r_itf, SYSTEM_STR_MAX_LEN);

    return 1;
}

int hwapi_itf_list(char *w_obj, hwitf_t *ids, int max_itf)
{
    int i, k;

    k = 0;
    for (i = 0; i < MAX_INT_ITF && k < max_itf; i++) {
        if (!strncmp(int_itf[i].w_obj, w_obj, SYSTEM_STR_MAX_LEN)) {

            ids[k++] = int_itf[i].key_id;
        }
    }
    return k;
}

/** Get itf id given an itf name and object name.
 */
hwitf_t hwapi_itf_find(char *obj_name, char *itf_name, char mode)
{
    int i;

    /* first of all, search itf in interfaces database */
    i = MAX_INT_ITF;
    if (mode & FLOW_WRITE_ONLY) {
        i = 0;
        while (i < MAX_INT_ITF
                && (strncmp(itf_name, int_itf[i].w_itf, SYSTEM_STR_MAX_LEN)
                || strncmp(obj_name, int_itf[i].w_obj, SYSTEM_STR_MAX_LEN))) {
            i++;
        }
    }
    if ((mode & FLOW_READ_ONLY) && (i == MAX_INT_ITF)) {
        i = 0;
        while (i < MAX_INT_ITF
                && (strncmp(itf_name, int_itf[i].r_itf, SYSTEM_STR_MAX_LEN)
                || strncmp(obj_name, int_itf[i].r_obj, SYSTEM_STR_MAX_LEN))) {
            i++;
        }
    }
    if (i == MAX_INT_ITF) {
        #ifdef DEBUG_ITF
        printf("HWAPI: notfound itf %d %s:%s r/w\n", mode, itf_name, obj_name);
        #endif
        return 0;
    }
    return int_itf[i].key_id;
}

/** @} */

/** @defgroup hw Hardware Monitoring Functions
 *
 * A set of functions are available to gather information about
 * underlying hardware characteristics and current status.
 *
 * @{
 */

/** Physical Interface Information.
 *
 * Obtains information of the Physical External interfaces. It is read from
 * the physical interfaces configuration file (RUNPH Launcher).
 *
 * User must provide the maximum amount of external interfaces he wants to read.
 * It depends on the length of the xitf buffer (number of elements)
 *
 * @param xitf Pointer to information structure (check phal_hw_api.h)
 * @param max_itf Maximum number of interfaces (buffer length, in elements)
 *
 * @return >=0 Number of interfaces read
 * @return <0 error
 */
int hwapi_hwinfo_xitf(struct hwapi_xitf_i *xitf, int max_itf)
{
    int i;
    assert(xitf && max_itf >= 0);

    if (max_itf > hw_api_db->net_db.nof_ext_itf) {
        max_itf = hw_api_db->net_db.nof_ext_itf;
    }

    /* copy all interfaces */
    for (i = 0; i < max_itf; i++) {
        memcpy(&xitf[i], &hw_api_db->net_db.ext_itf[i].info,
                sizeof (struct hwapi_xitf_i));
    }
    return max_itf;
}

int hwapi_hwinfo_isDebugging()
{
	return hw_api_db->cpu_info.debug_level?1:0;
}

/** Get Processor Information.
 *
 *
 * @todo Definition of structure, dynamic change? battery? etc.
 *
 *
 * Function used to gather processing resource (CPU) information.
 * Data is writted to the struct pointed by info:
 *
 *
 * @see hwapi_cpu_i
 * @param info CPU information struct
 *
 */
void hwapi_hwinfo_cpu(struct hwapi_cpu_i * info)
{
    assert(info);
    struct msqid_ds msgstatus;

    /* fill info, copy it from hw_api_db struct */
    memcpy(info, &hw_api_db->cpu_info, sizeof (struct hwapi_cpu_i));

    hw_api_db->cpu_info.cpu_usage = 0;
    int i;
    for (i = 0; i < MAX_PROCS; i++) {
        if (proc_db[i].obj_id) {
            hw_api_db->cpu_info.cpu_usage += proc_db[i].sys_cpu;
        }
    }

    msgctl(data_msg_id, IPC_STAT, &msgstatus);
    hw_api_db->cpu_info.used_fifo = msgstatus.__msg_cbytes;
    hw_api_db->cpu_info.total_fifo = msgstatus.msg_qbytes;
}

void hwapi_hwinfo_setid(int pe_id)
{
    hw_api_db->cpu_info.pe_id = pe_id;
}


/** @} */

/** @defgroup proc Process Management Functions
 *
 * This group of functions provide tools for creating, monitoring and
 * changing the status of processes (objects) running in the local processor.
 *
 * @{
 */

int hwapi_proc_needsrelocate()
{
    return 0;
}

/** Request space for an executable
 *
 * Requests space to allocate executable program space.
 * Space is splitted in a set of slots. Objects reserve one
 * or more slots so that: numslots*slotsz>program_len
 * see hwapi_dsp.h
 *
 * When this function is called, space is reserved. It won't be
 * released until a call to hwapi_proc_remove() function.
 * So if the loading processs is canceled user should call the that
 * function so the space is realeased for next calls.
 * @param request_length Requested length
 * @param addr allocated address
 * @param space allocated space
 */
int hwapi_proc_request(struct hwapi_proc_launch *pinfo)
{
	return 1;
}

/** Create a New Process.
 *
 * This function creates a process and launches it. As the process resides in user
 * memory (pdata buffer), it must be writed to the disk before launching it. This file
 * will be deleted when the process does.
 *
 * @todo (in Linux) path to downloaded software should be either relative or guessed but not a constant
 *
 * @param pdata Pointer to program data
 * @param pinfo Pointer to process information structure
 * @param exec_position Position in the execution queue
 * @param core_idx Index of the core where the process must be executed
 *
 * @return >0 Pid of the process
 * @return <0 error
 */
int hwapi_proc_create(char *pdata, struct hwapi_proc_launch *pinfo, int exec_position, int core_idx)
{
    int n;
    int i;
    int f;
    int pid;
    int blen, wlen;
    int c;
    char ib;
    cpu_set_t set;

    assert(pdata && pinfo);

    if (!pinfo->app_name || !pinfo->exe_name || !pinfo->obj_name) {
        printf("HWAPI: Error launching executable. Some parameters are empty\n");
        return -1;
    }

    blen = pinfo->blen;
    wlen = pinfo->wlen;

    i = 0;
    while (i < MAX_PROCS && proc_db[i].pid && proc_db[i].obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        printf("HWAPI: No more space in process db.\n");
        return -1;
    }

    /* check if file is already created */
    sprintf(aux_string, "%s/%s", RCV_PROC_PATH, pinfo->exe_name);
    f = open(aux_string, O_CREAT | O_WRONLY | O_EXCL, S_IRWXU);
    /* check error */
    if (f < 0 && errno != EEXIST) {
        perror("fopen");
        return -1;
    }

    /* write buffer to temporal file unless file exists */
    if (f > 0) {
        n = write(f, pdata, blen);
        if (n != blen) {
            perror("write");
            printf("removing file...\n");
            if (remove(aux_string)) {
                perror("remove");
            }
        }
    }

    /* close file */
    close(f);

    memset(&proc_db[i], 0, sizeof (struct hwapi_proc_i));

    proc_db[i].app_id = pinfo->app_id;
    proc_db[i].obj_id = pinfo->obj_id;
    proc_db[i].change_progress = 1;
    proc_db[i].status.cur_status = 0;
    proc_db[i].ideal_period = hw_api_db->cpu_info.tslen_usec;
    proc_db[i].status.next_status = 1;
    proc_db[i].status.next_tstamp = get_tstamp();
    proc_db[i].obj_tstamp = get_tstamp();
    proc_db[i].exec_position = exec_position;
    proc_db[i].core_idx = core_idx;
    proc_db[i].usec_limit = pinfo->usec_limit;
    strncpy(proc_db[i].path, aux_string, 128);
    strncpy(proc_db[i].obj_name, pinfo->obj_name, 24);
    strncpy(proc_db[i].app_name, pinfo->app_name, 24);

    /* tell background daemon to launch the process */
    ib = i;
    if (send_parent_cmd(HWD_CREATE_PROCESS, &ib, 1, (char*) & pid, 4) != 1) {
        printf("HWAPI: Error creating child\n");
        memset(&proc_db[i], 0, sizeof (struct hwapi_proc_i));
        return -1;
    }

    /* wait creation of the process */
    c = 0;
    while (pid != proc_db[i].pid && c < 40 && pid > 0) {
        sleep_us(*ts_len/2);
        c++;
    }

    /* if error, remove file and reset database structure */
    if (pid != proc_db[i].pid) {
        printf("HWAPI: Child did not started, removing it...\n");
        memset(&proc_db[i], 0, sizeof (struct hwapi_proc_i));

        return -1;
    }

    /* set processor affinity */
    CPU_ZERO(&set);
    CPU_SET(core_idx,&set);
    if (sched_setaffinity(pid, sizeof(cpu_set_t),&set)) {
        printf("HWAPI: Error setting process affinity to %d\n",core_idx);
        perror("sched_setaffinity");
    }

    return pid;
}

/** Remove Process Function:
 *
 * Removes a process from the system and de-allocates all resources
 * it was using.
 *
 * @param obj_id Id of the object to remove
 *
 * @return 1 success
 * @return 0 error
 */
int hwapi_proc_remove(int obj_id)
{
    int i;

    /* search it in the database */
    i = 0;
    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        printf("HWAPI: Error removing object from db. Id 0x%x not found.\n", obj_id);
        return 0;
    }

    /* clear db */
    proc_db[i].obj_id = 0;

    return 1;
}

int hwapi_proc_kill(int obj_id)
{

    int c;
    int i,j;

    /* search it in the database */
    i = 0;
    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        printf("HWAPI: Error removing object from db. Id 0x%x not found.\n", obj_id);
        return 0;
    }

    if (!proc_db[i].pid) {
        return 1;
    }

    /* send sigterm */
    kill(proc_db[i].pid, SIGTERM);
    c = 0;
    while (proc_db[i].pid) {
        sleep_ms(100);
        c++;
        if (c == 10) {
            printf("HWAPI: Error terminating process %d, sending kill signal...\n",
                    proc_db[i].pid);
            kill(proc_db[i].pid, SIGKILL);

            return 0;
        }
    }

    return 1;
}
/** Force a relinquish to an object
 */
int hwapi_proc_relinquish(int obj_id)
{

    int c;
    int i,j;

    /* search it in the database */
    i = 0;
    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        printf("HWAPI: Error removing object from db. Id 0x%x not found.\n", obj_id);
        return 0;
    }

    if (!proc_db[i].pid) {
        return 1;
    }

    /* send sigusr1 */
    union sigval x;
    x.sival_int=0;
    if (sigqueue(proc_db[i].pid, SIGUSR2, x)) {
        perror("sigqueue");
    }

    return 1;
}

/** Get object new status
 *
 * Obtains the status the object must run in the current timestamp.
 *
 * This function is called by Services API during the Status()
 * call, in every object execution's loop.
 *
 * During an status change procedure, the new status won't be
 * returned after the timestamp is same or greater than the one
 * passed as the third parameter in the hwapi_proc_status_new() function
 *
 */
int hwapi_proc_status_get(void)
{
    int r;

    wait_sem(hw_api_db->status_sem_id);
    r = my_proc->status.cur_status;
    if (my_proc->status.next_status != my_proc->status.cur_status) {
        if (my_proc->change_progress < 2) {
            if (get_tstamp() >= my_proc->status.next_tstamp) {
                my_proc->change_progress = 2;
                r = my_proc->status.next_status;
            }
        } else {
            my_proc->status.cur_status = my_proc->status.next_status;
            r = my_proc->status.cur_status;
        }
    }

    post_sem(hw_api_db->status_sem_id);

    return r;
}

/** Set a new status for an object
 * @returns 1 if changed ok, 0 if can't change, -1 if error
 */
int hwapi_proc_status_new(int obj_id, int next_status, int next_tstamp)
{
    int i, r;
    i = 0;

    /* caller is the object and is terminating, return inmediatly */
    if (my_proc && isterming)
        return 1;

    if (!next_tstamp) {
        next_tstamp = get_tstamp();
    }

    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        #ifdef DEB
        printf("HWAPI: Error object 0x%x not found\n", obj_id);
        #endif
        return -1;
    }

    wait_sem(hw_api_db->status_sem_id);

    if ((!proc_db[i].change_progress
            && proc_db[i].status.next_status == proc_db[i].status.cur_status)
            || next_tstamp == -1) {

        proc_db[i].status.next_status = next_status;
        proc_db[i].status.next_tstamp = next_tstamp;
        proc_db[i].change_progress = 1;

        r = next_status;
    } else {
        r = 0;
    }

    post_sem(hw_api_db->status_sem_id);

    return r;
}

/** Confirm a successfull status change.
 * @returns 1 if changed ok, 0 if still not changed.
 */
int hwapi_proc_status_ack(int obj_id)
{
    int i, r;
    i = 0;
    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        printf("HWAPI: Error obj id not found 0x%x\n", obj_id);
        return -1;
    }

    wait_sem(hw_api_db->status_sem_id);

    if (proc_db[i].status.cur_status == proc_db[i].status.next_status
            && proc_db[i].change_progress == 2) {
        proc_db[i].change_progress = 0;
        r = 1;
    } else {
        r = 0;
    }

    post_sem(hw_api_db->status_sem_id);

    return r;
}

/** Proces Monitoring.
 *
 * Function used to gather information about current process status,
 * future status, cpu timing and other related information
 *
 * @param proc Pointer to hwapi_proc_i structure
 * @param obj_id id of the object assigned to the process
 *
 * @return 1 ok
 * @return 0 error (not found)
 */
int hwapi_proc_info(int obj_id, struct hwapi_proc_i *pinfo)
{
    int i;
    i = 0;

    assert(pinfo);

    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        return 0;
    }

    memcpy(pinfo, &proc_db[i], sizeof (struct hwapi_proc_i));

    return 1;
}

/** My process information.
 *
 * Fills hwapi_proc_i structure with the caller's info.
 *
 * @param proc Pointer to hwapi_proc_i structure
 *
 */
int hwapi_proc_myinfo(struct hwapi_proc_i *pinfo)
{
    assert(pinfo && my_proc);
    memcpy(pinfo, my_proc, sizeof (struct hwapi_proc_i));

    return 1;
}

void hwapi_proc_tstamp_set(objid_t obj_id, int tstamp)
{
    int i;

    i=0;
    while (i < MAX_PROCS && proc_db[i].obj_id != obj_id) {
        i++;
    }
    if (i == MAX_PROCS) {
        #ifdef DEB
        printf("HWAPI: Error object 0x%x not found\n", obj_id);
        #endif
    }

    proc_db[i].obj_tstamp = tstamp;
}

void hwapi_proc_mytstampinc()
{
    assert(my_proc);

    my_proc->obj_tstamp++;
}

/** Get Processes List.
 *
 */
int hwapi_proc_list(struct hwapi_proc_i *pinfo, int nelems)
{
    int i, k;

    assert(pinfo);

    k = 0;
    for (i = 0; i < MAX_PROCS; i++) {
        if (proc_db[i].obj_id) {
            if (k < nelems) {
                wait_sem(hw_api_db->status_sem_id);
                memcpy(&pinfo[k], &proc_db[i],
                        sizeof (struct hwapi_proc_i));
                post_sem(hw_api_db->status_sem_id);
            }
            k++;
        }
    }
    if (k > nelems) {

        printf("HWAPI: Caution more processes runnning but buffer too short (%d<%d)\n",
                nelems, k);
        k = nelems;
    }
    return k;
}

/** @} */

/** @defgroup stats Global Variable Functions
 *
 * The Hardware API enables processes to share global variables in an easy, fast
 * and efficient way.
 *
 *
 * @{
 */

/** Create a new shared variable.
 *
 * @param size Size of the variable, in bytes
 * @returns Id for the variable, 0 if error
 */
varid_t hwapi_var_create(char *name, int size, int opts)
{
    varid_t i;
    int offset;

    assert(var_db && name && size >= 0);

    /* alloc data*/
    offset = mem_alloc(hw_api_db->var_table, size);
    if (!offset) {
        printf("HWAPI: Error allocating space for variable\n");
        return 0;
    }

    i = 1;
    while (i < MAX_VARS && var_db[i].stat_id) {
        i++;
    }
    if (i == MAX_VARS) {
        printf("HWAPI: Can't create more variables\n");
        return 0;
    }

    /* fill database */
    strncpy(var_db[i].name, name, VAR_NAME_LEN);
    var_db[i].obj_id = my_proc?my_proc->obj_id:0;
    var_db[i].stat_id = i;
    var_db[i].size = size;
    var_db[i].table_offset = offset;
    var_db[i].opts = opts;

    /* stat id is position */
    return i;
}

/** Delete a variable
 */
int hwapi_var_delete(varid_t stat_id)
{
    assert(var_db);

    if (stat_id < 1 || stat_id > MAX_VARS) {
        return 0;
    }

    /* dealloc buffer */
    if (var_db[stat_id].table_offset) {
        mem_free(hw_api_db->var_table, var_db[stat_id].table_offset);
    }

    memset(&var_db[stat_id], 0, sizeof (struct hwapi_var_i));

    return 1;
}

/** Get a variable option
 */
int hwapi_var_getopt(varid_t stat_id)
{
    assert(var_db);

    if (stat_id < 1 || stat_id > MAX_VARS) {

        return 0;
    }
    return var_db[stat_id].opts;
}

/** List all shared variables.
 *
 * Obtains a list of all variables created in the system
 * matching the given option (set to 0 to list all)
 */
int hwapi_var_list(int opts, struct hwapi_var_i *var_list, int nof_elems)
{
    int i, k;

    assert(var_db && var_list);

    k = 0;

    for (i = 1; i < MAX_VARS && k < nof_elems; i++) {
        if (var_db[i].obj_id && (opts == var_db[i].opts || !opts)) {

            memcpy(&var_list[k], &var_db[i],
                    sizeof (struct hwapi_var_i));
            k++;
        }
    }

    return k;
}

/** Modify a variable option
 */
int hwapi_var_setopt(varid_t stat_id, int new_opt)
{
    assert(var_db);

    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error setting stat option: Invalid stat id %d\n",stat_id);
        return 0;
    }
    var_db[stat_id].opts = new_opt;

    return 1;
}

/** Read a variable value
 * @param value_sz Size of the buffer, In bytes
 */
int hwapi_var_read(varid_t stat_id, void *value, int buff_sz)
{
    int len;

    assert(var_db && value && buff_sz >= 0);

    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error reading stat: Invalid var id %d, process %d\n",stat_id,getpid());
        return -1;
    }
    len = var_db[stat_id].cur_size;
    if (len > buff_sz) {
        len = buff_sz;
    }

    mem_cpy((mem_o) hw_api_db->var_table, value, var_db[stat_id].table_offset + hw_api_db->var_table, len);

    return len;
}

int hwapi_var_getsz(varid_t stat_id) {
    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error reading stat size: Invalid stat id %d\n",stat_id);
        return -1;
    }
    return var_db[stat_id].cur_size;
}

int hwapi_var_readts(varid_t stat_id, void *value, int buff_sz, int *tstamp)
{
    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error reading stat ts: Invalid stat id %d\n",stat_id);
        return -1;
    }
    if (tstamp) {
        *tstamp = var_db[stat_id].write_tstamp;
    }
    return hwapi_var_read(stat_id, value, buff_sz);
}

int hwapi_var_get_tstamp(varid_t stat_id)
{

    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error getting stat ts: Invalid stat id %d\n",stat_id);
        return -1;
    }
    return var_db[stat_id].write_tstamp;
}

/** Write a variable value
 * @param value_sz Size of the variable, In bytes
 */
int hwapi_var_write(varid_t stat_id, void *value, int value_sz)
{
    int len;

    assert(var_db && value && value_sz >= 0);

    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error writting stat: Invalid stat id %d, process %d\n",stat_id,getpid());
        return -1;
    }

    len = value_sz;
    
    if (len >= var_db[stat_id].size) {
        len = var_db[stat_id].size;
    }

    var_db[stat_id].cur_size = len;

    mem_cpy((mem_o) hw_api_db->var_table, var_db[stat_id].table_offset + hw_api_db->var_table, value, len);

    return len;
}

int hwapi_var_writets(varid_t stat_id, void *value, int value_sz)
{
    return hwapi_var_writemyts(stat_id, value, value_sz, my_proc->obj_tstamp);
}

int hwapi_var_writemyts(varid_t stat_id, void *value, int value_sz, int tstamp)
{

    if (stat_id < 1 || stat_id > MAX_VARS) {
        printf("HWAPI: Error writting stat ts: Invalid stat id %d\n",stat_id);
        return -1;
    }
    if (!my_proc) {
        return -1;
    }
    var_db[stat_id].write_tstamp = tstamp;

    return hwapi_var_write(stat_id, value, value_sz);
}

/* @} */


/** @defgroup mem Memory management
 *
 * Functions to use simulated dynamic memory. ALOE does not use dynamic memory
 * for any allocation to prevent system memory fragmentation and detect leaks better.
 *
 * To facilitate programming and introduce the benefits of dynamic memory,
 * ALOE enables the programmer to pre-allocate segments of memory of certain block size.
 * Then, when we request a memory chunk the HW API will search if a segment for this
 * block size has been pre-allocated, returning error or allocating memory to system heap
 * depending on the constant MEM_NOCHUNK_ERROR
 *
 * This will prevent memory fragmentation because only certain fragments are allowed
 * (the ones which have been preallocated).
 *
 *
 * @{
 */

/** Pre-allocate a heap of certain block size
 *
 * @param nof_elems Number of blocks to allocate
 * @param elem_sz Size of the block
 */
int hwapi_mem_prealloc(int nof_blocks, int block_sz)
{
    int i = 0;
    char *buffer;
    int buffer_sz;

    while (i < MAX_MEM_SEGMENTS && memseg[i].ptr)
        i++;
    if (i == MAX_MEM_SEGMENTS) {
        printf("HWAPI: No more memory pre-allocations allowed\n");
        return 0;
    }
    buffer_sz = mem_calc_buffer(nof_blocks, block_sz);
    buffer = calloc(buffer_sz, 1);
    if (!buffer) {
        printf("HWAPI: Error allocating memory (%d of %d)\n", nof_blocks, block_sz);
        return 0;
    }
    memseg[i].ptr = mem_new(buffer, buffer_sz, block_sz, 0);
    if (!memseg[i].ptr) {
        printf("HWAPI: Error creating memory segment\n");
        return 0;
    }
    memseg[i].elem_sz = block_sz;

    return 1;
}


/** dissaloc a pre-allocated memory segment of certain block
 * @param block_sz Size of the block
 */
int hwapi_mem_disalloc(int block_sz)
{
    int i = 0;
    char *x;

    while (i < MAX_MEM_SEGMENTS && memseg[i].elem_sz != block_sz)
        i++;
    if (i == MAX_MEM_SEGMENTS) {
        printf("HWAPI: Didn't find pre-allocated segment of block %d\n", block_sz);
        return 0;
    }
    x = memseg[i].ptr;
    mem_delete(&memseg[i].ptr);
    free(x);
    memset(&memseg[i], 0, sizeof (struct memseg));

    return 1;
}
int memCnt = 0;

void hwapi_mem_init(void) {
    memset(memseg, 0, sizeof (struct memseg) * MAX_MEM_SEGMENTS);
}

void * hwapi_mem_alloc_var(int size)
{

}

/** Allocate a segment of memory
 * @param size Size of the segment to allocate
 */
void * hwapi_mem_alloc(int size)
{
    int i, offset;

    if (hwapi_initated) {
        memCnt++;
        /* find one of equal elem sz */
        for (i = 0; i < MAX_MEM_SEGMENTS; i++) {
            if (memseg[i].elem_sz == size) {
                offset = mem_calloc(memseg[i].ptr, size);
                if (offset > 0) {
                    return (offset + memseg[i].ptr);
                }
            }
        }

    #ifdef MEM_NOCHUNK_ERROR
        return NULL;
    #endif

#ifdef VERBOSE_MEM
        if (!mem_silent) {
            printf("HWAPI: Caution pid %d is allocating memory to system heap (size %d not pre-allocated).\n", getpid(), size);
            print_bt(NULL);
        }
#endif

    }
    return calloc(1, size);
}

/** Free a segment of allocated memory allocated with hwapi_mem_alloc
 *
 * @param ptr Pointer of the segment allocated with hwapi_mem_alloc
 */
void hwapi_mem_free(void *ptr)
{
    int i;

    memCnt--;

    /* find one of equal elem sz */
    for (i = 0; i < MAX_MEM_SEGMENTS; i++) {
        if ((char*) ptr > (char*) memseg[i].ptr && memseg[i].ptr) {
            if (mem_free(memseg[i].ptr, (char*) ptr - (char*) memseg[i].ptr)) {
                break;
            }
        }
    }

    if (i == MAX_MEM_SEGMENTS) {
        #ifdef MEM_NOCHUNK_ERROR
            printf("HWAPI: Caution dellaocatin memory not allocated\n");
            return;
        #endif
#ifdef VERBOSE_MEM
        if (!mem_silent) {
            printf("HWAPI: Caution de-allocating memory from system heap (0x%x)\n", (unsigned int)  ptr);
            print_bt(NULL);
        }
#endif
        free(ptr);
    }
}
/**@} */

/* functions for debugging */
void hwapi_mem_silent(int silent)
{

    mem_silent = silent;
}


int hwapi_mem_used_chunks()
{

    return memCnt;
}

int hwapi_dac_getInputLen()
{
    return hw_api_db->dac.NsamplesIn;
}

int hwapi_dac_getOutputLen()
{
    return hw_api_db->dac.NsamplesOut;
}

float *hwapi_dac_getOutputBuffer()
{
    return hw_api_db->dac.dacoutbuff;
}

float *hwapi_dac_getInputBuffer()
{
    return hw_api_db->dac.dacinbuff;
}


/**********************************************************

 int send_parent_cmd(int cmd, int *data, int size);

 Internal function, send a command to parent process (runph)

 **********************************************************/
char send_parent_cmd(char cmd, char *data, int sz_data, char *result, int sz_res)
{
    int n;

    /* clear msb bit */
    msg.head.dst = MSG_MAGIC;
    msg.head.src = getpid();

    /* check size */
    if (sz_data < 0 || sz_data > MSG_SZ) {
        printf("HWAPI: Error with size %d\n", sz_data);
        return -1;
    }

    msg.body[0] = cmd;

    /* copy body */
    memcpy(&msg.body[1], data, sz_data);

    /* send message */
    n = msgsnd(api_msg_id, &msg, sz_data + MSG_HEAD_SZ + 1, 0);
    if (n == -1) {
        perror("msgsnd");
        return -1;
    }

    kill(getppid(), SIGUSR1);

    /* return message type is my pid */
    n = msgrcv(api_msg_id, &msg, MSG_SZ, getpid(), 0);
    if (n == -1) {
        perror("Receiving message from parent");
        return -1;
    }

    /* copy result */
    memcpy(result, &msg.body[1], sz_res);

    /* return value is first word */
    return msg.body[0];
}

/***********************
 *   SLEEP FUNCTIONS   *
 ***********************/
void sleep_ms(int msec)
{
    struct timespec treq, trem;
    if (msec <= 0)
        return;
    treq.tv_sec = msec / 1000;
    treq.tv_nsec = (msec % 1000) * 1000000;
    nanosleep(&treq, &trem);
}

void sleep_us(int usec)
{
    struct timespec treq, trem;
    if (usec <= 0)
        return;
    treq.tv_sec = usec / 1000000;
    treq.tv_nsec = (usec % 1000000) * 1000;
    clock_nanosleep(CLOCK_MONOTONIC, 0, &treq, &trem);
}

void get_time(time_t * t)
{
    time_t tact;
    struct timespec x;
    //gettimeofday(&tact, NULL);
    clock_gettime(CLOCK_MONOTONIC,&x);
    tact.tv_sec = x.tv_sec;
    tact.tv_usec = x.tv_nsec/1000;
    timersub(&tact, &hw_api_db->init_time, t);
}

void set_time(time_t * t)
{
    time_t tact;
    time_t tt;
    struct timespec x;

    get_time(&tt);

    clock_gettime(CLOCK_MONOTONIC,&x);
    tact.tv_sec=x.tv_sec;
    tact.tv_usec=x.tv_nsec/1000;
    timersub(&tact, t, &hw_api_db->init_time);
    hw_api_db->sync_ts = time_to_tstamp(t);

}

int hwapi_last_sync() {
    return hw_api_db->sync_ts;
}

void set_tslot(int len) {
  *ts_len = len;
}

void init_time(void)
{
    struct timespec x;
    clock_gettime(CLOCK_MONOTONIC,&x);
    hw_api_db->init_time.tv_sec = x.tv_sec;
    hw_api_db->init_time.tv_usec = x.tv_nsec/1000;
}


void add_time(time_t * t1, time_t * t2, time_t * r)
{

    timeradd(t1, t2, r);
}

void sub_time(time_t * t1, time_t * t2, time_t * r)
{

    timersub(t1, t2, r);
}

void get_time_interval(time_t * tdata)
{

    /*First element stores the result,
     *second element is the start time and
     *third element is the end time.
     */
    tdata[0].tv_sec = tdata[2].tv_sec - tdata[1].tv_sec;
    tdata[0].tv_usec = tdata[2].tv_usec - tdata[1].tv_usec;
    if (tdata[0].tv_usec < 0) {

        tdata[0].tv_sec--;
        tdata[0].tv_usec += 1000000;
    }
}

int time_to_tstamp(time_t *tdata)
{
	unsigned long long int x;
    if (!ts_len) {
        ts_len = &hw_api_db->cpu_info.tslen_usec;
    }
    x = ((unsigned long long int) tdata->tv_sec)*1000000 + tdata->tv_usec;

    return (int) (x/(*ts_len));

//    return (int) ((tdata->tv_sec*1000000+tdata->tv_usec)/(*ts_len));
}

void tstamp_to_time(int tstamp, time_t *tdata)
{
	unsigned long long int x;
    if (!ts_len) {
        ts_len = &hw_api_db->cpu_info.tslen_usec;
    }
    x = ((unsigned long long int) tstamp * (*ts_len));
    tdata[0].tv_sec = (int) (x/ 1000000);
    tdata[0].tv_usec = (int) (x - tdata[0].tv_sec * 1000000);
}

int get_tstamp(void)
{
    time_t tdata;
    get_time(&tdata);

    return time_to_tstamp(&tdata);
}

/** @} */

void wait_sem(int sem_id)
{
    wait_sem_n(sem_id,0);
}

int wait_sem_n(int sem_id, int n)
{

    if (!sem_id)
        return 0;

    struct sembuf op1;

    op1.sem_num = n;
    op1.sem_op = -1; 
    op1.sem_flg = 0; 

    if ((n = semop(sem_id, &op1, 1)) == -1) {
        if (errno==EINTR && hw_api_db->cpu_info.debug_level) {
            return 1;
        } else {
            perror("Error setting semaphore: ");
            printf("pid=%d\n",getpid());
            return 0;
        }
    }
    return 1;
}

void post_sem(int sem_id)
{
    post_sem_n(sem_id,0);
}

void get_sem(int sem_id, int n)
{
    return semctl(sem_id,n,GETVAL);
}

int post_sem_n(int sem_id,int n)
{
    struct sembuf op1;
    union semun op2;
    int x;

    if (!sem_id) {
        printf("Error sem_id is 0\n");
        return 0;
    }


    x = semctl(sem_id,n,GETVAL);
    if (x>=1) {
        if (x>1) {
            op2.val = 1;
            if (semctl(sem_id,n,SETVAL,&op2)==-1) {
                perror("semctl");
            }
        }
        return 1;
    }
    op1.sem_num = n;
    op1.sem_op = 1; 
    op1.sem_flg = 0; 

    if ((n = semop(sem_id, &op1, 1)) == -1) {
        if (errno==EINTR && hw_api_db->cpu_info.debug_level) {
            return 1;
        } else {
            perror("Error setting semaphore: ");
            printf("pid=%d\n",getpid());
            return 0;
        }
    }

    return 1;
}