/* vxbFtQspi.c - QSPI driver */

/*
 *  
 * This program is OPEN SOURCE software: you can redistribute it and/or modify it; 
 * This program 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.
 *
 */


/* includes */

#include <vxWorks.h>
#include <vsbConfig.h>
#include <intLib.h>
#include <stdlib.h>
#include <string.h>
#include <semLib.h>
#include <taskLib.h>
#include <stdio.h>
#include <vxbTimerLib.h>
#include <cacheLib.h>
#include <hwif/util/hwMemLib.h>
#include <hwif/util/vxbParamSys.h>
#include <hwif/vxbus/vxBus.h>
#include <hwif/vxbus/vxbPlbLib.h>
#include <../src/hwif/h/vxbus/vxbAccess.h>
#include <hwif/vxbus/vxbSpiLib.h>
#include <vxbFtQspi.h>

/* debug macro */

#undef  SPI_DBG_ON
#ifdef  SPI_DBG_ON

#undef  LOCAL
#define LOCAL

#define SPI_DBG_OFF            0x00000000
#define SPI_DBG_ISR            0x00000001
#define SPI_DBG_RW             0x00000002
#define SPI_DBG_ERR            0x00000004
#define SPI_DBG_RTN            0x00000008
#define SPI_DBG_INFO           0x00000010
#define SPI_DBG_ALL            0xffffffff

LOCAL UINT32 spiDbgMask =  SPI_DBG_ALL;
IMPORT FUNCPTR _func_logMsg;

#define SPI_DBG(mask, string, X1, X2, X3, X4, X5, X6)           \
    if ((spiDbgMask & mask) || (mask == SPI_DBG_ALL))           \
        if (_func_logMsg != NULL)                               \
            (* _func_logMsg)(string, (int)X1, (int)X2, (int)X3, \
                             (int)X4, (int)X5, (int)X6)
#else
#define SPI_DBG(mask, string, X1, X2, X3, X4, X5, X6)
#endif  /* SPI_DBG_ON */

#if _BYTE_ORDER == _BIG_ENDIAN
#    define SPI_REG_HANDLE_SWAP(x) VXB_HANDLE_SWAP(x)
#else
#    define SPI_REG_HANDLE_SWAP(x) (x)
#endif /* _BYTE_ORDER == _BIG_ENDIAN */

#define IS_SPI_FULL_DUPLEX(mode)  (((mode) & SPI_FULL_DUPLEX) != 0)

/* VxBus methods */

LOCAL void vxbFtQspiInstInit (VXB_DEVICE_ID pDev);
LOCAL void vxbFtQspiInstInit2 (VXB_DEVICE_ID pDev);
LOCAL void vxbFtQspiInstConnect (VXB_DEVICE_ID pDev);
LOCAL VXB_SPI_BUS_CTRL * vxbFtQspiCtrlGet (VXB_DEVICE_ID pDev);
LOCAL void vxbFtQspiShow (VXB_DEVICE_ID, int);
LOCAL STATUS vxbFtQspiInstUnlink (VXB_DEVICE_ID pDev, void * unused);

/* forward declarations */
LOCAL void vxbFtQspiCtrlInit (VXB_DEVICE_ID pDev);

LOCAL STATUS vxbFtQspiTransfer (VXB_DEVICE_ID pDev, SPI_HARDWARE * pSpiDev,
                                  SPI_TRANSFER * pPkg);
/* locals */

LOCAL struct drvBusFuncs vxbFtQspiVxbFuncs = {
    vxbFtQspiInstInit,      /* devInstanceInit */ 
    vxbFtQspiInstInit2,     /* devInstanceInit2 */
    vxbFtQspiInstConnect    /* devConnect */
};

LOCAL device_method_t vxbFtQspiDeviceMethods[] = {
    DEVMETHOD (vxbSpiControlGet, vxbFtQspiCtrlGet),
    DEVMETHOD (busDevShow,       vxbFtQspiShow),
    DEVMETHOD (vxbDrvUnlink,     vxbFtQspiInstUnlink),
    DEVMETHOD_END
};

LOCAL struct vxbPlbRegister vxbFtQspiDevRegistration = {
    {
    NULL,                       /* pNext */
    VXB_DEVID_DEVICE,           /* devID */
    VXB_BUSID_PLB,              /* busID = PLB */
    VXB_VER_4_0_0,              /* vxbVersion */
    FT_QSPI_NAME,               /* drvName */
    &vxbFtQspiVxbFuncs,         /* pDrvBusFuncs */
    &vxbFtQspiDeviceMethods[0], /* pMethods */
    NULL,                       /* devProbe  */
    NULL,                       /* pParamDefaults */
    },
};

