#include <stdio.h>
#include <string.h>
#include <libmcpu.h>
#include "../utils/mcpu-asm.h"

#define CHECK(c,m) do{if(!(c)){fprintf(stderr,"FAIL: %s\n",m);return 1;}}while(0)

static const char src[]=
".text\n"
"        LDI     r64@0x00000, 5\n"
"        ADJSP   -16\n"
"        STSP    0, r64@0x00000\n"
"        CALL    demo\n"
"        ADJSP   16\n"
"        MOV     r64@0x00008, r64@0x7fff8\n"
"        HALT\n"
"demo:\n"
"        PUSHFP\n"
"        SETFP\n"
"        ADJSP   -16\n"
"        LDFP    r64@0x00100, 16\n"
"        STFP    -8, r64@0x00100\n"
"        LEAFP   r64@0x00108, -8\n"
"        LEASP   r64@0x00110, 0\n"
"        LDI     r64@0x00118, 24\n"
"        PSUB    r64@0x00120, r64@0x00110, r64@0x00118\n"
"        SETSP   r64@0x00120\n"
"        LDI     r64@0x00128, 37\n"
"        LEASP   r64@0x00130, 0\n"
"        STOREP  r64@0x00130, r64@0x00128\n"
"        LDFP    r64@0x00138, -8\n"
"        LOADP   r64@0x00140, r64@0x00130\n"
"        IADD    r64@0x7fff8, r64@0x00138, r64@0x00140\n"
"        SETSP   r64@0x00110\n"
"        RESTSP\n"
"        POPFP\n"
"        RET\n";

static __mpu_uint64_t
reg_u64( unsigned address )
{
  __mpu_uint64_t v = 0;
  memcpy( &v, __cpu_reg_ptr( 64, address ), sizeof( v ) );
  return( v );
}

static __mpu_int64_t
reg_i64( unsigned address )
{
  __mpu_int64_t v = 0;
  memcpy( &v, __cpu_reg_ptr( 64, address ), sizeof( v ) );
  return( v );
}

static void
set_reg_u64( unsigned address, __mpu_uint64_t v )
{
  memcpy( __cpu_reg_ptr( 64, address ), &v, sizeof( v ) );
}

static void
regop( cpu_operand *o, unsigned address )
{
  memset( o, 0, sizeof( *o ) );
  o->type = CPU_OPERAND_REGISTER;
  o->value.reg.bits = 64;
  o->value.reg.address = address;
}

