/*  ============================================================================
 *   Copyright (c) Texas Instruments Inc 2011
 *
 *   Use of this software is controlled by the terms and conditions found in the
 *   license agreement under which this software has been supplied.
* ============================================================================
Example to show the performance of Multicore Navigator on Keystone DSP
* =============================================================================
 *  Revision History
 *  ===============
 *  May 15, 2011 Brighton Feng   File Created
 * ============================================================================
 */

#include <stdio.h>
#include <csl_cacheAux.h>
#include <csl_cache.h>
#include <csl_qm_queue.h>
#include "KeyStone_common.h"
#include "MNav_QM_Intc_setup.h"
#include "MNav_Test.h"

Uint32 uiaDescriptorBuffer[TEST_DESC_BUF_SIZE];
Uint32 uiaDescriptorPintPongBufferHi[(TEST_DESC_BUF_SIZE+1)*2];
Uint32 uiaDescriptorPintPongBufferLo[(TEST_DESC_BUF_SIZE+1)*2];
Qmss_AccCmdCfg hiQuAccCfg=
{
    /** Accumulator channel affected (0-47) */
    0,//channel
    /** Accumulator channel command - Qmss_AccCmd_ENABLE_CHANNEL : Enable channel 
     * Qmss_AccCmd_DISABLE_CHANNEL : Disable channel */
    Qmss_AccCmd_ENABLE_CHANNEL,//command
    /** This field specifies which queues are to be included in the queue group. 
     * Bit 0 corresponds to the base queue index, and bit 31 corresponds to the base 
     * queue index plus 31. For any bit set in this mask, the corresponding queue index 
     * is included in the monitoring function.
     *
     * This field is ignored in single-queue mode.*/
    1,//queueEnMask
    /** Physical pointer to list ping/pong buffer. NULL when channel disabled */
    (Uint32)uiaDescriptorPintPongBufferHi+0x13000000,//listAddress
    /** Queue Manager and Queue Number index to monitor. This serves as a base queue index when the 
     * channel in multi-queue mode, and must be a multiple of 32 when multi-queue mode is enabled. */
    QMSS_HIGH_PRIORITY_QUEUE_BASE,//queMgrIndex
    /** Max entries per list buffer page */
    1+1,//maxPageEntries
    /** Number of timer ticks to delay interrupt */
    1,//timerLoadCount
    /** Interrupt pacing mode. Specifies when the interrupt should be trigerred */
    Qmss_AccPacingMode_NONE,//interruptPacingMode
    /** List entry size. Specifies the size of each data entry */
    Qmss_AccEntrySize_REG_D,//listEntrySize
    /** List count Mode. The number of entries in the list */
    Qmss_AccCountMode_NULL_TERMINATE, //listCountMode
    /** Queue mode. Moitor single or multiple queues */
    Qmss_AccQueueMode_SINGLE_QUEUE //multiQueueMode
};

Qmss_AccCmdCfg loQuAccCfg=
{
    /** Accumulator channel affected (0-47) */
    32,//channel
    /** Accumulator channel command - Qmss_AccCmd_ENABLE_CHANNEL : Enable channel 
     * Qmss_AccCmd_DISABLE_CHANNEL : Disable channel */
    Qmss_AccCmd_ENABLE_CHANNEL,//command
    /** This field specifies which queues are to be included in the queue group. 
     * Bit 0 corresponds to the base queue index, and bit 31 corresponds to the base 
     * queue index plus 31. For any bit set in this mask, the corresponding queue index 
     * is included in the monitoring function.
     *
     * This field is ignored in single-queue mode.*/
    0xFFFFFFFF,//queueEnMask
    /** Physical pointer to list ping/pong buffer. NULL when channel disabled */
    (Uint32)uiaDescriptorPintPongBufferLo+0x13000000,//listAddress
    /** Queue Manager and Queue Number index to monitor. This serves as a base queue index when the 
     * channel in multi-queue mode, and must be a multiple of 32 when multi-queue mode is enabled. */
    QMSS_LOW_PRIORITY_QUEUE_BASE,//queMgrIndex
    /** Max entries per list buffer page */
    1+1,//maxPageEntries
    /** Number of timer ticks to delay interrupt */
    1,//timerLoadCount
    /** Interrupt pacing mode. Specifies when the interrupt should be trigerred */
    Qmss_AccPacingMode_NONE,//interruptPacingMode
    /** List entry size. Specifies the size of each data entry */
    Qmss_AccEntrySize_REG_D,//listEntrySize
    /** List count Mode. The number of entries in the list */
    Qmss_AccCountMode_NULL_TERMINATE, //listCountMode
    /** Queue mode. Moitor single or multiple queues */
    Qmss_AccQueueMode_MULTI_QUEUE //multiQueueMode
};

