/* This file is in the public domain. */

#include <stdlib.h>
#include <string.h>
#include <strings.h>

#include "machine.h"
#include "fnprintf.h"

static const char *regs[]
 = {  "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7",
      "r8", "r9","r10","r11","r12", "sp", "lr", "pc" };
static const char *conds[]
 = { "eq", "ne", "cs", "cc", "mi", "pl", "vs", "vc",
     "hi", "ls", "ge", "lt", "gt", "le", "", 0 };
#define VALID_SPEC_REG 0: case 1: case 2: case 3: case 5: case 6: case 7: case 8: case 9: case 16: case 20
static const char *spec_reg[]
 = { [0] = "apsr",
     [1] = "iapsr",
     [2] = "eapsr",
     [3] = "xpsr",
     [5] = "ipsr",
     [6] = "epsr",
     [7] = "iepsr",
     [8] = "msp",
     [9] = "psp",
     [16] = "primask",
     [20] = "control" };
#define WRAP32(v) ((((unsigned int)(v)-1U)&31U)+1)

static unsigned int sign_extend(unsigned int v, unsigned int signbit)
{
 return((v&signbit)?(v|((~0U)&~(signbit-1))):v);
}

#if 0
static void addr_mode_1(void (*fn)(char), unsigned long int opc, const char *mnem)
{
 unsigned int v;
 unsigned int n;

 if ((opc & 0x02000090) == 0x00000090) abort();
 fnprintf(fn,"%s%s%-7s%s, %s, ",mnem,conds[opc>>28],(opc&0x00100000)?"s":"",regs[(opc>>12)&15],regs[(opc>>16)&15]);
 if (opc & 0x02000000)
  { v = opc & 0x000000ff;
    n = (opc >> 3) & 30;
    if (n) v = (v >> n) | ((v << (32 - n)) & 0xffffffffU);
    if (v < 256)
     { fnprintf(fn,"#%u",v);
     }
    else
     { fnprintf(fn,"#0x%x",v);
     }
  }
 else
  { switch (opc & 0x00000070)
     { case 0x00000000:
	  fnprintf(fn,"%s",regs[opc&15]);
	  if ((opc & 0x00000ff0) != 0x00000000) fnprintf(fn,", lsl #%ld",(opc>>7)&31);
	  break;
       case 0x00000010:
	  fnprintf(fn,"%s, lsl %s",regs[opc&15],regs[(opc>>8)&15]);
	  break;
       case 0x00000020:
	  fnprintf(fn,"%s, lsr #%d",regs[opc&15],WRAP32((opc>>7)&31));
	  break;
       case 0x00000030:
	  fnprintf(fn,"%s, lsr %s",regs[opc&15],regs[(opc>>8)&15]);
	  break;
       case 0x00000040:
	  fnprintf(fn,"%s, asr #%d",regs[opc&15],WRAP32((opc>>7)&31));
	  break;
       case 0x00000050:
	  fnprintf(fn,"%s, asr %s",regs[opc&15],regs[(opc>>8)&15]);
	  break;
       case 0x00000060:
	  if ((opc & 0x00000ff0) == 0x00000060)
	   { fnprintf(fn,"%s, rrx",regs[opc&15]);
	   }
	  else
	   { fnprintf(fn,"%s, ror #%ld",regs[opc&15],(opc>>7)&31);
	   }
	  break;
       case 0x00000070:
	  fnprintf(fn,"%s, ror %s",regs[opc&15],regs[(opc>>8)&15]);
	  break;
     }
  }
}
#endif

