git.rot13.org Git - goodfet/blob - client/GoodFETARM7.py

   1 #!/usr/bin/env python
   2 # GoodFET ARM Client Library
   3 #
   4 # Contributions and bug reports welcome.
   5 #
   6 # todo:
   7 #  * full cycle debugging.. halt to resume
   8 #  * ensure correct PC handling
   9 #  * flash manipulation (probably need to get the specific chip for this one)
  10 #  * set security (chip-specific)
  11
  12 import sys
  13 import time
  14 import struct
  15
  16 from GoodFET import GoodFET
  17 from intelhex import IntelHex
  18
  19
  20 #Global Commands
  21 READ  = 0x00
  22 WRITE = 0x01
  23 PEEK  = 0x02
  24 POKE  = 0x03
  25 SETUP = 0x10
  26 START = 0x20
  27 STOP  = 0x21
  28 CALL  = 0x30
  29 EXEC  = 0x31
  30 NOK   = 0x7E
  31 OK    = 0x7F
  32
  33 # ARM7TDMI JTAG commands
  34 IR_SHIFT =                  0x80
  35 DR_SHIFT =                  0x81
  36 RESETTAP =                  0x82
  37 RESETTARGET =               0x83
  38 DR_SHIFT_MORE =             0x87
  39 GET_REGISTER =              0x8d
  40 SET_REGISTER =              0x8e
  41 DEBUG_INSTR =               0x8f
  42 # Really ARM specific stuff
  43 WAIT_DBG =                  0x91
  44 CHAIN0 =                    0x93
  45 SCANCHAIN1 =                0x94
  46 EICE_READ =                 0x95
  47 EICE_WRITE =                0x96
  48
  49 IR_EXTEST           =  0x0
  50 IR_SCAN_N           =  0x2
  51 IR_SAMPLE           =  0x3
  52 IR_RESTART          =  0x4
  53 IR_CLAMP            =  0x5
  54 IR_HIGHZ            =  0x7
  55 IR_CLAMPZ           =  0x9
  56 IR_INTEST           =  0xC
  57 IR_IDCODE           =  0xE
  58 IR_BYPASS           =  0xF
  59
  60 DBG_DBGACK =    1
  61 DBG_DBGRQ =     2
  62 DBG_IFEN =      4
  63 DBG_cgenL =     8
  64 DBG_TBIT =      16
  65
  66
  67 EICE_DBGCTRL =                     0  # read 3 bit - Debug Control
  68 EICE_DBGCTRL_BITLEN =              3
  69 EICE_DBGSTATUS =                   1  # read 5 bit - Debug Status
  70 EICE_DBGSTATUS_BITLEN =            5
  71 EICE_DBGCCR =                      4  # read 6 bit - Debug Comms Control Register
  72 EICE_DBGCCR_BITLEN =               6
  73 EICE_DBGCDR =                      5  # r/w 32 bit - Debug Comms Data Register
  74 EICE_WP0ADDR =                     8  # r/w 32 bit - Watchpoint 0 Address
  75 EICE_WP0ADDRMASK =                 9  # r/w 32 bit - Watchpoint 0 Addres Mask
  76 EICE_WP0DATA =                     10 # r/w 32 bit - Watchpoint 0 Data
  77 EICE_WP0DATAMASK =                 11 # r/w 32 bit - Watchpoint 0 Data Masl
  78 EICE_WP0CTRL =                     12 # r/w 9 bit - Watchpoint 0 Control Value
  79 EICE_WP0CTRLMASK =                 13 # r/w 8 bit - Watchpoint 0 Control Mask
  80 EICE_WP1ADDR =                     16 # r/w 32 bit - Watchpoint 0 Address
  81 EICE_WP1ADDRMASK =                 17 # r/w 32 bit - Watchpoint 0 Addres Mask
  82 EICE_WP1DATA =                     18 # r/w 32 bit - Watchpoint 0 Data
  83 EICE_WP1DATAMASK =                 19 # r/w 32 bit - Watchpoint 0 Data Masl
  84 EICE_WP1CTRL =                     20 # r/w 9 bit - Watchpoint 0 Control Value
  85 EICE_WP1CTRLMASK =                 21 # r/w 8 bit - Watchpoint 0 Control Mask
  86
  87 MSB         = 0
  88 LSB         = 1
  89 NOEND       = 2
  90 NORETIDLE   = 4
  91
  92 F_TBIT      = 1<<40
  93
  94 PM_usr = 0b10000
  95 PM_fiq = 0b10001
  96 PM_irq = 0b10010
  97 PM_svc = 0b10011
  98 PM_abt = 0b10111
  99 PM_und = 0b11011
 100 PM_sys = 0b11111
 101
 102 proc_modes = {
 103     0:      ("UNKNOWN, MESSED UP PROCESSOR MODE","fsck", "This should Never happen.  MCU is in funky state!"),
 104     PM_usr: ("User Processor Mode", "usr", "Normal program execution mode"),
 105     PM_fiq: ("FIQ Processor Mode", "fiq", "Supports a high-speed data transfer or channel process"),
 106     PM_irq: ("IRQ Processor Mode", "irq", "Used for general-purpose interrupt handling"),
 107     PM_svc: ("Supervisor Processor Mode", "svc", "A protected mode for the operating system"),
 108     PM_abt: ("Abort Processor Mode", "abt", "Implements virtual memory and/or memory protection"),
 109     PM_und: ("Undefined Processor Mode", "und", "Supports software emulation of hardware coprocessor"),
 110     PM_sys: ("System Processor Mode", "sys", "Runs privileged operating system tasks (ARMv4 and above)"),
 111 }
 112
 113 PSR_bits = [
 114     None, None, None, None, None, "Thumb", "nFIQ_int", "nIRQ_int",
 115     "nImprDataAbort_int", "BIGendian", None, None, None, None, None, None,
 116     "GE_0", "GE_1", "GE_2", "GE_3", None, None, None, None,
 117     "Jazelle", None, None, "Q (DSP-overflow)", "oVerflow", "Carry", "Zero", "Neg",
 118     ]
 119
 120 ARM_INSTR_NOP =             0xe1a00000L
 121 ARM_INSTR_BX_R0 =           0xe12fff10L
 122 ARM_INSTR_STR_Rx_r14 =      0xe58f0000L # from atmel docs
 123 ARM_READ_REG =              ARM_INSTR_STR_Rx_r14
 124 ARM_INSTR_LDR_Rx_r14 =      0xe59f0000L # from atmel docs
 125 ARM_WRITE_REG =             ARM_INSTR_LDR_Rx_r14
 126 ARM_INSTR_LDR_R1_r0_4 =     0xe4901004L
 127 ARM_READ_MEM =              ARM_INSTR_LDR_R1_r0_4
 128 ARM_INSTR_STR_R1_r0_4 =     0xe4801004L
 129 ARM_WRITE_MEM =             ARM_INSTR_STR_R1_r0_4
 130 ARM_INSTR_STRB_R1_r0_1 =    0xe4c01001L
 131 ARM_WRITE_MEM_BYTE =        ARM_INSTR_STRB_R1_r0_1
 132 ARM_INSTR_MRS_R0_CPSR =     0xe10f0000L
 133 ARM_INSTR_MSR_cpsr_cxsf_R0 =0xe12ff000L
 134 ARM_INSTR_STMIA_R14_r0_rx = 0xE88e0000L      # add up to 65k to indicate which registers...
 135 ARM_INSTR_LDMIA_R14_r0_rx = 0xE89e0000L      # add up to 65k to indicate which registers...
 136 ARM_STORE_MULTIPLE =        ARM_INSTR_STMIA_R14_r0_rx
 137 ARM_INSTR_SKANKREGS =       0xE88F7fffL
 138 ARM_INSTR_CLOBBEREGS =      0xE89F7fffL
 139
 140 ARM_INSTR_B_IMM =           0xea000000L
 141 ARM_INSTR_B_PC =            0xea000000L
 142 ARM_INSTR_BX_PC =           0xe1200010L      # need to set r0 to the desired address
 143 THUMB_INSTR_LDR_R0_r0 =     0x68006800L
 144 THUMB_WRITE_REG =           THUMB_INSTR_LDR_R0_r0
 145 THUMB_INSTR_STR_R0_r0 =     0x60006000L
 146 THUMB_READ_REG =            THUMB_INSTR_STR_R0_r0
 147 THUMB_INSTR_MOV_R0_PC =     0x46b846b8L
 148 THUMB_INSTR_MOV_PC_R0 =     0x46474647L
 149 THUMB_INSTR_BX_PC =         0x47784778L
 150 THUMB_INSTR_NOP =           0x1c001c00L
 151 THUMB_INSTR_B_IMM =         0xe000e000L
 152 ARM_REG_PC =                15
 153
 154 nRW         = 0
 155 MAS0        = 1
 156 MAS1        = 2
 157 nOPC        = 3
 158 nTRANS      = 4
 159 EXTERN      = 5
 160 CHAIN       = 6
 161 RANGE       = 7
 162 ENABLE      = 8
 163
 164 DBGCTRLBITS = {
 165         'nRW':nRW,
 166         'MAS0':MAS0,
 167         'MAS1':MAS1,
 168         'nOPC':nOPC,
 169         'nTRANS':nTRANS,
 170         'EXTERN':EXTERN,
 171         'CHAIN':CHAIN,
 172         'RANGE':RANGE,
 173         'ENABLE':ENABLE,
 174         1<<nRW:'nRW',
 175         1<<MAS0:'MAS0',
 176         1<<MAS1:'MAS1',
 177         1<<nOPC:'nOPC',
 178         1<<nTRANS:'nTRANS',
 179         1<<EXTERN:'EXTERN',
 180         1<<CHAIN:'CHAIN',
 181         1<<RANGE:'RANGE',
 182         1<<ENABLE:'ENABLE',
 183         }
 184
 185 LDM_BITMASKS = [(1<<x)-1 for x in xrange(16)]
 186 #### TOTALLY BROKEN, NEED VALIDATION AND TESTING
 187 PCOFF_DBGRQ = 4 * 4
 188 PCOFF_WATCH = 4 * 4
 189 PCOFF_BREAK = 4 * 4
 190
 191
 192 def debugstr(strng):
 193     print >>sys.stderr,(strng)
 194
 195 def PSRdecode(psrval):
 196     output = [ "(%s mode)"%proc_modes[psrval&0x1f][1] ]
 197     for x in xrange(5,32):
 198         if psrval & (1<<x):
 199             output.append(PSR_bits[x])
 200     return " ".join(output)
 201
 202 fmt = [None, "B", "<H", None, "<L", None, None, None, "<Q"]
 203
 204 def chop(val,byts):
 205     s = struct.pack(fmt[byts], val)
 206     return [ord(b) for b in s ]
 207
 208 class GoodFETARM7(GoodFET):
 209     """A GoodFET variant for use with ARM7TDMI microprocessor."""
 210     def __init__(self):
 211         GoodFET.__init__(self)
 212         self.storedPC =         0xffffffff
 213         self.current_dbgstate = 0xffffffff
 214         self.flags =            0xffffffff
 215         self.nothing =          0xffffffff
 216         self.stored_regs = []
 217     def __del__(self):
 218         try:
 219             if (self.ARMget_dbgstate()&9) == 9:
 220                 self.resume()
 221         except:
 222             sys.excepthook(*sys.exc_info())
 223     def setup(self):
 224         """Move the FET into the JTAG ARM application."""
 225         #print "Initializing ARM."
 226         self.writecmd(0x13,SETUP,0,self.data)
 227     def flash(self,file):
 228         """Flash an intel hex file to code memory."""
 229         print "Flash not implemented.";
 230     def dump(self,fn,start=0,stop=0xffffffff):
 231         """Dump an intel hex file from code memory."""
 232
 233         print "Dumping from %04x to %04x as %s." % (start,stop,f);
 234         # FIXME: get mcu state and return it to that state
 235         self.halt()
 236
 237         h = IntelHex(None);
 238         i=start;
 239         while i<=stop:
 240             data=self.ARMreadChunk(i, 48, verbose=0);
 241             print "Dumped %06x."%i;
 242             for dword in data:
 243                 if i<=stop and dword != 0xdeadbeef:
 244                     h.puts( i, struct.pack("<I", dword) )
 245                 i+=4;
 246         # FIXME: get mcu state and return it to that state
 247         self.resume()
 248         h.write_hex_file(fn);
 249
 250         print "Dump not implemented.";
 251     def ARMshift_IR(self, IR, noretidle=0):
 252         self.writecmd(0x13,IR_SHIFT,2, [IR, LSB|noretidle])
 253         return self.data
 254     def ARMshift_DR(self, data, bits, flags):
 255         self.writecmd(0x13,DR_SHIFT,14,[bits&0xff, flags&0xff, 0, 0, data&0xff,(data>>8)&0xff,(data>>16)&0xff,(data>>24)&0xff, (data>>32)&0xff,(data>>40)&0xff,(data>>48)&0xff,(data>>56)&0xff,(data>>64)&0xff,(data>>72)&0xff])
 256         return self.data
 257     def ARMshift_DR_more(self, data, bits, flags):
 258         self.writecmd(0x13,DR_SHIFT_MORE,14,[bits&0xff, flags&0xff, 0, 0, data&0xff,(data>>8)&0xff,(data>>16)&0xff,(data>>24)&0xff, (data>>32)&0xff,(data>>40)&0xff,(data>>48)&0xff,(data>>56)&0xff,(data>>64)&0xff,(data>>72)&0xff])
 259         return self.data
 260     def ARMwaitDBG(self, timeout=0xff):
 261         self.current_dbgstate = self.ARMget_dbgstate()
 262         while ( not ((self.current_dbgstate & 9L) == 9)):
 263             timeout -=1
 264             self.current_dbgstate = self.ARMget_dbgstate()
 265         return timeout
 266     def ARMident(self):
 267         """Get an ARM's ID."""
 268         self.ARMshift_IR(IR_IDCODE,0)
 269         self.ARMshift_DR(0,32,LSB)
 270         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
 271         return retval
 272     def ARMidentstr(self):
 273         ident=self.ARMident()
 274         ver     = (ident >> 28)
 275         partno  = (ident >> 12) & 0xffff
 276         mfgid   = (ident >> 1)  & 0x7ff
 277         return "Chip IDCODE:    0x%x\tver: %x\tpartno: %x\tmfgid: %x" % (ident, ver, partno, mfgid);
 278     def ARMeice_write(self, reg, val):
 279         data = chop(val,4)
 280         data.extend([reg])
 281         retval = self.writecmd(0x13, EICE_WRITE, 5, data)
 282         return retval
 283     def ARMeice_read(self, reg):
 284         self.writecmd(0x13, EICE_READ, 1, [reg])
 285         retval, = struct.unpack("<L",self.data)
 286         return retval
 287     def ARMget_dbgstate(self):
 288         """Read the config register of an ARM."""
 289         self.ARMeice_read(EICE_DBGSTATUS)
 290         self.current_dbgstate = struct.unpack("<L", self.data[:4])[0]
 291         return self.current_dbgstate
 292     status = ARMget_dbgstate
 293     def statusstr(self):
 294         """Check the status as a string."""
 295         status=self.status()
 296         str=""
 297         i=1
 298         while i<0x20:
 299             if(status&i):
 300                 str="%s %s" %(self.ARMstatusbits[i],str)
 301             i*=2
 302         return str
 303     def ARMget_dbgctrl(self):
 304         """Read the config register of an ARM."""
 305         self.ARMeice_read(EICE_DBGCTRL)
 306         retval = struct.unpack("<L", self.data[:4])[0]
 307         return retval
 308     def ARMset_dbgctrl(self,config):
 309         """Write the config register of an ARM."""
 310         self.ARMeice_write(EICE_DBGCTRL, config&7)
 311     def ARMgetPC(self):
 312         """Get an ARM's PC. Note: real PC gets all wonky in debug mode, this is the "saved" PC"""
 313         return self.storedPC
 314     getpc = ARMgetPC
 315     def ARMsetPC(self, val):
 316         """Set an ARM's PC.  Note: real PC gets all wonky in debug mode, this changes the "saved" PC which is used when exiting debug mode"""
 317         self.storedPC = val
 318     def ARMget_register(self, reg):
 319         """Get an ARM's Register"""
 320         self.writecmd(0x13,GET_REGISTER,1,[reg&0xf])
 321         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
 322         return retval
 323     def ARMset_register(self, reg, val):
 324         """Get an ARM's Register"""
 325         self.writecmd(0x13,SET_REGISTER,8,[val&0xff, (val>>8)&0xff, (val>>16)&0xff, val>>24, reg,0,0,0])
 326         retval = struct.unpack("<L", "".join(self.data[0:4]))[0]
 327         return retval
 328     def ARMget_registers(self):
 329         """Get ARM Registers"""
 330         regs = [ self.ARMget_register(x) for x in range(15) ]
 331         regs.append(self.ARMgetPC())            # make sure we snag the "static" version of PC
 332         return regs
 333     def ARMset_registers(self, regs, mask):
 334         """Set ARM Registers"""
 335         for x in xrange(15):
 336           if (1<<x) & mask:
 337             self.ARMset_register(x,regs.pop(0))
 338         if (1<<15) & mask:                      # make sure we set the "static" version of PC or changes will be lost
 339           self.ARMsetPC(regs.pop(0))
 340     def ARMdebuginstr(self,instr,bkpt):
 341         if type (instr) == int or type(instr) == long:
 342             instr = struct.pack("<L", instr)
 343         instr = [int("0x%x"%ord(x),16) for x in instr]
 344         instr.extend([bkpt])
 345         self.writecmd(0x13,DEBUG_INSTR,len(instr),instr)
 346         return (self.data)
 347     def ARM_nop(self, bkpt=0):
 348         if self.status() & DBG_TBIT:
 349             return self.ARMdebuginstr(THUMB_INSTR_NOP, bkpt)
 350         return self.ARMdebuginstr(ARM_INSTR_NOP, bkpt)
 351     def ARMrestart(self):
 352         self.ARMshift_IR(IR_RESTART)
 353     def ARMset_watchpoint0(self, addr, addrmask, data, datamask, ctrl, ctrlmask):
 354         self.ARMeice_write(EICE_WP0ADDR, addr);           # write 0 in watchpoint 0 address
 355         self.ARMeice_write(EICE_WP0ADDRMASK, addrmask);   # write 0xffffffff in watchpoint 0 address mask
 356         self.ARMeice_write(EICE_WP0DATA, data);           # write 0 in watchpoint 0 data
 357         self.ARMeice_write(EICE_WP0DATAMASK, datamask);   # write 0xffffffff in watchpoint 0 data mask
 358         self.ARMeice_write(EICE_WP0CTRL, ctrl);           # write 0x00000100 in watchpoint 0 control value register (enables watchpoint)
 359         self.ARMeice_write(EICE_WP0CTRLMASK, ctrlmask);   # write 0xfffffff7 in watchpoint 0 control mask - only detect the fetch instruction
 360         return self.data
 361     def ARMset_watchpoint1(self, addr, addrmask, data, datamask, ctrl, ctrlmask):
 362         self.ARMeice_write(EICE_WP1ADDR, addr);           # write 0 in watchpoint 1 address
 363         self.ARMeice_write(EICE_WP1ADDRMASK, addrmask);   # write 0xffffffff in watchpoint 1 address mask
 364         self.ARMeice_write(EICE_WP1DATA, data);           # write 0 in watchpoint 1 data
 365         self.ARMeice_write(EICE_WP1DATAMASK, datamask);   # write 0xffffffff in watchpoint 1 data mask
 366         self.ARMeice_write(EICE_WP1CTRL, ctrl);           # write 0x00000100 in watchpoint 1 control value register (enables watchpoint)
 367         self.ARMeice_write(EICE_WP1CTRLMASK, ctrlmask);   # write 0xfffffff7 in watchpoint 1 control mask - only detect the fetch instruction
 368         return self.data
 369     def THUMBgetPC(self):
 370         THUMB_INSTR_STR_R0_r0 =     0x60006000L
 371         THUMB_INSTR_MOV_R0_PC =     0x46b846b8L
 372         THUMB_INSTR_BX_PC =         0x47784778L
 373         THUMB_INSTR_NOP =           0x1c001c00L
 374
 375         r0 = self.ARMget_register(0)
 376         self.ARMdebuginstr(THUMB_INSTR_MOV_R0_PC, 0)
 377         retval = self.ARMget_register(0)
 378         self.ARMset_register(0,r0)
 379         return retval
 380     def ARMcapture_system_state(self, pcoffset):
 381         self.c0Data, self.flags, self.c0Addr = self.ARMchain0(0)
 382         if self.ARMget_dbgstate() & DBG_TBIT:
 383             pcoffset += 8
 384         else:
 385             pcoffset += 4
 386         self.storedPC = self.ARMget_register(15) + pcoffset
 387         self.last_dbg_state = self.ARMget_dbgstate()
 388         self.cpsr = self.ARMget_regCPSR()
 389         #print "ARMcapture_system_state: stored pc: 0x%x  last_dbg_state: 0x%x" % (self.storedPC, self.last_dbg_state)
 390
 391     #def ARMhaltcpu(self):
 392     def halt(self):
 393         """Halt the CPU."""
 394         if self.ARMget_dbgstate()&DBG_DBGACK:
 395             if not len(self.stored_regs):
 396                 #print "stored regs: " + repr(self.stored_regs)
 397                 self.stored_regs = self.ARMget_registers()[:15]
 398                 print self.print_stored_registers()
 399             return
 400         print "halting cpu"
 401         self.ARMset_dbgctrl(2)
 402         if (self.ARMwaitDBG() == 0):
 403             raise Exception("Timeout waiting to enter DEBUG mode on HALT")
 404         self.ARMset_dbgctrl(0)
 405
 406         self.ARMcapture_system_state(PCOFF_DBGRQ)
 407         #print "storedPC: %x (%x)      flags: %x    nothing: %x" % (self.storedPC, self.c0Data, self.flags, self.c0Addr)
 408         if self.ARMget_dbgstate() & DBG_TBIT:
 409             self.ARMsetModeARM()
 410             if self.storedPC ^ 4:
 411                 self.ARMset_register(15,self.storedPC&0xfffffffc)
 412         self.stored_regs = self.ARMget_registers()[:15]
 413         #print "stored regs: " + repr(self.stored_regs)
 414         #print self.print_stored_registers()
 415         #print "CPSR: (%s) %s"%(self.ARMget_regCPSRstr())
 416     #halt = ARMhaltcpu
 417
 418     #def ARMreleasecpu(self):
 419     def resume(self):
 420         """Resume the CPU."""
 421         # FIXME: restore CPSR
 422         # FIXME: true up PC to exactly where we left off...
 423         if not self.ARMget_dbgstate()&DBG_DBGACK:
 424             return
 425         print "resume"
 426         if len(self.stored_regs):
 427             #print self.print_stored_registers()
 428             self.ARMset_registers(self.stored_regs, 0x7fff)
 429         else:
 430             print "skipping restore of stored registers due to empty list ?  WTFO?"
 431
 432         currentPC, self.currentflags, nothing = self.ARMchain0(self.storedPC,self.flags, self.c0Addr)
 433         if not(self.flags & F_TBIT):                                    # need to be in arm mode
 434             if self.currentflags & F_TBIT:                              # currently in thumb mode
 435                 self.ARMsetModeARM()
 436             # branch to the right address
 437             self.ARMset_register(15, self.storedPC)
 438             #print hex(self.storedPC)
 439             #print hex(self.ARMget_register(15))
 440             #print hex(self.ARMchain0(self.storedPC,self.flags)[0])
 441             self.ARMchain0(self.storedPC,self.flags)
 442             self.ARM_nop(0)
 443             self.ARM_nop(1)
 444             self.ARMdebuginstr(ARM_INSTR_B_IMM | 0xfffff0,0)
 445             self.ARM_nop(0)
 446             self.ARMrestart()
 447
 448         elif self.flags & F_TBIT:                                       # need to be in thumb mode
 449             if not (self.currentflags & F_TBIT):                        # currently in arm mode
 450                 self.ARMsetModeThumb()
 451             r0=self.ARMget_register(0)
 452             self.ARMset_register(0, self.storedPC)
 453             self.ARMdebuginstr(THUMB_INSTR_MOV_PC_R0,0)
 454             self.ARM_nop(0)
 455             self.ARM_nop(1)
 456             #print hex(self.storedPC)
 457             #print hex(self.ARMget_register(15))
 458             #print hex(self.ARMchain0(self.storedPC,self.flags)[0])
 459             self.ARMchain0(self.storedPC,self.flags)[0]
 460             self.ARMdebuginstr(THUMB_INSTR_B_IMM | (0x7fc07fc),0)
 461             self.ARM_nop(0)
 462             self.ARMrestart()
 463
 464         #print >>sys.stderr,"Debug Status:\t%s\n" % self.statusstr()
 465         #print >>sys.stderr,"CPSR: (%s) %s"%(self.ARMget_regCPSRstr())
 466
 467     #resume = ARMreleasecpu
 468
 469     def resettap(self):
 470         self.writecmd(0x13, RESETTAP, 0,[])
 471
 472     def ARMsetModeARM(self):
 473         r0 = None
 474         if ((self.current_dbgstate & DBG_TBIT)):
 475             #debugstr("=== Switching to ARM mode ===")
 476             self.ARM_nop(0)
 477             self.ARMdebuginstr(THUMB_INSTR_BX_PC,0)
 478             self.ARM_nop(0)
 479             self.ARM_nop(0)
 480         self.resettap()
 481         self.current_dbgstate = self.ARMget_dbgstate();
 482         return self.current_dbgstate
 483
 484     def ARMsetModeThumb(self):                               # needs serious work and truing
 485         self.resettap()
 486         #debugstr("=== Switching to THUMB mode ===")
 487         if ( not (self.current_dbgstate & DBG_TBIT)):
 488             self.storedPC |= 1
 489             r0 = self.ARMget_register(0)
 490             self.ARMset_register(0, self.storedPC)
 491             self.ARM_nop(0)
 492             self.ARMdebuginstr(ARM_INSTR_BX_R0,0)
 493             self.ARM_nop(0)
 494             self.ARM_nop(0)
 495             self.resettap()
 496             self.ARMset_register(0,r0)
 497         self.current_dbgstate = self.ARMget_dbgstate();
 498         return self.current_dbgstate
 499
 500     def ARMget_regCPSRstr(self):
 501         psr = self.ARMget_regCPSR()
 502         return hex(psr), PSRdecode(psr)
 503
 504     def ARMget_regCPSR(self):
 505         """Get an ARM's Register"""
 506         r0 = self.ARMget_register(0)
 507         self.ARM_nop( 0) # push nop into pipeline - clean out the pipeline...
 508         self.ARMdebuginstr(ARM_INSTR_MRS_R0_CPSR, 0) # push MRS_R0, CPSR into pipeline - fetch
 509         self.ARM_nop( 0) # push nop into pipeline - decoded
 510         self.ARM_nop( 0) # push nop into pipeline - execute
 511         retval = self.ARMget_register(0)
 512         self.ARMset_register(0, r0)
 513         return retval
 514
 515     def ARMset_regCPSR(self, val):
 516         """Get an ARM's Register"""
 517         r0 = self.ARMget_register(0)
 518         self.ARMset_register(0, val)
 519         self.ARM_nop( 0)        # push nop into pipeline - clean out the pipeline...
 520         self.ARMdebuginstr(ARM_INSTR_MSR_cpsr_cxsf_R0, 0) # push MSR cpsr_cxsf, R0 into pipeline - fetch
 521         self.ARM_nop( 0)        # push nop into pipeline - decoded
 522         self.ARM_nop( 0)        # push nop into pipeline - execute
 523         self.ARMset_register(0, r0)
 524         return(val)
 525
 526     '''
 527     def ARMreadMem(self, adr, wrdcount=1):
 528         retval = []
 529         r0 = self.ARMget_register(0);        # store R0 and R1
 530         r1 = self.ARMget_register(1);
 531         #print >>sys.stderr,("CPSR:\t%x"%self.ARMget_regCPSR())
 532         self.ARMset_register(0, adr);        # write address into R0
 533         self.ARMset_register(1, 0xdeadbeef)
 534         for word in range(adr, adr+(wrdcount*4), 4):
 535             #sys.stdin.readline()
 536             self.ARM_nop(0)
 537             self.ARM_nop(1)
 538             self.ARMdebuginstr(ARM_READ_MEM, 0); # push LDR R1, [R0], #4 into instruction pipeline  (autoincrements for consecutive reads)
 539             self.ARM_nop(0)
 540             self.ARMrestart()
 541             self.ARMwaitDBG()
 542             #print hex(self.ARMget_register(1))
 543
 544             # FIXME: this may end up changing te current debug-state.  should we compare to current_dbgstate?
 545             #print repr(self.data[4])
 546             if (len(self.data)>4 and self.data[4] == '\x00'):
 547               print >>sys.stderr,("FAILED TO READ MEMORY/RE-ENTER DEBUG MODE")
 548               raise Exception("FAILED TO READ MEMORY/RE-ENTER DEBUG MODE")
 549               return (-1);
 550             else:
 551               retval.append( self.ARMget_register(1) )  # read memory value from R1 register
 552               #print >>sys.stderr,("CPSR: %x\t\tR0: %x\t\tR1: %x"%(self.ARMget_regCPSR(),self.ARMget_register(0),self.ARMget_register(1)))
 553         self.ARMset_register(1, r1);       # restore R0 and R1
 554         self.ARMset_register(0, r0);
 555         return retval
 556         '''
 557
 558     def ARMreadStream(self, addr, bytecount):
 559         baseaddr    = addr & 0xfffffffc
 560         endaddr     = ((addr + bytecount + 3) & 0xfffffffc)
 561         diffstart   = 4 - (addr - baseaddr)
 562         diffend     = 4 - (endaddr - (addr + bytecount ))
 563
 564
 565         out = []
 566         data = [ x for x in self.ARMreadChunk( baseaddr, ((endaddr-baseaddr) / 4) ) ]
 567         #print data, hex(baseaddr), hex(diffstart), hex(endaddr), hex(diffend)
 568         if len(data) == 1:
 569             #print "single dword"
 570             out.append( struct.pack("<I", data.pop(0)) [4-diffstart:diffend] )
 571         else:
 572             #print "%d dwords" % len(data)
 573             if diffstart:
 574                 out.append( struct.pack("<I", data.pop(0)) [4-diffstart:] )
 575                 bytecount -= (diffstart)
 576                 #print out
 577
 578             for ent in data[:-1]:
 579                 out.append( struct.pack("<I", data.pop(0) ) )
 580                 bytecount -= 4
 581                 #print out
 582
 583             if diffend and bytecount>0:
 584                 out.append( struct.pack("<I", data.pop(0)) [:diffend] )
 585                 #print out
 586         return ''.join(out)
 587
 588     def ARMprintChunk(self, adr, wordcount=1, verbose=False, width=8):
 589         for string in self.ARMreprChunk(adr, wordcount, verbose=False, width=8):
 590             sys.stdout.write(string)
 591
 592     def ARMreprChunk(self, adr, wordcount=1, verbose=False, width=8):
 593         adr &= 0xfffffffc
 594         endva = adr + (4*wordcount)
 595         output = [ "Dwords from 0x%x through 0x%x" % (adr, endva) ]
 596         idx = 0
 597         for data in self.ARMreadChunk(adr, wordcount, verbose):
 598             if (idx % width) == 0:
 599                 yield ( "\n0x%.8x\t" % (adr + (4*idx)) )
 600             yield ( "%.8x   " % (data) )
 601             idx += 1
 602
 603         yield("\n")
 604
 605     def ARMreadChunk(self, adr, wordcount=1, verbose=True):
 606         """ Only works in ARM mode currently
 607         WARNING: Addresses must be word-aligned!
 608         """
 609         regs = self.ARMget_registers()
 610         self.ARMset_registers([0xdeadbeef for x in xrange(14)], 0xe)
 611         count = wordcount
 612         while (wordcount > 0):
 613             if (verbose and wordcount%64 == 0):  sys.stderr.write(".")
 614             count = (wordcount, 0xe)[wordcount>0xd]
 615             bitmask = LDM_BITMASKS[count]
 616             self.ARMset_register(14,adr)
 617             self.ARM_nop(1)
 618             self.ARMdebuginstr(ARM_INSTR_LDMIA_R14_r0_rx | bitmask ,0)
 619             #FIXME: do we need the extra nop here?
 620             self.ARMrestart()
 621             self.ARMwaitDBG()
 622             for x in range(count):
 623                 yield self.ARMget_register(x)
 624             wordcount -= count
 625             adr += count*4
 626             #print hex(adr)
 627         # FIXME: handle the rest of the wordcount here.
 628         self.ARMset_registers(regs,0xe)
 629
 630     '''def ARMreadStream(self, adr, bytecount):
 631         #data = [struct.unpack("<L", x) for x in self.ARMreadChunk(adr, (bytecount-1/4)+1)]
 632         diff = adr % 4
 633         address = adr - diff
 634         data = [ struct.pack("<L", x) for x in self.ARMreadChunk(address, (bytecount-1/4)+1) ]
 635         return "".join(data)[diff:bytecount]'''
 636
 637     ARMreadMem = ARMreadChunk
 638     peek = ARMreadMem
 639
 640     def ARMwriteChunk(self, adr, wordarray):
 641         """ Only works in ARM mode currently
 642         WARNING: Addresses must be word-aligned!
 643         """
 644         regs = self.ARMget_registers()
 645         wordcount = len(wordarray)
 646         while (wordcount > 0):
 647             if (wordcount%64 == 0):  sys.stderr.write(".")
 648             count = (wordcount, 0xe)[wordcount>0xd]
 649             bitmask = LDM_BITMASKS[count]
 650             self.ARMset_register(14,adr)
 651             #print len(wordarray),bin(bitmask)
 652             self.ARMset_registers(wordarray[:count],bitmask)
 653             self.ARM_nop(1)
 654             self.ARMdebuginstr(ARM_INSTR_STMIA_R14_r0_rx | bitmask ,0)
 655             #FIXME: do we need the extra nop here?
 656             self.ARMrestart()
 657             self.ARMwaitDBG()
 658             wordarray = wordarray[count:]
 659             wordcount -= count
 660             adr += count*4
 661             #print hex(adr)
 662         # FIXME: handle the rest of the wordcount here.
 663         '''
 664     def ARMwriteMem(self, adr, wordarray, instr=ARM_WRITE_MEM):
 665         r0 = self.ARMget_register(0);        # store R0 and R1
 666         r1 = self.ARMget_register(1);
 667         #print >>sys.stderr,("CPSR:\t%x"%self.ARMget_regCPSR())
 668         for widx in xrange(len(wordarray)):
 669             address = adr + (widx*4)
 670             word = wordarray[widx]
 671             self.ARMset_register(0, address);        # write address into R0
 672             self.ARMset_register(1, word);        # write address into R0
 673             self.ARM_nop(0)
 674             self.ARM_nop(1)
 675             self.ARMdebuginstr(instr, 0); # push STR R1, [R0], #4 into instruction pipeline  (autoincrements for consecutive writes)
 676             self.ARM_nop(0)
 677             self.ARMrestart()
 678             self.ARMwaitDBG()
 679             #print >>sys.stderr,hex(self.ARMget_register(1))
 680         self.ARMset_register(1, r1);       # restore R0 and R1
 681         self.ARMset_register(0, r0);
 682         '''
 683     ARMwriteMem = ARMwriteChunk
 684
 685     def ARMwriteStream(self, addr, datastr):
 686         #bytecount = len(datastr)
 687         #baseaddr    = addr & 0xfffffffc
 688         #diffstart   = addr - baseaddr
 689         #endaddr     = ((addr + bytecount) & 0xfffffffc) + 4
 690         #diffend     = 4 - (endaddr - (addr+bytecount))
 691         bytecount = len(datastr)
 692         baseaddr    = addr & 0xfffffffc
 693         endaddr     = ((addr + bytecount + 3) & 0xfffffffc)
 694         diffstart   = 4 - (addr - baseaddr)
 695         diffend     = 4 - (endaddr - (addr + bytecount ))
 696
 697         print hex(baseaddr), hex(diffstart), hex(endaddr), hex(diffend)
 698         out = []
 699         if diffstart:
 700             dword = self.ARMreadChunk(baseaddr, 1)[0] & (0xffffffff>>(8*diffstart))
 701             dst = "\x00" * (4-diffstart) + datastr[:diffstart]; print hex(dword), repr(dst)
 702             datachk = struct.unpack("<I", dst)[0]
 703             out.append( dword+datachk )
 704             datastr = datastr[diffstart:]
 705             bytecount -= diffstart
 706         for ent in xrange(baseaddr+4, endaddr-4, 4):
 707             print repr(datastr)
 708             dword = struct.unpack("<I", datastr[:4])[0]
 709             out.append( dword )
 710             datastr = datastr[4:]
 711             bytecount -= 4
 712         if diffend and bytecount:
 713             dword = self.ARMreadChunk(endaddr-4, 1)[0] & (0xffffffff<<(8*diffend))
 714             dst = datastr + "\x00" * (4-diffend); print hex(dword), repr(dst)
 715             datachk = struct.unpack("<I", dst)[0]
 716             out.append( dword+datachk )
 717         print repr([hex(x) for x in out])
 718         return self.ARMwriteChunk(baseaddr, out)
 719
 720
 721     def writeMemByte(self, adr, byte):
 722         self.ARMwriteMem(adr, byte, ARM_WRITE_MEM_BYTE)
 723
 724
 725     ARMstatusbits={
 726                   0x10 : "TBIT",
 727                   0x08 : "cgenL",
 728                   0x04 : "Interrupts Enabled (or not?)",
 729                   0x02 : "DBGRQ",
 730                   0x01 : "DGBACK"
 731                   }
 732     ARMctrlbits={
 733                   0x04 : "disable interrupts",
 734                   0x02 : "force dbgrq",
 735                   0x01 : "force dbgack"
 736                   }
 737     def ARMresettarget(self, delay=1000):
 738         return self.writecmd(0x13,RESETTARGET,2, [ delay&0xff, (delay>>8)&0xff ] )
 739
 740     def ARMchain0(self, address, bits=0x819684c054, data=0):
 741         bulk = chop(address,4)
 742         bulk.extend(chop(bits,8))
 743         bulk.extend(chop(data,4))
 744         #print >>sys.stderr,(repr(bulk))
 745         self.writecmd(0x13,CHAIN0,16,bulk)
 746         d1,b1,a1 = struct.unpack("<LQL",self.data)
 747         return (a1,b1,d1)
 748
 749     def start(self, ident=False):
 750         """Start debugging."""
 751         self.writecmd(0x13,START,0,self.data)
 752         if ident:
 753             print >>sys.stderr,"Identifying Target:"
 754             print >>sys.stderr, self.ARMidentstr()
 755             print >>sys.stderr,"Debug Status:\t%s\n" % self.statusstr()
 756
 757     def stop(self):
 758         """Stop debugging."""
 759         self.writecmd(0x13,STOP,0,self.data)
 760     #def ARMstep_instr(self):
 761     #    """Step one instruction."""
 762     #    self.writecmd(0x13,STEP_INSTR,0,self.data)
 763     #def ARMflashpage(self,adr):
 764     #    """Flash 2kB a page of flash from 0xF000 in XDATA"""
 765     #    data=[adr&0xFF,
 766     #          (adr>>8)&0xFF,
 767     #          (adr>>16)&0xFF,
 768     #          (adr>>24)&0xFF]
 769     #    print "Flashing buffer to 0x%06x" % adr
 770     #    self.writecmd(0x13,MASS_FLASH_PAGE,4,data)
 771
 772     def print_registers(self):
 773         return [ hex(x) for x in self.ARMget_registers() ]
 774
 775     def print_stored_registers(self):
 776         return [ hex(x) for x in self.stored_regs ]
 777
 778
 779
 780 ######### command line stuff #########
 781 from intelhex import IntelHex16bit, IntelHex
 782
 783 def arm7_syntax():
 784     print "Usage: %s verb [objects]\n" % sys.argv[0]
 785     print "%s info" % sys.argv[0]
 786     print "%s dump $foo.hex [0x$start 0x$stop]" % sys.argv[0]
 787     print "%s erase" % sys.argv[0]
 788     print "%s eraseinfo" % sys.argv[0]
 789     print "%s flash $foo.hex [0x$start 0x$stop]" % sys.argv[0]
 790     print "%s verify $foo.hex [0x$start 0x$stop]" % sys.argv[0]
 791     print "%s poke 0x$adr 0x$val" % sys.argv[0]
 792     print "%s peek 0x$start [0x$stop]" % sys.argv[0]
 793     print "%s reset" % sys.argv[0]
 794     sys.exit()
 795
 796 def arm7_main():
 797     ''' this function should be called from command line app '''
 798
 799     #Initialize FET and set baud rate
 800     client=GoodFETARM7()
 801     client.serInit()
 802
 803     client.setup()
 804     client.start()
 805
 806     arm7_cli_handler(client, sys.argv)
 807
 808 def arm7_cli_handler(client, argv):
 809     if(argv[1]=="info"):
 810         client.halt()
 811         print >>sys.stderr, client.ARMidentstr()
 812         print >>sys.stderr,"Debug Status:\t%s" % client.statusstr()
 813         print >>sys.stderr,"CPSR: (%s) %s\n"%(client.ARMget_regCPSRstr())
 814         client.resume()
 815
 816
 817     if(argv[1]=="dump"):
 818         f = sys.argv[2]
 819         start=0x00000000
 820         stop=0xFFFFFFFF
 821         if(len(sys.argv)>3):
 822             start=int(sys.argv[3],16)
 823         if(len(sys.argv)>4):
 824             stop=int(sys.argv[4],16)
 825
 826         print "Dumping from %04x to %04x as %s." % (start,stop,f)
 827         #h = IntelHex16bit(None)
 828         # FIXME: get mcu state and return it to that state
 829         client.halt()
 830
 831         try:
 832             h = IntelHex(None)
 833             i=start
 834             while i<=stop:
 835                 #data=client.ARMreadMem(i, 48)
 836                 data=client.ARMreadChunk(i, 48, verbose=0)
 837                 print "Dumped %06x."%i
 838                 for dword in data:
 839                     if i<=stop and dword != 0xdeadbeef:
 840                         h.puts( i, struct.pack("<I", dword) )
 841                     i+=4
 842             # FIXME: get mcu state and return it to that state
 843         except:
 844             print "Unknown error during read. Writing results to output file."
 845             print "Rename file with last address dumped %06x."%i
 846             pass
 847
 848         client.resume()
 849         h.write_hex_file(f)
 850
 851     '''
 852     if(sys.argv[1]=="erase"):
 853         print "Erasing main flash memory."
 854         client.ARMmasserase()
 855
 856     if(sys.argv[1]=="eraseinfo"):
 857         print "Erasing info memory."
 858         client.ARMinfoerase()
 859
 860
 861     '''
 862     if(sys.argv[1]=="ivt"):
 863         client.halt()
 864         client.ARMprintChunk(0x0,0x20)
 865         client.resume()
 866
 867     if(sys.argv[1]=="regs"):
 868         client.halt()
 869         for i in range(0,16):
 870             print "r%i=%04x" % (i,client.ARMget_register(i))
 871         client.resume()
 872
 873     if(sys.argv[1]=="flash"):
 874         f=sys.argv[2]
 875         start=0
 876         stop=0x10000
 877         if(len(sys.argv)>3):
 878             start=int(sys.argv[3],16)
 879         if(len(sys.argv)>4):
 880             stop=int(sys.argv[4],16)
 881
 882         client.halt()
 883         h = IntelHex16bit(f)
 884
 885         #Should this be default?
 886         #Makes flashing multiple images inconvenient.
 887         #client.ARMmasserase()
 888
 889         count=0; #Bytes in commit.
 890         first=0
 891         vals=[]
 892         last=0;  #Last address committed.
 893         for i in h._buf.keys():
 894             if((count>0x40 or last+2!=i) and count>0 and i&1==0):
 895                 #print "%i, %x, %x" % (len(vals), last, i)
 896                 client.ARMpokeflashblock(first,vals)
 897                 count=0
 898                 first=0
 899                 last=0
 900                 vals=[]
 901             if(i>=start and i<stop  and i&1==0):
 902                 val=h[i>>1]
 903                 if(count==0):
 904                     first=i
 905                 last=i
 906                 count+=2
 907                 vals+=[val&0xff,(val&0xff00)>>8]
 908                 if(i%0x100==0):
 909                     print "%04x" % i
 910         if count>0: #last commit, ivt
 911             client.ARMpokeflashblock(first,vals)
 912         client.resume()
 913
 914     if(sys.argv[1]=="verify"):
 915         f=sys.argv[2]
 916         start=0
 917         stop=0xFFFF
 918         if(len(sys.argv)>3):
 919             start=int(sys.argv[3],16)
 920         if(len(sys.argv)>4):
 921             stop=int(sys.argv[4],16)
 922
 923         client.halt()
 924         h = IntelHex16bit(f)
 925         for i in h._buf.keys():
 926             if(i>=start and i<stop and i&1==0):
 927                 peek=client.peek(i)
 928                 if(h[i>>1]!=peek):
 929                     print "ERROR at %04x, found %04x not %04x"%(i,peek,h[i>>1])
 930                 if(i%0x100==0):
 931                     print "%04x" % i
 932         client.resume()
 933
 934
 935     if(sys.argv[1]=="peek"):
 936         start = 0x0000
 937         if(len(sys.argv)>2):
 938             start=int(sys.argv[2],16)
 939
 940         stop = start+4
 941         if(len(sys.argv)>3):
 942             stop=int(sys.argv[3],16)
 943
 944         print "Peeking from %04x to %04x." % (start,stop)
 945         client.halt()
 946         for dword in client.ARMreadChunk(start, (stop-start)/4, verbose=0):
 947             print "%.4x: %.8x" % (start, dword)
 948             start += 4
 949         client.resume()
 950
 951     if(sys.argv[1]=="poke"):
 952         start=0x0000
 953         val=0x00
 954         if(len(sys.argv)>2):
 955             start=int(sys.argv[2],16)
 956         if(len(sys.argv)>3):
 957             val=int(sys.argv[3],16)
 958
 959         print "Poking %06x to become %04x." % (start,val)
 960         client.halt()
 961         #???while client.ARMreadMem(start)[0]&(~val)>0:
 962         client.ARMwriteChunk(start, [val])
 963         print "Poked to %.8x" % client.ARMreadMem(start)[0]
 964         client.resume()
 965
 966
 967     if(sys.argv[1]=="reset"):
 968         #Set PC to RESET vector's value.
 969
 970         #client.ARMsetPC(0x00000000)
 971         #client.ARMset_regCPSR(0)
 972         #client.ARMreleasecpu()
 973         client.ARMresettarget(1000)
 974
 975     #client.ARMreleasecpu()
 976     #client.ARMstop()
 977