volatile Uint32 intTSCL; 	//TSCL when interrupt happen

/*note: proxy registers can not be used for pop*/
void QueuePushCycleTest(QueueManageRegs * queueRegs, Uint32 uiDescriptorNumber,
	QueueManageRegs * queuePopRegs)
{
	int i, j, k=0;
	Uint32 cycles;
	volatile Uint32 * restrict uipREG_D_Descriptor= &queueRegs->REG_D_Descriptor;

	/*pop descriptors before push*/
	for(i=uiDescriptorNumber/2; i>0; i>>=1)
	{
		for(j=0; j<i; j++)
		{
			uiaDescriptorBuffer[k]= queuePopRegs->REG_D_Descriptor;
			if(NULL==uiaDescriptorBuffer[k++])
			{
				printf("Queue is empty!\n");
				return;
			}
		}
	}

	k= 0;
	for(i=uiDescriptorNumber/2; i>0; i>>=1)
	{
		cycles= TSCL;
		for(j=0; j<i; j++)
		{
			*uipREG_D_Descriptor= uiaDescriptorBuffer[k++];
		}
		cycles= TSC_count_cycle_from(cycles);
		printf("consumes %6d cycles to push         %4d descriptors, average %3d cycles\n", 
			cycles, i, cycles/i);
	}
}

/*note: proxy registers can not be used for pop*/
void QueuePopCycleTest(QueueManageRegs * queueRegs, Uint32 uiDescriptorNumber)
{
	int i, j, k=0;
	Uint32 cycles;

	for(i=uiDescriptorNumber/2; i>0; i>>=1)
	{
		cycles= TSCL;
		for(j=0; j<i; j++)
		{
			uiaDescriptorBuffer[k++]= queueRegs->REG_D_Descriptor;
		}
		cycles= TSC_count_cycle_from(cycles);
		printf("consumes %6d cycles to pop          %4d descriptors, average %3d cycles\n", 
			cycles, i, cycles/i);
	}

	/*push back the descriptors*/
	k= 0;
	for(i=uiDescriptorNumber/2; i>0; i>>=1)
	{
		for(j=0; j<i; j++)
		{
			queueRegs->REG_D_Descriptor= uiaDescriptorBuffer[k++];
		}
	}
}

void QueuePushToPopDelayTest(QueueManageRegs * queueRegs, Uint32 uiDescriptorNumber)
{
	int i, j;
	Uint32 cycles;
	Uint32 uiDescriptor;

	/*pop a descriptor before test*/
	uiDescriptor= queueRegs->REG_D_Descriptor;

	for(i=uiDescriptorNumber/2; i>0; i>>=1)
	{
		cycles= TSCL;
		for(j=0; j<i; j++)
		{
			queueRegs->REG_D_Descriptor= uiDescriptor;
			uiDescriptor= queueRegs->REG_D_Descriptor;
		}
		cycles= TSC_count_cycle_from(cycles);
		printf("consumes %6d cycles to push and pop %4d descriptors, average %3d cycles\n", 
			cycles, i, cycles/i);
	}

	/*return the descriptor to original queue*/
	queueRegs->REG_D_Descriptor= uiDescriptor;
}

