/* tsnclock_dkm.c - Wind River tsn clock demo */
/*
 * Copyright (c) 2017 Wind River Systems, Inc.
 *
 * The right to copy, distribute, modify or otherwise make use
 * of this software may be licensed only pursuant to the terms
 * of an applicable Wind River license agreement.
 */
/*
modification history
--------------------
03Jan17 xxy  Init version.
*/

#include <vxWorks.h>
#include <vxbTimerLib.h>
#include <edrLib.h>

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <endLib.h>
#include <tsnClkLib.h>

#define INCLUDE_USR_TSN_CLOCK_DEMO
#ifdef INCLUDE_USR_TSN_CLOCK_DEMO

#define tsnClockRecordMax 60

UINT64 usrTsnClock1TickCounts = 0;
UINT64 usrTsnClock2TickCounts = 0;

UINT32 tsnClock1RecordCount=0;
UINT32 tsnClock2RecordCount=0;

UINT64 usrTsnClockReadDelay = 0;

typedef struct tsnclockrec
	{
        struct timespec ts;
	}TSNCLOCKREC;

TSNCLOCKREC tsnClock1Record[tsnClockRecordMax];
TSNCLOCKREC tsnClock2Record[tsnClockRecordMax];

clockid_t       gTsnClock1Id=0;
clockid_t       gTsnClock2Id=0;

LOCAL void usrTsnClockIsr
    (
    void
    )
    {
    if( (gTsnClock1Id!= 0) && (tsnClock1RecordCount < tsnClockRecordMax))
    {
    (void)tsnClockTimeGet(gTsnClock1Id, &tsnClock1Record[tsnClock1RecordCount].ts);    
    tsnClock1RecordCount++;
    }
    
    usrTsnClock1TickCounts++;
    return;
    }

LOCAL void usrTsnClock2Isr
    (
    void
    )
    {
    if( (gTsnClock2Id!= 0) && (tsnClock2RecordCount < tsnClockRecordMax))
    {
    (void)tsnClockTimeGet(gTsnClock2Id, &tsnClock2Record[tsnClock2RecordCount].ts);    
    tsnClock2RecordCount++;
    }
    
    usrTsnClock2TickCounts++;
    return;
    }



/*******************************************************************************
*
* usrTsnClockTimeInit - init TSN 1588 timer demo.
*
*
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockTimeInit
* (
*    char        *pName,
*    int         unit
* )
*
*
*/

STATUS usrTsnClockTimeInit
(
    char        *pName,     /*eth device name */
    int         unit      /*eth device index */
)
{
    clockid_t  cid ;   /* ptr to return 1588 time clock ID */
    struct timespec ts1;
    struct timespec ts2;
    UINT64 diff = 0;
    UINT64 tryCounts = 0;
    
    UINT64 usrTsnClock1ReadDelay = 0;
    UINT64 usrTsnClock2ReadDelay = 0;
    
    if(gTsnClock1Id ==0 )
    {
        cid=tsnClockIdGet(pName,unit,0);
        if (cid == 0)
        {
            return (ERROR);
        }
        gTsnClock1Id=cid;
    }
    
    if(gTsnClock2Id ==0 )
    {
        cid=tsnClockIdGet(pName,unit,1);
        if (cid == 0)
        {
            return (ERROR);
        }
        gTsnClock2Id=cid;
     }
    /* calculate the clock time getting delay*/
    do{
        
        (void)tsnClockTimeGet(gTsnClock1Id,&ts1);
        (void)tsnClockTimeGet(gTsnClock1Id,&ts1);
        (void)tsnClockTimeGet(gTsnClock1Id,&ts2);


        usrTsnClock1ReadDelay= (UINT64)((((UINT64)ts2.tv_sec) * BILLION) + 
            (UINT64)ts2.tv_nsec) - (UINT64)((((UINT64)ts1.tv_sec) * BILLION)
            + (UINT64)ts1.tv_nsec);

        (void)tsnClockTimeGet(gTsnClock2Id,&ts1);
        (void)tsnClockTimeGet(gTsnClock2Id,&ts1);
        (void)tsnClockTimeGet(gTsnClock2Id,&ts2);
        
        usrTsnClock2ReadDelay= (UINT64)((((UINT64)ts2.tv_sec) * BILLION) + 
            (UINT64)ts2.tv_nsec) - (UINT64)((((UINT64)ts1.tv_sec) * BILLION)
            + (UINT64)ts1.tv_nsec);


        if(usrTsnClock2ReadDelay > usrTsnClock1ReadDelay)
        {
            diff = usrTsnClock2ReadDelay - usrTsnClock1ReadDelay;
        }
        else
        {
            diff = usrTsnClock1ReadDelay - usrTsnClock2ReadDelay;
        }
        tryCounts++;
        
    }while((diff > 100) && ( tryCounts < 10 ));

    if(tryCounts >=10)
    {
        printf("calculate the clock time getting delay failed\n");
        return (ERROR);
    }
    
    usrTsnClockReadDelay = (usrTsnClock1ReadDelay + usrTsnClock2ReadDelay)/2;
    
    printf("delay1: %llu, delay2:%llu, diff:%llu , mean: %llu\n",
        usrTsnClock1ReadDelay,
        usrTsnClock2ReadDelay,
        diff,
        usrTsnClockReadDelay);
    
    return (OK);
}