/* controllor read and write interface */

#define FT_QSPI_BAR(p)    ((char *)((p)->pRegBase[0]))
#define FT_QSPI_HANDLE(p) (((FT_QSPI_CTRL *)(p)->pDrvCtrl)->regHandle)

#undef CSR_READ_4
#define CSR_READ_4(pDev, addr)                                  \
    vxbRead32 (FT_QSPI_HANDLE(pDev),                             \
               (UINT32 *)((char *)FT_QSPI_BAR(pDev) + (addr)))

#undef CSR_WRITE_4
#define CSR_WRITE_4(pDev, addr, data)                           \
    vxbWrite32 (FT_QSPI_HANDLE(pDev),                            \
                (UINT32 *)((char *)FT_QSPI_BAR(pDev) + (addr)), (data))

#undef CSR_SETBIT_4
#define CSR_SETBIT_4(pDev, offset, val)                         \
    CSR_WRITE_4((pDev), (offset), CSR_READ_4((pDev), (offset)) | (val))

#undef CSR_CLRBIT_4
#define CSR_CLRBIT_4(pDev, offset, val)                         \
    CSR_WRITE_4((pDev), (offset), CSR_READ_4((pDev), (offset)) & ~(val))

/*****************************************************************************
*
* vxbFtQspiRegister - register with the VxBus subsystem
*
* This routine registers the SPI driver with VxBus Systems.
*
* RETURNS: N/A
*
* ERRNO: N/A
*/

void vxbFtQspiRegister (void)
    {
    vxbDevRegister ((struct vxbDevRegInfo *) &vxbFtQspiDevRegistration);    
    }

/*****************************************************************************
*
* vxbFtQspiInstInit - initialize QSPI controller
*
* This function implements the VxBus instInit handler for a QSPI controller
* device instance.
*
* Initialize the device by the following:
*
* - retrieve the resource from the hwconf
* - per-device init
* - announce SPI Bus and create device instance
*
* RETURNS: N/A
*
* ERRNO: N/A
*/

LOCAL void vxbFtQspiInstInit
    (
    VXB_DEVICE_ID pDev
    )
    {
    FT_QSPI_CTRL * pDrvCtrl;
    struct hcfDevice * pHcf;
    /*FUNCPTR clkFunc = NULL;*/
    int i;

    /* check for valid parameter */
    VXB_ASSERT_NONNULL_V (pDev);

    /* create controller driver context structure for core */

    pDrvCtrl = (FT_QSPI_CTRL *) hwMemAlloc (sizeof (FT_QSPI_CTRL));

    if (pDrvCtrl == NULL)
        {
        return;
        }

    /* save instance ID */

    pDev->pDrvCtrl = pDrvCtrl;
    pDrvCtrl->pDev = pDev;

    for (i = 0; i < VXB_MAXBARS; i++)
        {
        if (pDev->regBaseFlags[i] == VXB_REG_MEM)
            {
            break;
            }
        }

    if (i == VXB_MAXBARS)
        {
    #ifndef  _VXBUS_BASIC_HWMEMLIB
        hwMemFree ((char *)pDrvCtrl);
    #endif /* _VXBUS_BASIC_HWMEMLIB */

        pDev->pDrvCtrl = NULL;
        return;
        }

    pDrvCtrl->regBase = pDev->pRegBase[i];
    vxbRegMap (pDev, i, &pDrvCtrl->regHandle);
    pDrvCtrl->regHandle = (void *)
                          SPI_REG_HANDLE_SWAP ((ULONG)pDrvCtrl->regHandle);

    pHcf = (struct hcfDevice *) hcfDeviceGet (pDev);
    if (pHcf == NULL)
        {
    #ifndef  _VXBUS_BASIC_HWMEMLIB
        hwMemFree ((char *)pDrvCtrl);
    #endif /* _VXBUS_BASIC_HWMEMLIB */

        pDev->pDrvCtrl = NULL;
        return;
        }

    if (devResourceGet (pHcf, "capacity", HCF_RES_INT,
                        (void *) &pDrvCtrl->capacity) != OK)
        {
        pDrvCtrl->capacity = QSPI_FLASH_CAP_4MB;
        }
     
    if (devResourceGet (pHcf, "clkDiv", HCF_RES_INT,
                        (void *) &pDrvCtrl->clkDiv) != OK)
        {
        pDrvCtrl->clkDiv = QSPI_SCK_DIV_128;
        }
    
    if (devResourceGet (pHcf, "transMode", HCF_RES_INT,
                        (void *) &pDrvCtrl->transMode) != OK)
        {
        pDrvCtrl->transMode = QSPI_TRANSFER_1_1_1;
        }
    
    if (devResourceGet (pHcf, "addrMode", HCF_RES_INT,
                        (void *) &pDrvCtrl->addrMode) != OK)
        {
        pDrvCtrl->addrMode = QSPI_ADDR_SEL_3;
        }  
    
    if (devResourceGet (pHcf, "spiDevNum", HCF_RES_INT,
                        (void *) &pDrvCtrl->spiDevNum) != OK)
        {
        pDrvCtrl->spiDevNum = 0;
        }      
    
    /* SPI controller init */

    (void) vxbFtQspiCtrlInit (pDev);

    /* install the transfer routine for SPI Lib */

    pDrvCtrl->vxbSpiCtrl.spiTransfer = (void *)vxbFtQspiTransfer;

    /* announce that there's a SPI bus */

    (void) vxbBusAnnounce (pDev, VXB_BUSID_SPI);

    /* notify the bus subsystem of all devices on SPI */

    (void) spiBusAnnounceDevices (pDev);

    /* the first phase of the initialization */

    pDrvCtrl->initPhase = 1;
    }

