#include "PrForceSensorJr3PCI.h"
#include "PrJr3ForcePCIDefn.h"
#include "PrJr3PCIDSP.h"
#include <stdio.h>
#include <hw/pci.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>

static const int JR3_PCI_SUPP_ERROR = -1;

PrForceSensorJr3PCI::PrForceSensorJr3PCI( Channel channel ) 
    : PrForceSensorJr3() {
   
    channel_ = channel;
    getBaseAddr();
    if ( flag() == PR_DRIVER_OK ) {
        uploadDSPCode();
        if ( flag() != PR_DRIVER_OK )
            return;	
        PrForceSensorJr3::initialize();
    }
}

void
PrForceSensorJr3PCI::getBaseAddr( void ) {
    int baseAddrOffset = 0;
    if ( channel_ == ACCEL_SENSOR )
	baseAddrOffset = 0x80000;
    jr3BaseAddr_ = getBaseAddrWithOffset( baseAddrOffset );
    if ( jr3BaseAddr_ == JR3_PCI_SUPP_ERROR )
	flag( PR_DRIVER_ERROR );
}

int
PrForceSensorJr3PCI::getBaseAddrWithOffset( int baseAddrOffset ) {
    unsigned lastbus, version, hardware;
    unsigned busnum, devfuncnum;
    long baseAddr, size;
    long tmp;
    int i;
    int fd;
    char * mmapAddr;

    pci_attach( 0 );

    if ( pci_present( &lastbus, &version, &hardware ) != PCI_SUCCESS ) {
        fprintf( stderr, "No PCI BIOS found\n" );
        flag( PR_DRIVER_ERROR );
        return JR3_PCI_SUPP_ERROR;
    }

    if ( pci_find_device( PR_JR3_PCI_DEVICE_ID, PR_JR3_PCI_VENDOR_ID, 
            0, &busnum, &devfuncnum ) != PCI_SUCCESS ) {
        fprintf( stderr, "Cannot find JR3 interface card.\n" );
        flag( PR_DRIVER_ERROR );
        return JR3_PCI_SUPP_ERROR;
    }

    // Retrieve the base address

    unsigned int PCI_BASE_ADDRESS_0 = 0x10;

    if ( pci_read_config32( busnum, devfuncnum, PCI_BASE_ADDRESS_0, 1,
             (char *)(&baseAddr)) != PCI_SUCCESS ) {
        fprintf( stderr, "Failed to retrieve base address.\n" );
        flag( PR_DRIVER_ERROR );
        return JR3_PCI_SUPP_ERROR;
    }

    baseAddr = baseAddr & ~0xf;

    // Determine the size of the address space.
    // This is done by setting the address register to all ones, then read
    // it back.  The first non-zero bit indicates the size of the address
    // space.  After that we need to restore the original contents of the
    // address register.

    tmp = ~0;
    pci_write_config32( busnum, devfuncnum, PCI_BASE_ADDRESS_0, 1,
            (char *)(&tmp) );
    pci_read_config32( busnum, devfuncnum, PCI_BASE_ADDRESS_0, 1,
            (char *)(&tmp) );
    for ( i = 3; i < 31; i++ ) {
            size = 1 << i;
            if ( tmp & size )
                    break;
    }
    pci_write_config32( busnum, devfuncnum, PCI_BASE_ADDRESS_0, 1,
            (char *)(&baseAddr) );

    //printf( "Base address : %x\n", baseAddr );
    //printf( "Address space size : %d bytes\n", size );

    // Second channel is mapped to 512Kb above the base address.
    baseAddr += baseAddrOffset;
 
    // Now map the device memory to process space.

    mmapAddr = (char *) mmap_device_memory( NULL, size/2, 
                PROT_READ | PROT_WRITE, 0, baseAddr );
    if ( mmapAddr == MAP_FAILED ) {
        printf( "mmap failed : %s\n", strerror( errno ) );
        flag( PR_DRIVER_ERROR );
        return JR3_PCI_SUPP_ERROR;
    }

    //printf( "Map address : %x\n", mmapAddr );

    return (int) mmapAddr;
}

unsigned short 
PrForceSensorJr3PCI::readJr3( int addr ) {
    return readJr3Dm( addr + PR_JR3_PCI_DM_ADDR_MASK );
}

void
PrForceSensorJr3PCI::writeJr3( int addr, int data ) {
    writeJr3Dm( addr + PR_JR3_PCI_DM_ADDR_MASK, data );
}

int 
PrForceSensorJr3PCI::toJr3PciAddr( int addr ) {
    return (addr << 2) + jr3BaseAddr_;
}

unsigned int 
PrForceSensorJr3PCI::readJr3Pm( int addr ) {
    return (*(unsigned short volatile *)(toJr3PciAddr(addr)) << 8) |
            *(unsigned char volatile *)(toJr3PciAddr(addr) + 
            PR_JR3_PCI_NO_ADDR_MASK );
}

void 
PrForceSensorJr3PCI::writeJr3Pm( int addr, int data1, int data2 ) {
    *(int volatile *) toJr3PciAddr(addr) = data1;
    *(int volatile *) (toJr3PciAddr(addr) + PR_JR3_PCI_NO_ADDR_MASK) = data2;
}

