/*
 * WRS_PTS_Lib
 *
 * Control different parts of the WRS hardware for the PTS
 *
 *  Created on: January, 2017
 *  Authors:
 *         - INCAA Computers
 *
 * 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 for the WRS PTS
 *
 * You should have received a copy of the GNU General Public License...
*/

//includes
#include <stdlib.h>
#include <getopt.h>
#include <assert.h>
#include <stddef.h>
#include <math.h>
#include <linux/if_ether.h>	/* struct ethhdr */

#include <libwr/shw_io.h>
#include <libwr/switch_hw.h>
#include <libwr/wrs-msg.h>
#include <libwr/shmem.h>
#include <libwr/hal_shmem.h>
#include <libwr/util.h>
#include <libwr/hal_client.h>
#include <libwr/ptpd_netif.h>
#include <libwr/pio.h>
#include <regs/gen10mhz-regs.h>
#include <regs/ppsg-regs.h>
#include <../spwm-regs.h>
#include <../i2c_sfp.h>
#include <../i2c_fpga_reg.h>
#include <../i2c.h>
#include <rt_ipc.h>
#include <fpga_io.h>
#include "ad9516.h"

//static variables
static int halCount = 1;
static int hal_shm_init_done = 0;
static int hal_nports_local;
static struct wrs_shm_head *hal_head;
static struct hal_port_state *hal_ports;
static volatile struct SPWM_WB *spwm_wbr;//FPGA PWM system
extern uint32_t pca9554_masks[];

static i2c_fpga_reg_t fpga_sensors_bus_master = {
	.base_address = FPGA_I2C_ADDRESS,
	.if_num = FPGA_I2C_SENSORS_IFNUM,
	.prescaler = 500,
};
static struct i2c_bus fpga_sensors_i2c = {
	.name = "fpga_sensors",
	.type = I2C_BUS_TYPE_FPGA_REG,
	.type_specific = &fpga_sensors_bus_master,
};
static i2c_fpga_reg_t fpga_bus0_reg = {
	.base_address = FPGA_I2C_ADDRESS,
	.if_num = FPGA_I2C0_IFNUM,
	.prescaler = 500,
};
static struct i2c_bus fpga_bus0 = {
	.name = "fpga_sensors_ic7",
	.type = I2C_BUS_TYPE_FPGA_REG,
	.type_specific = &fpga_bus0_reg,
};
static i2c_fpga_reg_t fpga_bus1_reg = {
	.base_address = FPGA_I2C_ADDRESS,
	.if_num = FPGA_I2C1_IFNUM,
	.prescaler = 500,
};
static struct i2c_bus fpga_bus1 = {
	.name = "sfp_i2c",
	.type = I2C_BUS_TYPE_FPGA_REG,
	.type_specific = &fpga_bus1_reg,
};

//data structures
typedef struct {
    int ID;
    int rand;
} pkt_test_t;

struct params {
    float freq_mhz;
    int period_ns;

    float duty;
    int h_width, l_width;

    int cshift_ns;
    int sigdel_taps;
    int ppshift_taps;
};

//defines
#define DEF_FREQ    10
#define DEF_DUTY    0.5
#define DEF_CSHIFT    30
#define DEF_SIGDEL    0
#define DEF_PPSHIFT    0
#define MAX_FREQ    250        /* min half-period is 2ns */
#define MIN_FREQ    0.004    /* max half-period is 65535*2ns */
#define CNT_RES        2
#define TEMP_SENSOR_ADDR_FPGA   0x4A /* (7bits addr) IC19 Below FPGA */
#define TEMP_SENSOR_ADDR_PLL    0x4C /* (7bits addr) IC18 PLLs */
#define TEMP_SENSOR_ADDR_PSL    0x49 /* (7bits addr) IC20 Power supply left */
#define TEMP_SENSOR_ADDR_PSR    0x4D /* (7bits addr) IC17 Power supply right */
uint8_t TEMP_SENSOR_ADDR_IC7  = 0x4A; /* (7bits addr) IC7 miniBackplane */
#define gen10_write(reg, val) \
    _fpga_writel(FPGA_BASE_GEN_10MHZ + offsetof(struct GEN10_WB, reg), val)