void QueuePushToAccumulationDelayTest(
	Uint32 uiSrcQu)
{
	int i;
	Uint32 cycles, uiDescriptor, uiPingPongIndex;
	volatile Uint32 * uipDescList;
	QueueManageRegs * restrict lowPriQueue;
	QueueManageRegs * restrict highPriQueue;

	/*configure high priority queue accumulation channel*/
	KeyStone_Qmss_Config_Acc_Channel(Qmss_PdspId_PDSP1, &hiQuAccCfg);

	/*configure low priority queue accumulation channel*/
#ifdef ACC_48_CHANNEL
	puts("\nQueue Push/accumulation test with ACC48 firmware");
	loQuAccCfg.channel=32;
	KeyStone_Qmss_Config_Acc_Channel(Qmss_PdspId_PDSP1, &loQuAccCfg);
#else
	puts("\nQueue Push/accumulation test with ACC32+ACC16 firmware");
	loQuAccCfg.channel=0;
	KeyStone_Qmss_Config_Acc_Channel(Qmss_PdspId_PDSP2, &loQuAccCfg);
#endif

	lowPriQueue= &queueManageData[QMSS_LOW_PRIORITY_QUEUE_BASE];
	highPriQueue= &queueManageData[QMSS_HIGH_PRIORITY_QUEUE_BASE];

	/*pop descriptors before push*/
	uiDescriptor= queueManageRegs[uiSrcQu].REG_D_Descriptor;

	for(i=0; i<10; i++) 	//try multiple times to get statistics 
	{
		/*test high priority queue accumulation*/
		uipDescList= (Uint32 *)hiQuAccCfg.listAddress;
		uiPingPongIndex= hiQuAccCfg.maxPageEntries*(i&1);
		uipDescList[uiPingPongIndex]= 0; 	//clear the descriptor list buffer
 		
		cycles= TSCL;
		highPriQueue->REG_D_Descriptor= uiDescriptor ; 	//push descriptor
 		asm(" IDLE"); //waiting for accumulation interupt
		cycles= ((unsigned int)((0xFFFFFFFFl+intTSCL)- (unsigned long long)cycles)+ 1);
		printf("consumes %6u cycles to push and accumulate one descriptor with high priority\n", 
			cycles);

		if(uiDescriptor !=uipDescList[uiPingPongIndex])
		{
			printf("descriptor does not match , expect 0x%x, get 0x%x\n",
				uiDescriptor , uipDescList[uiPingPongIndex]);
			break;
		}
 

		/*test low priority queue accumulation*/
		uipDescList= (Uint32 *)loQuAccCfg.listAddress;
		uiPingPongIndex= loQuAccCfg.maxPageEntries*(i&1);
		uipDescList[uiPingPongIndex]= 0; 	//clear the descriptor list buffer
 		
		cycles= TSCL;
		lowPriQueue->REG_D_Descriptor= uiDescriptor; 	//push descriptor
		asm(" IDLE"); //waiting for accumulation interupt
		cycles= ((unsigned int)((0xFFFFFFFFl+intTSCL)- (unsigned long long)cycles)+ 1);
		printf("consumes %6u cycles to push and accumulate one descriptor with  low priority\n", 
			cycles);

		if(uiDescriptor!=uipDescList[uiPingPongIndex])
		{
			printf("descriptor does not match, expect 0x%x, get 0x%x\n",
				uiDescriptor , uipDescList[uiPingPongIndex]);
			break;
		}
	}

	/*push descriptors back after test*/
	queueManageData[uiSrcQu].REG_D_Descriptor= uiDescriptor ;
 
	/*disable accumulation channels*/
	KeyStone_Qmss_disable_Acc_Channel(Qmss_PdspId_PDSP1, 0);
#ifdef ACC_48_CHANNEL
	KeyStone_Qmss_disable_Acc_Channel(Qmss_PdspId_PDSP1, 32);
#else
	KeyStone_Qmss_disable_Acc_Channel(Qmss_PdspId_PDSP2, 0);
#endif

}

