--- a/python/stlink.py	Sat Jan 12 09:39:23 2013 +0100
+++ b/python/stlink.py	Sat Jan 12 10:23:33 2013 +0100
@@ -116,17 +116,19 @@
    return sectors
 
 class STLink:
-   def __init__(self):
-      self.context = UsbContext()
+   """ ST link interface code """
+   def __init__(self, stlink2=None):
+      if not stlink2:
+         context = UsbContext()
+         stlink2s = list(filter(checkDevice, context.DeviceList))
+         if not stlink2s:
+            raise STLinkException('Could not find an ST link')
+         if len(stlink2s) > 1:
+            print('More then one stlink2 found, picking first one')
+         stlink2 = stlink2s[0]
+      self.stlink2 = stlink2
    def open(self):
-      context = UsbContext()
-      stlink2s = list(filter(checkDevice, context.DeviceList))
-      if not stlink2s:
-         raise STLinkException('Could not find an ST link')
-      if len(stlink2s) > 1:
-         print('More then one stlink2 found, picking first one')
-      stlink2 = stlink2s[0]
-      self.devHandle = stlink2.open()
+      self.devHandle = self.stlink2.open()
       if self.devHandle.Configuration != 1:
          self.devHandle.Configuration = 1
       self.devHandle.claimInterface(0)
@@ -245,23 +247,35 @@
       sectors = calcSectors(address, len(content))
       print('erasing {0} sectors'.format(len(sectors)))
       for sector, secsize in sectors:
-         print('erasing sector {0} of size {1}'.format(sector, secsize))
+         print('erasing sector {0} of {1} bytes'.format(sector, secsize))
          self.eraseFlashSector(sector)
 
       # program pages:
-      self.initFlashLoader()
       self.unlockFlashIf()
-      self.writeFlashCrPsiz(2)
+      self.writeFlashCrPsiz(2) # writes are 32 bits aligned
       self.setFlashCrPg()
 
+      print('writing {0} bytes'.format(len(content)), end='')
       offset = 0
+      t1 = time.time()
       while offset < len(content):
          size = len(content) - offset
          if size > 0x8000:
             size = 0x8000
 
-         self.runFlashLoader(address + offset, content[offset:offset + size])
+         chunk = content[offset:offset + size]
+         while len(chunk) % 4 != 0:
+            print('padding chunk')
+            chunk = chunk + bytes([0])
+
+         # Use simple mem32 writes:
+         self.write_mem32(address + offset, chunk)
+
          offset += size
+         print('.', end='', flush=True)
+      t2 = time.time()
+      print('Done!')
+      print('Speed: {0} bytes/second'.format(len(content)/(t2-t1)))
 
       self.lockFlash()
 
@@ -287,7 +301,7 @@
       ok = content == device_content
       print('Verify:', ok)
    def readFlash(self, address, size):
-      print('reading', address, size)
+      print('Reading {1} bytes from 0x{0:X}'.format(address, size), end='')
       offset = 0
       tmp_size = 0x1800
       t1 = time.time()
@@ -312,74 +326,57 @@
          offset += tmp_size
       t2 = time.time()
       assert size == len(image)
-      print('done! {0} bytes/second'.format(size/(t2-t1)))
+      print('Done!')
+      print('Speed: {0} bytes/second'.format(size/(t2-t1)))
       return image
-   def initFlashLoader(self):
-      # TODO: support other loader code.
-      self.write_mem32(STM32_SRAM_BASE, loader_code_stm32f4)
-   def runFlashLoader(self, targetaddress, buf):
-      bufAddress = STM32_SRAM_BASE + len(loader_code_stm32f4)
-      print('running flash loader for {0} bytes'.format(len(buf)))
-      self.write_buffer_to_sram(buf)
-      count = int(len(buf) / 4)
-      if len(buf) % 4 != 0: count += 1
-      self.write_reg(0, bufAddress)
-      self.write_reg(1, targetaddress)
-      self.write_reg(2, count)
-      self.write_reg(15, STM32_SRAM_BASE) # pc
-
-      self.run() # Let loose cpu!
-
-      while self.Status == CORE_RUNNING:
-         pass
-
-      r2 = self.read_reg(2)
-      if r2 != 0:
-         raise STLinkException("write error! {0}".format(r2))
-   def write_buffer_to_sram(self, buf):
-      bufAddress = STM32_SRAM_BASE + len(loader_code_stm32f4)
-      chunk = len(buf) >> 2
-      rem = len(buf) & 0x3
-      if chunk > 0:
-         self.write_mem32(bufAddress, buf[:chunk])
-      if rem > 0:
-         self.write_mem8(bufAddress + chunk, buf[chunk:chunk+rem])
-
    def readFlashSr(self):
       return self.read_debug32(FLASH_F4_SR)
    def readFlashCr(self):
       return self.read_debug32(FLASH_F4_CR)
