#!/usr/bin/env python

import sys
import struct
import binascii

from GoodFETARM7 import GoodFETARM
from intelhex import IntelHex16bit, IntelHex


if(len(sys.argv)==1):
    print "Usage: %s verb [objects]\n" % sys.argv[0]
    # halt, resume... but need for flash and dump to leave it as it was
    print "%s info" % sys.argv[0]
    print "%s dump $foo.hex [0x$start 0x$stop]" % sys.argv[0]
    print "%s erase" % sys.argv[0]
    print "%s eraseinfo" % sys.argv[0]
    print "%s flash $foo.hex [0x$start 0x$stop]" % sys.argv[0]
    print "%s verify $foo.hex [0x$start 0x$stop]" % sys.argv[0]
    print "%s poke 0x$adr 0x$val" % sys.argv[0]
    print "%s peek 0x$start [0x$stop]" % sys.argv[0]
    print "%s reset" % sys.argv[0]
    sys.exit()

#Initialize FET and set baud rate
client=GoodFETARM()
client.serInit()

client.setup()
client.start()

if(sys.argv[1]=="info"):
    client.halt()
    client.resume()


if(sys.argv[1]=="dump"):
    f = sys.argv[2]
    start=0x00000000
    stop=0xFFFFFFFF
    if(len(sys.argv)>3):
        start=int(sys.argv[3],16)
    if(len(sys.argv)>4):
        stop=int(sys.argv[4],16)
    
    print "Dumping from %04x to %04x as %s." % (start,stop,f)
    #h = IntelHex16bit(None)
    # FIXME: get mcu state and return it to that state
    client.halt()

    h = IntelHex(None)
    i=start
    while i<=stop:
        #data=client.ARMreadMem(i, 48)
        data=client.ARMreadChunk(i, 48, verbose=0)
        print "Dumped %06x."%i
        for dword in data:
            if i<=stop and dword != 0xdeadbeef:
                h.puts( i, struct.pack("<I", dword) )
            i+=4
    # FIXME: get mcu state and return it to that state
    client.resume()
    h.write_hex_file(f)

'''
if(sys.argv[1]=="erase"):
    print "Erasing main flash memory."
    client.ARMmasserase()

if(sys.argv[1]=="eraseinfo"):
    print "Erasing info memory."
    client.ARMinfoerase()

    
'''
if(sys.argv[1]=="ivt"):
    client.ARMreprChunk(0xFFC0,0xFFFF)

if(sys.argv[1]=="regs"):
    for i in range(0,16):
        print "r%i=%04x" % (i,client.ARMget_register(i))

if(sys.argv[1]=="flash"):
    f=sys.argv[2]
    start=0
    stop=0x10000
    if(len(sys.argv)>3):
        start=int(sys.argv[3],16)
    if(len(sys.argv)>4):
        stop=int(sys.argv[4],16)
    
    client.halt()
    h = IntelHex16bit(f)
    
    #Should this be default?
    #Makes flashing multiple images inconvenient.
    #client.ARMmasserase()
    
    count=0; #Bytes in commit.
    first=0
    vals=[]
    last=0;  #Last address committed.
    for i in h._buf.keys():
        if((count>0x40 or last+2!=i) and count>0 and i&1==0):
            #print "%i, %x, %x" % (len(vals), last, i)
            client.ARMpokeflashblock(first,vals)
            count=0
            first=0
            last=0
            vals=[]
        if(i>=start and i<stop  and i&1==0):
            val=h[i>>1]
            if(count==0):
                first=i
            last=i
            count+=2
            vals+=[val&0xff,(val&0xff00)>>8]
            if(i%0x100==0):
                print "%04x" % i
    if count>0: #last commit, ivt
        client.ARMpokeflashblock(first,vals)
    client.resume()

if(sys.argv[1]=="verify"):
    f=sys.argv[2]
    start=0
    stop=0xFFFF
    if(len(sys.argv)>3):
        start=int(sys.argv[3],16)
    if(len(sys.argv)>4):
        stop=int(sys.argv[4],16)
    
    client.halt()
    h = IntelHex16bit(f)
    for i in h._buf.keys():
        if(i>=start and i<stop and i&1==0):
            peek=client.peek(i)
            if(h[i>>1]!=peek):
                print "ERROR at %04x, found %04x not %04x"%(i,peek,h[i>>1])
            if(i%0x100==0):
                print "%04x" % i
    client.resume()


if(sys.argv[1]=="peek"):
    start = 0x0000
    if(len(sys.argv)>2):
        start=int(sys.argv[2],16)

    stop = start+4
    if(len(sys.argv)>3):
        stop=int(sys.argv[3],16)

    print "Peeking from %04x to %04x." % (start,stop)
    client.halt()
    for dword in client.ARMreadChunk(start, (stop-start)/4, verbose=0):
        print "%.4x: %.8x" % (start, dword)
        start += 4
    client.resume()

if(sys.argv[1]=="poke"):
    start=0x0000
    val=0x00
    if(len(sys.argv)>2):
        start=int(sys.argv[2],16)
    if(len(sys.argv)>3):
        val=int(sys.argv[3],16)
    
    print "Poking %06x to become %04x." % (start,val)
    client.halt()
    #???while client.ARMreadMem(start)[0]&(~val)>0:
    client.ARMwriteChunk(start, [val])
    print "Poked to %.8x" % client.ARMreadMem(start)[0]
    client.resume()


if(sys.argv[1]=="reset"):
    #Set PC to RESET vector's value.
    
    #client.ARMsetPC(0x00000000)
    #client.ARMset_regCPSR(0)
    #client.ARMreleasecpu()
    client.ARMresettarget(1000)

#client.ARMreleasecpu()
#client.ARMstop()