/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
#include <linux/config.h>
#include <asm/avalanche/avalanche_map.h>
#include <asm/mips-boards/prom.h>
#include <asm/avalanche/sangam/vlynq.h>
#include <asm/avalanche/sangam/hw_usb.h>

#if defined(CONFIG_AVALANCHE_VBUS_TEST)
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static unsigned int MipsVbusAccess(struct _usb_hw *USB_ctrl, unsigned int VbusAddr, int nbytes, enum _vbus_access_mode mode, unsigned int data ) {
                         /*--- nbytes =     1       2       4 ---*/
                         /*--- VbusAddr&3 = 0-3     0,2     0 ---*/
    unsigned int  ByteEnableMask;    /*--- 0001b   0011b   1111b ---*/
    unsigned int  BitMask;           /*--- 0xFF    0xFFFF  0xFFFFFFFF ---*/
    unsigned int  ByteWithinDword;   /*--- 0-3     0,2     0 ---*/
    unsigned int  CfgByteEnables;
    unsigned int  timeout_count;

    ByteWithinDword = VbusAddr & 0x03;
    VbusAddr &= ~0x03;          /*--- insure vbus address is 32-bit aligned ---*/
    VbusAddr &= 0x1FFFFFFF;  /*--- convert VBUS virtual address to physical address ---*/
     /*--- Calculate values ---*/
    ByteEnableMask = 0xF >> (4 - nbytes);
    BitMask = 0xFFFFFFFF >> (8 * (4 - nbytes));
    CfgByteEnables = (ByteEnableMask << ByteWithinDword);
     /*--- Set VBUS Address register ---*/
    USB_ctrl->usb_slave_vbus_addr_0 = (VbusAddr >>  0) & 0xFF;
    USB_ctrl->usb_slave_vbus_addr_1 = (VbusAddr >>  8) & 0xFF;
    USB_ctrl->usb_slave_vbus_addr_2 = (VbusAddr >> 16) & 0xFF;
    USB_ctrl->usb_slave_vbus_addr_3 = (VbusAddr >> 24) & 0xFF;

    if( mode == vbus_access_write) {
        data = (data & BitMask) << (8 * ByteWithinDword);
        USB_ctrl->usb_slave_vbus_data_0 = (data >>  0) & 0xFF;
        USB_ctrl->usb_slave_vbus_data_1 = (data >>  8) & 0xFF;
        USB_ctrl->usb_slave_vbus_data_2 = (data >> 16) & 0xFF;
        USB_ctrl->usb_slave_vbus_data_3 = (data >> 24) & 0xFF;
    } else {
        USB_ctrl->usb_slave_vbus_data_0 = 0;
        USB_ctrl->usb_slave_vbus_data_1 = 0;
        USB_ctrl->usb_slave_vbus_data_2 = 0;
        USB_ctrl->usb_slave_vbus_data_3 = 0;
    }
    /*--- Request a VBUS access ---*/
    USB_ctrl->usb_slave_vbus_ctl_reg.i = ((mode == vbus_access_read) ? (1 << 5) : 0) | CfgByteEnables;

    /*--- Wait for access to complete ---*/
    for(timeout_count = 1000 ; timeout_count > 0 && USB_ctrl->usb_slave_vbus_ctl_reg.bits.ready == 0 ; timeout_count--) {
        printk("[wait vbus]");
    }

    if( USB_ctrl->usb_slave_vbus_ctl_reg.bits.ready == 0 ) {
        printk("\n" KERN_ERR "ERROR: VBUS Timeout\n" );
        return 0xFFFFFFFF;
    }

    if( mode == vbus_access_read) {
        unsigned int val;
        val =   ((unsigned int)USB_ctrl->usb_slave_vbus_data_0 <<  0) |
                ((unsigned int)USB_ctrl->usb_slave_vbus_data_1 <<  8) |
                ((unsigned int)USB_ctrl->usb_slave_vbus_data_2 << 16) |
                ((unsigned int)USB_ctrl->usb_slave_vbus_data_3 << 24);
        val = (val >> (8 * ByteWithinDword)) & BitMask;
        return val;
    }

    return 0;
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static unsigned char MipsVbusRdByte(struct _usb_hw *USB_ctrl,  unsigned int VbusAddr ) {
    return (unsigned char)MipsVbusAccess(USB_ctrl,  VbusAddr, 1, vbus_access_read, 0 );
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static unsigned short MipsVbusRdWord(struct _usb_hw *USB_ctrl,  unsigned int VbusAddr ) {
    return (unsigned short)MipsVbusAccess(USB_ctrl,  VbusAddr, 2, vbus_access_read, 0 );
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static unsigned int MipsVbusRdDword(struct _usb_hw *USB_ctrl,  unsigned int VbusAddr ) {
    return MipsVbusAccess(USB_ctrl,  VbusAddr, 4, vbus_access_read, 0 );
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static void MipsVbusWrByte(struct _usb_hw *USB_ctrl,  unsigned int VbusAddr, unsigned char data ) {
    MipsVbusAccess(USB_ctrl,  VbusAddr, 1, vbus_access_write, (unsigned int)data );
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static void MipsVbusWrWord(struct _usb_hw *USB_ctrl,  unsigned int VbusAddr, unsigned short data ) {
    MipsVbusAccess(USB_ctrl,  VbusAddr, 2, vbus_access_write, (unsigned int)data );
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static void MipsVbusWrDword(struct _usb_hw *USB_ctrl,  unsigned int VbusAddr, unsigned int data ) {
    MipsVbusAccess(USB_ctrl,  VbusAddr, 4, vbus_access_write, data );
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
int sangam_board_test_vbus(struct _usb_hw *USB_ctrl) {
    unsigned int  dp[] = { 0x00000000, 0x55555555, 0xAAAAAAAA, 0xFFFFFFFF };  // data patterns
    unsigned int  vbusaddr;       // byte-aligned 32-bit address for VBUS access
    unsigned int  vbusaddrbase;   //
    unsigned int  TestVar[2];
    int     i, j, error;
    unsigned char   bvalw,bvalr;
    unsigned short  wvalw,wvalr;
    unsigned int  dvalw,dvalr;

    error = 0;
    vbusaddrbase = (unsigned int)TestVar;

    printk( "\n Testing byte reads/writes: " );
    for(i = 0; i < 4; i++) {
        for(j = 0; j < sizeof(dp)/sizeof(unsigned int); j++) {
            dvalr = *(volatile unsigned int *)vbusaddrbase & ~(0xFF<<(8*i));  // save current 32-bit value
            bvalw=(unsigned char)dp[j];
            vbusaddr = vbusaddrbase + i;
            MipsVbusWrByte(USB_ctrl, vbusaddr,bvalw);
            bvalr = MipsVbusRdByte(USB_ctrl, vbusaddr);
            dvalw = *(volatile unsigned int *)vbusaddrbase & ~(0xFF<<(8*i));
            if( bvalr != bvalw ) {
                printk( "\n  ERROR: Byte #%d, wr=0x%02x, rd=0x%02x", i, bvalw, bvalr );
                error |= 1;
            }
            if( dvalr != dvalw ) {
                printk( "\n  ERROR: Unexpected data modified: Byte #%d, Was=0x%08x, Is=0x%08x", i, dvalr, dvalw );
                error |= 1;
            }
        }
    }
    if(error==0) printk( "Passed." );

    printk( "\n Testing word reads/writes: " );
    for(i = 0; i < 2; i++) {
        for(j = 0; j < sizeof(dp)/sizeof(unsigned int); j++ )
        {
            dvalr = *(volatile unsigned int *)vbusaddrbase & ~(0xFFFF<<(8*i*2));  // read current 32-bit value
            wvalw=(unsigned short)dp[j];
            vbusaddr = vbusaddrbase + (i*2);
            MipsVbusWrWord(USB_ctrl, vbusaddr,wvalw);
            wvalr = MipsVbusRdWord(USB_ctrl, vbusaddr);
            dvalw = *(volatile unsigned int *)vbusaddrbase & ~(0xFFFF<<(8*i*2));
            if( wvalr != wvalw ) {
                printk( "\n  ERROR: Word #%d, wr=0x%04x, rd=0x%04x", i, wvalw, wvalr );
                error |= 2;
            }
            if( dvalr != dvalw ) {
                printk( "\n  ERROR: Unexpected data modified: Word #%d, Was=0x%08x, Is=0x%08x", i, dvalr, dvalw );
                error |= 2;
            }
        }
    }
    if((error & 2) == 0) printk( "Passed." );

    printk( "\n Testing dword reads/writes: " );
    for(j = 0; j < sizeof(dp)/sizeof(unsigned int); j++) {
        dvalw=dp[j];
        vbusaddr = vbusaddrbase;
        MipsVbusWrDword(USB_ctrl, vbusaddr, dvalw);
        dvalr = MipsVbusRdDword(USB_ctrl, vbusaddr);
        if( dvalr != dvalw ) {
            printk( "\n  ERROR: Dword, wr=0x%08x, rd=0x%08x", dvalw, dvalr );
            error |= 4;
        }
    }
    if((error & 4) == 0) printk( "Passed." );
    return error;
}

EXPORT_SYMBOL(sangam_board_test_vbus);
#endif /*--- #if defined(CONFIG_AVALANCHE_VBUS_TEST) ---*/