/*******************************************************************************
*
* usrTsnClockTimeGet - get a TSN 1588 timer time.
*
* This routine get a TSN 1588 timer time.
*
* The <timerNo> argument, refer to the 1588 timer unit,0 or 1
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \i <timerNo>
* 1588 clock timer number .
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockTimeGet
* (
*    char        *pName,
*    int         unit,
*    int         timerNo,
* )
*
*
*/

STATUS usrTsnClockTimeGet
(
    char        *pName,     /*eth device name */
    int         unit,      /*eth device index */
    int         timerNo     /* 1588 clock timer nuber */
)
{
    clockid_t  cid ;   /* ptr to return 1588 time clock ID */
    struct timespec   ts;      /* current time */
    STATUS ret ;
    

    cid=tsnClockIdGet(pName,unit,timerNo);
    if (cid == 0)
    {
        (void)printf("usrTsnClockTimeGetDemo %s%d-%d failed.\n",pName,unit,timerNo);
        return (ERROR);
    }
    
    ret= tsnClockTimeGet (cid,& ts);
    
    if( ret == ERROR )
         return  ERROR;
    
    (void)printf("usrTsnClockTimeGetDemo  %d: %09ld \n",ts.tv_sec,ts.tv_nsec);
    return (OK);
}

/*******************************************************************************
*
* usrTsnClockRateGet - get a TSN 1588 timer ticks per second.
*
* This routine get get a TSN 1588 timer ticks per second.
*
* The <timerNo> argument, refer to the 1588 timer unit,0 or 1
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \i <timerNo>
* 1588 clock timer number .
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockRateGet
* (
*    char        *pName,
*    int         unit,
*    int         timerNo,
* )
*
*
*/

STATUS usrTsnClockRateGet
(
    char        *pName,     /*eth device name */
    int         unit,      /*eth device index */
    int         timerNo     /* 1588 clock timer nuber */
)
{
    clockid_t  cid ;   /* ptr to return 1588 time clock ID */
    STATUS ret ;
    

    cid=tsnClockIdGet(pName,unit,timerNo);
    if (cid == 0)
    {
        (void)printf("usrTsnClockTimeGetDemo %s%d-%d failed.\n",pName,unit,timerNo);
        return (ERROR);
    }
    
    ret= tsnClockRateGet (cid);
    
    if( ret == ERROR )
         return  ERROR;
    
    (void)printf("usrTsnClockRateGetDemo  %d \n",ret);
    return (OK);
}



/*******************************************************************************
*
* usrTsnClockAlloc - Allocate a timer for ticks or one shot interrupt.
*
* This routine enable a TSN 1588 timer.
*
* The <timerNo> argument, refer to the 1588 timer unit,0 or 1
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \i <timerNo>
* 1588 clock timer number .
* \i <ticksPerSecond>
* Number of ticks per second,to use one shot interrupt set to be zeros
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockAlloc
* (
*    char        *pName,
*    int         unit,
*    int         timerNo,
*    UINT32      ticksPerSecond
* )
*
*
*/

STATUS usrTsnClockAlloc
(
    char        *pName,     /*eth device name */
    int         unit ,     /*eth device index */
    int         timerNo,     /* 1588 clock timer nuber */
    UINT32      ticksPerSecond /* number of clock interrupts per second */
)
{
    clockid_t  cid =0 ;
    STATUS ret ;
    
    cid=tsnClockIdGet(pName,unit,timerNo);
    if (cid == 0)
    {
        (void)printf("tsnClockIdGet %s%d-%d failed.\n",pName,unit,timerNo);
        return (ERROR);
    }
    
    ret = tsnTimerAllocate(cid);
    if (ret == ERROR)
    {
        (void)printf("tsnClockOpen cid: 0x%x failed.\n",cid);
        return (ERROR);
    }
    if(timerNo == 1)
    {
        ret = tsnClockConnect(cid, (FUNCPTR)usrTsnClock2Isr, 0);
    }
    else
    {
        ret = tsnClockConnect(cid, (FUNCPTR)usrTsnClockIsr, 0);
    }
    
    if (ret == ERROR)
    {
        (void)printf("tsnClockConnect cid: 0x%x failed.\n",cid);
        (void)tsnTimerRelease(cid);
        return (ERROR);
    }

    ret=tsnClockRateSet(cid,ticksPerSecond);
    if( ret == ERROR )
    {
        (void)printf("tsnClockRateSet cid: 0x%x failed.\n",cid);
        (void)tsnTimerRelease(cid);
         return  ERROR;
    }
   
    return (OK);
}


/*******************************************************************************
*
* usrTsnClockEnable - enable a TSN 1588 timer .
*
* This routine enable a TSN 1588 timer.
*
* The <timerNo> argument, refer to the 1588 timer unit,0 or 1
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \i <timerNo>
* 1588 clock timer number .
* \i <seconds>
* offset to current second (second)
* \i <nanoseconds>
* absolute start nanosecond (nanosecond)
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockEnable
* (
*    char        *pName,
*    int         unit,
*    int         timerNo,
*    UINT32      seconds,
*    UINT32      nanoseconds
* )
*
*
*/