#define gen10_read(reg) \
    _fpga_readl(FPGA_BASE_GEN_10MHZ + offsetof(struct GEN10_WB, reg))
#define HAL_PROCESS_NAME "/wr/bin/wrsw_hal"
#define PROCESS_COMMAND_HAL "/bin/ps axo command"
#define MONIT_PROCESS_NAME "/usr/bin/monit"
#define PROCESS_COMMAND_MONIT "/bin/ps axo stat o command"
#define FPGA_BASE_GPIO_STATUS   0x7F000
#define FPGA_BASE_GPIO_CONTROL  0x7E000
#define FPGA_BASE_SYS_MON_CORE  0x7D000
#define GPIO_COR  0x0 //Set output reg
#define GPIO_SOR  0x4 //Clear output reg
#define GPIO_ROR  0x4 //Read output reg
#define SFP_LED_SYNCED_MASK(t)	((t) ? (1 << 6) : (1 << 0))
#define SFP_LED_LINK_MASK(t)	((t) ? (1 << 4) : (1 << 1))
#define SFP_LED_WRMODE_MASK(t)	((t) ? (1 << 5) : (1 << 3))
#define SFP_TX_DISABLE_MASK(t)	((t) ? (1 << 7) : (1 << 2))
#define AT91C_BASE_SYS_RAW  0xFFFFC000
#define AT91C_BASE_PMC_RAW  0xFFFFFC00
#define AT91C_BASE_PIOA     0xFFFFF200
#define BASE_GPIO_RT        0x10300
#define GPIO_SYS_CLK_SEL    0
#define GPIO_PLL_RESET_N    1
#define GPIO_CPU_RESET      2
#define GPIO_PERIPH_RESET_N 3

//needed for using HAL SHMEM
static int hal_shm_init(void)
{
    int ii;
    struct hal_shmem_header *h;
    int n_wait = 0;
    int ret;

    /* wait for HAL */
    while ((ret = wrs_shm_get_and_check(wrs_shm_hal, &hal_head)) != 0) {
        n_wait++;
        if (n_wait > 10) {
            if (ret == WRS_SHM_OPEN_FAILED) {
                fprintf(stderr, "wr_phytool: Unable to open "
                    "HAL's shm !\n");
            }
            if (ret == WRS_SHM_WRONG_VERSION) {
                fprintf(stderr, "wr_phytool: Unable to read "
                    "HAL's version!\n");
            }
            if (ret == WRS_SHM_INCONSISTENT_DATA) {
                fprintf(stderr, "wr_phytool: Unable to read "
                    "consistent data from HAL's shmem!\n");
            }
            return(-1);
        }
        sleep(1);
    }

    /* check hal's shm version */
    if (hal_head->version != HAL_SHMEM_VERSION) {
        fprintf(stderr, "wr_mon: unknown hal's shm version %i "
            "(known is %i)\n",
            hal_head->version, HAL_SHMEM_VERSION);
        return -1;
    }

    h = (void *)hal_head + hal_head->data_off;

    while (1) { /* wait forever for HAL to produce consistent nports */
        ii = wrs_shm_seqbegin(hal_head);
        /* Assume number of ports does not change in runtime */
        hal_nports_local = h->nports;
        if (!wrs_shm_seqretry(hal_head, ii))
            break;
        fprintf(stderr, "INFO: wr_phytool Wait for HAL.\n");
        sleep(1);
    }

    /* Even after HAL restart, HAL will place structures at the same
     * addresses. No need to re-dereference pointer at each read. */
    hal_ports = wrs_shm_follow(hal_head, h->ports);
    if (hal_nports_local > HAL_MAX_PORTS) {
        fprintf(stderr,
            "FATAL: wr_phytool Too many ports reported by HAL."
            "%d vs %d supported\n",
            hal_nports_local, HAL_MAX_PORTS);
        return -1;
    }
    return 0;
}