#if 0
static void addr_mode_4(void (*fn)(char), unsigned long int opc)
{
 const char *pref;
 int i;

 fnprintf(fn,"%sm%s",(opc&0x00100000)?"ld":"st",conds[opc>>28]);
 if ((opc & 0x000f0000) == 0x000d0000)
  { switch (opc & 0x01900000)
     { case 0x00000000: case 0x01900000: fnprintf(fn,"ed"); break;
       case 0x00100000: case 0x01800000: fnprintf(fn,"fa"); break;
       case 0x00800000: case 0x01100000: fnprintf(fn,"ea"); break;
       case 0x00900000: case 0x01000000: fnprintf(fn,"fd"); break;
     }
  }
 else
  { switch (opc & 0x01800000)
     { case 0x00000000: fnprintf(fn,"da"); break;
       case 0x00800000: fnprintf(fn,"ia"); break;
       case 0x01000000: fnprintf(fn,"db"); break;
       case 0x01800000: fnprintf(fn,"ib"); break;
     }
  }
 fnprintf(fn,"     %s%s, {"/*}*/,regs[(opc>>16)&15],(opc&0x00200000)?"!":"");
 pref = "";
 for (i=0;i<16;i++)
  { if ((opc >> i) & 1)
     { fnprintf(fn,"%s%s",pref,regs[i]);
       pref = ",";
     }
  }
 fnprintf(fn,/*{*/"}");
}
#endif

#if 0
static int do_dis_arm(ADDR addr, void (*fn)(char), CFLAG flg)
{
 unsigned long int opc;

 addr -= corebase;
 if (addr+4 > coresize) return(-1);
 opc = corefetchu(addr,4);
 do
  { if ((opc & 0xf0000000) == 0xf0000000)
     {
     }
    else
     { if ((opc & 0x0e000000) == 0x08000000)
	{ addr_mode_4(fn,opc);
	  break;
	}
       else if ((opc & 0x0de00000) == 0x00000000)
	{ if ((opc & 0x02000090) != 0x00000090)
	   { addr_mode_1(fn,opc,"and");
	     break;
	   }
	}
     }
    fnprintf(fn,"<unknown %08lx>",opc);
  } while (0);
 if (addr & 3)
  { fnprintf(fn,"  {misaligned}");
    return(1);
  }
 if (flg & CFF_INST_PARALLEL)
  { fnprintf(fn,"  {parallel}");
    return(1);
  }
 flags[addr+1] = CF_PREV;
 flags[addr+2] = CF_PREV;
 flags[addr+3] = CF_PREV;
 return(4);
}
#endif

static void print_reg_mask(void (*fn)(char), unsigned int m)
{
 static const char *pref;
 int i;

 fnprintf(fn,"{"/*}*/);
 pref = "";
 for (i=0;i<16;i++)
  { if ((m >> i) & 1)
     { fnprintf(fn,"%s%s",pref,regs[i]);
       pref = ",";
     }
  }
 fnprintf(fn,/*{*/"}");
}

