#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <unistd.h>
#include <libmcpu.h>
#include <cpu-context.h>
#include <cpu-syscalls.h>

__thread struct __cpu_context cctx;
__thread struct __cpu_context *pcctx;

static __mpu_uint32_t
__cpu_get_mpu_psw( void )
{
  __mpu_uint32_t psw = 0;
  if( __mpu_gta() ) psw |= PSW_AF_MASK;
  if( __mpu_gtc() ) psw |= PSW_CF_MASK;
  if( __mpu_gto() ) psw |= PSW_OF_MASK;
  if( __mpu_gts() ) psw |= PSW_SF_MASK;
  if( __mpu_gtp() ) psw |= PSW_PF_MASK;
  if( __mpu_gtz() ) psw |= PSW_ZF_MASK;
  if( __mpu_gtr() ) psw |= PSW_RF_MASK;
  if( __mpu_gtv() ) psw |= PSW_VF_MASK;
  if( __mpu_gtdom() ) psw |= PSW_DOMF_MASK;
  if( __mpu_gtsng() ) psw |= PSW_SNGF_MASK;
  if( __mpu_gtovf() ) psw |= PSW_OVFF_MASK;
  if( __mpu_gtudf() ) psw |= PSW_UDFF_MASK;
  if( __mpu_gttls() ) psw |= PSW_TLSF_MASK;
  if( __mpu_gtpls() ) psw |= PSW_PLSF_MASK;
  if( __mpu_gtind() ) psw |= PSW_INDF_MASK;
  if( __mpu_gtinx() ) psw |= PSW_INXF_MASK;
  return( psw );
}

static void
__cpu_set_mpu_psw( __mpu_uint32_t p )
{
#define SF(mask,st,cl) do{if(p&(mask)) st(); else cl();}while(0)
  SF( PSW_AF_MASK, __mpu_sta, __mpu_cla );
  SF( PSW_CF_MASK, __mpu_stc, __mpu_clc );
  SF( PSW_OF_MASK, __mpu_sto, __mpu_clo );
  SF( PSW_SF_MASK, __mpu_sts, __mpu_cls );
  SF( PSW_PF_MASK, __mpu_stp, __mpu_clp );
  SF( PSW_ZF_MASK, __mpu_stz, __mpu_clz );
  SF( PSW_RF_MASK, __mpu_str, __mpu_clr );
  SF( PSW_VF_MASK, __mpu_stv, __mpu_clv );
  SF( PSW_DOMF_MASK, __mpu_stdom, __mpu_cldom );
  SF( PSW_SNGF_MASK, __mpu_stsng, __mpu_clsng );
  SF( PSW_OVFF_MASK, __mpu_stovf, __mpu_clovf );
  SF( PSW_UDFF_MASK, __mpu_studf, __mpu_cludf );
  SF( PSW_TLSF_MASK, __mpu_sttls, __mpu_cltls );
  SF( PSW_PLSF_MASK, __mpu_stpls, __mpu_clpls );
  SF( PSW_INDF_MASK, __mpu_stind, __mpu_clind );
  SF( PSW_INXF_MASK, __mpu_stinx, __mpu_clinx );
#undef SF
}

/*
 * Host backing store.  Architectural size and host mapping capacity are kept
 * separate deliberately: the latter is rounded to the host page size only so
 * mmap()/mprotect()/munmap() can operate on complete pages.  CPU bounds checks
 * must always use the architectural size fields.
 */
static size_t
host_page_size( void )
{
  static size_t page;
  long n;
  if( page ) return( page );
  n = sysconf( _SC_PAGESIZE );
  if( n <= 0 ) return( 0 );
  page = (size_t)n;
  return( page );
}

static void
mem_unmap( __mpu_UNIT_t **p, __mpu_uint64_t *sz, __mpu_uint64_t *cap )
{
  if( *p && *cap && (__mpu_uint64_t)(size_t)*cap == *cap )
    (void)munmap( (void *)*p, (size_t)*cap );
  *p = 0;
  *sz = 0;
  *cap = 0;
}