//apply settings to AD9516
static int apply_settings(struct params *p)
{
    gen10_write(PR, p->h_width);
    gen10_write(DCR, p->l_width);
    gen10_write(CSR, p->cshift_ns/CNT_RES);
    gen10_write(IOR, p->sigdel_taps);
    gen10_write(PPS_IOR, p->ppshift_taps);
    sleep(1);
    /* now read the actual delay (in taps) from IODelays */
    p->sigdel_taps = gen10_read(IOR);
    p->sigdel_taps >>= GEN10_IOR_TAP_CUR_SHIFT;
    p->ppshift_taps = gen10_read(PPS_IOR);
    p->ppshift_taps >>= GEN10_PPS_IOR_TAP_CUR_SHIFT;

    return 0;
}

//calc settings for AD9516
static int calc_settings(struct params *req, struct params *calc)
{
    calc->freq_mhz = req->freq_mhz;
    req->period_ns = 1000 / calc->freq_mhz;
    calc->h_width = req->period_ns/CNT_RES * req->duty;
    calc->l_width = req->period_ns/CNT_RES - calc->h_width;

    /* last step, calculate the actual frequency and period based on the
     * actual h_width and l_width */
    calc->period_ns = (calc->h_width + calc->l_width) * CNT_RES;
    calc->freq_mhz = 1000 / calc->period_ns;
    calc->duty = (float)calc->h_width*CNT_RES / calc->period_ns;

    /* just copy the values that don't change */
    calc->cshift_ns    = req->cshift_ns;
    calc->sigdel_taps  = req->sigdel_taps;
    calc->ppshift_taps = req->ppshift_taps;

    return 0;
}

/* Texas Instruments TMP100 temperature sensor driver */
static uint32_t tmp100_read_reg(int dev_addr, uint8_t reg_addr, int n_bytes)
{
    uint8_t data[8];
    uint32_t rv = 0, i;

    data[0] = reg_addr;
    i2c_write(&fpga_sensors_i2c, dev_addr, 1, data);
    i2c_read(&fpga_sensors_i2c, dev_addr, n_bytes, data);

    for (i = 0; i < n_bytes; i++) {
        rv <<= 8;
        rv |= data[i];
    }

    return rv;
}

/* Texas Instruments TMP100 temperature sensor driver for IC7 */
static uint32_t tmp100_read_reg_ic7(int dev_addr, uint8_t reg_addr, int n_bytes)
{
    uint8_t data[8];
    uint32_t rv = 0, i;

    data[0] = reg_addr;
    i2c_write(&fpga_bus0, dev_addr, 1, data);
    i2c_read(&fpga_bus0, dev_addr, n_bytes, data);

    for (i = 0; i < n_bytes; i++) {
        rv <<= 8;
        rv |= data[i];
    }

    return rv;
}

/* Texas Instruments TMP100 temperature sensor driver for IC7 */
static void tmp100_write_reg_ic7(int dev_addr, uint8_t reg_addr, uint8_t value)
{
	uint8_t data[2];

	data[0] = reg_addr;
	data[1] = value;
	i2c_write(&fpga_bus0, dev_addr, 2, data);
}

//check if monit service is running
static int check_monit(void)
{
	FILE *f;
	char command[41]; /* 1 for null char */
	char stat[5]; /* 1 for null char */
	int ret = 0;

	f = popen(PROCESS_COMMAND_MONIT, "r");
	if (!f) {
		pr_error("Error while checking the presence of HAL!\n");
		exit(1);
	}
	while (ret != EOF) {
		/* read first word from line (process name) ignore rest of
		 * the line */
		ret = fscanf(f, "%4s %40s%*[^\n]", stat, command);

		if (ret != 2)
			continue; /* error... or EOF */
		if (!strcmp(MONIT_PROCESS_NAME, command)) {
			if (strcmp(stat, "T")) {
				/* if monit in "T" then not really running */
				pclose(f);
				return 1;
			}
		}
	}
	pclose(f);
	return 0;
}

