// File for Belle2 slow control access with PCIe40 
#include "pcie40_b2slc.h"
#include <unistd.h>
#include "pcie40_ecs.h"

/* ---------------------------------------------------------------------- *\
   readfee8
   returns -1 in case of error
\* ---------------------------------------------------------------------- */
int pcie40_readfee8( int dev , int adr) {
  // PCIe40
  if ( ( adr <=0 ) || ( adr > 0x7F ) ) return -1 ;
  // Reset the FIFO (Emit and reception)
  unsigned ret = 0 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
		   1 << SLC_WFIFO_RESET_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  // Fill the FIFO with the requested information: write MSB first
  int data_word_1 = 0xFFFE ;
  int data_word_2 = 0x0001 ;
  int data_word_3 = 0x8000 | ( adr & 0x7F ) ;
  int data_word_4 = 0x000c ;
  int data_word_5 = 0xEEEE ;

  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_1 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_2 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_3 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_4 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_5 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;

  // start emit
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
  		   1 << SLC_WFIFO_EMIT_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  // Wait for the result to come back (1 word in the fifo)
  int i ;
  for ( i=0 ; i<10 ; i++ ) {
    usleep( 10 ) ; //10 ms
    ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_STATUS ) ;
    if ( ret == 1 ) break;
  }

  if (i == 10) return -1;

  // Read the value 
  ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_ADD );
  
  // check address consistency
  if ( ( ( ret & 0x7F00 ) >> 8 ) != adr ) return -1 ;

  return ( ret & 0xFF ) ;
}

//==============================================================================
// Write 8b
//==============================================================================
int pcie40_writefee8( int dev , int adr , int val ) {
  // PCIe40
  if ( ( adr <=0 ) || ( adr > 0x7F ) ) return -1 ;
  // Reset the FIFO
  unsigned ret = 0 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
		   1 << SLC_WFIFO_RESET_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  // Fill the FIFO with the requested information: write MSB first
  int data_word_1 = 0xFFFE ;
  int data_word_2 = 0x0001 ;
  int data_word_3 = 0x8000 | ( adr & 0x7F ) ;
  int data_word_4 = 0x0000 | ( val & 0xFF ) ;
  int data_word_5 = 0xEEEE ;

  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_1 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_2 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_3 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_4 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_5 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;

  // start emit
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
  		   1 << SLC_WFIFO_EMIT_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  return 0 ;
}

//==============================================================================
// Read 32b
//==============================================================================
int pcie40_readfee32( int dev , int adr , int *valp ) {
  // PCIe40
  if ( ( adr <0 ) || ( adr > 0xFFFF ) ) return -1 ;
  // Reset the FIFO
  unsigned ret = 0 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
		   1 << SLC_WFIFO_RESET_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  // Fill the FIFO with the requested information: write MSB first
  int data_word_1 = 0xFFFB ;
  int data_word_2 = 0x7000 | ( ( adr & 0xFF00 ) >> 8 ) ;
  int data_word_3 = 0x7000 | ( adr & 0xFF ) ;
  int data_word_5 = 0xEEEE ;

  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_1 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_2 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_3 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_5 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;

  // start emit
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
  		   1 << SLC_WFIFO_EMIT_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  // Wait for the result to come back
  int i ;
  for ( i=0 ; i<10 ; i++ ) {
    usleep( 10 ) ; //10 ms
    ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_STATUS ) ;
    if ( ret == 4 ) break;
  }

  if (i == 10) return -1;

  // Read the value 
  int ret1 = ecs_read( dev , SLC_BAR , SLC_RFIFO_ADD );
  int ret2 = ecs_read( dev , SLC_BAR , SLC_RFIFO_ADD );

  int add1 = ( ret1 & 0x7F000000 ) >> 24 ;
  int add2 = ( ret1 & 0x7F00 ) >> 8 ;
  int add3 = ( ret2 & 0x7F000000 ) >> 24 ;
  int add4 = ( ret2 & 0x7F00 ) >> 8 ;

  if ( ( add1 != 0x6C ) || ( add2 != 0x6D ) || ( add3 != 0x6A ) || ( add4 != 0x6B ) )
    return -1 ;

  int res1 = ( ret1 & 0xFF0000 ) >> 16 ;
  int res2 = ( ret1 & 0xFF ) ;
  int res3 = ( ret2 & 0xFF0000 ) >> 16 ;
  int res4 = ( ret2 & 0xFF ) ;
 
  return ( res2 << 24 ) | ( res1 << 16 ) | ( res4 << 8 ) | ( res3 ) ;
}

//==============================================================================
// Read 32b
//==============================================================================
int pcie40_writefee32( int dev , int adr , int val ) { 
  // PCIe40
  if ( ( adr <0 ) || ( adr > 0xFFFF ) ) return -1 ;
  // Reset the FIFO
  unsigned ret = 0 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
		   1 << SLC_WFIFO_RESET_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  int val1 = ( val & 0xFF ) ;
  int val2 = ( val & 0xFF00 ) >> 8 ;
  int val3 = ( val & 0xFF0000 ) >> 16 ;
  int val4 = ( val & 0xFF000000 ) >> 24 ;

  // Fill the FIFO with the requested information: write MSB first
  int data_word_1 = 0xFFFB ;
  int data_word_2 = 0x7000 | ( ( adr & 0xFF00 ) >> 8 ) ;
  int data_word_3 = 0x7000 | ( adr & 0xFF ) ;
  int data_word_4 = 0x7000 | val1 ;
  int data_word_5 = 0x7000 | val2 ;
  int data_word_6 = 0x7000 | val3 ;
  int data_word_7 = 0x7000 | val4 ;
  int data_word_8 = 0xEEEE ;


  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_1 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_2 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_3 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_4 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_5 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_6 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_7 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , 
		   (int) ( data_word_8 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;

  // start emit
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 
  		   1 << SLC_WFIFO_EMIT_BIT ) ;
  if ( ret != 0 ) return -1 ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 0 ) ;
  if ( ret != 0 ) return -1 ;

  return ret;
}

int pcie40_writestream( int dev , char * filename ) {
  return 0 ;
}