/*******************************************************************************
*
* vxbFtQspiCtrlInit -  QSPI controller initialization
*
* This routine performs the QSPI controller initialization.
*
* RETURNS: N/A
*
* ERRNO: N/A
*/

LOCAL void vxbFtQspiCtrlInit
    (
    VXB_DEVICE_ID               pDev
    )
    {
    FT_QSPI_CTRL *      pDrvCtrl;
    UINT8 ndev;

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;

    ndev = pDrvCtrl->spiDevNum & 0x3;
    if (ndev > 0) {
        ndev -= 1;
    }
    CSR_WRITE_4 (pDev, REG_QSPI_CAP, CAP_FLASH_NUM(ndev) | CAP_FLASH_CAP(pDrvCtrl->capacity));

    pDrvCtrl->initDone = TRUE;
    }

/*******************************************************************************
*
* vxbFtQspiInstInit2 - second level initialization routine of QSPI controller
*
* This routine performs the second level initialization of the QSPI controller.
*
* RETURNS: N/A
*
* ERRNO: N/A
*/

LOCAL void vxbFtQspiInstInit2
    (
    VXB_DEVICE_ID pDev
    )
    {
    FT_QSPI_CTRL * pDrvCtrl;

    /* check for valid parameter */

    VXB_ASSERT_NONNULL_V (pDev);

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;

    /* used for mutex accessing of the controller */

    pDrvCtrl->muxSem = semMCreate (SEM_Q_PRIORITY);

    if (pDrvCtrl->muxSem == NULL)
        {
        SPI_DBG (SPI_DBG_ERR, "semMCreate failed for muxSem\n",
                 0, 0, 0, 0, 0, 0);
        return;
        }

    pDrvCtrl->initPhase = 2;
    }

/*******************************************************************************
*
* vxbFtQspiInstConnect - third level initialization
*
* This routine performs the third level initialization of the QSPI controller
* driver.
*
* RETURNS: N/A
*
* ERRNO : N/A
*/

LOCAL void vxbFtQspiInstConnect
    (
    VXB_DEVICE_ID       pDev
    )
    {
    FT_QSPI_CTRL * pDrvCtrl;

    pDrvCtrl = pDev->pDrvCtrl;
    pDrvCtrl->initPhase = 3;
    }

/*****************************************************************************
*
* vxbFtQspiInstUnlink - VxBus unlink handler
*
* This function shuts down a QSPI controller instance in response to an
* an unlink event from VxBus. This may occur if our VxBus instance has
* been terminated, or if the SPI driver has been unloaded.
*
* RETURNS: OK if device was successfully destroyed, otherwise ERROR
*
* ERRNO: N/A
*/