static int mem_alloc( __mpu_UNIT_t **p, __mpu_uint64_t *sz, __mpu_uint64_t *cap,
                     __mpu_uint64_t n)
{
  size_t page, need, mapped, max = (size_t)-1;
  void *q;

  mem_unmap( p, sz, cap );
  if( !n ) return( 0 );
  if( (__mpu_uint64_t)(size_t)n != n ) return( -1 );

  page = host_page_size();
  if( !page ) return( -1 );
  need = (size_t)n;
  if( need > max - ( page - 1U ) ) return( -1 );
  mapped = ( ( need + page - 1U ) / page ) * page;
  if( !mapped ) return( -1 );

  q = mmap( (void *)0, mapped, PROT_READ | PROT_WRITE,
         MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
  if( q == MAP_FAILED ) return( -1 );

  /* Anonymous mappings are zero-filled by the host kernel. */
  *p = (__mpu_UNIT_t *)q;
  *sz = n;
  *cap = (__mpu_uint64_t)mapped;
  return( 0 );
}

void
__cpu_init( void )
{
  pcctx = &cctx;
  __cpu_cwd_context_free();
  __cpu_fd_context_free();
  mem_unmap( &cctx._program, &cctx._program_size, &cctx._program_capacity );
  mem_unmap( &cctx._data, &cctx._data_size, &cctx._data_capacity );
  mem_unmap( &cctx._stack, &cctx._stack_size, &cctx._stack_capacity );
  mem_unmap( &cctx._control, &cctx._control_size, &cctx._control_capacity );
  mem_unmap( &cctx._input, &cctx._input_size, &cctx._input_capacity );
  mem_unmap( &cctx._output, &cctx._output_size, &cctx._output_capacity );
  mem_unmap( &cctx._trace, &cctx._trace_size, &cctx._trace_capacity );
  mem_unmap( &cctx._heap, &cctx._heap_size, &cctx._heap_capacity );
  memset( &cctx, 0, sizeof( cctx ) );
  pcctx = &cctx;
  __cpu_fd_context_init();
  __cpu_cwd_context_init();
  __mpu_init();
}

void
__cpu_free_context( void )
{
  if( !pcctx ) pcctx = &cctx;
  __cpu_cwd_context_free();
  __cpu_fd_context_free();
  mem_unmap( &pcctx->_program, &pcctx->_program_size, &pcctx->_program_capacity );
  mem_unmap( &pcctx->_data, &pcctx->_data_size, &pcctx->_data_capacity );
  mem_unmap( &pcctx->_stack, &pcctx->_stack_size, &pcctx->_stack_capacity );
  mem_unmap( &pcctx->_control, &pcctx->_control_size, &pcctx->_control_capacity );
  mem_unmap( &pcctx->_input, &pcctx->_input_size, &pcctx->_input_capacity );
  mem_unmap( &pcctx->_output, &pcctx->_output_size, &pcctx->_output_capacity );
  mem_unmap( &pcctx->_trace, &pcctx->_trace_size, &pcctx->_trace_capacity );
  mem_unmap( &pcctx->_heap, &pcctx->_heap_size, &pcctx->_heap_capacity );
  pcctx->_heap_brk = 0;
  pcctx->_heap_limit = 0;
  pcctx->_pc = 0;
  pcctx->_sp = 0;
  pcctx->_fp = 0;
  pcctx->_psw = 0;
  pcctx->_stop_reason = CPU_STOP_NONE;
  pcctx->_exit_status = 0;
  __mpu_free_context();
}

__mpu_uint64_t *
__ptr_cpu_pc( void )
{
  return( &cctx._pc );
}

__mpu_uint64_t *
__ptr_cpu_sp( void )
{
  return( &cctx._sp );
}

__mpu_uint64_t *
__ptr_cpu_fp( void )
{
  return( &cctx._fp );
}

__mpu_UNIT_t *
__ptr_cpu_registers( void )
{
  return( &cctx._regs[0] );
}

int
__cpu_program_alloc( __mpu_uint64_t n )
{
  return( mem_alloc( &cctx._program, &cctx._program_size, &cctx._program_capacity, n ) );
}

void
__cpu_program_free( void )
{
  mem_unmap( &cctx._program, &cctx._program_size, &cctx._program_capacity );
  cctx._pc = 0;
}

int
__cpu_program_protect( void )
{
  if( !cctx._program ) return( cctx._program_size == 0 ? 0 : -1 );
  if( !cctx._program_capacity ||
     (__mpu_uint64_t)(size_t)cctx._program_capacity != cctx._program_capacity)
    return( -1 );
  return( mprotect( (void *)cctx._program, (size_t)cctx._program_capacity, PROT_READ ) == 0 ? 0 : -1 );
}

__mpu_UNIT_t *
__cpu_program_ptr( void )
{
  return( cctx._program );
}

__mpu_uint64_t
__cpu_program_size( void )
{
  return( cctx._program_size );
}

int
__cpu_program_set( const __mpu_UNIT_t *d, __mpu_uint64_t n )
{
  if( __cpu_program_alloc( n ) ) return( -1 );
  if( n && d ) memcpy( cctx._program, d, (size_t)n );
  if( __cpu_program_protect() )
  {
    __cpu_program_free();
    return( -1 );
  }
  return( 0 );
}

int
__cpu_data_alloc( __mpu_uint64_t n )
{
  return( mem_alloc( &cctx._data, &cctx._data_size, &cctx._data_capacity, n ) );
}

void
__cpu_data_free( void )
{
  mem_unmap( &cctx._data, &cctx._data_size, &cctx._data_capacity );
}

__mpu_UNIT_t *
__cpu_data_ptr( void )
{
  return( cctx._data );
}

__mpu_uint64_t
__cpu_data_size( void )
{
  return( cctx._data_size );
}

int
__cpu_data_set( const __mpu_UNIT_t *d, __mpu_uint64_t n )
{
  if( __cpu_data_alloc( n ) ) return( -1 );
  if( n && d ) memcpy( cctx._data, d, (size_t)n );
  return( 0 );
}

int
__cpu_stack_alloc( __mpu_uint64_t n )
{
  int rc = mem_alloc( &cctx._stack, &cctx._stack_size, &cctx._stack_capacity, n );
  cctx._sp = ( rc == 0 ) ? n : 0;
  cctx._fp = ( rc == 0 ) ? n : 0;
  return( rc );
}

void
__cpu_stack_free( void )
{
  mem_unmap( &cctx._stack, &cctx._stack_size, &cctx._stack_capacity );
  cctx._sp = 0;
  cctx._fp = 0;
}

__mpu_UNIT_t *
__cpu_stack_ptr( void )
{
  return( cctx._stack );
}

__mpu_uint64_t
__cpu_stack_size( void )
{
  return( cctx._stack_size );
}

#define DEFINE_SEGMENT_API(name,field) \
int __cpu_##name##_alloc(__mpu_uint64_t n){return mem_alloc(&cctx._##field,&cctx._##field##_size,&cctx._##field##_capacity,n);} \
void __cpu_##name##_free(void){mem_unmap(&cctx._##field,&cctx._##field##_size,&cctx._##field##_capacity);} \
__mpu_UNIT_t *__cpu_##name##_ptr(void){return cctx._##field;} \
__mpu_uint64_t __cpu_##name##_size(void){return cctx._##field##_size;} \
int __cpu_##name##_set(const __mpu_UNIT_t *d,__mpu_uint64_t n){if(__cpu_##name##_alloc(n))return -1;if(n&&d)memcpy(cctx._##field,d,(size_t)n);return 0;}

DEFINE_SEGMENT_API( control, control )
DEFINE_SEGMENT_API( input, input )
DEFINE_SEGMENT_API( output, output )
DEFINE_SEGMENT_API( trace, trace )
#undef DEFINE_SEGMENT_API

static int
__cpu_heap_grow( __mpu_uint64_t new_size )
{
  size_t        page, need, mapped, max = (size_t)-1;
  __mpu_UNIT_t *p;

  if( new_size <= cctx._heap_capacity ) return( 0 );
  if( (__mpu_uint64_t)(size_t)new_size != new_size ) return( -1 );

  page = host_page_size();
  if( !page ) return( -1 );

  need = (size_t)new_size;
  if( need > max - (page - 1U) ) return( -1 );

  mapped = ((need + page - 1U) / page) * page;
  if( !mapped ) return( -1 );

  p = (__mpu_UNIT_t *)mmap( (void *)0, mapped, PROT_READ | PROT_WRITE,
                             MAP_PRIVATE | MAP_ANONYMOUS, -1, 0 );
  if( p == (__mpu_UNIT_t *)MAP_FAILED ) return( -1 );

  if( cctx._heap && cctx._heap_size )
    memcpy( p, cctx._heap, (size_t)cctx._heap_size );

  if( cctx._heap && cctx._heap_capacity )
    (void)munmap( (void *)cctx._heap, (size_t)cctx._heap_capacity );

  cctx._heap = p;
  cctx._heap_capacity = (__mpu_uint64_t)mapped;

  return( 0 );

} /* End of __cpu_heap_grow() */


int
__cpu_heap_alloc( __mpu_uint64_t initial_size, __mpu_uint64_t maximum_size )
{
  int  rc;

  if( maximum_size < initial_size ||
      maximum_size < CPU_HEAP_BEGIN || initial_size < CPU_HEAP_BEGIN )
    return( -1 );

  rc = mem_alloc( &cctx._heap, &cctx._heap_size,
                  &cctx._heap_capacity, initial_size );
  if( rc )
  {
    cctx._heap_brk = 0;
    cctx._heap_limit = 0;
    return( rc );
  }

  cctx._heap_brk = CPU_HEAP_BEGIN;
  cctx._heap_limit = maximum_size;

  return( 0 );

} /* End of __cpu_heap_alloc() */


void
__cpu_heap_free( void )
{
  mem_unmap( &cctx._heap, &cctx._heap_size, &cctx._heap_capacity );
  cctx._heap_brk = 0;
  cctx._heap_limit = 0;

} /* End of __cpu_heap_free() */


__mpu_uint64_t
__cpu_heap_brk( __mpu_uint64_t new_brk )
{
  __mpu_uint64_t  old_brk = cctx._heap_brk;

  if( !cctx._heap || cctx._heap_limit < CPU_HEAP_BEGIN )
    return( old_brk );

  if( new_brk < CPU_HEAP_BEGIN || new_brk > cctx._heap_limit )
    return( old_brk );

  if( new_brk == old_brk ) return( old_brk );

  if( new_brk < old_brk )
  {
    memset( cctx._heap + (size_t)new_brk, 0,
            (size_t)(old_brk - new_brk) );
    cctx._heap_brk = new_brk;
    return( new_brk );
  }

  if( new_brk <= cctx._heap_size )
  {
    cctx._heap_brk = new_brk;
    return( new_brk );
  }

  if( new_brk <= cctx._heap_capacity )
  {
    memset( cctx._heap + (size_t)cctx._heap_size, 0,
            (size_t)(new_brk - cctx._heap_size) );
    cctx._heap_size = new_brk;
    cctx._heap_brk = new_brk;
    return( new_brk );
  }

  if( __cpu_heap_grow( new_brk ) != 0 ) return( old_brk );

  cctx._heap_size = new_brk;
  cctx._heap_brk = new_brk;

  return( new_brk );

} /* End of __cpu_heap_brk() */


__mpu_UNIT_t *
__cpu_heap_ptr( void )
{
  return( cctx._heap );
}

__mpu_uint64_t
__cpu_heap_size( void )
{
  return( cctx._heap_size );
}

__mpu_uint64_t
__cpu_heap_limit( void )
{
  return( cctx._heap_limit );
}

cpu_stop_reason
__cpu_get_stop_reason( void )
{
  return( cctx._stop_reason );
}

__mpu_int32_t
__cpu_get_exit_status( void )
{
  return( cctx._exit_status );
}

void
__cpu_clear_stop_reason( void )
{
  cctx._stop_reason = CPU_STOP_NONE;
  cctx._exit_status = 0;
}

__mpu_uint32_t
__cpu_get_psw( void )
{
  return( ( __cpu_get_mpu_psw() & PSW_MPU_MASK ) | ( cctx._psw & PSW_CPU_MASK ) );
}

void
__cpu_set_psw( __mpu_uint32_t p )
{
  __cpu_set_mpu_psw( p & PSW_MPU_MASK );
  cctx._psw = p & PSW_CPU_MASK;
}

void
__cpu_clear_psw( void )
{
  __mpu_clear_mflags();
  cctx._psw = 0;
}

int
__cpu_gth( void )
{
  return( (int)__CFLAG( PSW_HF_MASK, PSW_HF_SHIFT ) );
}

int
__cpu_gtt( void )
{
  return( (int)__CFLAG( PSW_TF_MASK, PSW_TF_SHIFT ) );
}

int
__cpu_gti( void )
{
  return( (int)__CFLAG( PSW_IF_MASK, PSW_IF_SHIFT ) );
}

void
__cpu_sth( void )
{
  __SET_CFLAG( PSW_HF_MASK );
}

void
__cpu_stt( void )
{
  __SET_CFLAG( PSW_TF_MASK );
}

void
__cpu_sti( void )
{
  __SET_CFLAG( PSW_IF_MASK );
}

void
__cpu_clh( void )
{
  __CLEAR_CFLAG( PSW_HF_MASK );
}

void
__cpu_clt( void )
{
  __CLEAR_CFLAG( PSW_TF_MASK );
}

void
__cpu_cli( void )
{
  __CLEAR_CFLAG( PSW_IF_MASK );
}

void
__cpu_cmh( void )
{
  __INVERT_CFLAG( PSW_HF_MASK );
}

void
__cpu_cmt( void )
{
  __INVERT_CFLAG( PSW_TF_MASK );
}

void
__cpu_cmi( void )
{
  __INVERT_CFLAG( PSW_IF_MASK );
}

int
__cpu_gtmode( void )
{
  return( (int)( ( cctx._psw & PSW_MODE_MASK ) >> PSW_MODE_SHIFT ) );
}

int
__cpu_stmode( unsigned int m )
{
  if( m > 3 ) return( -1 );
  cctx._psw &= ~PSW_MODE_MASK;
  cctx._psw |= ( (__mpu_uint32_t)m << PSW_MODE_SHIFT ) & PSW_MODE_MASK;
  return( 0 );
}

int
__cpu_ptr_make( __mpu_uint64_t *ptr, unsigned int segment, __mpu_uint64_t offset )
{
  if( !ptr || segment > CPU_PTR_SEG_MAX || offset > CPU_PTR_OFFSET_MASK ) return( -1 );
  *ptr = ( (__mpu_uint64_t)segment << CPU_PTR_SEG_SHIFT ) | offset;
  return( 0 );
}

unsigned int
__cpu_ptr_segment( __mpu_uint64_t ptr )
{
  return( (unsigned int)( ( ptr & CPU_PTR_SEG_MASK ) >> CPU_PTR_SEG_SHIFT ) );
}

__mpu_uint64_t
__cpu_ptr_offset( __mpu_uint64_t ptr )
{
  return( ptr & CPU_PTR_OFFSET_MASK );
}