// Read and write from the data memory using bus relative addressing

unsigned short
PrForceSensorJr3PCI::readJr3Dm( int addr ) {
    return *(unsigned short volatile *) toJr3PciAddr(addr);
}

void 
PrForceSensorJr3PCI::writeJr3Dm( int addr, int data ) {
    *(int volatile *) toJr3PciAddr(addr) = data; 
}

void
PrForceSensorJr3PCI::resetJr3( void ) {
    writeJr3Dm( PR_JR3_PCI_RESET_ADDR, 0 );
}

void
PrForceSensorJr3PCI::uploadDSPCodeToOneChannel( void ) {
    int count;
    int i = 0;

    // First line is a line count

    count = dspCode[i++];

    // Read in file while the count is not 0xffff

    while ( count != 0xffff ) {
        int addr;

        // After the count is the address

	addr = dspCode[i++];
        // printf( "addr: %4.4x  cnt: %5d\n", addr, count );

        while ( count > 0 ) {
            if ( addr & 0x4000 ) {
                int data;
                
                printf( "Writing to data memory from the IDM file has "
                    "not been tested/encountered.  Check this code "
                    "in PrForceSensorJr3PCI.cpp if you're seeing this "
                    "message and having problems.\n" );

                // Data memory is 16 bits and is on one line.
       
		data = dspCode[i++];
                writeJr3Dm( addr, data );
               
                // Verify the write

                if ( data != readJr3Dm( addr ) ) { 
                    printf( "addr : %4.4x  out: %4.4x  in: %4.4x\n",
                        addr, data, readJr3Dm( data ) );
                    flag( PR_DRIVER_ERROR );
                }
            }
            else {
                int data1, data2;

                // Program memory is 24 bits and is on two lines

		data1 = dspCode[i++];
		data2 = dspCode[i++];
                writeJr3Pm( addr, data1, data2 );
                count -= 2;

                // Verify the write
                int data = (data1 << 8) | (data2 & 0xff);
                if ( readJr3Pm( addr ) != data ) {
                    printf( "addr: %4.4x  out: %6.6x  in: %6.6x\n", addr,
                        data, readJr3Pm( addr ) );
                    flag( PR_DRIVER_ERROR );
                }
            }
            addr++;
        }
	count = dspCode[i++];
    }
}

void
PrForceSensorJr3PCI::uploadDSPCode( void ) {
    unsigned short count_x1 = readJr3( PR_JR3_COUNTX_ADDR );

    int k = 1;
    for ( int i = 0; i < 1000; i++ )
	k = k+ k;   // Just a delay loop

    unsigned short count_x2 = readJr3( PR_JR3_COUNTX_ADDR );

    if ( count_x1 != count_x2 ) {
	// This means the DSP is alive, so we don't have to
	// upload.
	return;
    }

    // Upload to both the low 512Kb and high 512Kb channels.

    resetJr3();
    uploadDSPCodeToOneChannel();

    unsigned int baseAddrOffset = 0;
    if ( channel_ == FORCE_SENSOR ) {
	// We've already initialized the lower 512Kb, so do the
	// upper 512Kb.
	baseAddrOffset = 0x80000;
    }

    unsigned int tempBaseAddr = getBaseAddrWithOffset( baseAddrOffset );
    if ( tempBaseAddr == JR3_PCI_SUPP_ERROR ) {
	printf( "Error uploading DSP code.\n" );
	return;
    }

    unsigned int tmp = jr3BaseAddr_;
    jr3BaseAddr_ = tempBaseAddr;
    uploadDSPCodeToOneChannel();
    jr3BaseAddr_ = tmp;

    // Wait for 4 seconds because the Jr3 needs to "boot up".
    printf( "Loading JR3 DSP code. Waiting for 4 seconds..." );
    fflush( stdout );
    sleep( 4 );
    printf( "Done\n" );
    fflush( stdout );
}

void
PrForceSensorJr3PCI::setFullScales( void ) {
    writeJr3( PR_JR3_FORCE_UNIT_ADDR, PR_JR3_FORCE_UNIT );  // Use Nm 

    writeJr3( PR_JR3_FULL_SCALE_ADDR + 0, PR_JR3_FULL_SCALE_FX );
    writeJr3( PR_JR3_FULL_SCALE_ADDR + 1, PR_JR3_FULL_SCALE_FY );
    writeJr3( PR_JR3_FULL_SCALE_ADDR + 2, PR_JR3_FULL_SCALE_FZ );
    writeJr3( PR_JR3_FULL_SCALE_ADDR + 3, PR_JR3_FULL_SCALE_MX );
    writeJr3( PR_JR3_FULL_SCALE_ADDR + 4, PR_JR3_FULL_SCALE_MY );
    writeJr3( PR_JR3_FULL_SCALE_ADDR + 5, PR_JR3_FULL_SCALE_MZ );

    writeJr3( PR_JR3_COMM0_ADDR, PR_JR3_SET_FULL_SCALE );

    while ( readJr3( PR_JR3_COMM0_ADDR ) != 0 ) {
        // Busy wait
    }
}
