ref: 41e27b2d10c6a60c49931332e8677438736a1e36
dir: /os/sa1110/sa1110break.c/
#include "u.h" #include "../port/lib.h" #include "mem.h" #include "dat.h" #include "fns.h" #include "ureg.h" #include "../port/error.h" // // from trap.c // extern int (*breakhandler)(Ureg *ur, Proc*); extern Instr BREAK; extern void portbreakinit(void); // // Instructions that can have the PC as a destination register // enum { IADD = 1, IBRANCH, ILDM, ILDR, IMOV, // // These should eventually be implemented // IADC, IAND, IBIC, IEOR, ILDRT, IMRS, IMVN, IORR, IRSB, IRSC, ISBC, ISUB, }; static int instrtype(Instr i); static ulong iadd(Ureg *ur, Instr i); static ulong ibranch(Ureg *ur, Instr i); static ulong ildm(Ureg *ur, Instr i); static ulong ildr(Ureg *ur, Instr i); static ulong imov(Ureg *ur, Instr i); static ulong shifterval(Ureg *ur, Instr i); static int condpass(Instr i, ulong psr); static ulong *address(Ureg *ur, Instr i); static ulong* multiaddr(Ureg *ur, Instr i); static int nbits(ulong v); #define COND_N(psr) (((psr) >> 31) & 1) #define COND_Z(psr) (((psr) >> 30) & 1) #define COND_C(psr) (((psr) >> 29) & 1) #define COND_V(psr) (((psr) >> 28) & 1) #define REG(i, a, b) (((i) & BITS((a), (b))) >> (a)) #define REGVAL(ur, r) (*((ulong*)(ur) + (r))) #define LSR(v, s) ((ulong)(v) >> (s)) #define ASR(v, s) ((long)(v) >> (s)) #define ROR(v, s) (LSR((v), (s)) | (((v) & ((1 << (s))-1)) << (32 - (s)))) void machbreakinit(void) { portbreakinit(); breakhandler = breakhit; } Instr machinstr(ulong addr) { if (addr < KTZERO) error(Ebadarg); return *(Instr*)addr; } void machbreakset(ulong addr) { if (addr < KTZERO) error(Ebadarg); *(Instr*)addr = BREAK; segflush((void*)addr, sizeof(Instr)); } void machbreakclear(ulong addr, Instr i) { if (addr < KTZERO) error(Ebadarg); *(Instr*)addr = i; segflush((void*)addr, sizeof(Instr)); } // // Return the address of the instruction that will be executed after the // instruction at address ur->pc. // // This means decoding the instruction at ur->pc. // // In the simple case, the PC will simply be the address of the next // sequential instruction following ur->pc. // // In the complex case, the instruction is a branch of some sort, so the // value of the PC after the instruction must be computed by decoding // and simulating the instruction enough to determine the PC. // ulong machnextaddr(Ureg *ur) { Instr i; i = machinstr(ur->pc); switch(instrtype(i)) { case IADD: return iadd(ur,i); case IBRANCH: return ibranch(ur,i); case ILDM: return ildm(ur,i); case ILDR: return ildr(ur,i); case IMOV: return imov(ur,i); case IADC: case IAND: case IBIC: case IEOR: case ILDRT: case IMRS: case IMVN: case IORR: case IRSB: case IRSC: case ISBC: case ISUB: // XXX - Tad: unimplemented // // any of these instructions could possibly have the // PC as Rd. Eventually, these should all be // checked just like the others. default: return ur->pc+4; } return 0; } static int instrtype(Instr i) { if(i & BITS(26,27) == 0) { switch((i >> 21) & 0xF) { case 0: return IAND; case 1: return IEOR; case 2: return ISUB; case 3: return IRSB; case 4: return IADD; case 5: return IADC; case 6: return ISBC; case 7: return IRSC; case 0xD: return IMOV; case 0xC: return IORR; case 0xE: return IBIC; case 0xF: return IMVN; } if(((i & BIT(25)|BITS(23,24)|BITS(20,21))) >> 20 == 0x10) return IMRS; return 0; } if(((i & BITS(27,25)|BIT(20)) >> 20) == 0x81) return ILDM; if(((i & BITS(26,27)|BIT(22)|BIT(20)) >> 20) == 0x41) return ILDR; if(((i & BITS(25,27)) >> 25) == 5) return IBRANCH; return 0; } static ulong iadd(Ureg *ur, Instr i) { ulong Rd = REG(i, 12, 15); ulong Rn = REG(i, 16, 19); if(Rd != 15 || !condpass(i, ur->psr)) return ur->pc+4; return REGVAL(ur, Rn) + shifterval(ur, i); } static ulong ibranch(Ureg *ur, Instr i) { if(!condpass(i, ur->psr)) return ur->pc+4; return ur->pc + ((signed long)(i << 8) >> 6) + 8; } static ulong ildm(Ureg *ur, Instr i) { if((i & BIT(15)) == 0) return ur->pc+4; return *(multiaddr(ur, i) + nbits(i & BITS(15, 0))); } static ulong ildr(Ureg *ur, Instr i) { if(REG(i, 12, 19) != 15 || !condpass(i, ur->psr)) return ur->pc+4; return *address(ur, i); } static ulong imov(Ureg *ur, Instr i) { if(REG(i, 12, 15) != 15 || !condpass(i, ur->psr)) return ur->pc+4; return shifterval(ur, i); } static int condpass(Instr i, ulong psr) { uchar n = COND_N(psr); uchar z = COND_Z(psr); uchar c = COND_C(psr); uchar v = COND_V(psr); switch(LSR(i,28)) { case 0: return z; case 1: return !z; case 2: return c; case 3: return !c; case 4: return n; case 5: return !n; case 6: return v; case 7: return !v; case 8: return c && !z; case 9: return !c || z; case 10: return n == v; case 11: return n != v; case 12: return !z && (n == v); case 13: return z && (n != v); case 14: return 1; case 15: return 0; } } static ulong shifterval(Ureg *ur, Instr i) { if(i & BIT(25)) { // IMMEDIATE ulong imm = i & BITS(0,7); ulong s = (i & BITS(8,11)) >> 7; // this contains the * 2 return ROR(imm, s); } else { ulong Rm = REGVAL(ur, REG(i, 0, 3)); ulong s = (i & BITS(7,11)) >> 7; switch((i & BITS(6,4)) >> 4) { case 0: // LSL return Rm << s; case 1: // LSLREG s = REGVAL(ur, s >> 1) & 0xFF; if(s >= 32) return 0; return Rm << s; case 2: // LSRIMM return LSR(Rm, s); case 3: // LSRREG s = REGVAL(ur, s >> 1) & 0xFF; if(s >= 32) return 0; return LSR(Rm, s); case 4: // ASRIMM if(s == 0) { if(Rm & BIT(31) == 0) return 0; return 0xFFFFFFFF; } return ASR(Rm, s); case 5: // ASRREG s = REGVAL(ur, s >> 1) & 0xFF; if(s >= 32) { if(Rm & BIT(31) == 0) return 0; return 0xFFFFFFFF; } return ASR(Rm, s); case 6: // RORIMM if(s == 0) return (COND_C(ur->psr) << 31) | LSR(Rm, 1); return ROR(Rm, s); case 7: // RORREG s = REGVAL(ur, s >> 1) & 0xFF; if(s == 0 || (s & 0xF) == 0) return Rm; return ROR(Rm, s & 0xF); } } } static ulong* address(Ureg *ur, Instr i) { ulong Rn = REGVAL(ur, REG(i, 16, 19)); if(i & BIT(24) == 0) // POSTIDX return (ulong*)REGVAL(ur, Rn); if(i & BIT(25) == 0) { // OFFSET if(i & BIT(23)) return (ulong*)(REGVAL(ur, Rn) + (i & BITS(0, 11))); return (ulong*)(REGVAL(ur, Rn) - (i & BITS(0, 11))); } else { // REGOFF ulong Rm = REGVAL(ur, REG(i, 0, 3)); ulong index = 0; switch(i & BITS(5,6) >> 5) { case 0: index = Rm << ((i & BITS(7, 11)) >> 7); break; case 1: index = LSR(Rm, ((i & BITS(7, 11)) >> 7)); break; case 2: index = ASR(Rm, ((i & BITS(7, 11)) >> 7)); break; case 3: if(i & BITS(7, 11) == 0) index = (COND_C(ur->psr) << 31) | LSR(Rm, 1); else index = ROR(Rm, (i & BITS(7, 11)) >> 7); break; } if(i & BIT(23)) return (ulong*)(Rn + index); return (ulong*)(Rn - index); } } static ulong* multiaddr(Ureg *ur, Instr i) { ulong Rn = REGVAL(ur, REG(i, 16, 19)); switch((i >> 23) & 3) { case 0: return (ulong*)(Rn - (nbits(i & BITS(0,15))*4)+4); case 1: return (ulong*)Rn; case 2: return (ulong*)(Rn - (nbits(i & BITS(0,15))*4)); case 3: return (ulong*)(Rn + 4); } } static int nbits(ulong v) { int n = 0; int i; for(i = 0; i < 32; i++) { if(v & 1) ++n; v = LSR(v, 1); } return n; }