gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    T-Kernel 2.0 Software Package
    4:  *
    5:  *    Copyright 2011 by Ken Sakamura.
    6:  *    This software is distributed under the latest version of T-License 2.x.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by T-Engine Forum(http://www.t-engine.org/) at 2011/05/17.
   10:  *    Modified by T-Engine Forum at 2013/03/04.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: 
   16: /*
   17:  *      step.c
   18:  *
   19:  *       calculate step address
   20:  */
   21: 
   22: #include "../cmdsvc.h"
   23: 
   24: #define aINSTSZ         4              // ARM instruction size
   25: #define tINSTSZ         2              // THUMB instruction size
   26: #define REGBIT(reg)     (1 << (reg))        // register bit
   27: #define REGSZ           4                // register size
   28: #define regPC           15               // PC register
   29: #define regSP           13               // SP register
   30: 
   31: LOCAL   UW        curCPSR;               // cpsr
   32: LOCAL   UW        nextPC;                        // the next PC value
   33: LOCAL   W nextLen;                // size of the next instruction(2 or 4)
   34: LOCAL   UW        repInst;               // instruction to replace
   35: LOCAL   W repReg;                 // register to be replaced
   36: LOCAL   W trcNext;                // NEXT trace mode
   37: 
   38: /*
   39:         extraction of fields of an instruction
   40: */
   41: LOCAL   UW        getInstField(UW mask, W sht)
   42: {
   43:         return (repInst & mask) >> sht;
   44: }
   45: /*
   46:         set the register field of the instruction to be replaced
   47: */
   48: LOCAL   void      setRepInst(UW mask, W sht)
   49: {
   50:         repInst = (repInst & ~mask) | ((repReg & 0xF) << sht);
   51: }
   52: /*
   53:         Obtain an unused register for replacement
   54: */
   55: LOCAL   W getRepReg(UW reg)
   56: {
   57:         W      i;
   58: 
   59:         for (i = 0; i < 16 && (reg & 0x1); i++, reg >>= 1);
   60:         return i + 0x10;       // register number + flag
   61: }
   62: /*
   63:         Validate instruction execution condition
   64: */
   65: LOCAL   W checkCond(W cond)
   66: {
   67:         UW     sr = curCPSR;
   68: 
   69:         switch(cond) {
   70:         case 0:                // EQ:        z
   71:                 if (sr & PSR_Z) return 1;                     break;
   72:         case 1:                // NE:        !z
   73:                 if (!(sr & PSR_Z)) return 1;                  break;
   74:         case 2:                // CS: c
   75:                 if (sr & PSR_C) return 1;                     break;
   76:         case 3:                // CC: !c
   77:                 if (!(sr & PSR_C)) return 1;                  break;
   78:         case 4:                // MI: n
   79:                 if (sr & PSR_N) return 1;                     break;
   80:         case 5:                // PL: !n
   81:                 if (!(sr & PSR_N)) return 1;                  break;
   82:         case 6:                // VS: v
   83:                 if (sr & PSR_V) return 1;                     break;
   84:         case 7:                // VC: !v
   85:                 if (!(sr & PSR_V)) return 1;                  break;
   86:         case 8:                // HI: c && !z
   87:                 if ((sr & (PSR_C | PSR_Z)) == PSR_C) return 1;        break;
   88:         case 9:                // LS: !c || z
   89:                 if (!(sr& PSR_C) || (sr & PSR_Z)) return 1;   break;
   90:         case 12:       // GT: !z && (n == v)
   91:                 if (sr & PSR_Z) return 0;
   92:         case 10:       // GE: n == v
   93:                 sr &= PSR_N | PSR_V;
   94:                 if (sr == 0 || sr == (PSR_N | PSR_V)) return 1;       break;
   95:         case 13:       // LE: z || (n != v)
   96:                 if (sr & PSR_Z) return 1;
   97:         case 11:       // LT: n != v
   98:                 sr &= PSR_N | PSR_V;
   99:                 if (sr == PSR_N || sr == PSR_V) return 1;     break;
  100:         case 14:       // AL:
  101:         case 15:       // NV:
  102:                 return 1;
  103:         }
  104:         return 0;
  105: }
  106: /*
  107:         non-branching instruction
  108: */
  109: LOCAL   void      noBranch(UW inst)
  110: {
  111: }
  112: /*
  113:         ARM: Bcond / BLX(1) instruction
  114: */
  115: LOCAL   void      armBInst(UW inst)
  116: {
  117:         W      off;
  118: 
  119:         // BL is not handled during NEXT trace
  120:         if (trcNext) {
  121:                 if (inst & 0x01000000) return;        // BL
  122:                 if (inst >= 0xF0000000) return;       // BLX(1)
  123:         }
  124:         off = (inst & 0x00FFFFFF) << 2;
  125:         if (off & 0x02000000) off -= 0x04000000;       // sign extension
  126:         nextPC += aINSTSZ + off;
  127: 
  128:         if (inst >= 0xF0000000) {      // BLX(1)
  129:                 nextPC += getInstField(0x01000000, 24) << 1;
  130:                 // adjust for Thumb mode (on two bytes boundary)
  131:                 nextLen = tINSTSZ;    // THUMB mode
  132:         }
  133: }
  134: /*
  135:         ARM: BX / BLX(2) instruction
  136: */
  137: LOCAL   void      armBxInst(UW inst)
  138: {
  139:         // BL is not handled during NEXT trace
  140:         if (trcNext) {
  141:                 if (inst & 0x00000020) return;        // BLX(2)
  142:         }
  143:         inst &= 0x0000000F;
  144:         if (inst == regPC) {
  145:                 nextPC += aINSTSZ;
  146:         } else {
  147:                 nextPC = getRegister(inst);
  148:         }
  149:         if (nextPC & 1) nextLen = tINSTSZ;     // THUMB mode
  150: }
  151: /*
  152:         ARM: data processing instruction rd = pc
  153: */
  154: LOCAL   void      armOpInst(UW inst)
  155: {
  156:         W      rn, rm;
  157:         UW     usereg;
  158: 
  159:         // TST, TEQ, CMP, CMPN instructions are not handled
  160:         rm = inst & 0x01E00000;
  161:         if (rm >= 0x01000000 && rm <= 0x01600000) return;
  162: 
  163:         // OP1 register
  164:         rn = getInstField(0x000F0000, 16);
  165:         usereg = REGBIT(rn);
  166: 
  167:         // OP2 register
  168:         if (!(inst & 0x02000000)) {
  169:                 rm = getInstField(0x0000000F, 0);
  170:                 usereg |= REGBIT(rm);
  171:                 if (inst & 0x00000010)        // shift length register
  172:                         usereg |= REGBIT(getInstField(0x00000F00, 8));
  173:         }
  174:         //register to be replaced
  175:         repReg = getRepReg(usereg);
  176: 
  177:         // Dest register replacement
  178:         setRepInst(0x0000F000, 12);
  179: 
  180:         // OP1 register replacement
  181:         if (rn == regPC) setRepInst(0x000F0000, 16);
  182: 
  183:         // OP2 register replacement
  184:         if (rm == regPC) setRepInst(0x0000000F, 0);
  185: 
  186:         // if S bit is set, we obtain the next mode from spsr.
  187:         if ((inst & 0x00100000) && (getCurSPSR() & PSR_T)) nextLen = tINSTSZ;
  188: }
  189: /*
  190:         ARM: LDR pc instruction
  191: */
  192: LOCAL   void      armLdrInst(UW inst)
  193: {
  194:         W      rn, roff;
  195:         UW     usereg;
  196: 
  197:         // base register
  198:         rn = getInstField(0x000F0000, 16);
  199:         usereg = REGBIT(rn);
  200: 
  201:         // OFF register
  202:         roff = 0;
  203:         if (inst & 0x02000000) {
  204:                 roff = getInstField(0x0000000F, 0);
  205:                 usereg |= REGBIT(roff);
  206:         }
  207: 
  208:         //register to be replaced
  209:         repReg = getRepReg(usereg);
  210: 
  211:         // Dest register replacement
  212:         setRepInst(0x0000F000, 12);
  213: 
  214:         // base register replacement
  215:         if (rn == regPC) setRepInst(0x000F0000, 16);
  216: 
  217:         // offset register replacement
  218:         if (roff == regPC) setRepInst(0x0000000F, 0);
  219: }
  220: /*
  221:         ARM: LDM {pc} instruction
  222: */
  223: LOCAL   void      armLdmInst(UW inst)
  224: {
  225:         W      i, off;
  226:         UW     baddr;
  227: 
  228:         // memory base address
  229:         baddr = getRegister(getInstField(0x000F0000, 16));
  230: 
  231:         // obtain PC address offset
  232:         off = (inst & 0x01000000) ? REGSZ : 0; // preindex
  233: 
  234:         if (inst & 0x00800000) {       // UP
  235:                 for (i = 0; i < regPC; i++) {
  236:                         if (inst & REGBIT(i)) off += REGSZ;
  237:                 }
  238:                 baddr += off;
  239:         } else {                       // DOWN
  240:                 baddr -= off;
  241:         }
  242: 
  243:         // Extract the value set to PC
  244:         readMem(baddr, &nextPC, REGSZ, 2);
  245: 
  246:         // if S bit is set, we obtain the next mode from spsr.
  247:         if ((inst & 0x00400000) && (getCurSPSR() & PSR_T)) nextLen = tINSTSZ;
  248: }
  249: /*
  250:         ARM: MRS / MRC instruction rd = pc
  251: */
  252: LOCAL   void      armMrcsInst(UW inst)
  253: {
  254:         // register to be replaced
  255:         repReg = getRepReg(0);
  256: 
  257:         // Dest register replacement
  258:         setRepInst(0x0000F000, 12);
  259: }
  260: #if CPU_ARMv6
  261: /*
  262:         ARM: RFE instruction
  263: */
  264: LOCAL   void      armRfeInst(UW inst)
  265: {
  266:         W      off;
  267:         UW     baddr, saddr, paddr, spsr;
  268: 
  269:         // memory base address
  270:         baddr = getRegister(getInstField(0x000F0000, 16));
  271: 
  272:         // obtain PC address offset
  273:         off = (inst & 0x01000000) ? REGSZ : 0; // preindex
  274: 
  275:         if (inst & 0x00800000) {               // UP
  276:                 paddr = baddr + off;
  277:                 saddr = baddr + off + REGSZ;
  278:         } else {                               // DOWN
  279:                 paddr = baddr - off - REGSZ;
  280:                 saddr = baddr - off;
  281:         }
  282: 
  283:         // Extract the value set to PC
  284:         readMem(paddr, &nextPC, REGSZ, 2);
  285: 
  286:         // obtain the next mode from the saved spsr inside stack.
  287:         readMem(saddr, &spsr, REGSZ, 2);
  288:         if (spsr & PSR_T) nextLen = tINSTSZ;
  289: }
  290: #endif
  291: /*
  292:         THUMB: Bcond instruction
  293: */
  294: LOCAL   void      thumbBcondInst(UW inst)
  295: {
  296:         W      cond, off;
  297: 
  298:         cond = getInstField(0x0F00, 8);
  299: 
  300:         // undefined instruction is not supported
  301:         if (cond == 14) return;
  302: 
  303:         // check conditions
  304:         if (!checkCond(cond)) return;
  305: 
  306:         off = (inst & 0x00FF) << 1;
  307:         if (off >= 0x100) off -= 0x200;                // sign extension
  308:         nextPC += tINSTSZ + off;
  309: }
  310: /*
  311:         THUMB: B instruction
  312: */
  313: LOCAL   void      thumbBInst(UW inst)
  314: {
  315:         W      off;
  316: 
  317:         off = (inst & 0x07FF) << 1;
  318:         if (off >= 0x800) off -= 0x1000;       // sign extension
  319:         nextPC += tINSTSZ + off;
  320: }
  321: /*
  322:         THUMB: BX / BLX(2) instruction
  323: */
  324: LOCAL   void      thumbBxInst(UW inst)
  325: {
  326:         // BL is not handled during NEXT trace
  327:         if (inst & 0x0080) {   // BLX(2)
  328:                 if (trcNext) return;
  329:         }
  330:         inst = getInstField(0x0078, 3);
  331:         if (inst == regPC) {
  332:                 nextPC += tINSTSZ;
  333:         } else {
  334:                 nextPC = getRegister(inst);   //  including Hi register
  335:         }
  336:         if (!(nextPC & 1)) nextLen = aINSTSZ;
  337: }
  338: /*
  339:         THUMB: BL / BLX(1) instruction
  340: */
  341: LOCAL   void      thumbBlInst(UW inst)
  342: {
  343:         W      off;
  344:         UH     inst2;
  345: 
  346:         // BL is not handled during NEXT trace
  347:         if (trcNext) return;
  348: 
  349:         // Extract 2nd instruction
  350:         readMem(nextPC, &inst2, tINSTSZ, 2);
  351: 
  352:         off = (inst & 0x07FF) << 12;
  353:         if (off >= 0x400000) off -= 0x800000;  // sign extension
  354:         nextPC += tINSTSZ + off + ((inst2 & 0x07FF) << 1);
  355:         if (!(inst2 & 0x1000)) nextLen = aINSTSZ;      // BLX(1)
  356: }
  357: /*
  358:         THUMB: ADD|CMP|MOV Rd/Rn,Rm instruction rd = pc
  359: */
  360: LOCAL   void      thumbOp6Inst(UW inst)
  361: {
  362:         UW     op, dreg, mreg;
  363: 
  364:         op = inst & 0x0300;
  365:         if (op == 0x0100) return;      // compare instruction
  366: 
  367:         dreg = getInstField(0x0007, 0);
  368:         if (inst & 0x0080) dreg |= 0x8;        // Hi register
  369:         if (dreg != regPC) return;     // not PC register
  370: 
  371:         mreg = getInstField(0x0078, 3);        // including Hi register
  372:         nextPC = getRegister(mreg) & ~(tINSTSZ - 1);
  373: 
  374:         // calculate PC
  375:         if (op == 0x0000) nextPC += getRegister(dreg); // ADD
  376: }
  377: /*
  378:         THUMB: POP instruction pc
  379: */
  380: LOCAL   void      thumbPopInst(UW inst)
  381: {
  382:         W      i;
  383:         UW     sp;
  384: 
  385:         sp = getRegister(regSP);
  386:         for (i = 0; i < 8; i++) {
  387:                 if (inst & REGBIT(i)) sp += REGSZ;
  388:         }
  389:         // extract PC
  390:         readMem(sp, &nextPC, REGSZ, 2);
  391: }
  392: 
  393: // Arm instruction decode table
  394: typedef struct {
  395:         UW     mask;                       // mask
  396:         UW     code;                       // code
  397:         void   (*calcNextPC)(UW inst);   // calculate PC
  398: } INST_T;
  399: 
  400: LOCAL   const     INST_T      instArm[] = {
  401:       { 0xFFFFFFFF, 0xE1A00000, noBranch},      // NOP
  402:       { 0xFFF000F0, 0xE1200070, noBranch},      // BKPT(ARM5T)
  403:       { 0x0FFFFFF0, 0x012FFF10, armBxInst},     // BX
  404:       { 0xFE000000, 0xFA000000, armBInst},      // BLX(1)(ARM5T)
  405:       { 0x0FF000F0, 0x01200030, armBxInst},     // BLX(2)(ARM5T)
  406:       { 0x0E000000, 0x0A000000, armBInst},      // B,BL
  407:       { 0x0F000000, 0x0F000000, noBranch},      // SWI
  408:       { 0x0C10F000, 0x0410F000, armLdrInst},    // LDR pc
  409:       { 0x0E108000, 0x08108000, armLdmInst},    // LDM {pc}
  410: //    { 0x0F000010, 0x0E000000, noBranch},      // CDP
  411: //    { 0x0E000000, 0x0C000000, noBranch},      // LDC/STC
  412:       { 0x0F10F010, 0x0E10F010, armMrcsInst},   // MRC pc
  413:       { 0x0FBFFFFF, 0x010FF000, armMrcsInst},   // MRS pc
  414: //    { 0x0F0000F0, 0x00000090, noBranch},      // MULL
  415:       { 0x0E000090, 0x00000090, noBranch},      // LDR/STR Half/SByte
  416: //    { 0x0DB0F000, 0x0120F000, noBranch},      // MSR
  417: //    { 0x0FB00FF0, 0x01000090, noBranch},      // SWP
  418:       { 0x0C00F000, 0x0000F000, armOpInst},     // ADD/SUB.. rd = pc
  419: #if CPU_ARMv6
  420:       { 0xFE50FFFF, 0xF8100A00, armRfeInst},    // RFE(ARMv6)
  421: #endif
  422:       { 0 }
  423: };
  424: 
  425: // Thumb instruction decode table
  426: LOCAL   const     INST_T      instThumb[] = {
  427: //    { 0xF801, 0xE800, thumbBlInst},   // BLX(1)(ARM5T) 2Nd Inst
  428:       { 0xFF00, 0xBE00, noBranch},      // BKPT(ARM5T)
  429:       { 0xFFFF, 0x46C0, noBranch},      // NOP
  430:       { 0xFF00, 0xDF00, noBranch},      // SWI
  431:       { 0xF800, 0xE000, thumbBInst},    // B <label>
  432:       { 0xF000, 0xD000, thumbBcondInst},// B<cond> <label>
  433:       { 0xF000, 0xF000, thumbBlInst},   // BL,BLX(1)(ARM5T)
  434:       { 0xFF80, 0x4780, thumbBxInst},   // BLX(2)(ARM5T)
  435:       { 0xFF00, 0x4700, thumbBxInst},   // BX
  436: //    { 0xFC00, 0x1800, noBranch},      // OP(1)ADD|SUB Rd,Rn,Rm
  437: //    { 0xFC00, 0x1C00, noBranch},      // OP(2)ADD|SUB Rd,Rn,#imm3
  438: //    { 0xE000, 0x2000, noBranch},      // OP(3)<OP> Rd/Rn,#imm8
  439: //    { 0xE000, 0x0000, noBranch},      // OP(4)LSL|LSR|ASR Rd, Rn, Rn,#shift
  440: //    { 0xFC00, 0x4000, noBranch},      // OP(5)<OP> Rd/Rn,Rm/Rs
  441:       { 0xFC00, 0x4400, thumbOp6Inst},  // OP(6)ADD|CMP|MOV Rd/Rn,Rm
  442: //    { 0xF000, 0xA000, noBranch},      // OP(7)ADD Rd,SP|PC,#imm8
  443: //    { 0xFF00, 0xB000, noBranch},      // OP(8)ADD|SUB SP,SP,#imm7
  444: //    { 0xE000, 0x6000, noBranch},      // LS(1)LDR|STR(B) Rd,[Rn,#off5]
  445: //    { 0xF000, 0x8000, noBranch},      // LS(2)LDRH|STRH Rd,[Rn,#off5]
  446: //    { 0xF000, 0x5000, noBranch},      // LS(3)LDR|STR(S){H|B} Rd,[Rn,Rm]
  447: //    { 0xF800, 0x4800, noBranch},      // LS(4)LDR Rd,[PC,#off8]
  448: //    { 0xF000, 0x9000, noBranch},      // LS(5)LDR|STR Rd,[SP,#off8]
  449: //    { 0xF000, 0xC000, noBranch},      // LDMIA|STMIA Rn!,{<reg list>}
  450:       { 0xFD00, 0xBD00, thumbPopInst},  // POP {<reg list,PC}>}
  451:       { 0 }
  452: };
  453: /*
  454:         Obtain the next step address
  455: */
  456: EXPORT  W        getStepAddr(UW pc, UW cpsr, W mode, UW* npc, UW *rep)
  457: {
  458:         W      len;
  459:         UW     inst;
  460:         INST_T *tab;
  461: 
  462:         // set mode
  463:         len = (cpsr & PSR_T) ? tINSTSZ : aINSTSZ;
  464:         curCPSR = cpsr;                // cpsr
  465:         repReg = 0;            // register to replace
  466:         repInst = 0;           // instruction to replace
  467:         nextPC = pc + len;     // the next PC value for non-branching instruction
  468:         nextLen = len;         // size of the next instruction
  469:         trcNext = mode;                // NEXT trace
  470: 
  471:         // extract op code
  472:         if (readMem(pc, &inst, len, 2) != len) goto EXIT;
  473:         repInst = inst;                // instruction to replace
  474: 
  475:         if (len == 4) {                // ARM instruction
  476:                 // check the conditional execution
  477:                 if (!checkCond(getInstField(0xF0000000, 28))) goto EXIT;
  478:                 tab = (INST_T*)instArm;
  479:         } else {               // THUMB instruction
  480:                 tab = (INST_T*)instThumb;
  481:                 inst &= 0xFFFF;
  482:         }
  483: 
  484:         // search instruction and calculate next PC
  485:         for ( ; tab->mask != 0; tab++) {
  486:                 if ((inst & tab->mask) == tab->code) {
  487:                         (*(tab->calcNextPC))(inst);
  488:                         break;
  489:                 }
  490:         }
  491: EXIT:
  492:         *npc = nextPC & ~(nextLen - 1);
  493:         *rep = repInst;
  494:         return nextLen | (repReg << 4);
  495: }