--- a/python/stlink.py	Sun Jan 06 18:13:17 2013 +0100
+++ b/python/stlink.py	Mon Jan 07 19:30:01 2013 +0100
@@ -24,6 +24,7 @@
 DEBUG_EXIT = 0x21
 DEBUG_ENTER_SWD = 0xa3
 DEBUG_GETSTATUS = 0x01
+DEBUG_RESETSYS = 0x03
 DEBUG_READREG = 0x5
 DEBUG_WRITEREG = 0x6
 DEBUG_READMEM_32BIT = 0x7
@@ -39,19 +40,44 @@
 STM32_FLASH_BASE = 0x08000000
 STM32_SRAM_BASE  = 0x20000000
 
+FLASH_KEY1 = 0x45670123
+FLASH_KEY2 = 0xcdef89ab
 # flash registers:
-FLASH_F4_REGS_ADDR = 0x400223c0
+FLASH_F4_REGS_ADDR = 0x40023c00
 
+FLASH_F4_KEYR = FLASH_F4_REGS_ADDR + +0x04
 FLASH_F4_SR = FLASH_F4_REGS_ADDR + 0x0c
 FLASH_F4_CR = FLASH_F4_REGS_ADDR + 0x10
 
 FLASH_F4_CR_START = 16
 FLASH_F4_CR_LOCK = 31
 FLASH_F4_CR_SER = 1
+FLASH_CR_MER = 2
 FLASH_F4_CR_SNB = 3
 FLASH_F4_CR_SNB_MASK = 0x38
 FLASH_F4_SR_BSY = 16
 
