/*
 * sync.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/>.
 *
 */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>


#include "phal_hw_api.h"
#include "phid.h"
#include "sync.h"

#include "itf_utils.h"

#define ever (;;)

#define TEST

void answer_packet(hwitf_t id);

int sync_run;
int testing = 0;
char line_str[128];

#define BUFF_SZ 128



unsigned int itot;
int fd_w, fd_r;
hwitf_t id_w, id_r;
int period;
int round_trip_limit, interval;
time_t xtime, newt, difft;
struct syncdata sd;
time_t ttot;
int fbytes;
int ack;

int send_sync_req()
{
    int n;
    get_time(&sd.tx_time);
    n = hwapi_itf_snd(fd_w, &sd, sizeof (struct syncdata));
    return n;
}

void answer_packet(hwitf_t id)
{
    int n;

    n = hwapi_itf_rcv(fd_r, &sd, sizeof (struct syncdata));
    if (!n)
        return;
    if (n<sizeof (struct syncdata)) {
        printf("SYNC: Error receiving sync info from sync master\n");
        sync_run = 0;
    }
    get_time(&sd.rx_time);

    /*Compute loop time interval and adjust time*/
    interval = (sd.rx_time.tv_sec - sd.tx_time.tv_sec)*1000000 + sd.rx_time.tv_usec - sd.tx_time.tv_usec;
    if (interval < round_trip_limit) {
        newt.tv_sec = sd.ref_time.tv_sec + (sd.ref_time.tv_usec + interval / 2) / 1000000;
        newt.tv_usec = (sd.ref_time.tv_usec + interval / 2) % 1000000;
        set_time(&newt);

        difft.tv_sec = sd.ref_time.tv_sec - sd.tx_time.tv_sec;
        difft.tv_usec = sd.ref_time.tv_usec - sd.tx_time.tv_usec - interval/2;

#ifdef TEST					
        if (!(itot%16) || difft.tv_usec > interval) {
          printf("==- %d -==\n", itot);
          printf("SYNC: Approx. time misalignment: %d sec, %d usec\n", (int) difft.tv_sec, (int) difft.tv_usec);
          printf("SYNC: Max. estimated error: %d usec\n", interval / 2);
        }
#endif
    } else {
#ifdef TEST
        printf("%d >>>> round trip error too large (%d.%06d): %d\n", itot, (int) sd.rx_time.tv_sec, (int) sd.rx_time.tv_usec, interval);
#endif
    }
    ack = 1;
    itot++;
}

/***********
 *   MAIN  *
 ***********/

int main(int argc, char **argv)
{
    int last_tstamp, n;
    int period_ts;
    struct sync_register sync_reg;
    time_t t;
    struct hwapi_cpu_i cpu;

    if (!hwapi_init()) {
        xprintf("SYNC: Error initiating hwapi\n");
        exit(0);
    }

    /*Set default values*/
    period = DEFAULT_PERIOD;
    round_trip_limit = DEFAULT_ROUND_TRIP;

    t.tv_sec = 0;
    t.tv_usec = period * 1000;
    period_ts = time_to_tstamp(&t);

    printf("SYNC: Rounded period is %d time-slots\n", period_ts);

    if (!utils_create_attach_ext_id(SYNC_MASTER_ITF, BUFF_SZ,
            &fd_w, FLOW_WRITE_ONLY, &id_w)) {
        xprintf("SYNC: Error configuring slave sync output itf\n");
        exit(0);
    }
    if (!utils_create_attach_ext_id(SYNC_MASTER_ITF, BUFF_SZ,
            &fd_r, FLOW_READ_ONLY, &id_r)) {
        xprintf("SYNC: Error configuring slave sync output itf\n");
        exit(0);
    }

    xprintf("SYNC: Init Ok\n");

    sleep_ms(500);

    /* register to sync master */
    hwapi_hwinfo_cpu(&cpu);
    sync_reg.cmd = SYNC_REGISTER_MAGIC;
    sync_reg.itf_id = id_r;
    sync_reg.pe_id = cpu.pe_id;
    sync_reg.plat_family = cpu.plat_family;
    sync_reg.tslen_usec = cpu.tslen_usec;
    sync_reg.period = period;
    sync_reg.round_trip_limit = round_trip_limit;

    if (hwapi_itf_snd(fd_w, &sync_reg, sizeof (struct sync_register)) <= 0) {
        xprintf("SYNC: Error sending through itf\n");
        exit(0);
    }
    do {
        n = hwapi_itf_rcv(fd_r, &sync_reg, sizeof (struct sync_register));
        if (!n) {
            hwapi_relinquish_daemon();
        }
    } while (!n);
    if (n < 0 || sync_reg.cmd != SYNC_REGISTER_OK) {
        xprintf("SYNC: Error receiving register ack packet\n");
        exit(0);
    }

    if (!hwapi_itf_seturgent_out(id_w, sync_reg.itf_id)) {
        xprintf("SYNC: Error setting option to interface\n");
    }


    if (!hwapi_itf_seturgent_in(id_r, answer_packet)) {
        xprintf("SYNC: Error setting option to interface\n");
    }

    printf("SYNC: Registered ok to MASTER 0x%x local 0x%x remote 0x%x \n", sync_reg.pe_id, id_r, sync_reg.itf_id);

    /*For ever synchronise*/
    sd.key = SYNC_KEY;
    itot = 0;
    ack = 1;
    last_tstamp = get_tstamp();
    sync_run = 1;
    while (sync_run)
    {
        if (sync_run < 0) {
            sync_run = 0;
            sd.key = 0;
        }


        if ((get_tstamp() - last_tstamp) >= period_ts) {
            if (ack) {
                ack = 0;
                last_tstamp = get_tstamp();
                n = send_sync_req();
                if (n<sizeof (struct syncdata)) {
                    printf("SYNC: Error writing sync info to shell\n");
                    exit(-1);
                }
            }
        }
        /*Sleep for a period*/
        hwapi_relinquish_daemon();
    }


    return (0);
}