LOCAL STATUS vxbFtQspiInstUnlink
    (
    VXB_DEVICE_ID pDev,
    void *        unused
    )
    {
    FT_QSPI_CTRL * pDrvCtrl;

    /* check if the pDev pointer is valid */

    VXB_ASSERT (pDev != NULL, ERROR)

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;

    /*
     * The semaphore and interrupt resource are released here . The 
     * semaphore was created at phase 2 and interrupt was installed
     * at phase 3.
     */

    if (pDrvCtrl->initPhase >= 2)
        {
        (void)semTake (pDrvCtrl->muxSem, WAIT_FOREVER);

        (void) semDelete (pDrvCtrl->muxSem);
        pDrvCtrl->muxSem = NULL;
        }

#ifndef _VXBUS_BASIC_HWMEMLIB
    hwMemFree ((char *) pDrvCtrl);
#endif  /* _VXBUS_BASIC_HWMEMLIB */

    pDev->pDrvCtrl = NULL;
    pDrvCtrl->initPhase = 0;

    return (OK);
    }

/*******************************************************************************
*
* vxbFtQspiCtrlGet - get the QSPI controller struct
*
* This routine returns the QSPI controller struct pointer (VXB_SPI_BUS_CTRL *)
* to caller (SPI Lib) by vxbSpiControlGet method. Currently, this struct
* only contain the spiTransfer routine(eg: vxbFtQspiTransfer) for SPI Lib,
* other parameters can be easily added in this struct.
*
* RETURNS: the pointer of SPI controller struct
*
* ERRNO: N/A
*/

LOCAL VXB_SPI_BUS_CTRL * vxbFtQspiCtrlGet
    (
    VXB_DEVICE_ID pDev
    )
    {
    FT_QSPI_CTRL * pDrvCtrl;

    /* check if the pDev pointer is valid */

    VXB_ASSERT (pDev != NULL, NULL)

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;

    SPI_DBG (SPI_DBG_RTN, "vxbFtQspiCtrlGet(0x08%x) called\n",
            (_Vx_usr_arg_t)pDev, 2, 3, 4, 5, 6);

    return (&(pDrvCtrl->vxbSpiCtrl));
    }

/*******************************************************************************
*
* vxbFtQspiMemcpyToReg - Memcpy To Register
*
* Memory copy To Register
* Not to enable p_buffer when use this function
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL int vxbFtQspiMemcpyToReg
    (
    VXB_DEVICE_ID pDev,     
    UINT8 *buf, 
    UINT32 len
    )
{
    UINT32 val = 0;

    if (!buf || (len > 4))
        return ERROR;

    if (1 == len) {
        val = buf[0];
    } else if (2 == len) {
        val = buf[1];
        val = (val << 8) + buf[0];
    } else if (3 == len) {
        val = buf[2];
        val = (val << 8) + buf[1];
        val = (val << 8) + buf[0];
    } else if (4 == len) {
        val = buf[3];
        val = (val << 8) + buf[2];
        val = (val << 8) + buf[1];
        val = (val << 8) + buf[0];
    }

    CSR_WRITE_4 (pDev, REG_QSPI_LD_PORT, val);

    return OK;
}

/*******************************************************************************
*
* vxbFtQspiMemcpyFromReg - Memcpy from Register
*
* Memory copy from Register
* Need to enable p_buffer when use this function
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL STATUS vxbFtQspiMemcpyFromReg
    (
    VXB_DEVICE_ID pDev,
    UINT8 *buf, 
    UINT32 len
    )
    {
    int i;
    UINT32 val = 0;

    if (!buf)
        return ERROR;

    for (i = 0; i < len; i++)
    {
        if (0 == i % 4)
            val = CSR_READ_4 (pDev, REG_QSPI_LD_PORT);

        buf[i] = (UINT8) (val >> (i % 4) * 8) & 0xFF;
    }

    return OK;
    }

/*******************************************************************************
*
* vxbFtQspiFlashRead - flash read function
*
* Reads bytes data from flash addr to buf
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL STATUS  vxbFtQspiFlashRead    
    (
    VXB_DEVICE_ID       pDev
    )
    {
    QSPI_RD_CFG_REG_T rdCfgReg;
    UINT8 cmd;
    UINT32 addr;
    FT_QSPI_CTRL * pDrvCtrl;

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;
    
    cmd = pDrvCtrl->txBuf[0];   
    addr = pDrvCtrl->txBuf[1];
    addr = (addr << 8) + pDrvCtrl->txBuf[2];
    addr = (addr << 8) + pDrvCtrl->txBuf[3];
    if(pDrvCtrl->addrMode == QSPI_ADDR_SEL_4)
        {
        addr = (addr << 8) + pDrvCtrl->txBuf[4];
        }
        
    if ((addr < 0) || (pDrvCtrl->rxBuf == NULL) || (pDrvCtrl->rxLen < 0) || ((addr + pDrvCtrl->rxLen) > SPI_FLASH_SIZE))
        return ERROR;
  
    rdCfgReg.data = 0;
    rdCfgReg.val.rd_cmd = cmd;
    rdCfgReg.val.d_buffer = 1;
    rdCfgReg.val.rd_addr_sel = pDrvCtrl->addrMode;
    rdCfgReg.val.rd_sck_sel = pDrvCtrl->clkDiv;
    rdCfgReg.val.rd_transfer = pDrvCtrl->transMode;
    
    CSR_WRITE_4 (pDev, REG_QSPI_RD_CFG, rdCfgReg.data);
    
    /*printf("read:%x %x\n",CSR_READ_4 (pDev, REG_QSPI_RD_CFG),addr);*/

    memcpy (pDrvCtrl->rxBuf, (char *) (SPI_FLASH_BASE_ADRS + addr), pDrvCtrl->rxLen);

    return OK;
    }