+# flashloaders/stm32f4.s
+loader_code_stm32f4 = bytes([
+   0x07, 0x4b,
+
+   0x62, 0xb1,
+   0x04, 0x68,
+   0x0c, 0x60,
+
+   0xdc, 0x89,
+   0x14, 0xf0, 0x01, 0x0f,
+   0xfb, 0xd1,
+   0x00, 0xf1, 0x04, 0x00,
+   0x01, 0xf1, 0x04, 0x01,
+   0xa2, 0xf1, 0x01, 0x02,
+   0xf1, 0xe7,
+
+   0x00, 0xbe,
+
+   0x00, 0x3c, 0x02, 0x40
+])
+
 def calculate_F4_sector(address):
    """
       from 0x8000000 to 0x80FFFFF
@@ -97,6 +123,13 @@
       if self.devHandle.Configuration != 1:
          self.devHandle.Configuration = 1
       self.devHandle.claimInterface(0)
+
+      # First initialization:
+      if self.CurrentMode == DFU_MODE:
+         self.exitDfuMode()
+      if self.CurrentMode != DEBUG_MODE:
+         self.enterSwdMode()
+      self.reset()
    def close(self):
       pass
 
@@ -157,6 +190,10 @@
       cmd[0:2] = DEBUG_COMMAND, DEBUG_GETSTATUS
       reply = self.send_recv(cmd, 2)
       return reply[0]
+   def reset(self):
+      cmd = bytearray(16)
+      cmd[0:2] = DEBUG_COMMAND, DEBUG_RESETSYS
+      self.send_recv(cmd, 2)
 
    # debug commands:
    def step(self):
@@ -196,8 +233,12 @@
          self.eraseFlashSector(sector)
 
       # program pages:
+      self.initFlashLoader()
+      self.unlockFlashIf()
       # TODO
 
+      self.lockFlash()
+
       # verfify program:
       self.verifyFlash(address, content)
    def eraseFlashSector(self, sector):
@@ -207,33 +248,50 @@
       self.setFlashCrStart()
       self.waitFlashBusy()
       self.lockFlash()
+   def eraseFlash(self):
+      self.waitFlashBusy()
+      self.unlockFlashIf()
+      self.setFlashCrMer()
+      self.setFlashCrStart()
+      self.waitFlashBusy()
+      self.clearFlashCrMer()
+      self.lockFlash()
    def verifyFlash(self, address, content):
-      print('verifying', address, len(content))
+      device_content = self.readFlash(address, len(content))
+      ok = content == device_content
+      print('Verify:', ok)
+   def readFlash(self, address, size):
+      print('reading', address, size)
       offset = 0
-      cmp_size = 0x1800
+      tmp_size = 0x1800
       t1 = time.time()
-      while offset < len(content):
+      image = bytes()
+      while offset < size:
          # Correct for last page:
-         if offset + cmp_size > len(content):
-            cmp_size = len(content) - offset
+         if offset + tmp_size > size:
+            tmp_size = size - offset
 
          # align size to 4 bytes:
-         aligned_size = cmp_size
+         aligned_size = tmp_size
          while aligned_size % 4 != 0:
             aligned_size += 1
 
          mem = self.read_mem32(address + offset, aligned_size)
-         ok = mem[:cmp_size] == content[offset:offset+cmp_size]
+         image += mem[:tmp_size]
 
          # indicate progress:
-         okc = '.' if ok else 'x'
-         print(okc, end='', flush=True)
+         print('.', end='', flush=True)
 
          # increase for next piece:
-         offset += cmp_size
+         offset += tmp_size
       t2 = time.time()
-      print('done! {0} bytes/second'.format(len(content)/(t2-t1)))
-      return content == mem
+      assert size == len(image)
+      print('done! {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)
+      self.bufAddress = STM32_SRAM_BASE + len(loader_code_stm32f4)
    def readFlashSr(self):
       return self.read_debug32(FLASH_F4_SR)
    def readFlashCr(self):
@@ -244,6 +302,14 @@
       x |= sector << FLASH_F4_CR_SNB
       x |= 1 << FLASH_F4_CR_SER
       self.write_debug32(FLASH_F4_CR, x)
+   def setFlashCrMer(self):
+      x = self.readFlashCr()
+      x |= 1 << FLASH_CR_MER
+      self.write_debug32(FLASH_F4_CR, x)
+   def clearFlashCrMer(self):
+      x = self.readFlashCr()
+      x &= ~(1 << FLASH_CR_MER)
+      self.write_debug32(FLASH_F4_CR, x)
    def setFlashCrStart(self):
       x = self.readFlashCr()
       x |= 1 << FLASH_F4_CR_START
@@ -262,14 +328,15 @@
       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):
-      # TODO
-      pass
+      print('locking')
+      x = self.readFlashCr() | (1 << FLASH_F4_CR_LOCK)
+      self.write_debug32(FLASH_F4_CR, x)
 
    # Helper 1 functions:
    def write_debug32(self, address, value):
@@ -294,6 +361,14 @@
       cmd[0:3] = DEBUG_COMMAND, DEBUG_READREG, reg
       reply = self.send_recv(cmd, 4)
       return struct.unpack('<I', reply)[0]
+   def write_mem32(self, address, content):
+      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))
+      self.send_recv(cmd)
+      self.send_recv(content)
    def read_mem32(self, address, length):
       assert length % 4 == 0
       cmd = bytearray(16)
@@ -308,7 +383,7 @@
       """ Helper function that transmits and receives data in bulk mode. """
       # TODO: we could use here the non-blocking libusb api.
       tx = bytes(tx)
-      assert len(tx) == 16
+      #assert len(tx) == 16
       self.devHandle.bulkWrite(2, tx) # write to endpoint 2
       if rxsize > 0:
          return self.devHandle.bulkRead(1, rxsize) # read from endpoint 1
@@ -339,9 +414,9 @@
 
    # test registers:
    sl.write_reg(3, 0x1337)
-   sl.write_reg(1, 0x1332)
+   sl.write_reg(2, 0x1332)
    assert sl.read_reg(3) == 0x1337
-   assert sl.read_reg(1) == 0x1332
+   assert sl.read_reg(2) == 0x1332
 
    sl.exitDebugMode()
    print('mode at end:', sl.CurrentModeString)