#!/usr/bin/python 

# Very simple command line interface for development

import sys
import matplotlib.pyplot as plt;
import math;
import cmd;
import elffile
import glob;
import os;
import serial
import struct
import time
import traceback

plt.ion()

OP_MODE       = 0
OP_PEEK       = 1
OP_POKE       = 2
OP_GET        = 3
OP_SET        = 4
OP_SWAP       = 5
OP_ERASE_FW   = 6
OP_WRITE_FW   = 7
OP_SAVE_FWCRC = 8

TERM = 0xf1
ESC  = 0xf2

port = '/dev/ttyUSB0'
if( len(sys.argv) > 1 ):
   port = sys.argv[1]

print 'Opening port %s' % port

ser = serial.Serial( port=port, baudrate=115200 )
ser.timeout = 0.8

fwname = 'mini.elf'
showSerial = False;

varDict = {}

class VarInfo:
   def __init__( self, id, name, fmt, type ):
      self.name = name;
      self.id = id;
      self.fmt = fmt;
      self.type = type;
      varDict[name] = self;

   def IsSigned( self ):
      return not (self.type[0] == 'u' )


allVars = [
   VarInfo(  0, "trace_ctrl",    '0x%04x', 'u16' ),
   VarInfo(  1, "trace_period",  '%d',     'u16' ),
   VarInfo(  2, "trace_samples", '%d',     'u16' ),
   VarInfo(  3, "trace_var1",    '%d',     'u16' ),
   VarInfo(  4, "trace_var2",    '%d',     'u16' ),
   VarInfo(  5, "trace_var3",    '%d',     'u16' ),
   VarInfo(  6, "trace_var4",    '%d',     'u16' ),
   VarInfo(  7, "loop_freq",     '%d',     'u16' ),
   VarInfo(  8, "pressure1",     '%f',     'flt' ),
   VarInfo(  9, "pressure2",     '%f',     'flt' ),
   VarInfo( 10, "pres_off1",     '%d',     'u32' ),
   VarInfo( 11, "pres_off2",     '%d',     'u32' ),
   VarInfo( 12, "poff_calc",     '%d',     'u16' ),
   VarInfo( 13, "calibration",   '%.4f',   'aryflt' ),
   VarInfo( 14, "vin",           '%d',     'i16' ),
   VarInfo( 15, "flow",          '%f',     'flt' ),
]

class TraceVar:
   def __init__( self, id, fmt ):
      self.id = id;
      self.fmt = fmt

   # Convert the raw 32-bit value read from memory
   # into a proper value based on the type of
   # variable this is
   def Convert( self, raw ):
      if( self.fmt == 'u32' ):
         return raw
      if( self.fmt == 'i32' ):
         if( raw & 0x80000000 ):
            return 2**32-raw
         return raw
      if( self.fmt == 'flt' ):
         return I2F( raw )

      print 'Unknown format: %s' % fmt
      return None

   def Format( self, dat ):
      if( self.fmt == 'flt' ):
         return '% .4f' % dat
      else:
         return '% 9d' % dat

# Trace variables
traceVars = {
   'none':       TraceVar(  0, '' ),
   'bat_volt' :  TraceVar(  1, 'flt' ),
   'pressure1':  TraceVar(  2, 'flt' ),
   'pressure2':  TraceVar(  3, 'flt' ),
   'pres_diff':  TraceVar(  4, 'flt' ),
   'flow_rate':  TraceVar(  5, 'flt' ),
   'pres_filt1': TraceVar(  6, 'flt' ),
   'pres_filt2': TraceVar(  7, 'flt' ),
};

# Find the trace variable given it's ID
def TraceVarByID( val ):
   for k in traceVars.keys():
      if( traceVars[k].id == val ):
         return traceVars[k]

   # If it's not in my dictionary, just default to unsigned 32-bit
   return TraceVar( val, 'flt' )

# Most recently set trace variables
activeTraceVars = [None]*4

class Error(Exception):
   def __init__(self, value):
      self.value = value
   def __str__( self ):
      return self.value;