/*******************************************************************************
*
* vxbFtQspiFlashPageWrite - Write one page to flash 
*
* Writes one page into flash,changing bits from 1 to 0
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL STATUS  vxbFtQspiFlashPageWrite
    (
    VXB_DEVICE_ID       pDev
    )
    {
    QSPI_WR_CFG_REG_T wrCfgReg;
    int index = 0;
    UINT32 *pu32Buf = NULL;
    UINT32 val = 0;
    UINT8 cmd;
    UINT32 len;
    UINT32 addr;
    UINT8* sendBuff;
    FT_QSPI_CTRL * pDrvCtrl;

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;
    
    cmd = pDrvCtrl->txBuf[0];
    addr = pDrvCtrl->txBuf[1];
    addr = (addr << 8) + pDrvCtrl->txBuf[2];
    addr = (addr << 8) + pDrvCtrl->txBuf[3];
    if(pDrvCtrl->addrMode == QSPI_ADDR_SEL_4)
        {
        addr = (addr << 8) + pDrvCtrl->txBuf[4];
        sendBuff = &pDrvCtrl->txBuf[5];
        len = pDrvCtrl->txLen - 5;
        }
    else
        {
        sendBuff = &pDrvCtrl->txBuf[4];
        len = pDrvCtrl->txLen - 4;
        }
    pu32Buf = (UINT32 *)sendBuff;
    
    if ((len > SPI_FLASH_PAGE_SIZE) || (len <= 0)  || (addr % 4 != 0))
        return ERROR;
    
    wrCfgReg.data = 0;
    wrCfgReg.val.wr_cmd = cmd;
    wrCfgReg.val.wr_wait = 1;
    wrCfgReg.val.wr_sck_sel = pDrvCtrl->clkDiv;
    wrCfgReg.val.wr_addrsel = pDrvCtrl->addrMode;
    wrCfgReg.val.wr_transfer = pDrvCtrl->transMode;
    wrCfgReg.val.wr_mode = 1;
    CSR_WRITE_4 (pDev, REG_QSPI_WR_CFG, wrCfgReg.data);
    
    /*printf("write:%x  %x\n",CSR_READ_4 (pDev, REG_QSPI_WR_CFG),addr);*/
        
    while(len)
        {
        if (len >= 4)
            {
            sysOutLong(SPI_FLASH_BASE_ADRS + addr + index, pu32Buf[index / 4]) ;

            len -= 4;
            index += 4;
            }
        else
            {
            if (len == 1)
                val = sendBuff[index] | 0xFFFFFF00;
            else if (len == 2)
                val = sendBuff[index] | (sendBuff[index + 1] << 8) | 0xFFFF0000;
            else
                val = sendBuff[index] | (sendBuff[index + 1] << 8) | (sendBuff[index + 2] << 16) | 0xFF000000;

            sysOutLong(SPI_FLASH_BASE_ADRS + addr + index, val) ;
            len = 0;
            }
        }
    CSR_WRITE_4 (pDev, REG_QSPI_FLUSH, 0x1);
    return OK;
    }

