simulation/gazebo: added controller for imu, range sensors and diff
[ros_wild_thumper.git] / scripts / bootloader.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import struct
5 import sys
6 import socket
7 from optparse import OptionParser
8 from time import sleep, time
9 from i2c import i2c
10
11 CMD_READ = 0x1
12 CMD_ERASE = 0x2
13 CMD_WRITE = 0x3
14 CMD_ERASE_ALL = 0x5
15 CMD_JUMP = 0x6
16 CMD_INFO = 0x99
17
18 PAGESIZE=64
19
20 class bootloader:
21         def __init__(self, addr):
22                 self.i2c_addr = addr    
23                 self.boot_addr = 0x0
24
25                 if not self.identify():
26                         raise "Bootloader not running"
27         
28         def read_mem(self, addr, num):
29                 self.run_cmd(CMD_READ, addr, num)
30                 return self.read(num)
31
32         def erase(self, addr):
33                 self.run_cmd(CMD_ERASE)
34         
35         def erase_all(self):
36                 self.run_cmd(CMD_ERASE_ALL)
37
38         def __compare_memarea(self, addr, data):
39                         mem_cmp = self.read_mem(addr, 64)
40                         if mem_cmp != data:
41                                 print "Expected:", data.encode("hex")
42                                 print "Got:     ", mem_cmp.encode("hex")
43                                 raise("Compare mismatch at 0x%x" % addr)
44                         return
45         
46         def __program_memarea(self, addr, data):
47                 self.run_cmd(CMD_WRITE, addr, 64, data)
48
49         def compare(self, filename):
50                 return self.__process_hex(filename, self.__compare_memarea)
51         
52         def program(self, filename):
53                 return self.__process_hex(filename, self.__program_memarea)
54
55         def __process_hex(self, filename, handle):
56                 next_addr = None
57                 buf = ""
58                 lFirstRow = None
59
60                 f = open(filename, "r")
61                 count=0
62                 for line in f:
63                         if line[0] != ':':
64                                 raise("Bad line start character")
65                         hex = line[1:].replace("\r\n", "")
66                         data = hex.decode("hex")
67                         num = ord(data[0])
68                         chksum = 0
69                         for c in data:
70                                 chksum+=ord(c)
71                         if chksum % 256 != 0:
72                                 raise("Checksum error")
73                         addr, typ, data, chksum = struct.unpack(">HB%ssB" % num, data[1:])
74
75                         if typ == 0: # Data Record
76                                 count+=len(data)
77                                 if next_addr is not None:
78                                         if next_addr != addr:
79                                                 raise "Gap in file"
80                                 buf_addr = addr-len(buf)
81                                 buf+=data
82                                 if len(buf) >= PAGESIZE:
83                                         if not lFirstRow:
84                                                 # do the first as last one
85                                                 lFirstRow = (buf_addr, buf[:PAGESIZE])
86                                         else:
87                                                 print "Addr 0x%x" % buf_addr
88                                                 handle(buf_addr, buf[:PAGESIZE])
89                                         buf = buf[PAGESIZE:]
90                         elif typ == 3: # Start Segment Address Record
91                                 self.boot_addr = int(data.encode("hex"), 16)
92                         elif typ == 1: # End of File Record
93                                 print "Addr (rest) 0x%x" % buf_addr
94                                 buf_addr+=PAGESIZE
95                                 diff = PAGESIZE-len(buf)
96                                 buf+=chr(0xff)*diff # fill with 0xff
97                                 handle(buf_addr, buf[:PAGESIZE])
98                                 if lFirstRow: # was first
99                                         buf_addr = lFirstRow[0]
100                                         buf = lFirstRow[1]
101                                         print "Addr (First) 0x%x" % buf_addr
102                                         handle(buf_addr, buf)
103                         else:
104                                 raise("Unknown type %d" % typ)
105                         
106                         next_addr = addr+num
107                 print "Byte count:", count
108                 f.close()
109
110         def jump(self, addr):
111                 self.run_cmd(CMD_JUMP)
112
113         def wait_ping(self):
114                 while(True):
115                         try:
116                                 self.identify()
117                                 break
118                         except:
119                                 sleep(1)
120
121         def load(self, filename):
122                 print "Erase..."
123                 self.erase_all()
124                 self.wait_ping()
125                 print "Erase Done."
126                 print "Program..."
127                 t1 = time()
128                 self.program(filename)
129                 print "Time: %.1fs" % (time() - t1)
130                 print "Compare..."
131                 t1 = time()
132                 self.compare(filename)
133                 print "Time: %.1fs" % (time() - t1)
134                 print "Jump:"
135                 self.jump(self.boot_addr)
136
137         def write(self, s):
138                 dev = i2c(self.i2c_addr)
139                 dev.write(s)
140                 dev.close()
141
142         def read(self, num):
143                 dev = i2c(self.i2c_addr)
144                 s = dev.read(num)
145                 dev.close()
146                 return s
147         
148         def run_cmd(self, cmd, addr=0x0, num=0, data=""):
149                 length = len(data)
150                 s1 = struct.pack("<BLB%ds" % (length), cmd, addr, num, data)
151                 self.write(s1)
152                 s2 = struct.pack("B", 0xff)
153                 self.write(s2)
154
155         def identify(self):
156                 self.run_cmd(CMD_INFO)
157                 s = self.read(10)
158                 return s == "Bootloader"
159
160 def to_bootloader(addr):
161         dev = i2c(addr)
162         s = struct.pack("B", 0xff)
163         dev.write(s)
164         dev.close()
165
166 if __name__ == "__main__":
167         usage = "usage: %prog [options] addr [ihex]"
168         parser = OptionParser(usage=usage)
169         parser.add_option("-b", "--start-bootloader", action="store_true", dest="bToBoot", default=False, help="Start Bootloader")
170
171         (options, args) = parser.parse_args()
172         if len(args) > 1:
173                 addr = int(args[0], 16)
174                 if options.bToBoot:
175                         to_bootloader(addr)
176                         sleep(1)
177                 if len(args) > 1:
178                         loader = bootloader(addr)
179                         loader.load(args[1])