jtagarm: a few cleanup things...
[goodfet] / client / goodfet.pic
index 5158912..b02b376 100755 (executable)
@@ -9,7 +9,7 @@
 #
 # Scott Livingston  <slivingston at caltech.edu>
 #
-# March, April 2010.
+# March-June 2010.
 
 
 import sys
@@ -24,8 +24,80 @@ from intelhex import IntelHex
 PICAPP = 0x34
 MONITORAPP = 0x00
 NOK = 0x7E
+
+VERB_CMDLIST = 0x88
+
+# 0x32 is the normal address of the TBLPAG register. On some PICs,
+# though, it appears at 0x54 (see DS39970B). Affected PIC families
+# include: PIC24FJxxx{DA1,DA2,GB2,GA3}xx
+tblpag = 0x32
+
+WRITE_MAX_POLL_ATTEMPTS = 100
+
 dev_table = { 0x00EE : "dsPIC33FJ128GP708",
-              0x00EF : "dsPIC33FJ128GP710" }
+              0x00EF : "dsPIC33FJ128GP710",
+              0x080A : "PIC24HJ12GP201",
+              0x080B : "PIC24HJ12GP202",
+              0x0444 : "PIC24FJ16GA002",
+              0x044C : "PIC24FJ16GA004",
+              0x0445 : "PIC24FJ32GA002",
+              0x044D : "PIC24FJ32GA004",
+              0x0446 : "PIC24FJ48GA002",
+              0x044E : "PIC24FJ48GA004",
+              0x0447 : "PIC24FJ64GA002",
+              0x044F : "PIC24FJ64GA004",
+              0x0405 : "PIC24FJ64GA006",
+              0x0408 : "PIC24FJ64GA008",
+              0x040B : "PIC24FJ64GA010",
+              0x0406 : "PIC24FJ96GA006",
+              0x0409 : "PIC24FJ96GA008",
+              0x040C : "PIC24FJ96GA010",
+              0x0407 : "PIC24FJ128GA006",
+              0x040A : "PIC24FJ128GA008",
+              0x040D : "PIC24FJ128GA010",
+              0x1000 : "PIC24FJ64GA106",
+              0x1008 : "PIC24FJ128GA106",
+              0x1010 : "PIC24FJ192GA106",
+              0x1018 : "PIC24FJ256GA106",
+              0x1002 : "PIC24FJ64GA108",
+              0x100A : "PIC24FJ128GA108",
+              0x1012 : "PIC24FJ192GA108",
+              0x101A : "PIC24FJ256GA108",
+              0x1006 : "PIC24FJ64GA110",
+              0x100E : "PIC24FJ128GA110",
+              0x1016 : "PIC24FJ192GA110",
+              0x101E : "PIC24FJ256GA110",
+              0x1001 : "PIC24FJ64GB106",
+              0x1009 : "PIC24FJ128GB106",
+              0x1011 : "PIC24FJ192GB106",
+              0x1019 : "PIC24FJ256GB106",
+              0x1003 : "PIC24FJ64GB108",
+              0x100B : "PIC24FJ128GB108",
+              0x1013 : "PIC24FJ192GB108",
+              0x101B : "PIC24FJ256GB108",
+              0x1007 : "PIC24FJ64GB110",
+              0x100F : "PIC24FJ128GB110",
+              0x1017 : "PIC24FJ192GB110",
+              0x101F : "PIC24FJ256GB110",
+              0x4109 : "PIC24FJ128DA106",
+              0x410D : "PIC24FJ256DA106",
+              0x410B : "PIC24FJ128DA110",
+              0x410F : "PIC24FJ256DA110",
+              0x4108 : "PIC24FJ128DA206",
+              0x410C : "PIC24FJ256DA206",
+              0x410A : "PIC24FJ128DA210",
+              0x410E : "PIC24FJ256DA210",
+              0x46C0 : "PIC24FJ64GA306",
+              0x46C4 : "PIC24FJ64GA308",
+              0x46C8 : "PIC24FJ64GA310",
+              0x46C2 : "PIC24FJ128GA306",
+              0x46C6 : "PIC24FJ128GA308",
+              0x46CA : "PIC24FJ128GA310",
+              0x4100 : "PIC24FJ128GB206",
+              0x4104 : "PIC24FJ256GB206",
+              0x4102 : "PIC24FJ128GB210",
+              0x4106 : "PIC24FJ256GB210",
+              }
 cfg_table = { 0xF80000 : "FBS",
               0xF80002 : "FSS",
               0xF80004 : "FGS",
@@ -61,6 +133,84 @@ Assumes ICSP session already started."""
         stopICSP()
         exit(-1)
 
+class PicResponseThunk(object):
+    """A holder for a response value from the PIC, eg from a REGOUT
+    command.
+
+    To retrieve the value, simply call the object. If the value has
+    not yet been retrieved, any pending commands in the command buffer
+    will be executed. You can determine whether the response has been
+    recieved by calling the have_response function.
+    
+    Objects of this class should only be created by the PicCmdBuffer
+    class"""
+
+    def __init__(self, cmdbuf):
+        self.holder = cmdbuf
+        self.value = None
+
+    def _set_value(self, value):
+        self.value = value
+    def have_response(self):
+        return self.value is not None
+    def __call__(self):
+        if self.value is None:
+            self.holder.force()
+        assert self.value is not None
+        return self.value
+
+class PicCmdBuffer(object):
+    def __init__(self, client):
+        self.client = client
+        self.buffered_commands = []
+        self.response_buffer = []
+        data = client.writecmd(MONITORAPP, 0xc2)
+        assert data is not None and len(data) is 2
+        # buffer size is the size of the goodfet's recieve buffer, in
+        # instructions
+        self.buffer_size = (ord(data[0]) + (ord(data[1]) << 8)) >> 2
+    def force(self):
+        assert len(self.buffered_commands) <= self.buffer_size
+        command = ''.join(self.buffered_commands)
+        self.buffered_commands = []
+        #print "Running: " + command.encode('hex')
+        data = self.client.writecmd(PICAPP, VERB_CMDLIST,
+                                    len(command), map(ord, command))
+        #print "Recieved: " + data.encode('hex')
+        assert len(data) == (len(self.response_buffer) + 1) * 2
+        orig_data = data
+        checksum = 0
+        for resp in self.response_buffer:
+            wd, data = data[0:2],data[2:]
+            val = ord(wd[0]) + (ord(wd[1]) << 8)
+            checksum += val
+            resp._set_value(val)
+        assert len(data) == 2
+        checksum += ord(data[0]) + (ord(data[1]) << 8)
+        checksum &= 0xffff
+        assert checksum == 0
+        self.response_buffer = []
+    def _add_command(self, command, data):
+        assert command in (0,1)
+        assert (data >> 24) == 0
+        full_cmd = [chr(command)]
+        for i in (0,1,2):
+            full_cmd.append(chr((data >> (8*i)) & 0xff))
+        if len(self.buffered_commands) >= self.buffer_size:
+            self.force()
+        self.buffered_commands.append(''.join(full_cmd))
+    def SIX(self, *insns):
+        """Run a list of instructions"""
+        for insn in insns:
+            self._add_command(0, insn)
+        return self
+    def REGOUT(self):
+        "Read the VISI register. Returns a PicResponseThunk"
+        thunk = PicResponseThunk(self)
+        self.response_buffer.append(thunk)
+        self._add_command(1, 0)
+        return thunk
+    
 def build_instr_stream( cmd_li ):
     """Given a list of instruction words, returns a list of bytes of
 the same, in little endian ordering."""
@@ -80,6 +230,13 @@ def readreg( reg_num ):
     client.writecmd( PICAPP, 0x82, 3, [0x00,0x00,0x00] ) # NOP (pump clock)
     return readVISI()
 
+def writereg( reg_num, word ):
+    """Write 16-bit word to a working register (i.e., W0, W1, ..., W15)."""
+    instr = 0x200000 + ((word&0xffff)<<4) + (reg_num&0xf)
+    client.writecmd( PICAPP, 0x82, 3, [instr & 0xff,
+                                       (instr >> 8) & 0xff,
+                                       (instr >> 16) & 0xff] )
+
 def readVISI():
     """Read VISI register; assumes ICSP session already started."""
     data = client.writecmd( PICAPP, 0x83, 0 )
@@ -96,6 +253,11 @@ def readNVMCON():
     runlist( rd_nvmcon_li )
     return readVISI()
 
+def resetPC():
+    """Reset program counter during an active ISCP session."""
+    runlist( [0x040200,   # GOTO 0x0200
+              0x000000] )
+
 def startICSP():
     #print "Starting dsPIC33F/PIC24H ICSP session..."
     data = client.writecmd( PICAPP, 0x84, 0 )
@@ -110,6 +272,86 @@ def dumpVISI():
     result = readVISI()
     print "VISI: 0x%04X" % result
 
+def dump_pm( start_addr, stop_addr, pretty=False ):
+    """Dump routine, now encapsulated in a function.
+Returns an instance of IntelHex corresponding to result.
+
+Note that we start and stop an ICSP session here! This means an
+existing session will be broken."""
+    readpm_cmd_li = [0x200000, # MOV #addr<23:16>, W0
+                     0x880000 | tblpag << 3, # MOV W0, TBLPAG
+                     0x200006, # MOV #addr<15:0>, W6
+                     0xEB0380, # CLR W7
+                     0x000000, # NOP
+                     0xBA1B96, # TBLRDL [W6], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBADBB6, # TBLRDH.B [W6++], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBADBD6, # TBLRDH.B [++W6], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBA1BB6, # TBLRDL [W6++], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBA1B96, # TBLRDL [W6], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBADBB6, # TBLRDH.B [W6++], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBADBD6, # TBLRDH.B [++W6], [W7++]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xBA0BB6, # TBLRDL [W6++], [W7]
+                     0x000000, # NOP
+                     0x000000] # NOP
+
+    dumphex = IntelHex()
+
+    startICSP()
+
+    print "Reading program memory 0x%06X:0x%06X ..." % ( start_addr, stop_addr )
+    # Prep device
+    client.writecmd( PICAPP, 0x82, 3, [0x00,0x02,0x04] ) #GOTO 0x200 (reset)
+    client.writecmd( PICAPP, 0x82, 3, [0x00,0x02,0x04] ) #GOTO 0x200 (reset)
+    client.writecmd( PICAPP, 0x82, 3, [0x00,0x00,0x00] ) #NOP (pump clock)
+
+    last_highb = -1 # Only set TBLPAG when needed.
+    packed_instr_list = [0,0,0,0,0,0] # For packing 4 (24-bit) instructions in 6 (16-bit) words
+    for addr in range( start_addr&0xfffff8, stop_addr+8, 8 ):
+        if (addr>>16)&0xff != last_highb: 
+            last_highb = (addr>>16)&0xff;
+            specify_addr_cmd = [readpm_cmd_li[0] + ((addr & 0xff0000)>>12),
+                                readpm_cmd_li[1],
+                                readpm_cmd_li[2] + ((addr & 0xffff)<<4)]
+        else:
+            specify_addr_cmd = [readpm_cmd_li[2] + ((addr & 0xffff)<<4)]
+        runlist( specify_addr_cmd + readpm_cmd_li[3:] )
+
+        for reg_num in range(6): # Read W0:5, to get packed instructions
+            packed_instr_list[reg_num] = readreg( reg_num )
+        instr_list = words2instr( packed_instr_list )
+
+        for offset in range(4): # Print result
+            if addr+offset*2 < start_addr:
+                continue
+            if addr+offset*2 > stop_addr:
+                break
+            dumphex.puts( ((addr+offset*2)&0xffffff)*2,
+                          chr(instr_list[offset]&0xff)
+                          +chr((instr_list[offset]>>8)&0xff)
+                          +chr((instr_list[offset]>>16)&0xff)
+                          +chr(0) )
+            if pretty:
+                print "0x%06X    0x%06X" % (addr+offset*2,instr_list[offset])
+
+    stopICSP()
+
+    return dumphex
+
+    
 def instr2words( instr_list ):
     """Convert a list of 4 24-bit instructions to a list of 6 words
 (16-bit width).
@@ -148,11 +390,12 @@ Returns [-1] on failure."""
 #############
 
 if len(sys.argv) == 1:
-    print "Usage: %s verb [objects]\n" % sys.argv[0]
+    print "Usage: %s [-alt_tblpag] verb [objects]\n" % sys.argv[0]
     print "%s devid" % sys.argv[0]
     print "%s read 0x$addr" % sys.argv[0]
     print "%s dump $foo.hex [0x$start 0x$stop] [pretty]" % sys.argv[0]
     print "%s config" % sys.argv[0]
+    print "%s reset" % sys.argv[0]
     print "%s program $foo.hex" % sys.argv[0]
     print "%s verify $foo.hex" % sys.argv[0]
     print "%s write 0x$address 0x$value" % sys.argv[0]
@@ -163,7 +406,9 @@ if len(sys.argv) == 1:
     print "%s regout" % sys.argv[0]
     print """
 Note: use - for stdout.
-Warning: only supports dsPIC33F/PIC24H (maybe)...
+      use -alt_tblpag for PIC24FJxxx{DA1,DA2,GB2,GA3}xx with tblpag at 0x54
+Warning: only formally supports dsPIC33F/PIC24H,
+         but read/write flash memory works with PIC24F ...
 """
     sys.exit()
 
@@ -172,39 +417,66 @@ client = GoodFET()
 client.verbose = False # Dump activity to terminal
 client.serInit( timeout=10 ) # UART comm timeout (in seconds)
 
+if sys.argv[1] == "-alt_tblpag":
+    del sys.argv[1]
+    tblpag = 0x54
 
 # Handle each possible PIC verb in turn
-
 if sys.argv[1] == "devid": #0x81
     print "Requesting Application ID, DEVID and hardware revision..."
-    data = client.writecmd( PICAPP, 0x81, 0 )
-    
-    if len(data) > 0:
-        appid = ord(data[0])
-    else:
-        appid = -1
-    if len(data) > 2:
-        devid = ord(data[1]) + (ord(data[2]) << 8)
-    else:
-        devid = -1
-    if len(data) > 4:
-        hwrev = ord(data[3]) + (ord(data[4]) << 8)
-    else:
-        hwrev = -1
-    print "Application ID:   0x%02X" % appid
-    if dev_table.has_key( devid ):
-        print "DEVID:          0x%04X  (%s)" % ( devid, dev_table[devid] )
-    else:
-        print "DEVID:          0x%04X  (unknown)" %  devid
-    print "revision:       0x%04X"% hwrev
+    startICSP()
+    cmdbuf = PicCmdBuffer(client)
+    cmdbuf.SIX(0x040200, # GOTO 0x200
+               0x000000, # NOP
+               0x200FF0, # MOV #0xff, W0
+               0x880000 | tblpag << 3, # MOV W0, TBLPAG
+               0x200006, # MOV #0x0000, W6
+               0x207847, # MOV #VISI, W7
+               0x000000, # NOP
+               0xBA0B96, # TBLRDL [W6],[W7]
+               0x000000, # NOP
+               0x000000) # NOP
+    devid = cmdbuf.REGOUT()
+    cmdbuf.SIX(0x200026, # MOV #0x0002, W6
+               0x000000, # NOP
+               0xBA0B96, # TBLRDL [W6],[W7]
+               0x000000, # NOP
+               0x000000) # NOP
+    devidrev = cmdbuf.REGOUT()
+    cmdbuf.SIX(0x200800, # MOV #0x80, W0
+               0x880000 | tblpag << 3, # MOV W0, TBLPAG
+               0x207F00, # MOV #0x07F0, W6
+               0x000000, # NOP
+               0xBA0B96, # TBLRDL [W6],[W7]
+               0x000000, # NOP
+               0x000000) # NOP
+    appid = cmdbuf.REGOUT()
+    print "Application ID:   0x%02x" % appid()
+    print "DEVID:            0x%04x  (%s)" % (
+        devid(), dev_table.get(devid(), "unknown"))
+    print "Revision:  0x%04x" % devidrev()
+    stopICSP()
     #print "\n(Note that -1 indicates failure to read a value.)"
 
+elif sys.argv[1] == "test":
+    startICSP()
+    cmdbuf = PicCmdBuffer(client)
+    cmdbuf.SIX(0x2BEEF6, # MOV #0xBEEF, W6
+               0x000000,
+               0x780B86, # MOV W6, [W7]
+               0x000000, # NOP
+               0x000000) # NOP
+    assert cmdbuf.REGOUT()() == 0xbeef
+    stopICSP()
+    
+elif sys.argv[1] == "reset":
+    client.writecmd( PICAPP, 0x87, 0 )
+
 elif sys.argv[1] == "config": # Dump configuration registers
     prep_cmd_li = [0x040200, # GOTO 0x0200
-                   0x040200, # GOTO 0x0200
-                   0x000000, # NOP
+                   0x000000, #
                    0x200F80, # MOV #0x00F8, W0
-                   0x880190, # MOV W0, TBLPAG
+                   0x880000 | tblpag << 3, # MOV W0, TBLPAG
                    0xEB0300, # CLR W6
                    0x207847, # MOV #VISI, W7
                    0x000000] # NOP
@@ -225,11 +497,11 @@ elif sys.argv[1] == "config": # Dump configuration registers
         result = readVISI()
         addr = 0xF80000+k*2
         if cfg_table.has_key( addr ):
-            print "0x%06X (%s): 0x%04X" % ( addr,
+            print "0x%06X (%s): 0x%02X" % ( addr,
                                             cfg_table[addr].ljust(cfg_table["width"]),
                                             result )
         else:
-            print "0x%06X: 0x%04X" % ( addr, result )
+            print "0x%06X: 0x%02X" % ( addr, result )
 
     stopICSP()
 
@@ -243,89 +515,138 @@ elif sys.argv[1] == "program":
     except IOError:
         print "Error while attempting to read from %s" % sys.argv[2]
         exit(-1)
+    ph_addrs = proghex.addresses()
+
+    # Load starting program memory address
+    wr_pm64_li_setaddr = [0x040200, # GOTO 0x0200
+                          0x000000, #
+                          0x24001A, # MOV $0x4001, W10
+                          0x883B0A, # MOV W10, NVMCON
+                          0x200000, # + ((addr>>12)&0xff0),  # MOV #addr<23:16>, W0
+                          0x880000 | tblpag << 3, # MOV W0, TBLPAG
+                          0x200007] # + ((addr&0xffff)<<4), # MOV #addr<15:0>, W7
+
+    # Load 4 instructions into write latches
+    wr_pm64_li_wrlat = [0xEB0300, # CLR W6
+                        0x000000, # NOP
+                        0xBB0BB6, # TBLWTL [W6++], [W7]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBBDBB6, # TBLWTH.B [W6++], [W7++]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBBEBB6, # TBLWTH.B [W6++], [++W7]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBB1BB6, # TBLWTL [W6++], [W7++]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBB0BB6, # TBLWTL [W6++], [W7]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBBDBB6, # TBLWTH.B [W6++], [W7++]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBBEBB6, # TBLWTH.B [W6++], [++W7]
+                        0x000000, # NOP
+                        0x000000, # NOP
+                        0xBB1BB6, # TBLWTL [W6++], [W7++]
+                        0x000000, # NOP
+                        0x000000] # NOP
 
-    wr_pm_li = [0x040200, # GOTO 0x0200
-                0x040200, # GOTO 0x0200
-                0x000000, # NOP
-
-                0x24003A, # MOV $0x4003, W10
-                0x883B0A, # MOV W10, NVMCON
-                
-                0x200000,# + ((addr>>12)&0xff0),  # MOV #addr<23:16>, W0
-                0x880190, # MOV W0, TBLPAG
-                0x200007,# + ((addr&0xffff)<<4), # MOV #addr<15:0>, W7
-
-                # Here we load the instruction into registers W0:W1
-                0x200000,# + ((new_words[0]&0xffff)<<4),
-                0x200001,# + ((new_words[1]&0xffff)<<4),
-
-                0xEB0300, # CLR W6
-                0x000000, # NOP
-                0xBB0BB6, # TBLWTL [W6++], [W7]
-                0x000000, # NOP
-                0x000000, # NOP
-                0xBBDBB6, # TBLWTH.B [W6++], [W7++]
-                0x000000, # NOP
-                0x000000, # NOP
-
-                0xA8E761, # BSET NVMCON, #WR
-                0x000000, # NOP
-                0x000000, # NOP
-                0x000000, # NOP
-                0x000000] # NOP
-
+    wr_pm64_li_wr = [0xA8E761, # BSET NVMCON, #WR
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0x000000] # NOP
+    
     startICSP()
+    
+    for last_code_addr in reversed(ph_addrs):
+        if (last_code_addr>>1) < 0xf80000:
+            last_row_addr = (last_code_addr>>1) & 0xffff80
+            break
+    for addr in range((ph_addrs[0]>>1)&0xffff80,last_code_addr+2,128):
+        # Prevent crossing addresses where upper byte
+        # (i.e., address<23:16>) changes.
+        #if addr+126 > 0xffff:
+        #    stop_addr = addr | 0xffff
+        #else:
+        #    stop_addr = addr+126
+
+        print "Writing address 0x%06X" % (addr)
+
+        runlist( wr_pm64_li_setaddr[:4]
+                 + [0x200000 + ((addr>>12)&0xff0),
+                    wr_pm64_li_setaddr[5],
+                    0x200007 + ((addr&0xffff)<<4)] )
+                 
+        instr_list = [0,0,0,0]
+        for mid_addr in range(addr,addr+126,8):
+            for offset in range(4):
+                if (mid_addr+offset*2)<<1 not in ph_addrs:
+                    instr_list[offset] = 0xffffff
+                else:
+                    instr_list[offset] = proghex[(mid_addr+offset*2)<<1]
+                    instr_list[offset] |= proghex[((mid_addr+offset*2)<<1)+1] << 8
+                    instr_list[offset] |= proghex[((mid_addr+offset*2)<<1)+2] << 16
+            packed_instr_list = instr2words( instr_list )
+            runlist( [0x200000 + ((packed_instr_list[0]&0xffff)<<4),
+                      0x200001 + ((packed_instr_list[1]&0xffff)<<4),
+                      0x200002 + ((packed_instr_list[2]&0xffff)<<4),
+                      0x200003 + ((packed_instr_list[3]&0xffff)<<4),
+                      0x200004 + ((packed_instr_list[4]&0xffff)<<4),
+                      0x200005 + ((packed_instr_list[5]&0xffff)<<4)]
+                     + wr_pm64_li_wrlat )
+        
+        runlist( wr_pm64_li_wr )
+        attempts = 0
+        status = readNVMCON()
+        while status & 0x8000:
+            attempts += 1
+            #print " Status was 0x%04X for address 0x%06X" % (status, addr)
+            if attempts >= WRITE_MAX_POLL_ATTEMPTS:
+                print "Error writing to address 0x%06X (Status: 0x%04X)\nTry erase and program again." % (addr, status)
+                stopICSP()
+                exit(-1)
+            client.writecmd( PICAPP, 0x82, 3, [0x00,0x00,0x00] )
+            status = readNVMCON()
 
-    for addr in proghex.addresses():
-        # Ignore non-aligned steps
-        if addr % 4 != 0:
-            continue
+    # Configuration registers must be treated separately
+    for addr in ph_addrs[ph_addrs.index(last_code_addr):]:
 
-        # Configuration registers must be treated separately
-        if (addr>>1) >= 0xf80000 and (addr>>1) <= 0xf80017:
-            wr_cfg_li = [0x040200, # GOTO 0x0200
-                         0x040200, # GOTO 0x0200
-                         0x000000, # NOP
-                         0x200007 + ((addr<<3)&0xffff0), # MOV #addr<15:0>, W7
-                         0x24000A, # MOV #0x4000, W10
-                         0x883B0A, # MOV W10, NVMCON
-                         0x200F80, # MOV #0xF8, W0
-                         0x880190, # MOV W0, TBLPAG
-                         0x200000 + ((proghex[addr]&0x00ff)<<4), # MOV #new_val<7:0>, W0
-                         0xBB0B80, # TBLWTL W0, [W7]
-                         0x000000, # NOP
-                         0x000000, # NOP
-                         0xA8E761, # BSET NVMCON, #WR
-                         0x000000, # NOP
-                         0x000000, # NOP
-                         0x000000, # NOP
-                         0x000000] # NOP
-            runlist( wr_cfg_li )
-            status = readNVMCON()
-            while status & 0x8000:
-                client.writecmd( PICAPP, 0x82, 3, [0x00,0x00,0x00] )
-                status = readNVMCON()
+        print "Configuration register 0x%06X" % ( addr )
+  
+        if addr%4 != 0:
             continue
-
-        # Build instruction from 3 separate bytes
-        instr = proghex[addr]
-        instr |= proghex[addr+1] << 8
-        instr |= proghex[addr+2] << 16
-
-        # Change HEX file address to actual program memory address
-        addr_pm = addr >> 1
-
-        # Program this instruction word
-        addr_highb_cmd  = 0x200000 + ((addr_pm>>12)&0xff0)
-        addr_loww_cmd   = 0x200007 + ((addr_pm&0xffff)<<4)
-        instr_loww_cmd  = 0x200000 + ((instr&0xffff)<<4)
-        instr_highb_cmd = 0x200001 + ((instr>>12)&0xff0)
-        runlist( wr_pm_li[:5] + [addr_highb_cmd] + [wr_pm_li[6]]
-                 + [addr_loww_cmd,
-                    instr_loww_cmd, instr_highb_cmd]
-                 + wr_pm_li[10:] )
+        if (addr>>1) > 0xf80017:
+            break
+        wr_cfg_li = [0x040200, # GOTO 0x0200
+                     0x000000, #
+                     0x200007 + ((addr<<3)&0xffff0), # MOV #addr<15:0>, W7
+                     0x24000A, # MOV #0x4000, W10
+                     0x883B0A, # MOV W10, NVMCON
+                     0x200F80, # MOV #0xF8, W0
+                     0x880000 | tblpag<< 3, # MOV W0, TBLPAG
+                     0x200000 + ((proghex[addr]&0x00ff)<<4), # MOV #new_val<7:0>, W0
+                     0xBB0B80, # TBLWTL W0, [W7]
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0xA8E761, # BSET NVMCON, #WR
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0x000000, # NOP
+                     0x000000] # NOP
+        runlist( wr_cfg_li )
+        attempts = 0
         status = readNVMCON()
         while status & 0x8000:
+            attempts += 1
+            #print " Status was 0x%04X for address 0x%06X" % (status, addr)
+            if attempts >= WRITE_MAX_POLL_ATTEMPTS:
+                print "Error writing to address 0x%06X (Status: 0x%04X)\nTry erase and program again." % (addr, status)
+                stopICSP()
+                exit(-1)
             client.writecmd( PICAPP, 0x82, 3, [0x00,0x00,0x00] )
             status = readNVMCON()
 
@@ -343,62 +664,35 @@ elif sys.argv[1] == "verify":
         print "Error while attempting to read from %s" % sys.argv[2]
         exit(-1)
 
-    readpm_cmd_li = [0x040200, # GOTO 0x0200
-                     0x040200, # GOTO 0x0200
-                     0x000000, # NOP
-                     0x200000, # MOV #addr_pm<23:16>, W0
-                     0x880190, # MOV W0, TBLPAG
-                     0x200006, # MOV #addr_pm<15:0>, W6
-                     0xEB0380, # CLR W7
-                     0xEB0080, # CLR W1
-                     0x000000, # NOP
-                     0xBA1B96, # TBLRDL [W6], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBACB96, # TBLRDH.B [W6], [W7]
-                     0x000000, # NOP
-                     0x000000] # NOP
-
     print "WARNING: verifying config registers not yet supported."
 
-    startICSP()
-
-    # Step through addresses in HEX file and compare to those in target PIC
     for addr in proghex.addresses():
-        # Ignore non-aligned steps
-        if addr % 4 != 0:
+        if addr % 4 != 0 or (addr>>1) < 0x200:
             continue
+        #elif first_addr == None:
+        #    first_addr = addr>>1
+        if (addr>>1) >= 0xf80000:
+            break
+        last_addr = addr>>1
+    dumphex = dump_pm( (proghex.addresses()[0]>>1),
+                       last_addr )
 
-        # Configuration registers must be treated separately
-        if (addr>>1) >= 0xf80000 and (addr>>1) <= 0xf80017:
-            # Need to read datasheets to see which bits are masked.
-            # Then add dictionary to handle this.
-            # (Straightforward but tedious.)
-            continue
-        else: 
-            # Build instruction from 3 separate bytes
-            instr = proghex[addr]
-            instr |= proghex[addr+1] << 8
-            instr |= proghex[addr+2] << 16
-
-        # Change HEX file address to actual program memory address
-        addr_pm = addr >> 1
-        
-        runlist( readpm_cmd_li[:3] + [readpm_cmd_li[3]+((addr_pm&0xff0000)>>12)]
-                 + [readpm_cmd_li[4]] + [readpm_cmd_li[5]+((addr_pm&0xffff)<<4)]
-                 + readpm_cmd_li[6:] )
-        loww = readreg(0)  # Read W0, which has lower 16 bits
-        highb = readreg(1) # Read W1, which has high 8 bits
-        result = (highb<<16) | loww
-        
+    # Step through addresses in HEX file and compare to those in target PIC
+    for addr in proghex.addresses():
+        if addr>>1 >= 0xF80000:
+            break # verifying config registers not yet supported
         # Compare; fail if mismatch
-        if instr != result:
-            print "Fail at address 0x%06X: found 0x%06X, expected 0x%06X." % (addr_pm, result, instr )
-            stopICSP()
+        if proghex[addr] != dumphex[addr]:
+            addr &= addr&0xfffffffc
+            found_instr = dumphex[addr]
+            found_instr |= dumphex[addr+1]<<8
+            found_instr |= dumphex[addr+2]<<16
+            exp_instr = proghex[addr]
+            exp_instr |= proghex[addr+1]<<8
+            exp_instr |= proghex[addr+2]<<16
+            print "Fail at address 0x%06X: found 0x%06X, expected 0x%06X." % ( addr>>1, found_instr, exp_instr )
             exit(-1)
 
-    stopICSP()
-
     print "PASSED" # Be verbose.
 
 
@@ -416,13 +710,12 @@ elif sys.argv[1] == "write":
     #new_words = instr2words( new_instr_li )
         
     wr_pm_li = [0x040200, # GOTO 0x0200
-                0x040200, # GOTO 0x0200
-                0x000000, # NOP
+                0x000000, #
 
                 0x24003A, # MOV $0x4003, W10
                 0x883B0A, # MOV W10, NVMCON
                 0x200000 + ((addr>>12)&0xff0),  # MOV #addr<23:16>, W0
-                0x880190, # MOV W0, TBLPAG
+                0x880000 | tblpag << 3, # MOV W0, TBLPAG
                 0x200007 + ((addr&0xffff)<<4), # MOV #addr<15:0>, W7
 
                 # Here we load the instruction into registers W0:W1
@@ -496,13 +789,12 @@ elif sys.argv[1] == "write_config":
         reg_addr = cfg_table.keys()[cfg_table.values().index(sys.argv[2])]
     
     wr_cfg_li = [0x040200, # GOTO 0x0200
-                 0x040200, # GOTO 0x0200
-                 0x000000, # NOP
+                 0x000000, #
                  0x200007 + ((reg_addr&0xffff)<<4), # MOV #addr<15:0>, W7
                  0x24000A, # MOV #0x4000, W10
                  0x883B0A, # MOV W10, NVMCON
                  0x200F80, # MOV #0xF8, W0
-                 0x880190, # MOV W0, TBLPAG
+                 0x880000 | tblpag << 3, # MOV W0, TBLPAG
                  0x200000 + ((new_val&0xffff)<<4), # MOV #<new_val>, W0
                  0xBB0B80, # TBLWTL W0, [W7]
                  0x000000, # NOP
@@ -530,10 +822,9 @@ elif sys.argv[1] == "read": # Read an address of program memory
     addr = int(float.fromhex(sys.argv[2]))
 
     readpm_cmd_li = [0x040200,                         # GOTO 0x0200
-                     0x040200,                         # GOTO 0x0200
-                     0x000000,                         # NOP
+                     0x000000,                         #
                      0x200000+((addr & 0xff0000)>>12), # MOV #addr<23:16>, W0
-                     0x880190,                         # MOV W0, TBLPAG
+                     0x880000 | tblpag << 3,                         # MOV W0, TBLPAG
                      0x200006+((addr & 0xffff)<<4),    # MOV #addr<15:0>, W6
                      0xEB0380,                         # CLR W7
                      0xEB0080,                         # CLR W1
@@ -574,82 +865,14 @@ elif sys.argv[1] == "dump": # Read section of program memory
         start_addr = 0x0
         stop_addr = 0xFFFFFE
 
-    readpm_cmd_li = [0x200000, # MOV #addr<23:16>, W0
-                     0x880190, # MOV W0, TBLPAG
-                     0x200006, # MOV #addr<15:0>, W6
-                     0xEB0380, # CLR W7
-                     0x000000, # NOP
-                     0xBA1B96, # TBLRDL [W6], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBADBB6, # TBLRDH.B [W6++], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBADBD6, # TBLRDH.B [++W6], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBA1BB6, # TBLRDL [W6++], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBA1B96, # TBLRDL [W6], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBADBB6, # TBLRDH.B [W6++], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBADBD6, # TBLRDH.B [++W6], [W7++]
-                     0x000000, # NOP
-                     0x000000, # NOP
-                     0xBA0BB6, # TBLRDL [W6++], [W7]
-                     0x000000, # NOP
-                     0x000000] # NOP
-
-    dumphex = IntelHex()
-
-    startICSP()
-
-    print "Reading program memory 0x%06X:0x%06X ..." % ( start_addr, stop_addr )
-    # Prep device
-    client.writecmd( PICAPP, 0x82, 3, [0x00,0x02,0x04] ) #GOTO 0x200 (reset)
-    client.writecmd( PICAPP, 0x82, 3, [0x00,0x02,0x04] ) #GOTO 0x200 (reset)
-    client.writecmd( PICAPP, 0x82, 3, [0x00,0x00,0x00] ) #NOP (pump clock)
-
-    last_highb = -1 # Only set TBLPAG when needed.
-    packed_instr_list = [0,0,0,0,0,0] # For packing 4 (24-bit) instructions in 6 (16-bit) words
-    for addr in range( start_addr&0xfffff8, stop_addr+8, 8 ):
-        if (addr>>16)&0xff != last_highb: 
-            last_highb = (addr>>16)&0xff;
-            specify_addr_cmd = [readpm_cmd_li[0] + ((addr & 0xff0000)>>12),
-                                readpm_cmd_li[1],
-                                readpm_cmd_li[2] + ((addr & 0xffff)<<4)]
-        else:
-            specify_addr_cmd = [readpm_cmd_li[2] + ((addr & 0xffff)<<4)]
-        runlist( specify_addr_cmd + readpm_cmd_li[3:] )
-
-        for reg_num in range(6): # Read W0:5, to get packed instructions
-            packed_instr_list[reg_num] = readreg( reg_num )
-        instr_list = words2instr( packed_instr_list )
-
-        for offset in range(4): # Print result
-            if addr+offset*2 < start_addr:
-                continue
-            if addr+offset*2 > stop_addr:
-                break
-            dumphex.puts( ((addr+offset*2)&0xffffff)*2,
-                          chr(instr_list[offset]&0xff)
-                          +chr((instr_list[offset]>>8)&0xff)
-                          +chr((instr_list[offset]>>16)&0xff)
-                          +chr(0) )
-            if print_term:
-                print "0x%06X    0x%06X" % (addr+offset*2,instr_list[offset])
-    
+    dumphex = dump_pm( start_addr, stop_addr, print_term )
+        
     if not print_term:
         if fname == "-":
             dumphex.tofile( sys.stdout, format="hex" )
         else:
             dumphex.tofile( fname, format="hex" )
 
-    stopICSP()
 
 elif sys.argv[1] == "erase": # Bulk (all program memory) or page erase
 
@@ -660,8 +883,7 @@ elif sys.argv[1] == "erase": # Bulk (all program memory) or page erase
 
     if addr is None: # Bulk erase (all of program memory)
         erase_cmd_li = [0x040200, # GOTO 0x0200
-                        0x040200, # GOTO 0x0200
-                        0x000000, # NOP
+                        0x000000, #
                         0x2404FA, # MOV $0x404F, W10
                         0x883B0A, # MOV W10, NVMCON
                         0xA8E761, # BSET NVMCON, #WR
@@ -672,12 +894,11 @@ elif sys.argv[1] == "erase": # Bulk (all program memory) or page erase
     else: # Page erase
         print "Erasing page of 0x%06X" % addr
         erase_cmd_li = [0x040200, # GOTO 0x0200
-                        0x040200, # GOTO 0x0200
-                        0x000000, # NOP
+                        0x000000, #
                         0x24042A, # MOV $0x4042, W10
                         0x883B0A, # MOV W10, NVMCON
                         0x200000+((addr & 0xff0000)>>12), # MOV #addr<23:16>, W0
-                        0x880190, # MOV W0, TBLPAG
+                        0x880000 | tblpag << 3, # MOV W0, TBLPAG
                         0x200001+((addr&0xffff)<<4), # MOV addr<15:0>, W1
                         0x000000, # NOP
                         0xBB0881, # TBLTWL W1, [W1]