/*******************************************************************************
*
* vxbFtQspiFlashRegSet - set registers of flash 
*
* Set registers of flash 
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL STATUS vxbFtQspiFlashRegSet
    (
    VXB_DEVICE_ID       pDev
    )
    {
    QSPI_CMD_PORT_REG_T cmdPortReg;
    UINT8 cmd;
    UINT32 dataLen;
    UINT8* sendBuff = NULL;
    FT_QSPI_CTRL * pDrvCtrl;

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;
    
    cmd = pDrvCtrl->txBuf[0];
    dataLen = pDrvCtrl->txLen - 1;
    
    cmdPortReg.data = 0;
    cmdPortReg.val.cmd = cmd;
    cmdPortReg.val.wait = 1;
    cmdPortReg.val.sck_sel = pDrvCtrl->clkDiv;
    cmdPortReg.val.transfer = pDrvCtrl->transMode;
    cmdPortReg.val.cs = pDrvCtrl->channel;

    if(dataLen == 0)
        {
        CSR_WRITE_4 (pDev, REG_QSPI_CMD_PORT, cmdPortReg.data); 
        CSR_WRITE_4 (pDev, REG_QSPI_LD_PORT, 1);
        }       
    else
        {
        sendBuff = &pDrvCtrl->txBuf[1];
        cmdPortReg.val.data_transfer = 1;
        cmdPortReg.val.rw_mum = dataLen - 1;
        CSR_WRITE_4 (pDev, REG_QSPI_CMD_PORT, cmdPortReg.data); 
        vxbFtQspiMemcpyToReg(pDev, sendBuff, dataLen);
        }

    /*printf("Set Reg:%x \n",pDrvCtrl->txBuf[0]); */  
    return OK;
    }

/*******************************************************************************
*
* vxbFtQspiFlashRegSetWithAddr - set registers of flash 
*
* Set registers of flash with address
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL STATUS vxbFtQspiFlashRegSetWithAddr
    (
    VXB_DEVICE_ID       pDev
    )
    {
    QSPI_CMD_PORT_REG_T cmdPortReg;
    UINT8 cmd;
    UINT32 dataLen;
    UINT32 addr;
    UINT8* sendBuff = NULL;
    FT_QSPI_CTRL * pDrvCtrl;

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;
    
    cmd = pDrvCtrl->txBuf[0];
    addr = pDrvCtrl->txBuf[1];
    addr = (addr << 8) + pDrvCtrl->txBuf[2];
    addr = (addr << 8) + pDrvCtrl->txBuf[3];
    if(pDrvCtrl->addrMode == QSPI_ADDR_SEL_4)
        {
        addr = (addr << 8) + pDrvCtrl->txBuf[4];        
        dataLen = pDrvCtrl->txLen - 5;
        if(dataLen != 0)
            sendBuff = &pDrvCtrl->txBuf[5];
        }
    else
        {
        dataLen = pDrvCtrl->txLen - 4;
        if(dataLen != 0)        
            sendBuff = &pDrvCtrl->txBuf[4];
        }
    
    cmdPortReg.data = 0;
    cmdPortReg.val.cmd = cmd;
    cmdPortReg.val.wait = 1;
    cmdPortReg.val.sck_sel = pDrvCtrl->clkDiv;
    cmdPortReg.val.transfer = pDrvCtrl->transMode;
    cmdPortReg.val.cs = pDrvCtrl->channel;
    cmdPortReg.val.cmd_addr = 1;
    cmdPortReg.val.addr_sel = pDrvCtrl->addrMode;

    CSR_WRITE_4 (pDev, REG_QSPI_ADDR_PORT,addr);
    if(dataLen == 0)
        {
        CSR_WRITE_4 (pDev, REG_QSPI_CMD_PORT, cmdPortReg.data); 
        CSR_WRITE_4 (pDev, REG_QSPI_LD_PORT, 0);
        }       
    else
        {
        cmdPortReg.val.data_transfer = 1;
        cmdPortReg.val.rw_mum = dataLen - 1;
        CSR_WRITE_4 (pDev, REG_QSPI_CMD_PORT, cmdPortReg.data); 
        vxbFtQspiMemcpyToReg(pDev, sendBuff, dataLen);
        }

    /*printf("Set Reg addr:%x %x\n",pDrvCtrl->txBuf[0], addr);   */
    return OK;
    }