void QueuePushToPendInterruptDelayTest(QueueManageRegs * queueRegs)
{
	Uint32 cycles;
	Uint32 uiDescriptor;

	/*pop a descriptor for test*/
	uiDescriptor= queueRegs->REG_D_Descriptor;

	cycles= TSCL;
	/*push to queue 662, which has pend event to DSP INTC*/
	queueManageData[662].REG_D_Descriptor= uiDescriptor;
	asm(" IDLE"); //waiting for pending interupt
	cycles= ((unsigned int)((0xFFFFFFFFl+intTSCL)- (unsigned long long)cycles)+ 1);
	printf("consumes %6d cycles between push and pend interrupt\n", cycles);

	//return the descriptor to original queue
	queueRegs->REG_D_Descriptor= uiDescriptor;

}

void QueueReclamationDelayTest(Uint32 uiSrcQu)
{
	int i;
	
	Uint32 cycles;
	Uint32 uiDescriptor;
	Uint32 uiOriginalEntryCount, uiCurrentEntryCount;

	/*read original entry count*/
	uiOriginalEntryCount= queueStatusConfigRegs[uiSrcQu].REG_A_EntryCount;

	printf("\ndescriptor Reclamation test in queue %d\n", uiSrcQu);
	for(i=0; i<10; i++) 	//try multiple times to get statistics
	{
		/*pop a descriptor for test*/
		uiDescriptor= queueManageRegs[uiSrcQu].REG_D_Descriptor;

		cycles= TSCL;

		/*push to reclamation queue*/
		queueManageData[RECLAMATION_QUEUE].REG_D_Descriptor= uiDescriptor;

		/*wait the descriptor return to original queue*/
		do
		{
			uiCurrentEntryCount= queueStatusConfigRegs[uiSrcQu].REG_A_EntryCount;
		}while(uiCurrentEntryCount!= uiOriginalEntryCount);

		cycles= TSC_count_cycle_from(cycles);
		printf("consumes %6d cycles for one descriptor Reclamation\n", cycles);
	}
}

