ixQMgrAqmIfWordRead (registerAddress, &registerWord);    ixQMgrAqmIfWordWrite (registerAddress, (registerWord | actualBitOffset));}voidixQMgrAqmIfQInterruptDisable (IxQMgrQId qId){    volatile UINT32 *registerAddress;    UINT32 registerWord;    UINT32 actualBitOffset;    if (qId < IX_QMGR_MIN_QUEUPP_QID)    {    	registerAddress = (UINT32*)(aqmBaseAddress + IX_QMGR_QUEIEREG0_OFFSET);    }    else    {	registerAddress = (UINT32*)(aqmBaseAddress + IX_QMGR_QUEIEREG1_OFFSET);    }    actualBitOffset = 1 << (qId % IX_QMGR_MIN_QUEUPP_QID);    ixQMgrAqmIfWordRead (registerAddress, &registerWord);    ixQMgrAqmIfWordWrite (registerAddress, registerWord & (~actualBitOffset));}voidixQMgrAqmIfQueCfgWrite (IxQMgrQId qId,		       IxQMgrQSizeInWords qSizeInWords,		       IxQMgrQEntrySizeInWords entrySizeInWords,		       UINT32 freeSRAMAddress){    volatile UINT32 *cfgAddress = NULL;    UINT32 qCfg = 0;    UINT32 baseAddress = 0;    unsigned aqmEntrySize = 0;    unsigned aqmBufferSize = 0;    /* Build config register */    aqmEntrySize = entrySizeToAqmEntrySize (entrySizeInWords);    qCfg |= (aqmEntrySize&IX_QMGR_ENTRY_SIZE_MASK) <<	IX_QMGR_Q_CONFIG_ESIZE_OFFSET;    aqmBufferSize = bufferSizeToAqmBufferSize (qSizeInWords);    qCfg |= (aqmBufferSize&IX_QMGR_SIZE_MASK) << IX_QMGR_Q_CONFIG_BSIZE_OFFSET;    /* baseAddress, calculated relative to aqmBaseAddress and start address  */    baseAddress = freeSRAMAddress -	(aqmBaseAddress + IX_QMGR_QUECONFIG_BASE_OFFSET);		       /* Verify base address aligned to a 16 word boundary */    if ((baseAddress % IX_QMGR_BASE_ADDR_16_WORD_ALIGN) != 0)    {	IX_QMGR_LOG_ERROR0("ixQMgrAqmIfQueCfgWrite () address is not on 16 word boundary\n");    }    /* Now convert it to a 16 word pointer as required by QUECONFIG register */    baseAddress >>= IX_QMGR_BASE_ADDR_16_WORD_SHIFT;            qCfg |= (baseAddress << IX_QMGR_Q_CONFIG_BADDR_OFFSET);    cfgAddress = (UINT32*)(aqmBaseAddress +			IX_QMGR_Q_CONFIG_ADDR_GET(qId));    /* NOTE: High and Low watermarks are set to zero */    ixQMgrAqmIfWordWrite (cfgAddress, qCfg);}voidixQMgrAqmIfQueCfgRead (IxQMgrQId qId,		       unsigned int numEntries,		       UINT32 *baseAddress,		       unsigned int *ne,		       unsigned int *nf,		       UINT32 *readPtr,		       UINT32 *writePtr){    UINT32 qcfg;    UINT32 *cfgAddress = (UINT32*)(aqmBaseAddress + IX_QMGR_Q_CONFIG_ADDR_GET(qId));    unsigned int qEntrySizeInwords;    unsigned int qSizeInWords;    UINT32 *readPtr_ = NULL;	    /* Read the queue configuration register */    ixQMgrAqmIfWordRead (cfgAddress, &qcfg);        /* Extract the base address */    *baseAddress = (UINT32)((qcfg & IX_QMGR_BADDR_MASK) >>			    (IX_QMGR_Q_CONFIG_BADDR_OFFSET));    /* Base address is a 16 word pointer from the start of AQM SRAM.     * Convert to absolute word address.     */    *baseAddress <<= IX_QMGR_BASE_ADDR_16_WORD_SHIFT;    *baseAddress += (UINT32)IX_QMGR_QUECONFIG_BASE_OFFSET;    /*     * Extract the watermarks. 0->0 entries, 1->1 entries, 2->2 entries, 3->4 entries......     * If ne > 0 ==> neInEntries = 2^(ne - 1)     * If ne == 0 ==> neInEntries = 0     * The same applies.     */    *ne = ((qcfg) >> (IX_QMGR_Q_CONFIG_NE_OFFSET)) & IX_QMGR_NE_MASK;    *nf = ((qcfg) >> (IX_QMGR_Q_CONFIG_NF_OFFSET)) & IX_QMGR_NF_MASK;    if (0 != *ne)    {	*ne = 1 << (*ne - 1);	    }    if (0 != *nf)    {	*nf = 1 << (*nf - 1);    }    /* Get the queue entry size in words */    qEntrySizeInwords = ixQMgrQEntrySizeInWordsGet (qId);    /* Get the queue size in words */    qSizeInWords = ixQMgrQSizeInWordsGet (qId);    ixQMgrAqmIfEntryAddressGet (0/* Entry 0. i.e the readPtr*/,				qcfg,				qEntrySizeInwords,				qSizeInWords,				&readPtr_);    *readPtr = (UINT32)readPtr_;    *readPtr -= (UINT32)aqmBaseAddress;/* Offset, not absolute address */    *writePtr = (qcfg >> IX_QMGR_Q_CONFIG_WRPTR_OFFSET) & IX_QMGR_WRPTR_MASK;    *writePtr = *baseAddress + (*writePtr * (IX_QMGR_NUM_BYTES_PER_WORD));    return;}unsignedixQMgrAqmIfLog2 (unsigned number){    unsigned count = 0;    /*     * N.B. this function will return 0     * for ixQMgrAqmIfLog2 (0)     */    while (number/2)    {	number /=2;	count++;	    }    return count;}void ixQMgrAqmIfIntSrcSelReg0Bit3Set (void){    volatile UINT32 *registerAddress;    UINT32 registerWord;     /*     * Calculate the registerAddress     * multiple queues split accross registers     */    registerAddress = (UINT32*)(aqmBaseAddress +				IX_QMGR_INT0SRCSELREG0_OFFSET);        /* Read the current data */    ixQMgrAqmIfWordRead (registerAddress, &registerWord);    /* Set the write bits */    registerWord |= (1<<IX_QMGR_INT0SRCSELREG0_BIT3) ;    /*     * Write the data     */    ixQMgrAqmIfWordWrite (registerAddress, registerWord);}  voidixQMgrAqmIfIntSrcSelWrite (IxQMgrQId qId,			  IxQMgrSourceId sourceId){    ixQMgrAqmIfQRegisterBitsWrite (qId,				   IX_QMGR_INT0SRCSELREG0_OFFSET,				   IX_QMGR_INTSRC_NUM_QUE_PER_WORD,				   sourceId);}voidixQMgrAqmIfWatermarkSet (IxQMgrQId qId,			unsigned ne,			unsigned nf){    volatile UINT32 *address = 0;    UINT32 value = 0;    unsigned aqmNeWatermark = 0;    unsigned aqmNfWatermark = 0;    address = (UINT32*)(aqmBaseAddress +		     IX_QMGR_Q_CONFIG_ADDR_GET(qId));    aqmNeWatermark = watermarkToAqmWatermark (ne);    aqmNfWatermark = watermarkToAqmWatermark (nf);    /* Read the current watermarks */    ixQMgrAqmIfWordRead (address, &value);    /* Clear out the old watermarks */    value &=  IX_QMGR_NE_NF_CLEAR_MASK;        /* Generate the value to write */    value |= (aqmNeWatermark << IX_QMGR_Q_CONFIG_NE_OFFSET) |	(aqmNfWatermark << IX_QMGR_Q_CONFIG_NF_OFFSET);     ixQMgrAqmIfWordWrite (address, value);}PRIVATE voidixQMgrAqmIfEntryAddressGet (unsigned int entryIndex,			    UINT32 configRegWord,			    unsigned int qEntrySizeInwords,			    unsigned int qSizeInWords,			    UINT32 **address){    UINT32 readPtr;    UINT32 baseAddress;    UINT32 *topOfAqmSram;    topOfAqmSram = ((UINT32 *)aqmBaseAddress + IX_QMGR_AQM_ADDRESS_SPACE_SIZE_IN_WORDS);    /* Extract the base address */    baseAddress = (UINT32)((configRegWord & IX_QMGR_BADDR_MASK) >>			   (IX_QMGR_Q_CONFIG_BADDR_OFFSET));    /* Base address is a 16 word pointer from the start of AQM SRAM.     * Convert to absolute word address.     */    baseAddress <<= IX_QMGR_BASE_ADDR_16_WORD_SHIFT;    baseAddress += ((UINT32)aqmBaseAddress + (UINT32)IX_QMGR_QUECONFIG_BASE_OFFSET);    /* Extract the read pointer. Read pointer is a word pointer */    readPtr = (UINT32)((configRegWord >>			IX_QMGR_Q_CONFIG_RDPTR_OFFSET)&IX_QMGR_RDPTR_MASK);    /* Read/Write pointers(word pointers)  are offsets from the queue buffer space base address.     * Calculate the absolute read pointer address. NOTE: Queues are circular buffers.     */    readPtr  = (readPtr + (entryIndex * qEntrySizeInwords)) & (qSizeInWords - 1); /* Mask by queue size */    *address = (UINT32 *)(baseAddress + (readPtr * (IX_QMGR_NUM_BYTES_PER_WORD)));    switch (qEntrySizeInwords)    {	case IX_QMGR_Q_ENTRY_SIZE1:	    IX_OSAL_ASSERT((*address + IX_QMGR_ENTRY1_OFFSET) < topOfAqmSram);	    	    break;	case IX_QMGR_Q_ENTRY_SIZE2:	    IX_OSAL_ASSERT((*address + IX_QMGR_ENTRY2_OFFSET) < topOfAqmSram);	    break;	case IX_QMGR_Q_ENTRY_SIZE4:	    IX_OSAL_ASSERT((*address + IX_QMGR_ENTRY4_OFFSET) < topOfAqmSram);	    break;	default:	    IX_QMGR_LOG_ERROR0("Invalid Q Entry size passed to ixQMgrAqmIfEntryAddressGet");	    break;    }    }IX_STATUSixQMgrAqmIfQPeek (IxQMgrQId qId,		  unsigned int entryIndex,		  unsigned int *entry){    UINT32 *cfgRegAddress = (UINT32*)(aqmBaseAddress + IX_QMGR_Q_CONFIG_ADDR_GET(qId));    UINT32 *entryAddress = NULL;    UINT32 configRegWordOnEntry;    UINT32 configRegWordOnExit;    unsigned int qEntrySizeInwords;    unsigned int qSizeInWords;    /* Get the queue entry size in words */    qEntrySizeInwords = ixQMgrQEntrySizeInWordsGet (qId);    /* Get the queue size in words */    qSizeInWords = ixQMgrQSizeInWordsGet (qId);    /* Read the config register */    ixQMgrAqmIfWordRead (cfgRegAddress, &configRegWordOnEntry);    /* Get the entry address */    ixQMgrAqmIfEntryAddressGet (entryIndex,				configRegWordOnEntry,				qEntrySizeInwords,				qSizeInWords,				&entryAddress);    /* Get the lock or return busy */    if (IX_SUCCESS != ixOsalFastMutexTryLock(&ixQMgrAqmIfPeekPokeFastMutex[qId]))    {	return IX_FAIL;    }    while(qEntrySizeInwords--)    {	ixQMgrAqmIfWordRead (entryAddress++, entry++);    }    /* Release the lock */    ixOsalFastMutexUnlock(&ixQMgrAqmIfPeekPokeFastMutex[qId]);    /* Read the config register */    ixQMgrAqmIfWordRead (cfgRegAddress, &configRegWordOnExit);    /* Check that the read and write pointers have not changed */    if (configRegWordOnEntry != configRegWordOnExit)    {	return IX_FAIL;    }    return IX_SUCCESS;}IX_STATUSixQMgrAqmIfQPoke (IxQMgrQId qId,		  unsigned entryIndex,		  unsigned int *entry){    UINT32 *cfgRegAddress = (UINT32*)(aqmBaseAddress + IX_QMGR_Q_CONFIG_ADDR_GET(qId));    UINT32 *entryAddress = NULL;    UINT32 configRegWordOnEntry;    UINT32 configRegWordOnExit;    unsigned int qEntrySizeInwords;    unsigned int qSizeInWords;        /* Get the queue entry size in words */    qEntrySizeInwords = ixQMgrQEntrySizeInWordsGet (qId);    /* Get the queue size in words */    qSizeInWords = ixQMgrQSizeInWordsGet (qId);    /* Read the config register */    ixQMgrAqmIfWordRead (cfgRegAddress, &configRegWordOnEntry);    /* Get the entry address */    ixQMgrAqmIfEntryAddressGet (entryIndex,				configRegWordOnEntry,				qEntrySizeInwords,				qSizeInWords,				&entryAddress);    /* Get the lock or return busy */    if (IX_SUCCESS != ixOsalFastMutexTryLock(&ixQMgrAqmIfPeekPokeFastMutex[qId]))    {	return IX_FAIL;    }    /* Else read the entry directly from SRAM. This will not move the read pointer */    while(qEntrySizeInwords--)    {	ixQMgrAqmIfWordWrite (entryAddress++, *entry++);    }    /* Release the lock */    ixOsalFastMutexUnlock(&ixQMgrAqmIfPeekPokeFastMutex[qId]);    /* Read the config register */    ixQMgrAqmIfWordRead (cfgRegAddress, &configRegWordOnExit);    /* Check that the read and write pointers have not changed */    if (configRegWordOnEntry != configRegWordOnExit)    {	return IX_FAIL;    }    return IX_SUCCESS;}PRIVATE unsignedwatermarkToAqmWatermark (IxQMgrWMLevel watermark ){    unsigned aqmWatermark = 0;    /*     * Watermarks 0("000"),1("001"),2("010"),4("011"),     * 8("100"),16("101"),32("110"),64("111")     */    aqmWatermark = ixQMgrAqmIfLog2 (watermark * 2);        return aqmWatermark;}PRIVATE unsignedentrySizeToAqmEntrySize (IxQMgrQEntrySizeInWords entrySize){    /* entrySize  1("00"),2("01"),4("10") */    return (ixQMgrAqmIfLog2 (entrySize));}PRIVATE unsignedbufferSizeToAqmBufferSize (unsigned bufferSizeInWords){    /* bufferSize 16("00"),32("01),64("10"),128("11") */    return (ixQMgrAqmIfLog2 (bufferSizeInWords / IX_QMGR_MIN_BUFFER_SIZE));}/* * Reset AQM registers to default values. */PRIVATE voidixQMgrAqmIfRegistersReset (void){    volatile UINT32 *qConfigWordAddress = NULL;    unsigned int i;    /*     * Need to initialize AQM hardware registers to an initial     * value as init may have been called as a result of a soft     * reset. i.e. soft reset does not reset hardware registers.     */    /* Reset queues 0..31 status registers 0..3 */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUELOWSTAT0_OFFSET), 			 IX_QMGR_QUELOWSTAT_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUELOWSTAT1_OFFSET), 			 IX_QMGR_QUELOWSTAT_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUELOWSTAT2_OFFSET), 			 IX_QMGR_QUELOWSTAT_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUELOWSTAT3_OFFSET), 			 IX_QMGR_QUELOWSTAT_RESET_VALUE);    /* Reset underflow/overflow status registers 0..1 */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUEUOSTAT0_OFFSET), 			 IX_QMGR_QUEUOSTAT_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUEUOSTAT1_OFFSET), 			 IX_QMGR_QUEUOSTAT_RESET_VALUE);        /* Reset queues 32..63 nearly empty status registers */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUEUPPSTAT0_OFFSET),			 IX_QMGR_QUEUPPSTAT0_RESET_VALUE);    /* Reset queues 32..63 full status registers */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUEUPPSTAT1_OFFSET),			 IX_QMGR_QUEUPPSTAT1_RESET_VALUE);    /* Reset int0 status flag source select registers 0..3 */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_INT0SRCSELREG0_OFFSET),			 IX_QMGR_INT0SRCSELREG_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_INT0SRCSELREG1_OFFSET),			 IX_QMGR_INT0SRCSELREG_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_INT0SRCSELREG2_OFFSET),			 IX_QMGR_INT0SRCSELREG_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_INT0SRCSELREG3_OFFSET),			 IX_QMGR_INT0SRCSELREG_RESET_VALUE);	     /* Reset queue interrupt enable register 0..1 */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUEIEREG0_OFFSET),			 IX_QMGR_QUEIEREG_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QUEIEREG1_OFFSET),			 IX_QMGR_QUEIEREG_RESET_VALUE);    /* Reset queue interrupt register 0..1 */    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QINTREG0_OFFSET),			 IX_QMGR_QINTREG_RESET_VALUE);    ixQMgrAqmIfWordWrite((UINT32 *)(aqmBaseAddress + IX_QMGR_QINTREG1_OFFSET),			 IX_QMGR_QINTREG_RESET_VALUE);    /* Reset queue configuration words 0..63 */    qConfigWordAddress = (UINT32 *)(aqmBaseAddress + IX_QMGR_QUECONFIG_BASE_OFFSET);    for (i = 0; i < (IX_QMGR_QUECONFIG_SIZE / sizeof(UINT32)); i++)    {	ixQMgrAqmIfWordWrite(qConfigWordAddress,			     IX_QMGR_QUECONFIG_RESET_VALUE);	/* Next word */	qConfigWordAddress++;    }}