/*******************************************************************************
*
* vxbFtQspiFlashRegGet - get registers of flash 
*
* Get registers of flash 
* 
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/
LOCAL STATUS vxbFtQspiFlashRegGet
    (
    VXB_DEVICE_ID       pDev
    )
    {
    QSPI_CMD_PORT_REG_T cmdPortReg;
    FT_QSPI_CTRL * pDrvCtrl;

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;
    
    cmdPortReg.data = 0;
    cmdPortReg.val.cmd = pDrvCtrl->txBuf[0];
    cmdPortReg.val.wait = 1;
    cmdPortReg.val.sck_sel = pDrvCtrl->clkDiv;
    cmdPortReg.val.transfer = pDrvCtrl->transMode;
    cmdPortReg.val.cs = pDrvCtrl->channel;
    cmdPortReg.val.data_transfer = 1;
    cmdPortReg.val.p_buffer = 1;
    
    CSR_WRITE_4 (pDev, REG_QSPI_CMD_PORT, cmdPortReg.data);
    
    vxbFtQspiMemcpyFromReg(pDev ,pDrvCtrl->rxBuf, pDrvCtrl->rxLen);
    
    /*  printf("get reg :%x\n", pDrvCtrl->txBuf[0]);*/
    return OK;
    }

/*******************************************************************************
*
* vxbFtQspiTransfer - SPI transfer routine
*
* This routine is used to perform one transmission. It is the interface which 
* can be called by SPI device driver to send and receive data via the QSPI 
* controller.
*
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*/