+   def writeFlashCr(self, x):
+      self.write_debug32(FLASH_F4_CR, x)
    def writeFlashCrSnb(self, sector):
       x = self.readFlashCr()
       x &= ~FLASH_F4_CR_SNB_MASK
       x |= sector << FLASH_F4_CR_SNB
       x |= 1 << FLASH_F4_CR_SER
-      self.write_debug32(FLASH_F4_CR, x)
+      self.writeFlashCr(x)
    def setFlashCrMer(self):
       x = self.readFlashCr()
       x |= 1 << FLASH_CR_MER
-      self.write_debug32(FLASH_F4_CR, x)
+      self.writeFlashCr(x)
    def setFlashCrPg(self):
       x = self.readFlashCr()
       x |= 1 << FLASH_CR_PG
-      self.write_debug32(FLASH_F4_CR, x)
+      self.writeFlashCr(x)
    def writeFlashCrPsiz(self, n):
       x = self.readFlashCr()
       x &= (0x3 << 8)
       x |= n << 8
-      print('psiz', n)
-      self.write_debug32(FLASH_F4_CR, x)
+      self.writeFlashCr(x)
    def clearFlashCrMer(self):
       x = self.readFlashCr()
       x &= ~(1 << FLASH_CR_MER)
-      self.write_debug32(FLASH_F4_CR, x)
+      self.writeFlashCr(x)
    def setFlashCrStart(self):
       x = self.readFlashCr()
       x |= 1 << FLASH_F4_CR_START
-      self.write_debug32(FLASH_F4_CR, x)
+      self.writeFlashCr(x)
    def isFlashBusy(self):
       mask = 1 << FLASH_F4_SR_BSY
       sr = self.readFlashSr()
+      # Check for error bits:
+      errorbits = {}
+      errorbits[7] = 'Programming sequence error'
+      errorbits[6] = 'Programming parallelism error'
+      errorbits[5] = 'Programming alignment error'
+      errorbits[4] = 'Write protection error'
+      errorbits[1] = 'Operation error'
+      #errorbits[0] = 'End of operation'
+      for bit, msg in errorbits.items():
+         if sr & (1 << bit) == (1 << bit):
+            raise STLinkException(msg)
+
       return sr & mask == mask
    def waitFlashBusy(self):
       """ block until flash operation completes. """
@@ -391,13 +388,11 @@
       return cr & mask == mask
    def unlockFlashIf(self):
       if self.isFlashLocked():
-         print('unlocking')
          self.write_debug32(FLASH_F4_KEYR, FLASH_KEY1)
          self.write_debug32(FLASH_F4_KEYR, FLASH_KEY2)
          if self.isFlashLocked():
             raise STLinkException('Failed to unlock')
    def lockFlash(self):
-      print('locking')
       x = self.readFlashCr() | (1 << FLASH_F4_CR_LOCK)
       self.write_debug32(FLASH_F4_CR, x)
 
@@ -405,8 +400,7 @@
    def write_debug32(self, address, value):
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, JTAG_WRITEDEBUG_32BIT
-      cmd[2:6] = struct.pack('<I', address)
-      cmd[6:10] = struct.pack('<I', value)
+      cmd[2:10] = struct.pack('<II', address, value)
       self.send_recv(cmd, 2)
    def read_debug32(self, address):
       cmd = bytearray(16)
@@ -428,16 +422,14 @@
       assert len(content) % 4 == 0
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, DEBUG_WRITEMEM_32BIT
-      cmd[2:6] = struct.pack('<I', address)
-      cmd[6:8] = struct.pack('<H', len(content))
+      cmd[2:8] = struct.pack('<IH', address, len(content))
       self.send_recv(cmd)
       self.send_recv(content)
    def read_mem32(self, address, length):
       assert length % 4 == 0
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, DEBUG_READMEM_32BIT
-      cmd[2:6] = struct.pack('<I', address)
-      cmd[6:8] = struct.pack('<H', length) # uint16
+      cmd[2:8] = struct.pack('<IH', address, length)
       reply = self.send_recv(cmd, length) # expect memory back!
       return reply
 
@@ -487,4 +479,5 @@
    print('mode at end:', sl.CurrentModeString)
 
    sl.close()
+   print('Test succes!')