STATUS usrTsnClockEnable
(
    char        *pName,     /*eth device name */
    int         unit ,     /*eth device index */
    int         timerNo,     /* 1588 clock timer nuber */
    UINT32      seconds,     /* offset to current time (s) */
    UINT32      nanoseconds  /* absolute start time (ns) */
)
{
    clockid_t  cid =0 ;
    STATUS ret ;
    struct timespec ts;
    
    cid=tsnClockIdGet(pName,unit,timerNo);
    if (cid == 0)
    {
        (void)printf("tsnClockIdGet %s%d-%d failed.\n",pName,unit,timerNo);
        return (ERROR);
    }
    ret= tsnClockTimeGet (cid,& ts);
    
    if( ret == ERROR )
         return  ERROR;
    
    
    ts.tv_sec += seconds;
    ts.tv_nsec = nanoseconds;

    ret = tsnClockEnable(cid,&ts);
    
    (void)printf("usrTsnClockEnable launch time  %d: %09ld \n",ts.tv_sec,ts.tv_nsec);
    if (ret == ERROR)
    {
        (void)printf("tsnClockEnable cid: 0x%x failed.\n",cid);
        return (ERROR);
    }
    
    return (OK);
}

/*******************************************************************************
*
* usrTsnClockDisable - disable a TSN 1588 timer .
*
* This routine disable a TSN 1588 timer.
*
* The <timerNo> argument, refer to the 1588 timer unit,0 or 1
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \i <timerNo>
* 1588 clock timer number .
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockDisable
* (
*    char        *pName,
*    int         unit,
*    int         timerNo,
* )
*
*
*/

STATUS usrTsnClockDisable
(
    char        *pName,     /*eth device name */
    int         unit ,     /*eth device index */
    int         timerNo     /* 1588 clock timer nuber */
)
{
    clockid_t  cid =0 ;
        
    STATUS ret ;
    
    cid=tsnClockIdGet(pName,unit,timerNo);
    if (cid == 0)
    {
        (void)printf("tsnClockIdGet %s%d-%d failed.\n",pName,unit,timerNo);
        return (ERROR);
    }

    
    ret = tsnClockDisable(cid);
    
    return (ret);
}

/*******************************************************************************
*
* usrTsnClockRelease - release a TSN 1588 timer.
*
* This routine release a TSN 1588 timer.
*
* The <timerNo> argument, refer to the 1588 timer unit,0 or 1
*
* Parameters:
* \is
* \i <pName>
* A pointer to the ethernet device name.
* \i <unit>
* Ethernet device index.
* \i <timerNo>
* 1588 clock timer number .
* \ie
*
* RETURNS:
* 0 (OK), or -1 (ERROR) 
*
* ERRNO:
*
*
* SYNOPSIS
* STATUS usrTsnClockDisable
* (
*    char        *pName,
*    int         unit,
*    int         timerNo,
* )
*
*
*/

STATUS usrTsnClockRelease
(
    char        *pName,     /*eth device name */
    int         unit ,     /*eth device index */
    int         timerNo     /* 1588 clock timer nuber */
)
{
    clockid_t  cid =0 ;
        
    STATUS ret ;
    
    cid=tsnClockIdGet(pName,unit,timerNo);
    if (cid == 0)
    {
        (void)printf("tsnClockIdGet %s%d-%d failed.\n",pName,unit,timerNo);
        return (ERROR);
    }

    ret = tsnTimerRelease(cid);
    
    return (ret);
}


/******************************************************************************
*
* usrTsnClockshow - the routine of Tsn clock show
* RETURNS: N/A
*
* ERRNO: N/A
*/

void usrTsnClockshow( int unit)
{
    int ix =0;    
    UINT64 timeCounts = 0;
    
    if(unit ==0)
    {
        printf("\nclock1 interrrupt time sample:\n");
        for (ix = 0; ix < tsnClock1RecordCount; ix++ )
        {
            timeCounts= ((UINT64)(tsnClock1Record[ix].ts.tv_sec) *BILLION)
                + tsnClock1Record[ix].ts.tv_nsec - usrTsnClockReadDelay;
            
                printf(" %llu:%llu\n",
                    (timeCounts/BILLION),
                    (timeCounts%BILLION));
        }
        tsnClock1RecordCount=0;
    }
    else
    {
    
        printf("\n\n\nclock2 interrrupt time sample:\n");
        for (ix = 0; ix < tsnClock2RecordCount; ix++ )
        {
            timeCounts= ((UINT64)(tsnClock2Record[ix].ts.tv_sec) *BILLION)
                + tsnClock2Record[ix].ts.tv_nsec - usrTsnClockReadDelay;
            
                printf(" %llu:%llu\n",
                    (timeCounts/BILLION),
                    (timeCounts%BILLION));
    
        }
    
    tsnClock2RecordCount=0;
    }    
}



#endif