void PktDmaThroughput(Uint32 uiSrcQuNum, Uint32 uiTxQuNum, 
	Uint32 uiFlowID, Uint32 uiByteCount, Uint32 uiNumCh)
{
	int i, j;
	HostPacketDescriptor * hostDescriptor;
	MonolithicPacketDescriptor * monoDescriptor;
	Uint32 * uipSrcBufPtr, * uipDstBufPtr;
	Uint32 cycles, uiEntryCount;

	/*prepare source packets, each channel transfer one packet for this test*/
	for(j= 0; j< uiNumCh; j++)
	{
		hostDescriptor= (HostPacketDescriptor *)
			(queueManageRegs[uiSrcQuNum].REG_D_Descriptor&0xFFFFFFF0);
		if(NULL==hostDescriptor)
		{
			printf("Source queue descriptor %d is NULL\n", j);
			return;
		}
	
		/*invalid cache before read descriptor RAM*/
		if((Uint32)hostDescriptor>=0x0C000000&&(Uint32)hostDescriptor<0x0C400000)	//SL2
			CACHE_invL1d((void *)hostDescriptor, 64, CACHE_WAIT);
		else if((Uint32)hostDescriptor>=0x80000000) 	//DDR
		{
			CACHE_invL2((void *)hostDescriptor, 64, CACHE_WAIT);
		}
		InvalidPrefetchBuffer();

		/*get the source buffer*/
		if(Cppi_DescType_HOST==hostDescriptor->type_id)
		{
			uipSrcBufPtr= (Uint32 *)hostDescriptor->buffer_ptr;
			hostDescriptor->packet_length= uiByteCount;
			hostDescriptor->buffer_len= uiByteCount;
			uiaDescriptorBuffer[j]= (Uint32)hostDescriptor|FETCH_SIZE_32;
		}		
		else
		{
			monoDescriptor= (MonolithicPacketDescriptor *)hostDescriptor;
			monoDescriptor->packet_length= uiByteCount;
			uipSrcBufPtr= (Uint32 *)((Uint8* )monoDescriptor + 
				monoDescriptor->data_offset);
			uiaDescriptorBuffer[j]= (Uint32)hostDescriptor|FETCH_SIZE_16;
		}

		/*the SRC_TAG_LO field in the Tx descriptor is used as RX flow ID*/
		hostDescriptor->src_tag_lo= uiFlowID;

		/*write back data from cache to descriptor RAM*/
		if((Uint32)hostDescriptor>=0x0C000000&&(Uint32)hostDescriptor<0x0C400000)	//SL2
			CACHE_wbL1d((void *)hostDescriptor, 64, CACHE_WAIT);
		else if((Uint32)hostDescriptor>=0x80000000) 	//DDR
		{
			CACHE_wbL2((void *)hostDescriptor, 64, CACHE_WAIT);
		}

		/*initialize the source buffer*/
		for(i=0; i<uiByteCount/4; i++)
			uipSrcBufPtr[i]= (uiSrcQuNum<<16)|(uiFlowID<<8)|i;

		/*write back data from cache to buffer*/
		if((Uint32)uipSrcBufPtr>=0x0C000000&&(Uint32)uipSrcBufPtr<0x0C400000)	//SL2
			CACHE_wbL1d((void *)uipSrcBufPtr, uiByteCount, CACHE_WAIT);
		else if((Uint32)uipSrcBufPtr>=0x80000000) 	//DDR
		{
			CACHE_wbL2((void *)uipSrcBufPtr, uiByteCount, CACHE_WAIT);
		}
	}

	cycles= TSCL;
	/*push the packet descriptor to Packet DMA TX queue*/
	for(j= 0; j< uiNumCh; j++)
	{
		//each channel transfer one packet
		queueManageData[uiTxQuNum+j].REG_D_Descriptor= 
			uiaDescriptorBuffer[j];
	}
	_mfence(); 	//force all memory operation complete
	
	/*poll the packet descriptor in destination queue*/	
	do
	{
		/*received packet number should equal to channel number 
		since each channel transfer on packet*/
		uiEntryCount= queueStatusConfigRegs[PKT_DMA_TEST_DST_Q].REG_A_EntryCount;
	}while(uiEntryCount<uiNumCh);
	cycles= TSC_count_cycle_from(cycles);
	printf("Packet DMA achieves %4lld MB/s when transfer %5d bytes ",
		(unsigned long long)uiNumCh*uiByteCount*CPU_CLK_KHZ/cycles/1000,
		uiByteCount);

	/*check recieved data and recycle descriptors and buffers*/
	for(j= 0; j< uiNumCh; j++)
	{
		hostDescriptor= (HostPacketDescriptor *)
			(queueManageRegs[PKT_DMA_TEST_DST_Q].REG_D_Descriptor&0xFFFFFFF0);

		/*invalid cache before read descriptor RAM*/
		if((Uint32)hostDescriptor>=0x0C000000&&(Uint32)hostDescriptor<0x0C400000)	//SL2
			CACHE_invL1d((void *)hostDescriptor, 64, CACHE_WAIT);
		else if((Uint32)hostDescriptor>=0x80000000) 	//DDR
		{
			CACHE_invL2((void *)hostDescriptor, 64, CACHE_WAIT);
		}
		InvalidPrefetchBuffer();

		/*get the destination buffer*/
		if(Cppi_DescType_HOST==hostDescriptor->type_id)
		{
			uipDstBufPtr= (Uint32 *)hostDescriptor->buffer_ptr;
			uiaDescriptorBuffer[j]= (Uint32)hostDescriptor;
		}
		else
		{
			monoDescriptor= (MonolithicPacketDescriptor *)hostDescriptor;
			uipDstBufPtr= (Uint32 *)((Uint8 *)monoDescriptor + 
				monoDescriptor->data_offset);
			uiaDescriptorBuffer[j]= (Uint32)hostDescriptor;
		}
		printf("from 0x%8x to 0x%8x with channel %2d, ",
			(Uint32)uipSrcBufPtr, (Uint32)uipDstBufPtr, j);

		/*invalid old contents of destination buffer in cache before read*/
		if((Uint32)uipDstBufPtr>=0x0C000000&&(Uint32)uipDstBufPtr<0x0C400000)	//SL2
			CACHE_invL1d((void *)uipDstBufPtr, uiByteCount, CACHE_WAIT);
		else if((Uint32)uipDstBufPtr>=0x80000000) 	//DDR
		{
			CACHE_invL2((void *)uipDstBufPtr, uiByteCount, CACHE_WAIT);
		}
		InvalidPrefetchBuffer();

		/*compare the destination data against source data*/
		for(i=0; i<uiByteCount/4; i++)
		{
			if(uipSrcBufPtr[i]!= uipDstBufPtr[i])
			{
				printf("consumes %5d cycles\n", cycles);
				printf("destination data 0x%8x does not match source data 0x%8x at word %d of packet %d\n",
					uipDstBufPtr[i], uipSrcBufPtr[i], i, j);
				return;
			}
		}
		
		/*descriptor Reclamation*/
		//queueManageData[RECLAMATION_QUEUE].REG_D_Descriptor= uiaDescriptorBuffer[j];
		queueManageData[(hostDescriptor->pkt_return_qmgr<<12)|
			hostDescriptor->pkt_return_qnum].REG_D_Descriptor= 
			uiaDescriptorBuffer[j];
	}

	printf("consumes %5d cycles\n", cycles);
}