//check if hal service is running
static int check_hal(void)
{
	FILE *f;
	char key[41]; /* 1 for null char */
	int ret = 0;

	f = popen(PROCESS_COMMAND_HAL, "r");
	if (!f) {
		pr_error("Error while checking the presence of HAL!\n");
		exit(1);
	}
	while (ret != EOF) {
		/* read first word from line (process name) ignore rest of
		 * the line */
		ret = fscanf(f, "%40s%*[^\n]", key);
		if (ret != 1)
			continue; /* error... or EOF */
		if (!strcmp(HAL_PROCESS_NAME, key)) {
			pclose(f);
			return 1;
		}
	}
	pclose(f);
	return 0;
}

/* Configures a PWM output on gpio pin (pin). */
static void pwm_configure_fpga(int enmask, float rate)
{
    uint8_t u8speed = (uint8_t) ((rate >= 1) ? 0xff : (rate * 255.0));

    if ((enmask & 0x1) > 0)
        spwm_wbr->DR0 = u8speed;
    if ((enmask & 0x2) > 0)
        spwm_wbr->DR1 = u8speed;
}

//init sfp gpio txdisable to be input
static int shw_sfp_gpio_init_input(void)
{
	int i;
	uint8_t addr = 0x20;
	uint8_t conf_dir[] = { 0x3, (1<<2 | 1<<7) };//2x tx_disable as input

	/* configure the pins */
	i2c_transfer(&fpga_bus0, addr, 2, 0, conf_dir);

	for (i = 0; i < 8; i++)
		i2c_transfer(&fpga_bus1, addr + i, 2, 0, conf_dir);

    return 0;
}

//modified sfp gpio read for txdisable input
uint8_t shw_sfp_gpio_get_txdisable(int id)
{
	int top;
	uint8_t addr = 0x20;
	uint8_t send[2];
	uint8_t out = 0;
	uint8_t curr;
    struct i2c_bus *bus = &fpga_bus1;

	if (id < 2)
		bus = &fpga_bus0;

	if (id > 1) {
		addr += pca9554_masks[id] / 2;
		top = pca9554_masks[id] % 2;
	} else {
		top = id % 2;
	}

	send[0] = 0x0; //read input
	/* Read current state of pins */
	i2c_transfer(bus, addr, 1, 0, send);
	i2c_transfer(bus, addr, 0, 1, &curr);

	if (curr & SFP_LED_LINK_MASK(top))
		out |= SFP_LED_WRMODE1;
	if (curr & SFP_LED_WRMODE_MASK(top))
		out |= SFP_LED_WRMODE2;
	if (curr & SFP_LED_SYNCED_MASK(top))
		out |= SFP_LED_SYNCED;
	if (curr & SFP_TX_DISABLE_MASK(top))
		out |= SFP_TX_DISABLE;

	return out;
}

//get sfp gpio input
static int shw_sfp_get_generic(int num, int type)
{
	return shw_sfp_gpio_get_txdisable(num) & type;
}

//////////////
//public API//
//////////////