class cmdline( cmd.Cmd ):
   def __init__( self ):
      cmd.Cmd.__init__( self );
      self.UpdatePrompt()
      self.cmdloop();

   def UpdatePrompt( self, mode=None ):
      if( mode == None ):
         mode = SendCmd( OP_MODE )[0]
      if( mode == 1 ):
         self.prompt = 'BOOT] ';
      else:
         self.prompt = '] ';

   def cmdloop( self ):
      while( True ):

	 try:
	    return cmd.Cmd.cmdloop( self );

	 except:
            traceback.print_exc()
	    #print sys.exc_info()
	    pass;

   def do_debug( self, line ):
      global showSerial
      showSerial = not showSerial

   def do_run( self, line ):
      p = str.split(line);
      if( os.path.exists( p[0] ) ):
	 fname = p[0];
      elif( os.path.exists( 'scripts/' + p[0] ) ):
	 fname = 'scripts/' + p[0];
      else:
	 print 'Unknown file ' + p[0];
	 return;
      gbl = globals().copy();
      gbl['cmdline'] = line;
      gbl['parser'] = self
      execfile( fname, gbl );

   def complete_run( self, text, line, begidx, endidx ):
      return glob.glob( text + '*.py' ) + [x[8:] for x in glob.glob( 'scripts/' + text + '*.py' )];

   def do_exec( self, line ):
      exec(line);

   def do_peek( self, line ):
      """ Peek at a memory location"""
      param = str.split(line);
      addr = 0; ct = 1; fmt = '+XXXX';
      fname = None;
      if( len(param) > 3 ): fname = param[3];
      if( len(param) > 2 ): fmt = param[2];
      if( len(param) > 1 ): ct = int(param[1],0);
      if( len(param) > 0 ): addr = param[0]
      peek( addr, ct, fmt, fname );

   def do_poke( self, line ):
      param = str.split(line);
      if( len(param) < 2 ): return;
      ldat = False;

      ptype = 'byte'
      if( param[0] in ['long', 'short', 'float'] ):
         ptype = param[0]
	 param = param[1:];

      addr = param[0]

      if( ptype == 'float' ):
         data = [float(x) for x in param[1:] ];
      else:
         data = [int(x,0) for x in param[1:] ];
      poke( addr, data, ptype );

   def do_get( self, line ):
      param = str.split(line);
      if( len(param) < 1 ):
         print 'Usage: get <param>' 

      if( not param[0] in varDict ):
         print 'Unknow parameter'
         return

      val = GetVar( param[0] )

      v = varDict[ param[0] ]

      if( isinstance( val, list ) ):
         S = ''
         for i in val:
            S += v.fmt % i + ', '
         print S

      else:
         print v.fmt % val;

   def complete_get( self, text, line, begidx, endidx ):
      var = text;

      out = [];
      for i in varDict.keys():
	 if( i.startswith( var ) ):
	    out.append(i);

      return out;

   def complete_set( self, text, line, begidx, endidx ):
      return self.complete_get( text, line, begidx, endidx )

   def do_set( self, line ):
      param = str.split(line);
      if( len(param) < 2 ):
         print 'Usage: set <param> <value>' 

      if( not param[0] in varDict ):
         print 'Unknow parameter'
         return

      try:
         val = int( param[1], 0 )
      except:
         val = param[1]

      SetVar( param[0], val )

   def do_EOF( self, line ):
      return True;

   def do_exit( self, line ):
      return True;

   def do_trace( self, line ):

      if( len(line) < 1 ): line = 'graph'

      if not line in ['graph', 'download', 'mean' ]:
         print 'Unknown trace command'
         return

      dat = GetTrace()
      if( dat == None ):
         return

      if( line == 'graph' ):
         TraceGraph( dat )

      if( line == 'download' ):
         fp = open( 'trace.dat', 'w' )
         for j in range(len(dat[0])):
            S = ''
            for i in range(len(dat)):
               S += '%e ' % dat[i][j]
            fp.write( S + '\n' )
         fp.close()

      if( line == 'mean' ):
         for i in range(len(dat)):
            print '%d: mean %.3f, medial: %.3f std: %.5f' % (i, mean(dat[i]), median(dat[i]), std(dat[i]))

   def do_flash( self, line ):
      line = line.strip();
      if( len(line) < 1 ):
         print 'Please pass the name if the firmware file'
         return

      if( SendCmd( OP_MODE )[0] != 1 ):
         print 'Swapping to boot mode'
         dat = SendCmd( OP_SWAP, timeout=.1 )

      if( SendCmd( OP_MODE )[0] != 1 ):
         print 'Failure jumping to boot mode'
         return

      elf = elffile.ElfFile( line )

      print 'Erasing exiting firmware'
      SendCmd( OP_ERASE_FW, timeout=10 )

      done = False
      for s in elf.sect:
         if( s.addr < 1 or len(s.data) < 1 ):
            continue

         if( done ):
            print 'Error, at the moment I only handle one section.'
            break

         print 'Writing %d bytes to 0x%08x' % (len(s.data), s.addr)

         dat = s.data[:];
         done = True

         while( len(dat) & 7 ): 
            dat.append(0xff)

         L = len(dat)
         crc = CRCutil().calc(dat)

         A = s.addr;
         while( len(dat) ):
            ct = min(128, len(dat))
            d = dat[:ct]
            dat = dat[ct:]

            if( SendCmd( OP_WRITE_FW, Split32(A)+d ) == None ):
               break
            A += ct

      if( not done ):
         print 'No writable sections in the elf file'
         return 

      if( SendCmd( OP_SAVE_FWCRC, Split32(L)+Split32(crc) ) == None ):
         print 'Error saving firmware CRC'

      print 'Swaping back to normal mode'
      SendCmd( OP_SWAP, timeout=.1 )

      mode = SendCmd( OP_MODE )[0]
      if( mode != 0 ):
         print 'Failure jumping back to normal mode'

      self.UpdatePrompt( mode );

   def do_swap( self, line ):
      SendCmd( OP_SWAP, timeout=.1 )
      print SendCmd( OP_MODE )


def FmtPeek( dat, fmt='+XXXX', addr=0 ):

   fmtInfo = {
      '+' : ( '0x%08x: ', 0, 'addr' ),
      'x' : ( '0x%04x ',  2, 'GrabU16(dat)' ),
      'i' : ( '%5d ',     2, 'GrabI16(dat)' ),
      'u' : ( '%5u ',     2, 'GrabU16(dat)' ),
      'X' : ( '0x%08x ',  4, 'GrabU32(dat)' ),
      'I' : ( '%9d ',     4, 'GrabI32(dat)' ),
      'U' : ( '%9u ',     4, 'GrabU32(dat)' ),
      'n' : ( '\n',       0, 'None' ),
      'f' : ( '%8.4f ',   4, 'GrabFlt(dat)' ),
      'e' : ( '%12.4e ',   4, 'GrabFlt(dat)' ),
      'c' : ( '%s',       1, 'chr(GrabU8(dat))' ),
      'b' : ( '0x%02x ',  1, 'GrabU8(dat)' ),
   };

   ret = '';
   ndx = -1;
   while( len(dat) > 0 ):
      ndx += 1;
      if( ndx == len(fmt) ):
	 ret += '\n';
	 ndx = 0;

      ch = fmt[ndx];

      if( ch in fmtInfo ):
	 val = eval( fmtInfo[ch][2] );

	 if( val == None ):
	    ret += fmtInfo[ch][0];
	 else:
	    ret += fmtInfo[ch][0] % val;
	 addr += fmtInfo[ch][1];
      else:
	 ret += ch;

   return ret;

