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 TRON Forum(http://www.tron.org/) at 2015/06/01.
   11:  *
   12:  *----------------------------------------------------------------------
   13:  */
   14: 
   15: /*
   16:  *      break.c
   17:  *
   18:  *     break/trace processing (after ARMv6)
   19:  */
   20: 
   21: #include "../cmdsvc.h"
   22: #include <sys/sysinfo.h>
   23: 
   24: // SW breakpoint code (BKPT instruction)
   25: #define BREAK_ARM       0xE1200070
   26: #define BREAK_THUMB     0xBE00
   27: 
   28: /*
   29:         breakpoint data
   30: */
   31: typedef struct {
   32:         UW     addr;                       // break address
   33:         UW     code;                       // saved data
   34:         UW     atr;                        // break attribute
   35:         H      sz;                  //code size (2 or 4)
   36:         UB     cmd[L_BPCMD];               // executed command
   37: } BRKPT;
   38: 
   39: #define MAX_SBP         (8)            // maximum number of SW breakpoint
   40: #define MAX_IBP         (0)
   41: #define MAX_OBP         (0)
   42: #define MAX_BRKPT       (MAX_SBP + MAX_IBP + MAX_OBP)
   43: 
   44: LOCAL   BRKPT     brkPt[MAX_BRKPT + 1];       // breakpoint data
   45:                                         // the last is temorary break
   46: 
   47: /*
   48:         step point data
   49:         * used for trace and temporary step processing
   50: */
   51: typedef struct {
   52:         UW     addr;                       //step address
   53:         UW     code;                       // step save data
   54:         UW     pc;                 // address of replaced instruction
   55:         UW     inst;                       // replaced instruction
   56:         UW     regval;                     // replaced register value
   57:         H      reg;                 // replaced register number
   58:         H      sz;                  // code size ( 2 / 4)
   59: } STEPPT;
   60: 
   61: LOCAL   STEPPT    stepPt;                    // step point data
   62: 
   63: /*
   64:         break attribute
   65: */
   66: #define BA_S            0x1000            //software break
   67: #define BA_I            0x2000            // instruction break
   68: #define BA_O            0x4000            //operand break
   69: #define BA_SET          0x8000          //software break released flag
   70: #define BA_PRE          0x0100          // break before execution
   71: #define BA_R            0x0200            // break on read
   72: #define BA_W            0x0400            // break on write
   73: #define BA_RW           0x0600           // break on read/write
   74: #define BA_TMP          0x0800          // temporary break
   75: 
   76: #define MAX_BPATR       1
   77: 
   78: LOCAL   const struct {
   79:         UB     name[4];            // attribute name
   80:         UW     atr;                        // attribute code
   81: } brkAtr[MAX_BPATR] = {
   82:         {"S   ", 0x00000000 | BA_S | BA_PRE},          // software break
   83: };
   84: 
   85: /*
   86:         trace data
   87: */
   88: LOCAL   W traceMode;              // trace mode
   89: LOCAL   W traceStep;              // number of trace steps
   90: LOCAL   W stepFlg;                // temporary step execution flag
   91: LOCAL   union {
   92:                 UB    b[8];
   93:                 UW    w[2];              // to align on word boundary
   94:         } sbpCode;                     // SW break instructions (two)
   95: 
   96: /*
   97:         CP14 register manipulation
   98: */
   99: // no debug comprocessor
  100: LOCAL   void      setDSCR(UW val) {return;}
  101: LOCAL   UW        getDSCR(void) {return 0;}
  102: LOCAL   UW        getWFAR(void) {return 0;}
  103: LOCAL   void      setBVR(W num, UW val) {return;}
  104: LOCAL   void      setBCR(W num, UW val) {return;}
  105: LOCAL   UW        getBCR(W num) {return 0;}
  106: LOCAL   void      setWVR(W num, UW val) {return;}
  107: LOCAL   void      setWCR(W num, UW val) {return;}
  108: LOCAL   UW        getWCR(W num) {return 0;}
  109: 
  110: /*
  111:         check CP14 monitor debug mode
  112: */
  113: LOCAL   UW        CheckCP14(void)
  114: {
  115:         return getDSCR() & 0x00008000;
  116: }
  117: /*
  118:         set CP14 monitor debug mode
  119: */
  120: LOCAL   UW        EnableCP14(void)
  121: {
  122:         UW     dscr;
  123: 
  124:         dscr = getDSCR();
  125:         dscr |=  0x00008000;   // monitor debug mode on
  126:         dscr &= ~0x00004000;   // hold debug mode off
  127:         setDSCR(dscr);
  128: 
  129:         /* return the success/failure of setting */
  130:         return CheckCP14();
  131: }
  132: /*
  133:         reset CP14 monitor debug mode
  134: */
  135: LOCAL   void      DisableCP14(void)
  136: {
  137:         setDSCR(getDSCR() & ~0x00008000);
  138:         return;
  139: }
  140: /*
  141:         extract break attribute
  142: */
  143: EXPORT  W        getBreakAtr(UB *name)
  144: {
  145:         W      i;
  146: 
  147:         if (name[4] == ' ') {
  148:                 for (i = 0; i < MAX_BPATR; i++) {
  149:                         if (*((UW*)brkAtr[i].name) == *((UW*)name))
  150:                                 return brkAtr[i].atr;
  151:                 }
  152:         }
  153:         return E_BPATR;
  154: }
  155: /*
  156:         extract break attribute string (fixed length: 4 characters)
  157: */
  158: LOCAL   UB        *strBreakAtr(W atr)
  159: {
  160:         W      i;
  161: static  UB       str[5];
  162: 
  163:         atr &= ~BA_SET;
  164: 
  165:         for (i = 0; i < MAX_BPATR; i++) {
  166:                 if (brkAtr[i].atr == atr) {
  167:                         memcpy(str, brkAtr[i].name, 4);
  168:                         for (i = 4; str[--i] == ' '; );
  169:                         str[i + 1] = '\0';
  170:                         return str;
  171:                 }
  172:         }
  173:         return NULL;
  174: }
  175: /*
  176:         set breakpoint
  177: */
  178: EXPORT  ER       setBreak(UW addr, W atr, UB *cmd, W cmdlen)
  179: {
  180:         W      ibcnt, obcnt, sbcnt, sz;
  181:         UW     code;
  182:         BRKPT  *bp, *p;
  183: 
  184:         if (atr == 0) atr = BA_S | BA_PRE;     // default attribute
  185: 
  186:         // unaligned address (non-W alignment) is regarded as Thumb instruction
  187:         sz = (addr & 0x03) ? 2 : 4;
  188:         addr &= ~(sz - 1);
  189: 
  190:         if (atr & BA_TMP) {    // temporary break is used at fixed location
  191:                 bp = &brkPt[MAX_BRKPT];
  192:         } else {
  193:                 // find an empty slot in the table
  194:                 ibcnt = obcnt = sbcnt = 0;
  195:                 for (bp = NULL, p = brkPt; p < &brkPt[MAX_BRKPT]; p++) {
  196:                         if (p->addr == 0) {if (bp == NULL) bp = p;} // empty
  197:                         else if (p->addr == addr) bp = p;        // update
  198:                         else if (p->atr & BA_O) obcnt++;         // WP
  199:                         else if (p->atr & BA_I) ibcnt++;         // HW BP
  200:                         else sbcnt++;                                    // SW BP
  201:                 }
  202:                 // check for the maximum value
  203:                 if (atr & BA_O) {
  204:                         if (obcnt >= MAX_OBP) return E_HBPOVR;
  205:                 } else if (atr & BA_I) {
  206:                         if (ibcnt >= MAX_IBP) return E_HBPOVR;
  207:                 } else {
  208:                         if (sbcnt >= MAX_SBP) return E_SBPOVR;
  209:                 }
  210:         }
  211: 
  212:         if (atr & BA_S) {
  213:                 // validate PC
  214:                 // if (invalidPC(addr)) return E_BPBAD;
  215: 
  216:                 //check for read and and write access rights
  217:                 if (readMem(addr, &code, sz, 2) != sz) return E_BPBAD;
  218:                 if (writeMem(addr, &sbpCode.b[sz], sz, 2) != sz) return E_BPROM;
  219:                 writeMem(addr, &code, sz, 2);
  220:         } else {
  221:                 code = 0;
  222:         }
  223: 
  224:         //set breakpoint
  225:         bp->addr = addr;
  226:         bp->atr = atr | BA_SET;
  227:         bp->sz = sz;
  228:         bp->code = code;
  229:         memset(bp->cmd, 0, L_BPCMD);
  230:         if (cmdlen > 0) memcpy(bp->cmd, cmd, cmdlen);
  231:         return E_OK;
  232: }
  233: /*
  234:         clear breakpoint
  235: */
  236: EXPORT  ER       clearBreak(UW addr)
  237: {
  238:         BRKPT  *p;
  239: 
  240:         if (addr == 0) {       // clear all breakpoints
  241:                 memset(&brkPt[0], 0, sizeof(brkPt));
  242:                 return E_OK;
  243:         }
  244:         for (p = brkPt; p < &brkPt[MAX_BRKPT]; p++) {
  245:                 if (p->addr && p->addr == (addr & ~(p->sz - 1))) {
  246:                         memset(p, 0, sizeof(BRKPT));
  247:                         return E_OK;
  248:                 }
  249:         }
  250:         return E_BPUDF;
  251: }
  252: /*
  253:         list all breakpoints
  254: */
  255: EXPORT  void     dspBreak(void)
  256: {
  257:         BRKPT  *p;
  258: 
  259:         for (p = brkPt; p < &brkPt[MAX_BRKPT]; p++) {
  260:                 if (p->addr == 0) continue;
  261:                 // THUMB(sz == 2) is displayed using odd address
  262:                 DSP_F3(08X,(p->addr + ((p->sz & 2) >> 1)), CH,' ',
  263:                        S,strBreakAtr(p->atr));
  264:                 if (p->cmd[0] != '\0') {
  265:                         DSP_F3(S," \"", S,p->cmd, CH,'"');
  266:                 }
  267:                 DSP_LF;
  268:         }
  269: }
  270: /*
  271:         initialize breakpoint
  272: */
  273: EXPORT  void     initBreak(void)
  274: {
  275:         // clear all breakpoints
  276:         memset(&brkPt[0], 0, sizeof(brkPt));
  277: 
  278:         // clear all step points
  279:         memset(&stepPt, 0, sizeof(stepPt));
  280: 
  281:         // initialize others,
  282:         traceMode = traceStep = stepFlg = 0;
  283: 
  284:         // SW break instruction (undefined instruction)
  285:         *((UH*)&sbpCode.b[2]) = BREAK_THUMB;
  286:         *((UW*)&sbpCode.b[4]) = BREAK_ARM;
  287: }
  288: /*
  289:         release breakpoint temporarily (monitor entry)
  290: */
  291: EXPORT  W        resetBreak(UW vec)
  292: {
  293:         W      i, n, bpflg;
  294:         UW     code, pc;
  295:         BRKPT  *p;
  296: 
  297:         pc = getCurPCX();      // break address has been adjusted
  298:         bpflg = 0;
  299: 
  300:         /* release if monitor debug mode is used */
  301:         if (CheckCP14()) {
  302:                 // release hardware breakpoint
  303:                 for (i = 0; i < MAX_IBP; i++) {
  304:                         setBCR(i, getBCR(i) & ~1);
  305:                 }
  306: 
  307:                 // release watchpoint
  308:                 for (i = 0; i < MAX_OBP; i++) {
  309:                         setWCR(i, getWCR(i) & ~1);
  310:                 }
  311: 
  312:                 // monitor debug mode is set to off later
  313:         }
  314: 
  315:         // release steppoints
  316:         if (stepPt.addr != 0) {
  317:                 n = stepPt.sz;
  318:                 readMem(stepPt.addr, &code, n, 2);
  319:                 if (memcmp(&code, &sbpCode.b[n], n) == 0) {
  320:                         if (pc == stepPt.addr) bpflg = 0x100;
  321:                         writeMem(stepPt.addr, &stepPt.code, n, 2);
  322:                         if (stepPt.pc > 0) {
  323:                                 // restore the changed instruction (ARM instruction only)
  324:                                 writeMem(stepPt.pc, &stepPt.inst, 4, 2);
  325:                                 // restore the changed register
  326:                                 pc = getRegister(stepPt.reg);
  327:                                 setRegister(stepPt.reg, stepPt.regval);
  328:                         }
  329:                         // set the NEXT real PC
  330:                         if (bpflg != 0) setCurPCX(pc);
  331:                 }
  332:         }
  333: 
  334:         // in the case of trace/step execution, SW breakpoints have been released
  335:         if (! (traceMode || stepFlg)) {
  336: 
  337:                 // temporaly release SW/HW breakpoints (including the temporary breakpoints)
  338:                 for (p = brkPt; p <= &brkPt[MAX_BRKPT]; p++) {
  339:                         if (p->addr == 0) continue;
  340: 
  341:                         if (p->atr & BA_O) {
  342:                                 if (vec == EIT_DDEBUG)
  343:                                         bpflg = (p - brkPt) | 0x10;
  344:                         } else if (p->atr & BA_I) {
  345:                                 if (pc == p->addr)
  346:                                         bpflg = (p - brkPt) | 0x10;
  347:                         } else {
  348:                                 readMem(p->addr, &code, n = p->sz, 2);
  349:                                 if (memcmp(&code, &sbpCode.b[n], n) == 0) {
  350:                                         if (pc == p->addr)
  351:                                                 bpflg = (p - brkPt) | 0x10;
  352:                                         writeMem(p->addr, &p->code, n, 2);
  353:                                         p->atr |= BA_SET;
  354:                                 } else {
  355:                                         p->atr &= ~BA_SET;
  356:                                 }
  357:                                 // clear temporary breakpoint
  358:                                 if (p->atr & BA_TMP)
  359:                                         memset(p, 0, sizeof(BRKPT));
  360:                         }
  361:                 }
  362:         }
  363:         return bpflg;  // is PC breakpoint?
  364: }
  365: /*
  366:         setting step
  367: */
  368: LOCAL   void      setStep(UW pc, W mode)
  369: {
  370:         W      n;
  371:         UW     cpsr, inst;
  372: 
  373:         // ARM or THUMB
  374:         cpsr = getCurCPSR();
  375: 
  376:         // decode instruction and obtain the next branch target
  377:         n = getStepAddr(pc, cpsr, (mode == 2) ? 1 : 0, &stepPt.addr, &inst);
  378: 
  379:         if (n >= 0x10) {       // instruction modification
  380:                 // modify instruction (ARM instruction only)
  381:                 readMem(stepPt.pc = pc, &stepPt.inst, 4, 2);
  382:                 writeMem(pc, &inst, 4, 2);
  383:                 // restore the changed register
  384:                 stepPt.reg = (n >> 4) & 0x0F;
  385:                 stepPt.regval = getRegister(stepPt.reg);
  386:                 // Set PC witht the content of the replace register
  387:                 setRegister(stepPt.reg, pc + ((cpsr & PSR_T) ? 4 : 8));
  388:         }
  389:         //set break command
  390:         stepPt.sz = (n &= 0x0F);
  391:         readMem(stepPt.addr, &stepPt.code, n, 2);
  392:         writeMem(stepPt.addr, &sbpCode.b[n], n, 2);
  393: }
  394: /*
  395:         set breakpoint (monitor exit)
  396: */
  397: EXPORT  void     setupBreak(void)
  398: {
  399:         W      ibcnt, obcnt;
  400:         UW     bcr, wcr, pc;
  401:         BRKPT  *p;
  402: 
  403:         pc = getCurPCX();
  404: 
  405:         // clear steppoint
  406:         memset(&stepPt, 0, sizeof(stepPt));
  407: 
  408:         if (traceMode) {       // trace is executed
  409: 
  410:                 setStep(pc, traceMode);               // set up step
  411: 
  412:         } else {               // normal execution
  413: 
  414:                 // if an unexecuted break matches the PC value
  415:                 // temporarily set up step execution, and execute one instruction only
  416:                 if (stepFlg == 0) {
  417:                         for (p = brkPt; p <= &brkPt[MAX_BRKPT]; p++) {
  418:                                 if (p->addr == pc && (p->atr & BA_PRE)) {
  419:                                         setStep(pc, 0);    // set up temporary step execution
  420:                                         stepFlg = 1;
  421:                                         return;
  422:                                 }
  423:                         }
  424:                 }
  425: 
  426:                 ibcnt = obcnt = 0;
  427: 
  428:                 //set breakpoint
  429:                 // - unless we turn on monitor debug mode, WCR/WVR/BCR/BVR
  430:                 //   cannot be accessed
  431:                 // - depending on hardware, monitor debug mode cannot be
  432:                 //  set to on
  433:                 // So try setting monitor debug mode on, and only if it is successful,
  434:                 //  we try to set  WCR/WVR/BCR/BVR
  435:                 for (p = brkPt; p <= &brkPt[MAX_BRKPT]; p++) {
  436:                         if (p->addr == 0) continue;
  437: 
  438:                         if (p->atr & BA_O) {
  439:                                 if (!EnableCP14()) continue;
  440:                                 wcr = getWCR(obcnt);
  441:                                 wcr &= ~0x001FC1FF;
  442:                                 wcr |= ((p->atr & (BA_RW)) >> 9)  << 3;
  443:                                 switch (p->atr >> 24) {             // LE only
  444:                                 case 2: wcr |= 0x060 << (p->addr & 2); break;
  445:                                 case 4: wcr |= 0x1e0; break;
  446:                                 default: /* do nothing */ break;
  447:                                 }
  448:                                 setWVR(obcnt, p->addr & ~3);
  449:                                 setWCR(obcnt, wcr | 7);
  450:                                 obcnt++;
  451:                         } else if (p->atr & BA_I) {
  452:                                 if (!EnableCP14()) continue;
  453:                                 bcr = getBCR(ibcnt);
  454:                                 bcr &= ~0x007FC1E7;
  455:                                 bcr |= (p->addr & 2) ? 0x180 : 0x060;       // LE
  456:                                 setBVR(ibcnt, p->addr & ~3);
  457:                                 setBCR(ibcnt, bcr | 7);
  458:                                 ibcnt++;
  459:                         } else if (p->atr & BA_SET) {
  460:                                 readMem(p->addr, &p->code, p->sz, 2);
  461:                                 writeMem(p->addr, &sbpCode.b[p->sz], p->sz, 2);
  462:                         }
  463:                 }
  464: 
  465:                 // if hardware breakpoint is not used at all
  466:                 // monitor debug mode is turned off
  467:                 if (ibcnt == 0 && obcnt == 0) DisableCP14();
  468:         }
  469:         stepFlg = 0;           // clear temporary step execution flag
  470: }
  471: /*
  472:         stop tracing
  473: */
  474: EXPORT  void     stopTrace(void)
  475: {
  476:         traceMode = traceStep = 0;
  477: }
  478: /*
  479:         process program execution
  480: */
  481: EXPORT  ER       goTrace(W trace, UW pc, UW par)
  482: {
  483:         W      er;
  484: 
  485:         // set trace mode
  486:         if ((traceMode = trace) == 0) {                // normal execution
  487:                 // set temporary breakpoint
  488:                 if (par != 0) {
  489:                         er = setBreak(par, BA_S | BA_PRE | BA_TMP, NULL, 0);
  490:                         if (er < E_OK) return er;
  491:                 }
  492:         } else {               // trace execution
  493:                 traceStep = par;
  494:         }
  495:         setCurPC(pc);          // set execution start address
  496:         return E_OK;
  497: }
  498: /*
  499:         prcess break exception
  500: 
  501:         return 0: continue, 1: command execution (cmd : initial command line)
  502: */
  503: EXPORT  W        procBreak(W bpflg, UB **cmd)
  504: {
  505:         B      *mes;
  506:         BRKPT  *bp;
  507:         UW     pc, npc, wfar;
  508: 
  509:         bp = NULL;
  510: 
  511:         if (traceMode) {       // trace execution
  512:                 //PC holds the next PC value (by resetBreak())
  513:                 pc = getCurPC();
  514: 
  515:                 // disassembly display (next instruction)
  516:                 disAssemble(&pc, &npc, wrkBuf);
  517:                 DSP_F4(08X,pc, S,": ", S,wrkBuf, CH,'\n');
  518: 
  519:                 if (-- traceStep > 0) return 0;               // continue
  520:                 stopTrace();  // stop tracing
  521: 
  522:         } else {               // breakpoint
  523:                 // During temporary step execution, then do nothing and continue
  524:                 if (stepFlg) return 0;
  525: 
  526:                 pc = getCurPCX();     // break address has been adjusted
  527: 
  528:                 // this is not a breakpoint set by b command
  529:                 if ((bpflg & 0xF0) == 0) {
  530:                         DSP_F3(S,"Unknown break at H'", 08X,pc, CH,'\n');
  531:                         *cmd = NULL;
  532:                         return 1;
  533:                 }
  534: 
  535:                 bp = &brkPt[bpflg & 0xF];
  536:                 switch (bp->atr & (BA_S | BA_I | BA_O | BA_R | BA_W)) {
  537:                   case BA_S:          mes = "S";      break;
  538:                   case BA_I:          mes = "E";      break;
  539:                   case BA_O|BA_R:     mes = "R";  break;
  540:                   case BA_O|BA_W:     mes = "W";  break;
  541:                   case BA_O|BA_R|BA_W:        mes = "RW";    break;
  542:                   default:            mes = "?";        break;
  543:                 }
  544: 
  545:                 if ((bp->atr & BA_O) && CheckCP14()) {
  546:                         // the address of instruction that generated operand break
  547:                         // is fetched from WFAR
  548:                         wfar = getWFAR();
  549:                         wfar -= (getCurCPSR() & PSR_T) ? 4 : 8;
  550:                         DSP_F4(S,"Break (", S,mes, S,") at ", 08X,wfar);
  551:                         DSP_F3(S,"  (R15/PC:", 08X,pc, S,")\n");
  552:                 } else {
  553:                         DSP_F5(S,"Break (", S,mes, S,") at ", 08X,pc, CH,'\n');
  554:                 }
  555:         }
  556: 
  557:         // restore stopped instruction
  558:         *cmd = (bp && bp->cmd[0] != 0) ? bp->cmd : NULL;
  559:         return 1;      // wait for command
  560: }