//call all needed init functions
int init(void)
{
    printf("WRS_PTS_LIB init called\n");
    float temp;

    //disable some autostarted processes
    if (check_monit() > 0)
        system("/etc/init.d/monit.sh stop");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    //map fpga addresses to cpu addressspace
    printf("FPGA memmap init\n");
    if (shw_fpga_mmap_init() < 0)
        return -1;
    
    //check fpga loaded
    printf("Check FPGA loaded\n");
    if (_fpga_readl(0x30034) != 0xCAFEBABE)
        return -2;

    //init hardware interfaces
    printf("SHW io initialize\n");
    if (shw_io_init() < 0)
        return -3;

    printf("SHW io configure\n");
    if (shw_io_configure_all() < 0)
        return -4;

    printf("SHW SFP buses init\n");
    if (shw_sfp_buses_init() < 0)
        return -5;

    printf("FPGA sensors i2c bus init\n");
    if (i2c_init_bus(&fpga_sensors_i2c) < 0)
        return -6;
        
    printf("FPGA bus0 i2c init\n");
    if (i2c_init_bus(&fpga_bus0) < 0)
        return -7;

    printf("FPGA bus1 i2c init\n");
    if (i2c_init_bus(&fpga_bus1) < 0)
        return -8;

    printf("SFP GPIO init input\n");
    if (shw_sfp_gpio_init_input() < 0)
        return -9;
    
    printf("SHW init fans\n");
    if (shw_init_fans() < 0)
        return -10;
    
    //set ic7 config, others done in shw_init_fans
    printf("Set IC7 config\n");
    tmp100_write_reg_ic7(TEMP_SENSOR_ADDR_IC7, 1, 0x60);
    temp = (float)(tmp100_read_reg_ic7(TEMP_SENSOR_ADDR_IC7, 0, 2) >> 4) / 16.0;
    if (temp < 5.0 || temp > 100.0) //tmp100 instead of tmp101
    {
        TEMP_SENSOR_ADDR_IC7 = 0x4E;
        tmp100_write_reg_ic7(TEMP_SENSOR_ADDR_IC7, 1, 0x60);
    }

    //Point to the corresponding WB direction, PWM
    spwm_wbr =
        (volatile struct SPWM_WB *)(FPGA_BASE_ADDR +
                    FPGA_BASE_SPWM);

    //Configure SPWM register the 30~=(62.5MHz÷(8kHz×2^8))-1
    printf("Configure SPWM register\n");
    spwm_wbr->CR = SPWM_CR_PRESC_W(30) | SPWM_CR_PERIOD_W(255);

    //set default options gpio control, pts_mode+pll_sync_n
    printf("Set default PTS gpio options\n");
    _fpga_writel(FPGA_BASE_GPIO_CONTROL + GPIO_SOR, 1<<18 | 1<<21);

    //set pck0 to 30MHz(upllck/16), used for freq measurements
    printf("Enable PCK0 and set to 30MHz\n");
    _writel(_sys_base + AT91C_BASE_PMC_RAW - AT91C_BASE_SYS_RAW + 0x40, 0x13);//config clock
    _writel(_sys_base + AT91C_BASE_PIOA - AT91C_BASE_SYS_RAW + 0x74, (1 << 31));//select peripheral B(pck0)
    _writel(_sys_base + AT91C_BASE_PIOA - AT91C_BASE_SYS_RAW + 0x04, (1 << 31));//enable peripheral control
    _writel(_sys_base + AT91C_BASE_PMC_RAW - AT91C_BASE_SYS_RAW + 0x00, (1 << 8));//enable clock

    printf("Init succeeded\n");
    return 0;
}