static int do_dis_armv6m(ADDR addr, void (*fn)(char), CFLAG flg)
{
 unsigned long int opc;
 unsigned long int opc2;
 int rv;
 int i;
 const char *s;

 addr -= corebase;
 if (addr+2 > coresize) return(-1);
 opc = corefetchu(addr,2);
 rv = 2;
#define DONE break <"done">
 do <"done">
  { switch (opc >> 11)
     { case 0:
	  if ((opc >> 6) & 31)
	   { fnprintf(fn,"lsl     %s, %s, #%d",regs[opc&7],regs[(opc>>3)&7],WRAP32((opc>>6)&31));
	   }
	  else
	   { fnprintf(fn,"movs    %s, %s",regs[opc&7],regs[(opc>>3)&7]);
	   }
	  DONE;
       case 1:
	  fnprintf(fn,"lsr     %s, %s, #%d",regs[opc&7],regs[(opc>>3)&7],WRAP32((opc>>6)&31));
	  DONE;
       case 2:
	  fnprintf(fn,"asr     %s, %s, #%d",regs[opc&7],regs[(opc>>3)&7],WRAP32((opc>>6)&31));
	  DONE;
       case 3:
	  switch (opc >> 9)
	   { case 12:
		fnprintf(fn,"add     %s, %s, %s",regs[opc&7],regs[(opc>>3)&7],regs[(opc>>6)&7]);
		DONE;
	     case 13:
		fnprintf(fn,"sub     %s, %s, %s",regs[opc&7],regs[(opc>>3)&7],regs[(opc>>6)&7]);
		DONE;
	     case 14:
		fnprintf(fn,"add     %s, %s, #%lu",regs[opc&7],regs[(opc>>3)&7],(opc>>6)&3);
		DONE;
	     case 15:
		fnprintf(fn,"sub     %s, %s, #%lu",regs[opc&7],regs[(opc>>3)&7],(opc>>6)&3);
		DONE;
	   }
	  break;
       case 4: s = "mov";   if (0) {
       case 5: s = "cmp"; } if (0) {
       case 5: s = "add"; } if (0) {
       case 5: s = "sub"; }
	  fnprintf(fn,"%s     %s, #%ld",s,regs[(opc>>8)&7],opc&255);
	  DONE;
       case 8:
	  switch ((opc >> 6) & 31)
	   { case  0: s = "and";   if (0) {
	     case  1: s = "eor"; } if (0) {
	     case  2: s = "lsl"; } if (0) {
	     case  3: s = "lsr"; } if (0) {
	     case  4: s = "asr"; } if (0) {
	     case  5: s = "adc"; } if (0) {
	     case  6: s = "sbc"; } if (0) {
	     case  7: s = "ror"; } if (0) {
	     case  8: s = "tst"; } if (0) {
	     case  9: s = "neg"; } if (0) {
	     case 10: s = "cmp"; } if (0) {
	     case 11: s = "cmn"; } if (0) {
	     case 12: s = "orr"; } if (0) {
	     case 13: s = "mul"; } if (0) {
	     case 14: s = "bic"; } if (0) {
	     case 15: s = "mvn"; }
		fnprintf(fn,"%s     %s, %s",s,regs[opc&7],regs[(opc>>3)&7]);
		DONE;
	     case 16: case 17: case 18: case 19:
		if ((opc & 0x00ff) != 0x00ff)
		 { fnprintf(fn,"add     %s, %s ! high-regs",regs[((opc>>4)&8)|(opc&7)],regs[(opc>>3)&15]);
		   DONE;
		 }
		break;
	     case 20:
		break;
	     case 21: case 22: case 23:
		if ((opc & 0x00ff) != 0x00ff)
		 { fnprintf(fn,"cmp     %s, %s ! high-regs",regs[((opc>>4)&8)|(opc&7)],regs[(opc>>3)&15]);
		   DONE;
		 }
		break;
	     case 24: case 25: case 26: case 27:
		fnprintf(fn,"mov     %s, %s ! high-regs",regs[((opc>>4)&8)|(opc&7)],regs[(opc>>3)&15]);
		DONE;
	     case 28: case 29:
		if ((opc & 7) == 0)
		 { fnprintf(fn,"bx      %s",regs[(opc>>3)&15]);
		   DONE;
		 }
		break;
	     case 30: case 31:
		if ((opc & 7) == 0)
		 { fnprintf(fn,"blx     %s",regs[(opc>>3)&15]);
		   DONE;
		 }
		break;
	   }
	  break;
       case 9:
	  fnprintf(fn,"ldr     %s, ",regs[(opc>>8)&7]);
	  print_symbol_or_hex(fn,corebase+((addr+4)&~3)+((opc&255)*4));
	  DONE;
       case 10:
	  switch ((opc >> 9) & 3)
	   { case 0: s = "str";     if (0) {
	     case 1: s = "strh";  } if (0) {
	     case 2: s = "strb";  } if (0) {
	     case 3: s = "strsb"; }
		fnprintf(fn,"%-8s%s, [%s, %s]",s,regs[opc&7],regs[(opc>>3)&7],regs[(opc>>6)&7]);
		DONE;
	   }
	  break;
       case 11:
	  fnprintf(fn,"ldr%.2s    %s, [%s, %s]","  h b sh"+((opc>>8)&6),regs[opc&7],regs[(opc>>3)&7],regs[(opc>>6)&7]);
	  DONE;
       case 12: i = 4; s = "str";    if (0) {
       case 13: i = 4; s = "ldr";  } if (0) {
       case 14: i = 6; s = "strb"; } if (0) {
       case 15: i = 6; s = "ldrb"; } if (0) {
       case 16: i = 5; s = "strh"; } if (0) {
       case 17: i = 5; s = "ldrh"; }
	  fnprintf(fn,"%-8s%s, [%s",s,regs[opc&7],regs[(opc>>3)&7]);
	  if (opc & 0x07c0) fnprintf(fn,", #%ld",(opc&0x07c0)>>i);
	  fnprintf(fn,"]");
	  DONE;
       case 18:
	  fnprintf(fn,"str     %s, [sp, #%ld]",regs[(opc>>8)&7],opc&255);
	  DONE;
       case 19:
	  fnprintf(fn,"ldr     %s, [sp",regs[(opc>>8)&7]);
	  if (opc & 0x00ff) fnprintf(fn,", #%ld",opc&0xff);
	  fnprintf(fn,"]");
	  DONE;
       case 20:
	  fnprintf(fn,"adr     %s, ",regs[(opc>>8)&7]);
	  print_symbol_or_hex(fn,corebase+((addr+4)&~3)+((opc&255)*4));
	  DONE;
       case 21:
	  fnprintf(fn,"add     %s, sp, #%lu",regs[(opc>>8)&7],(opc&255)*4);
	  DONE;
       case 22:
	  switch ((opc>>9)&3)
	   { case 0:
		switch (opc & 0xff80)
		 { case 0xb080:
		      fnprintf(fn,"sub     sp, #%ld",(opc&0x7f)<<2);
		      DONE;
		 }
		break;
	     case 1:
		switch (opc & 0xffc0)
		 { case 0xb200: s = "sxth";   if (0) {
		   case 0xb240: s = "sxtb"; } if (0) {
		   case 0xb280: s = "uxth"; } if (0) {
		   case 0xb2c0: s = "uxtb"; }
		      fnprintf(fn,"%s    %s, %s",s,regs[opc&7],regs[(opc>>3)&7]);
		      DONE;
		 }
		break;
	     case 2:
		fnprintf(fn,"push    ");
		print_reg_mask(fn,(opc&255)|((opc&256)<<6));
		DONE;
	     case 3:
		switch (opc)
		 { case 0xb662:
		      fnprintf(fn,"cpsie");
		      DONE;
		   case 0xb672:
		      fnprintf(fn,"cpsid");
		      DONE;
		 }
		break;
	   }
	  break;
       case 23:
	  switch ((opc>>8)&7)
	   { case 2:
		switch ((opc >> 6) & 3)
		 { case 0:
		      fnprintf(fn,"rev     %s, %s",regs[opc&7],regs[(opc>>3)&7]);
		      DONE;
		   case 1:
		      fnprintf(fn,"rev16   %s, %s",regs[opc&7],regs[(opc>>3)&7]);
		      DONE;
		   case 3:
		      fnprintf(fn,"revsh   %s, %s",regs[opc&7],regs[(opc>>3)&7]);
		      DONE;
		 }
		break;
	     case 4: case 5:
		fnprintf(fn,"pop     ");
		print_reg_mask(fn,(opc&255)|((opc&256)<<7));
		DONE;
	     case 6:
		fnprintf(fn,"bkpt    %ld",opc&255);
		DONE;
	     case 7:
		switch (opc)
		 { case 0xbf00:
		      fnprintf(fn,"nop");
		      DONE;
		   case 0xbf10:
		      fnprintf(fn,"yield");
		      DONE;
		   case 0xbf20:
		      fnprintf(fn,"wfe");
		      DONE;
		   case 0xbf30:
		      fnprintf(fn,"wfi");
		      DONE;
		   case 0xbf40:
		      fnprintf(fn,"sev");
		      DONE;
		 }
		break;
	   }
	  break;
       case 24:
	  fnprintf(fn,"stm     %s!, ",regs[(opc>>8)&7]);
	  print_reg_mask(fn,opc&255);
	  DONE;
       case 25:
	  if (opc & 0x00ff)
	   { fnprintf(fn,"ldm     %s%s, ",regs[(opc>>8)&7],((opc>>((opc>>8)&7))&1)?"":"!");
	     print_reg_mask(fn,opc&0xff);
	     DONE;
	   }
	  break;
       case 26: case 27:
	  switch ((opc >> 8) & 15)
	   { default:
		fnprintf(fn,"b%s     ",conds[(opc>>8)&15]);
		print_symbol_or_hex(fn,addr+4+corebase+(sign_extend(opc&255,128)*2));
		DONE;
	     case 14:
		fnprintf(fn,"udf     #%ld",opc&255);
		DONE;
	     case 15:
		fnprintf(fn,"svc     #%ld",opc&255);
		DONE;
	   }
	  break;
       case 28:
	  fnprintf(fn,"b       ");
	  print_symbol_or_hex(fn,addr+4+corebase+sign_extend(opc&0x7ff,0x400));
	  DONE;
       case 29:
	  // UNDEFINED - see ARMv6-M PDF page 85
	  break;
       case 30:
	  if (addr+4 > coresize) return(-1);
	  opc2 = corefetchu(addr+2,2);
	  rv = 4;
	  switch (opc2 >> 12)
	   { case 0x8:
		if ((opc == 0xf3ef) && ((opc2 & 0x0f00) != 0x0d00))
		 { switch (opc2 & 0x00ff)
		    { case VALID_SPEC_REG:
			 fnprintf(fn,"mrs     %s, %s",regs[(opc2>>8)&15],spec_reg[opc2&0xff]);
			 DONE;
		    }
		 }
		else if (opc == 0xf3bf)
		 { switch (opc2)
		    { case 0x8f4f:
			 fnprintf(fn,"dsb     #sy");
			 DONE;
		      case 0x8f5f:
			 fnprintf(fn,"dmb     #sy");
			 DONE;
		      case 0x8f6f:
			 fnprintf(fn,"isb     #sy");
			 DONE;
		    }
		 }
		else if ((opc & 0xfff0) == 0xf380)
		 { switch (opc2 & 0xff00)
		    { case 0x8800:
			 fnprintf(fn,"msr     %s, %s",spec_reg[opc2&0xff],regs[opc&15]);
			 DONE;
		    }
		 }
		break;
	     case 0xa:
		if ((opc & 0xfff0) == 0xf7f0)
		 { fnprintf(fn,"udf     #0x%04lx",((opc&0xf)<<12)|(opc2&0xfff));
		   DONE;
		 }
		break;
	     case 0xd: case 0xf:
		fnprintf(fn,"bl      ");
		print_symbol_or_hex(fn,addr+4+corebase+sign_extend(
				((opc & 0x0400) << 14) |
				(((opc & 0x0400) << 13) ^ ((opc2 & 0x2000) << 10) ^ 0x00800000) |
				(((opc & 0x0400) << 12) ^ ((opc2 & 0x0800) << 11) ^ 0x00400000) |
				((opc & 0x03ff) << 12) |
				((opc2 & 0x07ff) << 1), 0x01000000));
		DONE;
	   }
	  break;
       case 31:
	  break;
     }
    fnprintf(fn,"<unknown %04lx>",opc);
  } while (0);
#undef DONE
 if (addr & 1)
  { fnprintf(fn,"  {misaligned}");
    return(1);
  }
 if (flg & CFF_INST_PARALLEL)
  { fnprintf(fn,"  {parallel}");
    return(1);
  }
 for (i=rv-1;i>0;i--) flags[addr+i] = CF_PREV;
 return(rv);
}

static void arm_init(void)
{
}

static int armv6m_dis(ADDR addr, void (*fn)(char))
{
 int rv;

 switch (flags[addr-corebase])
  { default:
       if (ISINST(flags[addr-corebase]))
	{ rv = do_dis_armv6m(addr,fn,flags[addr-corebase]);
	}
       else
	{ fnprintf(fn,"<bad flag %lu>",(unsigned long int)flags[addr-corebase]);
	  rv = 1;
	}
       break;
  }
 return(rv);
}

static unsigned long int armel_fetch(const void *base, int size)
#define bp(n) ((unsigned long int)(((const unsigned char *)base)[(n)]))
{
 switch (size)
  { case 1:
       return(bp(0));
       break;
    case 2:
       return((bp(1)<<8)|bp(0));
       break;
    case 4:
       return((bp(3)<<24)|(bp(2)<<16)|(bp(1)<<8)|bp(0));
       break;
  }
 abort();
}
#undef bp

static const char *armv6m_names[] = { "ARMv6-M", "armv6-m", 0 };
MACHINE machine_armv6m
 = { &armv6m_names[0],
     4,
     &arm_init,
     0,
     &armv6m_dis,
     &armel_fetch,
     0 };