00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "jtagpxa250.h"
00017 #include "stdio.h"
00018
00019 extern JTAGpxa250 *pxa250Ptr;
00020
00021
00022
00023 #define REG_PC 15
00024 #define REG_PSR 16
00025
00026 #define BREAKINST_ARM 0xe1200070
00027
00028 #define BREAKINST_THUMB 0xdf00
00029
00030 #define predicate(x) (x & 0xf0000000)
00031 #define PREDICATE_ALWAYS 0xe0000000
00032
00033 #define PCMASK (0x3)
00034
00035 #define pc_pointer(v) ((v) & ~PCMASK)
00036
00037 #define CC_C_BIT (1 << 29)
00038
00039 typedef unsigned int u32;
00040 typedef unsigned short u16;
00041
00042
00043 union debug_insn {
00044 u32 arm;
00045 u16 thumb;
00046 };
00047
00048 struct debug_entry {
00049 u32 address;
00050 union debug_insn insn;
00051 };
00052
00053 struct debug_info {
00054 int nsaved;
00055 struct debug_entry bp[2];
00056 };
00057
00058
00059 static inline unsigned int hweight16(unsigned int w)
00060 {
00061 unsigned int res = (w & 0x5555) + ((w >> 1) & 0x5555);
00062 res = (res & 0x3333) + ((res >> 2) & 0x3333);
00063 res = (res & 0x0F0F) + ((res >> 4) & 0x0F0F);
00064 return (res & 0x00FF) + ((res >> 8) & 0x00FF);
00065 }
00066
00067
00068
00069
00070
00071
00072
00073
00074 static inline long get_user_reg(int offset)
00075 {
00076 return pxa250Ptr->getSavePlace(offset);
00077 }
00078
00079
00080
00081
00082
00083
00084
00085 static inline int
00086 put_user_reg(int offset, long data)
00087 {
00088 pxa250Ptr->setSavePlace(offset,data);
00089 return 0;
00090 }
00091
00092 static inline int
00093 read_u32(unsigned long addr, u32 *res)
00094 {
00095 int ret;
00096
00097 if (addr & 0x3) {
00098 printf("FIXME: unalligned access not performed - returning crap\n");
00099 } else {
00100 pxa250Ptr->getData(addr, 1, res);
00101 }
00102
00103 return 0;
00104 }
00105
00106 static inline int
00107 read_instr(unsigned long addr, u32 *res)
00108 {
00109 int ret;
00110 u32 val;
00111 pxa250Ptr->getData(addr, 1, res);
00112
00113 return 0;
00114 }
00115
00116
00117
00118
00119 static unsigned long
00120 jelie_getrn(unsigned long insn)
00121 {
00122 unsigned int reg = (insn >> 16) & 15;
00123 unsigned long val;
00124
00125 val = get_user_reg(reg);
00126 if (reg == 15)
00127 val = pc_pointer(val + 8);
00128
00129 return val;
00130 }
00131
00132
00133
00134
00135 static unsigned long
00136 jelie_getaluop2(unsigned long insn)
00137 {
00138 unsigned long val;
00139 int shift;
00140 int type;
00141
00142 if (insn & 1 << 25) {
00143 val = insn & 255;
00144 shift = (insn >> 8) & 15;
00145 type = 3;
00146 } else {
00147 val = get_user_reg (insn & 15);
00148
00149 if (insn & (1 << 4))
00150 shift = (int)get_user_reg ((insn >> 8) & 15);
00151 else
00152 shift = (insn >> 7) & 31;
00153
00154 type = (insn >> 5) & 3;
00155 }
00156
00157 switch (type) {
00158 case 0: val <<= shift; break;
00159 case 1: val >>= shift; break;
00160 case 2:
00161 val = (((signed long)val) >> shift);
00162 break;
00163 case 3:
00164 val = (val >> shift) | (val << (32 - shift));
00165 break;
00166 }
00167 return val;
00168 }
00169
00170
00171
00172
00173 static unsigned long
00174 jelie_getldrop2(unsigned long insn)
00175 {
00176 unsigned long val;
00177 int shift;
00178 int type;
00179
00180 val = get_user_reg(insn & 15);
00181 shift = (insn >> 7) & 31;
00182 type = (insn >> 5) & 3;
00183
00184 switch (type) {
00185 case 0: val <<= shift; break;
00186 case 1: val >>= shift; break;
00187 case 2:
00188 val = (((signed long)val) >> shift);
00189 break;
00190 case 3:
00191 val = (val >> shift) | (val << (32 - shift));
00192 break;
00193 }
00194 return val;
00195 }
00196
00197 #define OP_MASK 0x01e00000
00198 #define OP_AND 0x00000000
00199 #define OP_EOR 0x00200000
00200 #define OP_SUB 0x00400000
00201 #define OP_RSB 0x00600000
00202 #define OP_ADD 0x00800000
00203 #define OP_ADC 0x00a00000
00204 #define OP_SBC 0x00c00000
00205 #define OP_RSC 0x00e00000
00206 #define OP_ORR 0x01800000
00207 #define OP_MOV 0x01a00000
00208 #define OP_BIC 0x01c00000
00209 #define OP_MVN 0x01e00000
00210
00211 static unsigned long
00212 get_branch_address(unsigned long pc, unsigned long insn)
00213 {
00214 u32 alt = 0;
00215
00216 switch (insn & 0x0e000000) {
00217 case 0x00000000:
00218 case 0x02000000: {
00219
00220
00221
00222 long aluop1, aluop2, ccbit;
00223
00224 if ((insn & 0xf000) != 0xf000)
00225 break;
00226
00227 aluop1 = jelie_getrn(insn);
00228 aluop2 = jelie_getaluop2(insn);
00229 ccbit = get_user_reg(REG_PSR) & CC_C_BIT ? 1 : 0;
00230
00231 switch (insn & OP_MASK) {
00232 case OP_AND: alt = aluop1 & aluop2; break;
00233 case OP_EOR: alt = aluop1 ^ aluop2; break;
00234 case OP_SUB: alt = aluop1 - aluop2; break;
00235 case OP_RSB: alt = aluop2 - aluop1; break;
00236 case OP_ADD: alt = aluop1 + aluop2; break;
00237 case OP_ADC: alt = aluop1 + aluop2 + ccbit; break;
00238 case OP_SBC: alt = aluop1 - aluop2 + ccbit; break;
00239 case OP_RSC: alt = aluop2 - aluop1 + ccbit; break;
00240 case OP_ORR: alt = aluop1 | aluop2; break;
00241 case OP_MOV: alt = aluop2; break;
00242 case OP_BIC: alt = aluop1 & ~aluop2; break;
00243 case OP_MVN: alt = ~aluop2; break;
00244 }
00245 break;
00246 }
00247
00248 case 0x04000000:
00249 case 0x06000000:
00250
00251
00252
00253 if ((insn & 0x0010f000) == 0x0010f000) {
00254 unsigned long base;
00255
00256 base = jelie_getrn(insn);
00257 if (insn & 1 << 24) {
00258 long aluop2;
00259
00260 if (insn & 0x02000000)
00261 aluop2 = jelie_getldrop2( insn);
00262 else
00263 aluop2 = insn & 0xfff;
00264
00265 if (insn & 1 << 23)
00266 base += aluop2;
00267 else
00268 base -= aluop2;
00269 }
00270 if (read_u32(base, &alt) == 0)
00271 alt = pc_pointer(alt);
00272 }
00273 break;
00274
00275 case 0x08000000:
00276
00277
00278
00279 if ((insn & 0x00108000) == 0x00108000) {
00280 unsigned long base;
00281 int nr_regs;
00282
00283 if (insn & (1 << 23)) {
00284 nr_regs = hweight16(insn & 65535) << 2;
00285
00286 if (!(insn & (1 << 24)))
00287 nr_regs -= 4;
00288 } else {
00289 if (insn & (1 << 24))
00290 nr_regs = -4;
00291 else
00292 nr_regs = 0;
00293 }
00294
00295 base = jelie_getrn(insn);
00296
00297 if (read_u32(base + nr_regs, &alt) == 0)
00298 alt = pc_pointer(alt);
00299 break;
00300 }
00301 break;
00302
00303 case 0x0a000000: {
00304
00305
00306
00307 signed long displ;
00308
00309
00310
00311
00312
00313
00314
00315 displ = (insn & 0x00ffffff) << 8;
00316 displ = (displ >> 6) + 8;
00317 if (displ != 0 && displ != 4)
00318 alt = pc + displ;
00319 }
00320 break;
00321 }
00322
00323 return alt;
00324 }
00325
00326 static int
00327 swap_insn(unsigned long addr, void *old_insn, void *new_insn, int size)
00328 {
00329
00330 pxa250Ptr->getData(addr,1,(unsigned int *)old_insn);
00331 pxa250Ptr->putData(addr,1,(unsigned int *)new_insn);
00332
00333 return 4;
00334 }
00335
00336 static void
00337 add_breakpoint(struct debug_info *dbg, unsigned long addr)
00338 {
00339 int nr = dbg->nsaved;
00340
00341 if (nr < 2) {
00342 u32 new_insn = BREAKINST_ARM;
00343 int res;
00344
00345 res = swap_insn(addr, &dbg->bp[nr].insn, &new_insn, 4);
00346
00347 if (res == 4) {
00348 dbg->bp[nr].address = addr;
00349 dbg->nsaved += 1;
00350 printf("software breakpoint on at %x\n",addr);
00351 }
00352 } else
00353 printf("too many breakpoints\n");
00354 }
00355
00356
00357
00358
00359
00360
00361 static void clear_breakpoint(struct debug_entry *bp)
00362 {
00363 unsigned long addr = bp->address;
00364 union debug_insn old_insn;
00365 int ret;
00366
00367 printf("software breakpoint off at %x\n",addr);
00368 ret = swap_insn(addr, &old_insn.arm,
00369 &bp->insn.arm, 4);
00370
00371 if (ret != 4 || old_insn.arm != BREAKINST_ARM) {
00372 printf("corrupted ARM breakpoint at "
00373 "0x%08lx (0x%08x)\n",addr, old_insn.arm);
00374 }
00375 }
00376
00377 struct debug_info dbg_s, *dbg=&dbg_s;
00378
00379 void jelie_set_bpt()
00380 {
00381 struct pt_regs *regs;
00382 unsigned long pc;
00383 u32 insn;
00384 int res;
00385
00386 pc=get_user_reg(REG_PC);
00387
00388 res = read_instr(pc, &insn);
00389 if (!res) {
00390 unsigned long alt;
00391
00392 dbg->nsaved = 0;
00393
00394 alt = get_branch_address(pc, insn);
00395 if (alt)
00396 add_breakpoint(dbg, alt);
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407 if (!alt || predicate(insn) != PREDICATE_ALWAYS)
00408 add_breakpoint(dbg, pc + 4);
00409 }
00410 }
00411
00412
00413
00414
00415
00416 void jelie_cancel_bpt()
00417 {
00418 int i, nsaved = dbg->nsaved;
00419
00420 dbg->nsaved = 0;
00421
00422 if (nsaved > 2) {
00423 printf("jelie_cancel_bpt: bogus nsaved: %d!\n", nsaved);
00424 nsaved = 2;
00425 }
00426
00427 for (i = 0; i < nsaved; i++)
00428 clear_breakpoint(dbg->bp+i);
00429 }
00430