//test sfp phy tx/rx
int sfp_comm_test(int ep)
{
    printf("WRS_PTS_LIB sfp_comm_test called\n");

    struct wr_tstamp ts_tx, ts_rx;
    struct wr_socket *sock;
    struct wr_sockaddr sock_addr, to, from;
    struct hal_port_state *port;
    char bufferTX[64], bufferRX[64];
    pkt_test_t ptest_TX, ptest_RX;
    int result = 0;
    char cmd[100];

    //start/restart hal deamon
    if (check_hal() == 0)
        system("/etc/init.d/hald.sh start");
    else { //periodic restart, seemed nessecary
        if (halCount >= 6) {
            halCount = 1;
            system("/etc/init.d/hald.sh restart");
        }
        else
            halCount++;
    }
    
    //handle unknown mac broadcast
    system("/wr/bin/rtu_stat unrec disable");
    sprintf(cmd, "/wr/bin/rtu_stat unrec enable %d", ep);
    system(cmd);

    //set config bit RTU "Forward unrecognized frames also to CPU"
    printf("Set config bit RTU to forward unrecognized frames\n");
    _fpga_writel(0x00060014, 0x20019);

    //set timestamps to zero
    memset(&ts_tx, 0, sizeof(ts_tx));
    memset(&ts_rx, 0, sizeof(ts_rx));

    //fill test packet
    srand(time(NULL));
    ptest_TX.ID = ep;
    ptest_TX.rand = rand();
    memcpy(bufferTX, &ptest_TX, sizeof(pkt_test_t));

    //prepare socket
    snprintf(sock_addr.if_name, sizeof(sock_addr.if_name), "wri%d", ep);
    sock_addr.family = PTPD_SOCK_RAW_ETHERNET; //socket type
    sock_addr.ethertype = 12345;
    memset(sock_addr.mac, 0xff, 6);

    //prepare send params
    to.mac[0] = 0x01;
    to.mac[1] = 0x02;
    to.mac[2] = 0x03;//random MAC -> unrecognized -> forwarded to CPU
    to.mac[3] = 0x04;
    to.mac[4] = 0x05;
    to.mac[5] = 0x06;
    to.ethertype = 12345;
    to.family = PTPD_SOCK_RAW_ETHERNET;

    //init ptpd+hal_shm
    if (ptpd_netif_init() < 0)
        return -1;
    if (hal_shm_init_done == 0)
    {
        if (hal_shm_init() < 0)
            return -1;
        hal_shm_init_done = 1;
    }

    //get port and create socket
    port = hal_lookup_port(hal_ports, hal_nports_local, sock_addr.if_name);
    sock = ptpd_netif_create_socket(PTPD_SOCK_RAW_ETHERNET, 0, &sock_addr, port);

    //do the test, send+receive
    result += ptpd_netif_sendto(sock, &to, bufferTX, sizeof(bufferTX), &ts_tx);
    printf("Send test packet, %d bytes\n", result);
    usleep(500);    
    result -= ptpd_netif_recvfrom(sock, &from, bufferRX, sizeof(bufferRX), &ts_rx, port);
    result -= sizeof(struct ethhdr); //sendto counts it, recvfrom not
    printf("Tried to receive test packet, resulting byte count is %d\n", result);
    memcpy(&ptest_RX, bufferRX, sizeof(pkt_test_t));

    //close socket
    ptpd_netif_close_socket(sock);
    
    //display results
    printf("tx id: %d and rx id: %d\n", ptest_TX.ID, ptest_RX.ID);
    printf("tx rand: %d and rx rand: %d\n", ptest_TX.rand, ptest_RX.rand);

    if ((ptest_RX.ID == ptest_TX.ID) && (ptest_RX.rand == ptest_TX.rand))
        return result;
    else
        return result - 1;
}

//configures a PWM output. Rate accepts range is from 0 (0%) to 1 (100%)
int shw_pwm_speed(int enmask, float rate)
{
    printf("WRS_PTS_LIB shw_pwm_speed called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    pwm_configure_fpga(enmask, rate);

    return 0;
}

//read different temperatures
int get_temps(float *temps)
{
    printf("WRS_PTS_LIB get_temps called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    temps[0] = (float)(tmp100_read_reg(TEMP_SENSOR_ADDR_FPGA, 0, 2) >> 4) / 16.0;
    temps[1] = (float)(tmp100_read_reg(TEMP_SENSOR_ADDR_PLL, 0, 2) >> 4) / 16.0;
    temps[2] = (float)(tmp100_read_reg(TEMP_SENSOR_ADDR_PSL, 0, 2) >> 4) / 16.0;
    temps[3] = (float)(tmp100_read_reg(TEMP_SENSOR_ADDR_PSR, 0, 2) >> 4) / 16.0;
    temps[4] = (float)(tmp100_read_reg_ic7(TEMP_SENSOR_ADDR_IC7, 0, 2) >> 4) / 16.0;
    temps[5] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE) >> 6) * 503.975 / 1024.0 - 273.15;

    return 0;
}

//read different voltages
int get_voltages(float *voltages)
{
    printf("WRS_PTS_LIB get_voltages called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    voltages[0] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 4) >> 6) / 1024.0 * 3.0;
    voltages[1] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 8) >> 6) / 1024.0 * 3.0;
    voltages[2] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 64) >> 6) * 0.000977 / (4.7 / (10 + 4.7));
    voltages[3] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 72) >> 6) * 0.000977 / (4.7 / (22 + 4.7));
    voltages[4] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 80) >> 6) * 0.000977 / (4.7 / (30 + 4.7));
    voltages[5] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 84) >> 6) * 0.000977 / (4.7 / (10 + 4.7));
    voltages[6] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 88) >> 6) * 0.000977 / (4.7 / (4.7 + 4.7));
    voltages[7] = (float)(_fpga_readl(FPGA_BASE_SYS_MON_CORE + 92) >> 6) * 0.000977 / (4.7 / (4.7 + 4.7));

    return 0;
}