elfFiles = {}
def loadElf( fname ):
   global elfFiles

   ct = os.stat( fname ).st_ctime

   if( not fname in elfFiles ):
      pass
   elif( elfFiles[fname]['ctime'] != ct ):
      pass
   else:
      return elfFiles[fname]['elf']

   e = {}
   e['ctime'] = ct
   e['elf'] = elffile.ElfFile( fname )
   elfFiles[fname] = e
   return e['elf']

def decodeAddr( addr, fw=None ):

   global fwname
   if( fw == None ):
      fw = fwname

   if( isinstance( addr, int ) ):
      return addr

   if( addr[0] == '&' ):
      elf = loadElf( fw )

      a = elf.getSymbol( addr[1:] )
      if( a != None ): 
         a = a.value;
      return a
   return int(addr,0)

def peek( addr, ct=1, fmt='+XXXX', fname=None, raw=False  ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return

   out = []

   A = addr;

   while( ct ):
      n = min( ct, 255 )
      dat = SendCmd( OP_PEEK, Split32( A )+[n] )
      out += dat
      ct -= len(dat)
      A += len(dat)

   if( raw ): return out

   s = FmtPeek( out, fmt, addr );
   if( fname == None ):
      print s
   else:
      fp = open( fname, 'w' );
      fp.write(s);
      fp.write('\n');
      fp.close()

def peek16( addr, ct=None, le=True, signed=False ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return

   if( ct == None ):
      ct = 1;
   out = peek( addr, 2*ct, raw=True )
   return Build16( out, le=True, signed=False )

def peek32( addr, ct=None, le=True, signed=False ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return

   if( ct == None ):
      ct = 1;
   out = peek( addr, 4*ct, raw=True )
   return Build32( out, le=True, signed=False )

#def peekf( addr, ct=None ):
#   dat = peekl(addr,ct, le=True, signed=False);
#   if( ct == None ):
#      return Util.I2F(dat)
#   ret = []
#   for d in dat:
#      ret.append( Util.I2F(d) )
#   return ret

def poke( addr, dat, ptype ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return
   if( isinstance(dat,int) or isinstance(dat,long) ):
      dat = [dat];

   if( ptype == 'long' ):
      dat = Split32(dat);
   if( ptype == 'short' ):
      dat = Split16(dat);
   if( ptype == 'float' ):
      dat = [F2I(x) for x in dat]
      dat = Split32(dat);

   SendCmd( OP_POKE, Split32( addr )+dat )

def poke32( addr, dat ):
   poke( addr, dat, 'long' )

def poke16( addr, dat ):
   poke( addr, dat, 'short' )

def GetVar( var ):
   if( not var in varDict ):
      return None;

   v = varDict[ var ]
   out = SendCmd( OP_GET, Split16( v.id ) )

   if( v.type in ['u16', 'u32'] ):
      return MakeInt( out, signed=False )

   if( v.type in  ['i16', 'i32'] ):
      return MakeInt( out, signed=True )

   if( v.type == 'flt' ):
      return BuildFlt( out )[0]

   if( v.type == 'ary32' ):
      return Build32( out, signed=True )

   if( v.type == 'aryflt' ):
      return BuildFlt( out )

   print "Can't handle %s yet" % v.type

def SetVar( var, value ):
   if( not var in varDict ):
      return

   v = varDict[ var ]

   if( var[:9] == 'trace_var' ):
      ndx = int(var[9:])-1
      if( value in traceVars ):
         activeTraceVars[ndx] = traceVars[value]
         value = traceVars[value].id
      else:
         activeTraceVars[ndx] = TraceVarByID(value)

   if( v.type in ['u16', 'i16'] ):
      bval = Split16( value )

   elif( v.type in ['u32', 'i32'] ):
      bval = Split32( value );

   elif( v.type == 'ary32' ):
      value = [int(x,0) for x in value.split(',')]
      bval = Split32( value )

   elif( v.type == 'aryflt' ):
      value = [float(x) for x in value.split(',')]
      bval = SplitFlt( value )

   else:
      print "Sorry, can't handle this one"
      return

   SendCmd( OP_SET, Split16( v.id )+bval )

def GetTrace():

   # See if there's any data collected
   n = GetVar( 'trace_samples' )
   if( n < 1 ):
      return None

   # Find the trace variables current enabled
   vct = 0;
   for i in range(4):
      v = GetVar( 'trace_var%d' % (i+1) );
      if( v==0 ):
         break
      vct += 1;

      # Find the trace variable object that goes with this ID.
      # We should already know this unless the trace was setup prior
      # to starting the interface
      if( activeTraceVars[i] == None or activeTraceVars[i].id != v ):
         activeTraceVars[i] = TraceVarByID( v )

   if( vct < 1 ):
      return None

   # n should be a multiple of the number of 
   # trace variables.  Just make sure that's the
   # case
   n -= n % vct;

   # Read the trace data as one big array
   dat = peek( 0x20006000, ct=n*4, raw=True )
   dat = Build32( dat, le=True, signed=False )

   # Chop it up into separate arrays for each trace
   # variable and reformat as necessary
   ret = []
   for i in range(vct):
      raw = dat[i::vct]
      if( activeTraceVars[i] != None ):
         raw = [activeTraceVars[i].Convert(x) for x in raw]
      ret.append(raw)

   return ret

def TraceGraph( dat=None ):

   if( dat == None ):
      dat = GetTrace()
      if( dat == None ):
         return

   freq = float(GetVar( 'loop_freq' ));
   per = GetVar( 'trace_period' )
   if( per < 1 ): per = 1;
   M = per / float(freq)

   time = [i*M for i in range(len(dat[0]))]
   plt.figure()
   for d in dat:
      plt.plot( time, d );
   plt.grid()
   plt.show();

def Cksum( buff ):
   s = 0x55
   for b in buff: s ^= b
   return s

def EscCmd( buff ):
   ret = []
   for i in buff:
      if( i==ESC or i==TERM ):
         ret.append(ESC)
      ret.append(i)
   ret.append(TERM)
   return ret;

def GetResp( show=False ):
   dat = []
   esc = False
   if( show ):
      print 'Getting resp: ',
   while( True ):
      x = ser.read(1)
      if( len(x) < 1 ):
         if( show ):
            print 'timeout'
         return dat
      x = ord(x)
      if( show ):
         print '0x%02x' % x,

      if( esc ):
         esc = False
         dat.append(x)
         continue

      if( x == ESC ):
         esc = True
         continue

      if( x == TERM ):
         if( show ):
            print
         return dat
      dat.append(x)

def SendCmd( op, data=[], timeout=None ):
   global showSerial, ser
   show = showSerial
   buff = [op] + data
   buff.insert( 1,Cksum(buff) )

   if( show ):
      S = 'CMD: '
      for b in buff: S += '0x%02x ' % b
      print S

   buff = EscCmd( buff )
   if( show ):
      S = 'ESC: '
      for b in buff: S += '0x%02x ' % b
      print S

   ser.write( ''.join( [chr(x) for x in buff]) )
   if( timeout != None ):
      if( show ):
         print 'Setting timeout to %.1f' % timeout
      oldto = ser.timeout
      ser.timeout = timeout

   rsp = GetResp( show )
   if( timeout != None ):
      ser.timeout = oldto

   if( len(rsp) < 1 ):
      return rsp
   if( Cksum(rsp) ):
      print 'Checksum error on response'
      return None

   if( len(rsp)>0 and rsp[0] ):
      #raise Error( "Error %d (0x%02x)" % (rsp[0],rsp[0]) )
      print "Error %d (0x%02x)" % (rsp[0],rsp[0]);
      return None
   return rsp[2:]

def I2F( ival ):
   s = struct.pack( 'I',ival&0xffffffff );
   return struct.unpack('f',s)[0]

def F2I( fval ):
   s = struct.pack( 'f',fval );
   return struct.unpack('I',s)[0]

def SplitInt( val, len=4, le=True, asStr=False ):
   ret = [];

   if( isinstance( val, list ) ):
      for v in val:
         ret += SplitInt( v, len, le, False );
   else:
      for i in range(len):
         ret.append( int(val & 0x00FF) );
         val >>= 8;

      if( not le ): ret = ret[::-1];
   if( asStr ): ret = "".join([chr(i) for i in ret]);
   return ret;

def Split16( x, le=True, asStr=False ): return SplitInt( x, 2, le, asStr );
def Split32( x, le=True, asStr=False ): return SplitInt( x, 4, le, asStr );
def SplitFlt( x ): return Split32( [F2I(i) for i in x] )

# Utility function to convert bytes to ints
def MakeInt( bytes, signed=True, le=True ):
   if( not le ): bytes = bytes[::-1];
   val = 0;
   shift = 0;
   for b in bytes:
      val |= (b<<shift);
      shift += 8;

   if( signed and bytes[-1] & 0x80 ):
      return val - (1<<shift)
   return val

def Build32( dat, le=True, signed=False ):
   ret = [];
   for i in range( len(dat)/4 ):
      ret.append( MakeInt( dat[ (4*i):(4*i+4) ], signed=signed, le=le ) );
   return ret;

def Build16( dat, le=True, signed=False ):
   ret = [];
   for i in range( len(dat)/2 ):
      ret.append( MakeInt( dat[ (2*i):(2*i+2) ], signed=signed, le=le ) );
   return ret;

def BuildFlt( dat ):
   tmp = Build32( dat, le=True, signed=False )
   return [I2F(x) for x in tmp]

def GrabElems( dat, ct ):
   if( len(dat) < ct ):
      dat.extend( (ct-len(dat))*[0] );
   ret = dat[:ct];
   del dat[0:ct];
   return ret;

def GrabI8( dat, le=True ):  return MakeInt( GrabElems(dat,1), signed=True,  le=le );
def GrabU8( dat, le=True ):  return MakeInt( GrabElems(dat,1), signed=False, le=le );
def GrabI16( dat, le=True ): return MakeInt( GrabElems(dat,2), signed=True,  le=le );
def GrabU16( dat, le=True ): return MakeInt( GrabElems(dat,2), signed=False, le=le );
def GrabI24( dat, le=True ): return MakeInt( GrabElems(dat,3), signed=True,  le=le );
def GrabU24( dat, le=True ): return MakeInt( GrabElems(dat,3), signed=False, le=le );
def GrabI32( dat, le=True ): return MakeInt( GrabElems(dat,4), signed=True,  le=le );
def GrabU32( dat, le=True ): return MakeInt( GrabElems(dat,4), signed=False, le=le );
def GrabI64( dat, le=True ): return MakeInt( GrabElems(dat,8), signed=True,  le=le );
def GrabU64( dat, le=True ): return MakeInt( GrabElems(dat,8), signed=False, le=le );
def GrabFlt( dat, le=True ): return I2F( GrabU32(dat, le=le) );

def mean( dat ):
   return float(sum(dat)) / len(dat)

def median( dat ):
   L = len(dat)
   S = sorted(dat)
   if( L & 1 ):
      return S[ int(L/2) ]
   return (S[ int(L/2)-1] + S[int(L/2)])/2

def std( dat ):
   m = mean(dat)

   x = 0
   for i in dat:
      x += (i-m)**2
   return math.sqrt( x/len(dat) )

class CRCutil:
   def __init__( self, poly=0xEDB88320 ):
      self.InitTable( poly )

   def calc( self, dat, init=0 ):
      crc = ~init & 0xFFFFFFFF;
      for d in dat:
         a = 0x00FF & (d^crc);
         crc = self.tbl[a] ^ (crc>>8);
      return ~crc & 0x00FFFFFFFF;

   def InitTable( self, poly ):
      self.tbl = []

      for i in range(256):
         crc = i;

         for j in range(8):
            lsbSet = crc & 1
            crc >>= 1;
            if( lsbSet ):
               crc ^= poly;
         self.tbl.append( crc )

cmdline();


1cf6ad83cecd3dc419075d16797263d46c155c7b
#!/usr/bin/python 

# Very simple command line interface for development

import sys
import matplotlib.pyplot as plt;
import math;
import cmd;
import elffile
import glob;
import os;
import serial
import struct
import time
import traceback

plt.ion()

OP_MODE       = 0
OP_PEEK       = 1
OP_POKE       = 2
OP_GET        = 3
OP_SET        = 4
OP_SWAP       = 5
OP_ERASE_FW   = 6
OP_WRITE_FW   = 7
OP_SAVE_FWCRC = 8

TERM = 0xf1
ESC  = 0xf2

port = '/dev/ttyUSB0'
if( len(sys.argv) > 1 ):
   port = sys.argv[1]

print 'Opening port %s' % port

ser = serial.Serial( port=port, baudrate=115200 )
ser.timeout = 0.8

fwname = 'mini.elf'
showSerial = False;

varDict = {}

class VarInfo:
   def __init__( self, id, name, fmt, type ):
      self.name = name;
      self.id = id;
      self.fmt = fmt;
      self.type = type;
      varDict[name] = self;

   def IsSigned( self ):
      return not (self.type[0] == 'u' )


allVars = [
   VarInfo(  0, "trace_ctrl",    '0x%04x', 'u16' ),
   VarInfo(  1, "trace_period",  '%d',     'u16' ),
   VarInfo(  2, "trace_samples", '%d',     'u16' ),
   VarInfo(  3, "trace_var1",    '%d',     'u16' ),
   VarInfo(  4, "trace_var2",    '%d',     'u16' ),
   VarInfo(  5, "trace_var3",    '%d',     'u16' ),
   VarInfo(  6, "trace_var4",    '%d',     'u16' ),
   VarInfo(  7, "loop_freq",     '%d',     'u16' ),
   VarInfo(  8, "pressure1",     '%f',     'flt' ),
   VarInfo(  9, "pressure2",     '%f',     'flt' ),
   VarInfo( 10, "pres_off1",     '%d',     'u32' ),
   VarInfo( 11, "pres_off2",     '%d',     'u32' ),
   VarInfo( 12, "poff_calc",     '%d',     'u16' ),
   VarInfo( 13, "calibration",   '%.4f',   'aryflt' ),
   VarInfo( 14, "vin",           '%d',     'i16' ),
   VarInfo( 15, "flow",          '%f',     'flt' ),
]

class TraceVar:
   def __init__( self, id, fmt ):
      self.id = id;
      self.fmt = fmt

   # Convert the raw 32-bit value read from memory
   # into a proper value based on the type of
   # variable this is
   def Convert( self, raw ):
      if( self.fmt == 'u32' ):
         return raw
      if( self.fmt == 'i32' ):
         if( raw & 0x80000000 ):
            return 2**32-raw
         return raw
      if( self.fmt == 'flt' ):
         return I2F( raw )

      print 'Unknown format: %s' % fmt
      return None

   def Format( self, dat ):
      if( self.fmt == 'flt' ):
         return '% .4f' % dat
      else:
         return '% 9d' % dat

# Trace variables
traceVars = {
   'none':       TraceVar(  0, '' ),
   'bat_volt' :  TraceVar(  1, 'flt' ),
   'pressure1':  TraceVar(  2, 'flt' ),
   'pressure2':  TraceVar(  3, 'flt' ),
   'pres_diff':  TraceVar(  4, 'flt' ),
   'flow_rate':  TraceVar(  5, 'flt' ),
   'pres_filt1': TraceVar(  6, 'flt' ),
   'pres_filt2': TraceVar(  7, 'flt' ),
};

# Find the trace variable given it's ID
def TraceVarByID( val ):
   for k in traceVars.keys():
      if( traceVars[k].id == val ):
         return traceVars[k]

   # If it's not in my dictionary, just default to unsigned 32-bit
   return TraceVar( val, 'flt' )

# Most recently set trace variables
activeTraceVars = [None]*4

class Error(Exception):
   def __init__(self, value):
      self.value = value
   def __str__( self ):
      return self.value;

class cmdline( cmd.Cmd ):
   def __init__( self ):
      cmd.Cmd.__init__( self );
      self.UpdatePrompt()
      self.cmdloop();

   def UpdatePrompt( self, mode=None ):
      if( mode == None ):
         mode = SendCmd( OP_MODE )[0]
      if( mode == 1 ):
         self.prompt = 'BOOT] ';
      else:
         self.prompt = '] ';

   def cmdloop( self ):
      while( True ):

	 try:
	    return cmd.Cmd.cmdloop( self );

	 except:
            traceback.print_exc()
	    #print sys.exc_info()
	    pass;

   def do_debug( self, line ):
      global showSerial
      showSerial = not showSerial

   def do_run( self, line ):
      p = str.split(line);
      if( os.path.exists( p[0] ) ):
	 fname = p[0];
      elif( os.path.exists( 'scripts/' + p[0] ) ):
	 fname = 'scripts/' + p[0];
      else:
	 print 'Unknown file ' + p[0];
	 return;
      gbl = globals().copy();
      gbl['cmdline'] = line;
      gbl['parser'] = self
      execfile( fname, gbl );

   def complete_run( self, text, line, begidx, endidx ):
      return glob.glob( text + '*.py' ) + [x[8:] for x in glob.glob( 'scripts/' + text + '*.py' )];

   def do_exec( self, line ):
      exec(line);

   def do_peek( self, line ):
      """ Peek at a memory location"""
      param = str.split(line);
      addr = 0; ct = 1; fmt = '+XXXX';
      fname = None;
      if( len(param) > 3 ): fname = param[3];
      if( len(param) > 2 ): fmt = param[2];
      if( len(param) > 1 ): ct = int(param[1],0);
      if( len(param) > 0 ): addr = param[0]
      peek( addr, ct, fmt, fname );

   def do_poke( self, line ):
      param = str.split(line);
      if( len(param) < 2 ): return;
      ldat = False;

      ptype = 'byte'
      if( param[0] in ['long', 'short', 'float'] ):
         ptype = param[0]
	 param = param[1:];

      addr = param[0]

      if( ptype == 'float' ):
         data = [float(x) for x in param[1:] ];
      else:
         data = [int(x,0) for x in param[1:] ];
      poke( addr, data, ptype );

   def do_get( self, line ):
      param = str.split(line);
      if( len(param) < 1 ):
         print 'Usage: get <param>' 

      if( not param[0] in varDict ):
         print 'Unknow parameter'
         return

      val = GetVar( param[0] )

      v = varDict[ param[0] ]

      if( isinstance( val, list ) ):
         S = ''
         for i in val:
            S += v.fmt % i + ', '
         print S

      else:
         print v.fmt % val;

   def complete_get( self, text, line, begidx, endidx ):
      var = text;

      out = [];
      for i in varDict.keys():
	 if( i.startswith( var ) ):
	    out.append(i);

      return out;

   def complete_set( self, text, line, begidx, endidx ):
      return self.complete_get( text, line, begidx, endidx )

   def do_set( self, line ):
      param = str.split(line);
      if( len(param) < 2 ):
         print 'Usage: set <param> <value>' 

      if( not param[0] in varDict ):
         print 'Unknow parameter'
         return

      try:
         val = int( param[1], 0 )
      except:
         val = param[1]

      SetVar( param[0], val )

   def do_EOF( self, line ):
      return True;

   def do_exit( self, line ):
      return True;

   def do_trace( self, line ):

      if( len(line) < 1 ): line = 'graph'

      if not line in ['graph', 'download', 'mean' ]:
         print 'Unknown trace command'
         return

      dat = GetTrace()
      if( dat == None ):
         return

      if( line == 'graph' ):
         TraceGraph( dat )

      if( line == 'download' ):
         fp = open( 'trace.dat', 'w' )
         for j in range(len(dat[0])):
            S = ''
            for i in range(len(dat)):
               S += '%e ' % dat[i][j]
            fp.write( S + '\n' )
         fp.close()

      if( line == 'mean' ):
         for i in range(len(dat)):
            print '%d: mean %.3f, medial: %.3f std: %.5f' % (i, mean(dat[i]), median(dat[i]), std(dat[i]))

   def do_flash( self, line ):
      line = line.strip();
      if( len(line) < 1 ):
         print 'Please pass the name if the firmware file'
         return

      if( SendCmd( OP_MODE )[0] != 1 ):
         print 'Swapping to boot mode'
         dat = SendCmd( OP_SWAP, timeout=.1 )

      if( SendCmd( OP_MODE )[0] != 1 ):
         print 'Failure jumping to boot mode'
         return

      elf = elffile.ElfFile( line )

      print 'Erasing exiting firmware'
      SendCmd( OP_ERASE_FW, timeout=10 )

      done = False
      for s in elf.sect:
         if( s.addr < 1 or len(s.data) < 1 ):
            continue

         if( done ):
            print 'Error, at the moment I only handle one section.'
            break

         print 'Writing %d bytes to 0x%08x' % (len(s.data), s.addr)

         dat = s.data[:];
         done = True

         while( len(dat) & 7 ): 
            dat.append(0xff)

         L = len(dat)
         crc = CRCutil().calc(dat)

         A = s.addr;
         while( len(dat) ):
            ct = min(128, len(dat))
            d = dat[:ct]
            dat = dat[ct:]

            if( SendCmd( OP_WRITE_FW, Split32(A)+d ) == None ):
               break
            A += ct

      if( not done ):
         print 'No writable sections in the elf file'
         return 

      if( SendCmd( OP_SAVE_FWCRC, Split32(L)+Split32(crc) ) == None ):
         print 'Error saving firmware CRC'

      print 'Swaping back to normal mode'
      SendCmd( OP_SWAP, timeout=.1 )

      mode = SendCmd( OP_MODE )[0]
      if( mode != 0 ):
         print 'Failure jumping back to normal mode'

      self.UpdatePrompt( mode );

   def do_swap( self, line ):
      SendCmd( OP_SWAP, timeout=.1 )
      print SendCmd( OP_MODE )


def FmtPeek( dat, fmt='+XXXX', addr=0 ):

   fmtInfo = {
      '+' : ( '0x%08x: ', 0, 'addr' ),
      'x' : ( '0x%04x ',  2, 'GrabU16(dat)' ),
      'i' : ( '%5d ',     2, 'GrabI16(dat)' ),
      'u' : ( '%5u ',     2, 'GrabU16(dat)' ),
      'X' : ( '0x%08x ',  4, 'GrabU32(dat)' ),
      'I' : ( '%9d ',     4, 'GrabI32(dat)' ),
      'U' : ( '%9u ',     4, 'GrabU32(dat)' ),
      'n' : ( '\n',       0, 'None' ),
      'f' : ( '%8.4f ',   4, 'GrabFlt(dat)' ),
      'e' : ( '%12.4e ',   4, 'GrabFlt(dat)' ),
      'c' : ( '%s',       1, 'chr(GrabU8(dat))' ),
      'b' : ( '0x%02x ',  1, 'GrabU8(dat)' ),
   };

   ret = '';
   ndx = -1;
   while( len(dat) > 0 ):
      ndx += 1;
      if( ndx == len(fmt) ):
	 ret += '\n';
	 ndx = 0;

      ch = fmt[ndx];

      if( ch in fmtInfo ):
	 val = eval( fmtInfo[ch][2] );

	 if( val == None ):
	    ret += fmtInfo[ch][0];
	 else:
	    ret += fmtInfo[ch][0] % val;
	 addr += fmtInfo[ch][1];
      else:
	 ret += ch;

   return ret;

elfFiles = {}
def loadElf( fname ):
   global elfFiles

   ct = os.stat( fname ).st_ctime

   if( not fname in elfFiles ):
      pass
   elif( elfFiles[fname]['ctime'] != ct ):
      pass
   else:
      return elfFiles[fname]['elf']

   e = {}
   e['ctime'] = ct
   e['elf'] = elffile.ElfFile( fname )
   elfFiles[fname] = e
   return e['elf']

def decodeAddr( addr, fw=None ):

   global fwname
   if( fw == None ):
      fw = fwname

   if( isinstance( addr, int ) ):
      return addr

   if( addr[0] == '&' ):
      elf = loadElf( fw )

      a = elf.getSymbol( addr[1:] )
      if( a != None ): 
         a = a.value;
      return a
   return int(addr,0)

def peek( addr, ct=1, fmt='+XXXX', fname=None, raw=False  ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return

   out = []

   A = addr;

   while( ct ):
      n = min( ct, 255 )
      dat = SendCmd( OP_PEEK, Split32( A )+[n] )
      out += dat
      ct -= len(dat)
      A += len(dat)

   if( raw ): return out

   s = FmtPeek( out, fmt, addr );
   if( fname == None ):
      print s
   else:
      fp = open( fname, 'w' );
      fp.write(s);
      fp.write('\n');
      fp.close()

def peek16( addr, ct=None, le=True, signed=False ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return

   if( ct == None ):
      ct = 1;
   out = peek( addr, 2*ct, raw=True )
   return Build16( out, le=True, signed=False )

def peek32( addr, ct=None, le=True, signed=False ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return

   if( ct == None ):
      ct = 1;
   out = peek( addr, 4*ct, raw=True )
   return Build32( out, le=True, signed=False )

#def peekf( addr, ct=None ):
#   dat = peekl(addr,ct, le=True, signed=False);
#   if( ct == None ):
#      return Util.I2F(dat)
#   ret = []
#   for d in dat:
#      ret.append( Util.I2F(d) )
#   return ret

def poke( addr, dat, ptype ):
   addr = decodeAddr(addr)
   if( addr == None ):
      print 'Unknown symbol'
      return
   if( isinstance(dat,int) or isinstance(dat,long) ):
      dat = [dat];

   if( ptype == 'long' ):
      dat = Split32(dat);
   if( ptype == 'short' ):
      dat = Split16(dat);
   if( ptype == 'float' ):
      dat = [F2I(x) for x in dat]
      dat = Split32(dat);

   SendCmd( OP_POKE, Split32( addr )+dat )

def poke32( addr, dat ):
   poke( addr, dat, 'long' )

def poke16( addr, dat ):
   poke( addr, dat, 'short' )

def GetVar( var ):
   if( not var in varDict ):
      return None;

   v = varDict[ var ]
   out = SendCmd( OP_GET, Split16( v.id ) )

   if( v.type in ['u16', 'u32'] ):
      return MakeInt( out, signed=False )

   if( v.type in  ['i16', 'i32'] ):
      return MakeInt( out, signed=True )

   if( v.type == 'flt' ):
      return BuildFlt( out )[0]

   if( v.type == 'ary32' ):
      return Build32( out, signed=True )

   if( v.type == 'aryflt' ):
      return BuildFlt( out )

   print "Can't handle %s yet" % v.type

def SetVar( var, value ):
   if( not var in varDict ):
      return

   v = varDict[ var ]

   if( var[:9] == 'trace_var' ):
      ndx = int(var[9:])-1
      if( value in traceVars ):
         activeTraceVars[ndx] = traceVars[value]
         value = traceVars[value].id
      else:
         activeTraceVars[ndx] = TraceVarByID(value)

   if( v.type in ['u16', 'i16'] ):
      bval = Split16( value )

   elif( v.type in ['u32', 'i32'] ):
      bval = Split32( value );

   elif( v.type == 'ary32' ):
      value = [int(x,0) for x in value.split(',')]
      bval = Split32( value )

   elif( v.type == 'aryflt' ):
      value = [float(x) for x in value.split(',')]
      bval = SplitFlt( value )

   else:
      print "Sorry, can't handle this one"
      return

   SendCmd( OP_SET, Split16( v.id )+bval )

def GetTrace():

   # See if there's any data collected
   n = GetVar( 'trace_samples' )
   if( n < 1 ):
      return None

   # Find the trace variables current enabled
   vct = 0;
   for i in range(4):
      v = GetVar( 'trace_var%d' % (i+1) );
      if( v==0 ):
         break
      vct += 1;

      # Find the trace variable object that goes with this ID.
      # We should already know this unless the trace was setup prior
      # to starting the interface
      if( activeTraceVars[i] == None or activeTraceVars[i].id != v ):
         activeTraceVars[i] = TraceVarByID( v )

   if( vct < 1 ):
      return None

   # n should be a multiple of the number of 
   # trace variables.  Just make sure that's the
   # case
   n -= n % vct;

   # Read the trace data as one big array
   dat = peek( 0x20006000, ct=n*4, raw=True )
   dat = Build32( dat, le=True, signed=False )

   # Chop it up into separate arrays for each trace
   # variable and reformat as necessary
   ret = []
   for i in range(vct):
      raw = dat[i::vct]
      if( activeTraceVars[i] != None ):
         raw = [activeTraceVars[i].Convert(x) for x in raw]
      ret.append(raw)

   return ret

def TraceGraph( dat=None ):

   if( dat == None ):
      dat = GetTrace()
      if( dat == None ):
         return

   freq = float(GetVar( 'loop_freq' ));
   per = GetVar( 'trace_period' )
   if( per < 1 ): per = 1;
   M = per / float(freq)

   time = [i*M for i in range(len(dat[0]))]
   plt.figure()
   for d in dat:
      plt.plot( time, d );
   plt.grid()
   plt.show();

def Cksum( buff ):
   s = 0x55
   for b in buff: s ^= b
   return s

def EscCmd( buff ):
   ret = []
   for i in buff:
      if( i==ESC or i==TERM ):
         ret.append(ESC)
      ret.append(i)
   ret.append(TERM)
   return ret;

def GetResp( show=False ):
   dat = []
   esc = False
   if( show ):
      print 'Getting resp: ',
   while( True ):
      x = ser.read(1)
      if( len(x) < 1 ):
         if( show ):
            print 'timeout'
         return dat
      x = ord(x)
      if( show ):
         print '0x%02x' % x,

      if( esc ):
         esc = False
         dat.append(x)
         continue

      if( x == ESC ):
         esc = True
         continue

      if( x == TERM ):
         if( show ):
            print
         return dat
      dat.append(x)

def SendCmd( op, data=[], timeout=None ):
   global showSerial, ser
   show = showSerial
   buff = [op] + data
   buff.insert( 1,Cksum(buff) )

   if( show ):
      S = 'CMD: '
      for b in buff: S += '0x%02x ' % b
      print S

   buff = EscCmd( buff )
   if( show ):
      S = 'ESC: '
      for b in buff: S += '0x%02x ' % b
      print S

   ser.write( ''.join( [chr(x) for x in buff]) )
   if( timeout != None ):
      if( show ):
         print 'Setting timeout to %.1f' % timeout
      oldto = ser.timeout
      ser.timeout = timeout

   rsp = GetResp( show )
   if( timeout != None ):
      ser.timeout = oldto

   if( len(rsp) < 1 ):
      return rsp
   if( Cksum(rsp) ):
      print 'Checksum error on response'
      return None

   if( len(rsp)>0 and rsp[0] ):
      #raise Error( "Error %d (0x%02x)" % (rsp[0],rsp[0]) )
      print "Error %d (0x%02x)" % (rsp[0],rsp[0]);
      return None
   return rsp[2:]

def I2F( ival ):
   s = struct.pack( 'I',ival&0xffffffff );
   return struct.unpack('f',s)[0]

def F2I( fval ):
   s = struct.pack( 'f',fval );
   return struct.unpack('I',s)[0]

def SplitInt( val, len=4, le=True, asStr=False ):
   ret = [];

   if( isinstance( val, list ) ):
      for v in val:
         ret += SplitInt( v, len, le, False );
   else:
      for i in range(len):
         ret.append( int(val & 0x00FF) );
         val >>= 8;

      if( not le ): ret = ret[::-1];
   if( asStr ): ret = "".join([chr(i) for i in ret]);
   return ret;

def Split16( x, le=True, asStr=False ): return SplitInt( x, 2, le, asStr );
def Split32( x, le=True, asStr=False ): return SplitInt( x, 4, le, asStr );
def SplitFlt( x ): return Split32( [F2I(i) for i in x] )

# Utility function to convert bytes to ints
def MakeInt( bytes, signed=True, le=True ):
   if( not le ): bytes = bytes[::-1];
   val = 0;
   shift = 0;
   for b in bytes:
      val |= (b<<shift);
      shift += 8;

   if( signed and bytes[-1] & 0x80 ):
      return val - (1<<shift)
   return val

def Build32( dat, le=True, signed=False ):
   ret = [];
   for i in range( len(dat)/4 ):
      ret.append( MakeInt( dat[ (4*i):(4*i+4) ], signed=signed, le=le ) );
   return ret;

def Build16( dat, le=True, signed=False ):
   ret = [];
   for i in range( len(dat)/2 ):
      ret.append( MakeInt( dat[ (2*i):(2*i+2) ], signed=signed, le=le ) );
   return ret;

def BuildFlt( dat ):
   tmp = Build32( dat, le=True, signed=False )
   return [I2F(x) for x in tmp]

def GrabElems( dat, ct ):
   if( len(dat) < ct ):
      dat.extend( (ct-len(dat))*[0] );
   ret = dat[:ct];
   del dat[0:ct];
   return ret;

def GrabI8( dat, le=True ):  return MakeInt( GrabElems(dat,1), signed=True,  le=le );
def GrabU8( dat, le=True ):  return MakeInt( GrabElems(dat,1), signed=False, le=le );
def GrabI16( dat, le=True ): return MakeInt( GrabElems(dat,2), signed=True,  le=le );
def GrabU16( dat, le=True ): return MakeInt( GrabElems(dat,2), signed=False, le=le );
def GrabI24( dat, le=True ): return MakeInt( GrabElems(dat,3), signed=True,  le=le );
def GrabU24( dat, le=True ): return MakeInt( GrabElems(dat,3), signed=False, le=le );
def GrabI32( dat, le=True ): return MakeInt( GrabElems(dat,4), signed=True,  le=le );
def GrabU32( dat, le=True ): return MakeInt( GrabElems(dat,4), signed=False, le=le );
def GrabI64( dat, le=True ): return MakeInt( GrabElems(dat,8), signed=True,  le=le );
def GrabU64( dat, le=True ): return MakeInt( GrabElems(dat,8), signed=False, le=le );
def GrabFlt( dat, le=True ): return I2F( GrabU32(dat, le=le) );

def mean( dat ):
   return float(sum(dat)) / len(dat)

def median( dat ):
   L = len(dat)
   S = sorted(dat)
   if( L & 1 ):
      return S[ int(L/2) ]
   return (S[ int(L/2)-1] + S[int(L/2)])/2

def std( dat ):
   m = mean(dat)

   x = 0
   for i in dat:
      x += (i-m)**2
   return math.sqrt( x/len(dat) )

class CRCutil:
   def __init__( self, poly=0xEDB88320 ):
      self.InitTable( poly )

   def calc( self, dat, init=0 ):
      crc = ~init & 0xFFFFFFFF;
      for d in dat:
         a = 0x00FF & (d^crc);
         crc = self.tbl[a] ^ (crc>>8);
      return ~crc & 0x00FFFFFFFF;

   def InitTable( self, poly ):
      self.tbl = []

      for i in range(256):
         crc = i;

         for j in range(8):
            lsbSet = crc & 1
            crc >>= 1;
            if( lsbSet ):
               crc ^= poly;
         self.tbl.append( crc )

cmdline();