LOCAL STATUS vxbFtQspiTransfer
    (
    VXB_DEVICE_ID       pDev,           /* controller pDev */
    SPI_HARDWARE *      pSpiDev,        /* device info */
    SPI_TRANSFER *      pPkg            /* transfer data info */
    )
    {
    STATUS              sts = OK;
    UINT32              cmd;
    UINT32              alignSize;
    FT_QSPI_CTRL * pDrvCtrl;
    /* check if the pointers are valid */

    if (pDev == NULL || pSpiDev == NULL || pPkg == NULL ||
    pSpiDev->devInfo == NULL)
        {
        SPI_DBG (SPI_DBG_ERR, "vxbFtQspiTransfer NULL pointer\n",
                 1, 2, 3, 4, 5, 6);
        return ERROR;
        }

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;
    if (pDrvCtrl == NULL)
        {
        SPI_DBG (SPI_DBG_ERR, "vxbFtQspiTransfer pDrvCtrl is NULL\n",
                 1, 2, 3, 4, 5, 6);
        return ERROR;
        }

    SPI_DBG (SPI_DBG_RTN, "vxbFtQspiTransfer txLen[%d] rxLen[%d]\n",
             pPkg->txLen, pPkg->rxLen, 3, 4, 5, 6);

    if (pPkg->txLen == 0 && pPkg->rxLen == 0)
        {
        SPI_DBG (SPI_DBG_ERR, "vxbFtQspiTransfer tx and rx both are 0\n",
                 1, 2, 3, 4, 5, 6);
        return ERROR;
        }

    if ((pPkg->txLen != 0 && pPkg->txBuf == NULL ) ||
        (pPkg->rxLen != 0 && pPkg->rxBuf == NULL))
        {
        SPI_DBG (SPI_DBG_ERR,
                 "vxbFtQspiTransfer invalid parameters[%x %x %x %x] \n",
                  pPkg->txBuf, pPkg->txLen, pPkg->rxLen, pPkg->rxBuf, 5, 6);

        return ERROR;
        }

    if (pSpiDev->devInfo->bitWidth <= 8)       /* 4 to 8 bits */
        {
        alignSize = sizeof(char);
        }
    else if (pSpiDev->devInfo->bitWidth <= 16) /* 9 to 16 bits */
        {
        alignSize = sizeof(UINT16);
        }
    else    /* 17 to 32 bits */
        {
        SPI_DBG (SPI_DBG_ERR, 
                 "vxbFtQspiTransfer: data word length must between 2-16\n",
                 1, 2, 3, 4, 5, 6);
        return ERROR;
        }

    /* check to see if the address is aligned with SPI bit width */

    if ((pPkg->txLen != 0 && 
        (((UINT32)pPkg->txBuf & (alignSize - 1)) != 0 ||
         (pPkg->txLen & (alignSize - 1)) != 0)) ||
        (pPkg->rxLen != 0 && 
         (((UINT32)pPkg->rxBuf & (alignSize - 1)) != 0 ||
          (pPkg->rxLen & (alignSize - 1)) != 0)))
        {
        SPI_DBG (SPI_DBG_ERR,
                 "vxbFtQspiTransfer address or len is not aligned:"
                 "[tx:%x-%x rx:%x-%x] with %d \n",
                 pPkg->txBuf, pPkg->txLen, pPkg->rxBuf, pPkg->rxLen, 
                 alignSize, 6);

        return ERROR;
        }

    pDrvCtrl->channel = pSpiDev->devInfo->chipSelect;
    if (pDrvCtrl->channel >= QSPI_MAX_CS_NUM)
        {
        SPI_DBG (SPI_DBG_ERR,
                 "vxbFtQspiTransfer invalid channel[%x] \n",
                 pDrvCtrl->channel, 2, 3, 4, 5, 6);

        return ERROR;
        }

    /*
     * The controller can not be used by multichannel at the same time.
     * If the initPhase < 2, there is no multi-task context. So, the
     * semTake is not needed.
     */

    if (pDrvCtrl->initPhase >= 2)
        {
        (void)semTake (pDrvCtrl->muxSem, WAIT_FOREVER);
        }

    pDrvCtrl->txBuf  = pPkg->txBuf;
    pDrvCtrl->txLen = pPkg->txLen;
    pDrvCtrl->rxBuf  = pPkg->rxBuf;
    pDrvCtrl->rxLen = pPkg->rxLen;
    cmd = pDrvCtrl->txBuf[0];
    
    switch (cmd)
        {
        case QSPI_FLASH_CMD_RDID:
        case QSPI_FLASH_CMD_RDSR1:          
             vxbFtQspiFlashRegGet(pDev);            
            break;
        case QSPI_FLASH_CMD_4BAM:
            vxbFtQspiFlashRegSet(pDev);
            pDrvCtrl->addrMode = QSPI_ADDR_SEL_4;
            break;
        case QSPI_FLASH_CMD_4BEX:
            vxbFtQspiFlashRegSet(pDev);
            pDrvCtrl->addrMode = QSPI_ADDR_SEL_3;
            break;
        case QSPI_FLASH_CMD_WREN:
        case QSPI_FLASH_CMD_WRDI:
        case QSPI_FLASH_CMD_BE: 
        case QSPI_FLASH_CMD_WRR:
             vxbFtQspiFlashRegSet(pDev); 
            break;
        case QSPI_FLASH_CMD_SE:
             if(pDrvCtrl->addrMode == QSPI_ADDR_SEL_4)
             {
                 pDrvCtrl->txBuf[0] = QSPI_FLASH_CMD_4SE;
             }
             vxbFtQspiFlashRegSetWithAddr(pDev);
            break;
        case QSPI_FLASH_CMD_PP:
             if(pDrvCtrl->addrMode == QSPI_ADDR_SEL_4)
             {
                 pDrvCtrl->txBuf[0] = QSPI_FLASH_CMD_4PP;
             }
             vxbFtQspiFlashPageWrite(pDev);
            break;
        case QSPI_FLASH_CMD_READ:
             if(pDrvCtrl->addrMode == QSPI_ADDR_SEL_4)
             {
                 pDrvCtrl->txBuf[0] = QSPI_FLASH_CMD_4READ;
             }
             vxbFtQspiFlashRead(pDev);
            break;          
        default:
            break;
        }
   /* printf(" xfer :%x\n",cmd);*/
    
    if (pPkg->usDelay > 0)
        {
        vxbUsDelay(pPkg->usDelay);
        }

    if (pDrvCtrl->initPhase >= 2)
        {
        semGive (pDrvCtrl->muxSem);
        }


    return sts;
    }




/*****************************************************************************
*
* vxbFtQspiShow - show the controller info
*
* This function shows the QSPI controller's info.
*
* RETURNS: N/A
*
* ERRNO: N/A
*/

LOCAL void vxbFtQspiShow
    (
    VXB_DEVICE_ID pDev,
    int verbose
    )
    {
    FT_QSPI_CTRL * pDrvCtrl;

    /* check for valid parameter */

    VXB_ASSERT_NONNULL_V (pDev);

    pDrvCtrl = (FT_QSPI_CTRL *) pDev->pDrvCtrl;

    printf ("        %s unit %d on %s @ %p with busInfo %p\n",
            pDev->pName,
            pDev->unitNumber,
            vxbBusTypeString (pDev->busID),
            pDev,
            pDev->u.pSubordinateBus);

    if (verbose > 0)
        {
        printf ("            BAR0 @ 0x%08x (memory mapped)\n",
                                                   pDev->pRegBase[0]);
        printf ("            pDrvCtrl @ 0x%08x\n", pDev->pDrvCtrl);
        printf ("            init  :  %s\n", 
                             pDrvCtrl->initDone ? "SUCCESS" : "FAIL");
        }
    }