//get sfp status
int get_sfp_status(uint32_t *presence, uint32_t *los_txfault, uint32_t *txdisable)
{
    printf("WRS_PTS_LIB get_sfp_status called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    *presence = 0x3FFFF & ~(
        shw_io_read(shw_io_sfp_presence1) << 0 | 
        shw_io_read(shw_io_sfp_presence2) << 1 |    
        shw_io_read(shw_io_sfp_presence3) << 2 |    
        shw_io_read(shw_io_sfp_presence4) << 3 |    
        shw_io_read(shw_io_sfp_presence5) << 4 |    
        shw_io_read(shw_io_sfp_presence6) << 5 |    
        shw_io_read(shw_io_sfp_presence7) << 6 |    
        shw_io_read(shw_io_sfp_presence8) << 7 |    
        shw_io_read(shw_io_sfp_presence9) << 8 |    
        shw_io_read(shw_io_sfp_presence10) << 9 |    
        shw_io_read(shw_io_sfp_presence11) << 10 |    
        shw_io_read(shw_io_sfp_presence12) << 11 |    
        shw_io_read(shw_io_sfp_presence13) << 12 |    
        shw_io_read(shw_io_sfp_presence14) << 13 |    
        shw_io_read(shw_io_sfp_presence15) << 14 |    
        shw_io_read(shw_io_sfp_presence16) << 15 |    
        shw_io_read(shw_io_sfp_presence17) << 16 |    
        shw_io_read(shw_io_sfp_presence18) << 17);

    *los_txfault = 0x3FFFF & ~(_fpga_readl(FPGA_BASE_GPIO_STATUS + GPIO_ROR));

    //port(0-17),type(1:green_1/2:orange1/4:green_2/8:txdisable)
    shw_sfp_gpio_init_input();
    int i=0, result=0;
    for (i=0; i<18; i++)
        result |= (shw_sfp_get_generic(i, 8) == 0) << i;

    *txdisable = result;

    return 0;
}