int
main( void )
{
  cpu_asm_image im;
  cpu_instruction i;
  __mpu_uint64_t ptr, saved_sp, saved_fp, badfp;
  __mpu_uint32_t psw;
  char err[256];

  __cpu_init();
  CHECK( __cpu_stack_alloc( 256 ) == 0, "stack alloc" );
  CHECK( __cpu_sp == 256, "empty stack SP" );
  CHECK( __cpu_fp == 256, "empty stack FP" );

  CHECK( cpu_assemble_text( src, &im, err, sizeof( err ) ) == 0, err );
  CHECK( __cpu_program_set( im.code, im.code_size ) == 0, "program set" );
  __cpu_pc = 0;
  __cpu_set_psw( PSW_CF_MASK | PSW_IF_MASK );
  psw = __cpu_get_psw();
  CHECK( __cpu_run() == CPU_EXEC_SUCCESS, "frame/dynamic stack program run" );
  CHECK( reg_i64( 0x00008 ) == 42, "frame example result" );
  CHECK( reg_i64( 0x7fff8 ) == 42, "canonical return value" );
  CHECK( __cpu_sp == 256, "caller cleanup restored SP" );
  CHECK( __cpu_fp == 256, "callee restored caller FP" );
  cpu_asm_image_free( &im );

  /* LEAFP is based on FP, not on the moving SP. */
  __cpu_sp = 160;
  __cpu_fp = 200;
  memset( &i, 0, sizeof( i ) );
  i.opcode = CPU_OP_LEAFP;
  regop( &i.operand[0], 0x00200 );
  i.operand[1].type = CPU_OPERAND_OFF64;
  i.operand[1].value.off64 = -24;
  __cpu_set_psw( PSW_CF_MASK | PSW_IF_MASK );
  psw = __cpu_get_psw();
  CHECK( __cpu_exec( &i ) == CPU_EXEC_SUCCESS, "LEAFP direct execution" );
  CHECK( __cpu_get_psw() == psw, "LEAFP preserves PSW" );
  ptr = reg_u64( 0x00200 );
  CHECK( __cpu_ptr_segment( ptr ) == CPU_PTR_SEG_STACK, "LEAFP STACK segment" );
  CHECK( __cpu_ptr_offset( ptr ) == 176, "LEAFP uses FP base" );
  CHECK( __cpu_sp == 160, "LEAFP leaves SP unchanged" );
  CHECK( __cpu_fp == 200, "LEAFP leaves FP unchanged" );

  /* SETSP accepts only a STACK PTR64 and is atomic on faults. */
  CHECK( __cpu_ptr_make( &ptr, CPU_PTR_SEG_STACK, 144 ) == 0, "make SETSP STACK ptr" );
  set_reg_u64( 0x00208, ptr );
  memset( &i, 0, sizeof( i ) );
  i.opcode = CPU_OP_SETSP;
  regop( &i.operand[0], 0x00208 );
  CHECK( __cpu_exec( &i ) == CPU_EXEC_SUCCESS, "SETSP valid STACK ptr" );
  CHECK( __cpu_sp == 144, "SETSP installs offset" );
  CHECK( __cpu_fp == 200, "SETSP preserves FP" );

  saved_sp = __cpu_sp;
  CHECK( __cpu_ptr_make( &ptr, CPU_PTR_SEG_DATA, 10 ) == 0, "make DATA ptr" );
  set_reg_u64( 0x00208, ptr );
  CHECK( __cpu_exec( &i ) == CPU_EXEC_POINTER_FAULT, "SETSP rejects DATA ptr" );
  CHECK( __cpu_sp == saved_sp, "SETSP pointer fault atomic" );

  CHECK( __cpu_ptr_make( &ptr, CPU_PTR_SEG_STACK, 300 ) == 0, "make out-of-range STACK ptr" );
  set_reg_u64( 0x00208, ptr );
  CHECK( __cpu_exec( &i ) == CPU_EXEC_STACK_FAULT, "SETSP checks allocated stack size" );
  CHECK( __cpu_sp == saved_sp, "SETSP stack fault atomic" );

  /* PUSHFP/POPFP preserve exact previous FP and validate a restored FP. */
  __cpu_sp = 128;
  __cpu_fp = 216;
  memset( &i, 0, sizeof( i ) );
  i.opcode = CPU_OP_PUSHFP;
  CHECK( __cpu_exec( &i ) == CPU_EXEC_SUCCESS, "PUSHFP" );
  CHECK( __cpu_sp == 120, "PUSHFP moves SP by eight bytes" );
  memcpy( &saved_fp, __cpu_stack_ptr() + 120, 8 );
  CHECK( saved_fp == 216, "PUSHFP stored FP" );

  memset( &i, 0, sizeof( i ) );
  i.opcode = CPU_OP_SETFP;
  CHECK( __cpu_exec( &i ) == CPU_EXEC_SUCCESS, "SETFP" );
  CHECK( __cpu_fp == 120, "SETFP copies SP" );

  memset( &i, 0, sizeof( i ) );
  i.opcode = CPU_OP_RESTSP;
  __cpu_sp = 80;
  CHECK( __cpu_exec( &i ) == CPU_EXEC_SUCCESS, "RESTSP" );
  CHECK( __cpu_sp == 120, "RESTSP copies FP" );

  memset( &i, 0, sizeof( i ) );
  i.opcode = CPU_OP_POPFP;
  CHECK( __cpu_exec( &i ) == CPU_EXEC_SUCCESS, "POPFP" );
  CHECK( __cpu_fp == 216, "POPFP restores caller FP" );
  CHECK( __cpu_sp == 128, "POPFP advances SP by eight bytes" );
  CHECK( __cpu_get_psw() == psw, "FP/SP management preserves PSW" );

  /* Invalid saved FP must not partially change FP or SP. */
  __cpu_sp = 64;
  __cpu_fp = 100;
  badfp = 257;
  memcpy( __cpu_stack_ptr() + 64, &badfp, 8 );
  saved_sp = __cpu_sp;
  saved_fp = __cpu_fp;
  CHECK( __cpu_exec( &i ) == CPU_EXEC_STACK_FAULT, "POPFP rejects invalid saved FP" );
  CHECK( __cpu_sp == saved_sp, "POPFP fault preserves SP" );
  CHECK( __cpu_fp == saved_fp, "POPFP fault preserves FP" );

  __cpu_free_context();
  CHECK( __cpu_fp == 0, "FP cleared by free context" );
  puts( "libcpu frame-pointer/dynamic STACK regression: PASS" );
  return( 0 );
}