interrupt void QueueAccumulationISR()
{
	int i;
	Uint32 qmIntStatus0, qmIntStatus1, qmIntStatus4;
	Uint32 uiMask;

	intTSCL= TSCL; 	//save the TSC when the interrupt happens

	/*read interrupt status*/
	qmIntStatus0= qmIntRegs->STATUS_REG0;
	qmIntStatus1= qmIntRegs->STATUS_REG1;
	qmIntStatus4= qmIntRegs->STATUS_REG4;

	/*clear interrupt status*/
	qmIntRegs->STATUS_CLR_REG0= qmIntStatus0;
	qmIntRegs->STATUS_CLR_REG1= qmIntStatus1;
	qmIntRegs->STATUS_CLR_REG4= qmIntStatus4;

	/*high priority interrupt*/
	uiMask=1;
	for(i=0; i<32; i++)
	{
		if(uiMask&qmIntStatus0)
		{
			
			qmIntRegs->INTCNT_REG[i]= 1 ;
			qmIntRegs->EOI_REG= i+2;
			//printf("high priority queue channel %d interrupt happens at %u\n", i, intTSCL);
		} 
		uiMask<<=1;
	}

	/*low priority interrupt*/
	uiMask=1;
	for(i=0; i<16; i++)
	{
		if(uiMask&qmIntStatus1)
		{
			
			qmIntRegs->INTCNT_REG[i+32]= 1 ;
			qmIntRegs->EOI_REG= i+34;
			//printf("low priority queues channel %d interrupt happens at %u\n", i, intTSCL);
		} 
		uiMask<<=1;
	}

	/*QMSS PKTDMA Rx Starvation Interrupts*/
	if(1&qmIntStatus4)
	{
		
		qmIntRegs->INTCNT_REG[0+48]= 1 ;
		qmIntRegs->EOI_REG= 0;
		printf("QMSS PKTDMA Rx SOP Starvation interrupt happens at %u\n", intTSCL);
	} 
	if(2&qmIntStatus4)
	{
		
		qmIntRegs->INTCNT_REG[1+48]= 1 ;
		qmIntRegs->EOI_REG= 1;
		printf("QMSS PKTDMA Rx MOP Starvation interrupt happens at %u\n", intTSCL);
	} 

}

interrupt void QueuePendISR()
{
	Uint32 uiDescriptor;
	
	intTSCL= TSCL; 	//save the TSC when the interrupt happens

	/*queue must be empty before clear interrupt*/
	//queueManageData[RECLAMATION_QUEUE].REG_D_Descriptor= 
	uiDescriptor= queueManageRegs[662].REG_D_Descriptor;

	/* Clear the status of the queue pend system event*/
	cpIntc0Regs->STATUS_CLR_INDEX_REG= uiQ662_pend_event_num; 
	//printf("Queue 662 pend interrupt happens at %u\n", intTSCL);
}