//read+check sfp eeprom
int get_eeprom_sfp(int port, uint8_t *sfp_header)
{
    printf("WRS_PTS_LIB get_eeprom_sfp called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    shw_sfp_read(port-1, I2C_SFP_ADDRESS, 0x0, sizeof(struct shw_sfp_header), sfp_header);

    //fix id+ext_id if for some reason wrong
    struct shw_sfp_header *header = (struct shw_sfp_header *)sfp_header;
    if (header->id != 3 && header->ext_id != 4){
        printf("fixing id's eeprom, was %d and %d\n", header->id, header->ext_id);
        uint8_t buffer[2] = {3, 4};
        shw_sfp_write(port-1, I2C_SFP_ADDRESS, 0x0, 0x2, &buffer[0]);
        shw_sfp_read(port-1, I2C_SFP_ADDRESS, 0x0, sizeof(struct shw_sfp_header), sfp_header);
    }

    return shw_sfp_header_verify_base((struct shw_sfp_header *)sfp_header);
}

//test AD9516 clk2 output
int set_ad9516_clk2(float freq, float duty, int cshift, int sigdel, int ppshift)
{
    printf("WRS_PTS_LIB set_ad9516_clk called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    struct params req = {DEF_FREQ, 0, DEF_DUTY, 0, 0, DEF_CSHIFT, DEF_SIGDEL, DEF_PPSHIFT};
    struct params calc;

    req.freq_mhz = freq;    
    req.duty = duty;
    req.cshift_ns = cshift;
    req.sigdel_taps = sigdel;
    req.ppshift_taps = ppshift;
    
    calc_settings(&req, &calc);
    
    apply_settings(&calc);

    return 0;
}

//set AD9516 through SPI
int set_ad9516(int output, int ratio, int phase_offset)
{
    printf("WRS_PTS_LIB set_ad9516 called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    return ad9516_set_output_divider(output, ratio, phase_offset);
}

//init AD9516 for test
int init_ad9516(void)
{
    printf("WRS_PTS_LIB init_ad9516 called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    return ad9516_init();
}

//set all leds color, 0=off, 1=green, 2=orange
int set_led(int color)
{
    printf("WRS_PTS_LIB set_led called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    uint32_t i, temp;

    //port leds
    shw_sfp_gpio_init();
    for (i = 0; i<18; i++)
    {
        //port(0-17),enable(0/1),type(1:green_1/2:orange1/4:green_2/8:txdisable)
        shw_sfp_set_generic(i, (color == 1 ? 1 : 0), 1);
        shw_sfp_set_generic(i, (color == 2 ? 1 : 0), 2);
        shw_sfp_set_generic(i, (color == 1 ? 1 : 0), 4);
    }

    //orange2 led, from fpga comm
    temp = _fpga_readl(FPGA_BASE_GPIO_CONTROL + GPIO_ROR);
    if (color == 2)
        _fpga_writel(FPGA_BASE_GPIO_CONTROL + GPIO_SOR, temp | 0x3FFFF);
    else
        _fpga_writel(FPGA_BASE_GPIO_CONTROL + GPIO_COR, 0x3FFFF);

    //status led
    shw_io_write(shw_io_led_state_o, (color == 2 ? 1 : 0));//red
    shw_io_write(shw_io_led_state_g, (color == 1 ? 1 : 0));//green

    return 0;
}

//get general button
int get_button(uint32_t *value)
{
    printf("WRS_PTS_LIB get_button called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    *value = shw_io_read(shw_io_arm_gen_but);

    return 0;
}

//write register to FPGA
int write_reg_fpga(int address, int data)
{
    printf("WRS_PTS_LIB write_reg_fpga called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    _fpga_writel(address, data);

    return 0;
}

//read register from FPGA
int read_reg_fpga(int address, uint32_t *value)
{
    printf("WRS_PTS_LIB read_reg_fpga called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    *value = _fpga_readl(address);

    return 0;
}

//write gpio control to FPGA
int write_gpio_control(int pin, int set)
{
    printf("WRS_PTS_LIB write_gpio_control called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    uint32_t temp;

    if (set == 0)
    {
        _fpga_writel(FPGA_BASE_GPIO_CONTROL + GPIO_COR, 
            1<<pin);
    }
    else
    {
        temp = _fpga_readl(FPGA_BASE_GPIO_CONTROL + GPIO_ROR);
        _fpga_writel(FPGA_BASE_GPIO_CONTROL + GPIO_SOR, 
            temp | 1<<pin);
    }

    return 0;
}

//read gpio status from FPGA
int read_gpio_status(uint32_t *value)
{
    printf("WRS_PTS_LIB read_gpio_status called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    *value = _fpga_readl(FPGA_BASE_GPIO_STATUS + GPIO_ROR);

    return 0;
}

//write gpio control rt
int write_gpio_rt(int pin, int set)
{
    printf("WRS_PTS_LIB write_gpio_rt called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    uint32_t temp;

    if (set == 0)
    {
        _fpga_writel(BASE_GPIO_RT + GPIO_COR, 
            1<<pin);
    }
    else
    {
        temp = _fpga_readl(BASE_GPIO_RT + GPIO_ROR);
        _fpga_writel(BASE_GPIO_RT + GPIO_SOR, 
            temp | 1<<pin);
    }

    return 0;
}

//execute command shell
int exec_cmd_shell(char *cmd)
{
    printf("WRS_PTS_LIB exec_cmd_shell called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    return system(cmd);
}

//write ad9516 register
int write_reg_ad9516(int reg, int val)
{
    printf("WRS_PTS_LIB write_reg_ad9516 called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    ad9516_set_reg(reg, val);

    return 0;
}

//read ad9516 register
int read_reg_ad9516(int reg, int *val)
{
    printf("WRS_PTS_LIB read_reg_ad9516 called\n");

    if (check_hal() > 0)
        system("/etc/init.d/hald.sh stop");

    *val = ad9516_read_reg(reg);

    return 0;
}
