// 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
  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
  int data_word_1 = 
    ( 0x73 ) | ( 0x07 << 8 ) | ( adr << 16 ) | ( 0x0c << 24 ) ;
  int data_word_2 = ( 0x08 << 0 ) ;
  // (temporary : 32 bits only)
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , (int) ( data_word_1 & 0xFFFFFFFF ) ) ;
  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 == 0x11 ) break;
  }

  if (i == 10) return -1;

  // Read the value 
  ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_ADD );

  return ret;
}

int pcie40_writefee8( int dev , int adr , int val ) {
  if ( ( adr <= 0 ) || ( adr > 0x7F ) ) return -1 ;
  int ret ;
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_RESET_ADD , 1 << SLC_WFIFO_RESET_BIT ) ;
  if ( ret != 0 ) return -1 ;
  
  int data_word_1 = 
    ( 0x73 ) | ( 0x0a << 8 ) | ( adr << 16 ) | ( ( val & 0xFF ) << 24 ) ;
  int data_word_2 = ( 0x08 << 0 ) ;
  // (temporary : 32 bits only)
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , (int) ( data_word_1 & 0xFFFFFFFF ) ) ;
  if ( ret != 0 ) return -1 ;
  
  return 0 ;
}

int pcie40_readfee32( int dev , int adr , int *valp ) {
  // 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 ;

  // Fill the FIFO with the requested information
  int data_word_1 = 
    ( 0x73 ) | ( 0x0c << 8 ) | ( ( adr & 0xFF ) << 16 ) | ( ( ( adr & 0xFF00 ) >> 8 ) << 24 ) ;
  int data_word_2 = ( 0x08 << 24 ) | ( 0x02 << 16 ) | ( 0x0c0c ) ;

  //
  ret = ecs_write( dev , SLC_BAR , SLC_WFIFO_ADD , ( data_word_1 & 0xFFFFFFFF ) | 
		   ( ( data_word_2 & 0xFFFFFFFF ) >> 32 ) ) ;
  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 == 0x11 ) break;
  }

  if (i == 10) return -1;

  // Read the value 
  ret = ecs_read( dev , SLC_BAR , SLC_RFIFO_ADD );

  return ret;
}

int pcie40_writefee32( int dev , int adr , int val ) { 
  return 0 ;
}

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