--- a/python/stlink.py	Mon Jan 07 19:30:01 2013 +0100
+++ b/python/stlink.py	Sat Jan 12 09:38:35 2013 +0100
@@ -10,6 +10,10 @@
    return device.VendorId == ST_VID and device.ProductId == STLINK2_PID
 
 DFU_MODE, MASS_MODE, DEBUG_MODE = 0, 1, 2
+
+CORE_RUNNING = 0x80
+CORE_HALTED = 0x81
+
 # Commands:
 GET_VERSION = 0xf1
 DEBUG_COMMAND = 0xf2
@@ -29,6 +33,8 @@
 DEBUG_WRITEREG = 0x6
 DEBUG_READMEM_32BIT = 0x7
 DEBUG_WRITEMEM_32BIT = 0x8
+DEBUG_RUNCORE = 0x9
+DEBUG_STEPCORE = 0xa
 
 JTAG_WRITEDEBUG_32BIT = 0x35
 JTAG_READDEBUG_32BIT = 0x36
@@ -51,6 +57,7 @@
 
 FLASH_F4_CR_START = 16
 FLASH_F4_CR_LOCK = 31
+FLASH_CR_PG = 0
 FLASH_F4_CR_SER = 1
 FLASH_CR_MER = 2
 FLASH_F4_CR_SNB = 3
@@ -131,6 +138,7 @@
          self.enterSwdMode()
       self.reset()
    def close(self):
+      # TODO
       pass
 
    # modes:
@@ -185,11 +193,19 @@
       revision = u32 & 0xf
       return implementer_id, variant, part, revision
 
-   def status(self):
+   def getStatus(self):
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, DEBUG_GETSTATUS
       reply = self.send_recv(cmd, 2)
       return reply[0]
+   Status = property(getStatus)
+   @property
+   def StatusString(self):
+      s = self.Status
+      statii = {CORE_RUNNING: 'CORE RUNNING', CORE_HALTED: 'CORE HALTED'}
+      if s in statii:
+         return statii[s]
+
    def reset(self):
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, DEBUG_RESETSYS
@@ -235,7 +251,17 @@
       # program pages:
       self.initFlashLoader()
       self.unlockFlashIf()
-      # TODO
+      self.writeFlashCrPsiz(2)
+      self.setFlashCrPg()
+
+      offset = 0
+      while offset < len(content):
+         size = len(content) - offset
+         if size > 0x8000:
+            size = 0x8000
+
+         self.runFlashLoader(address + offset, content[offset:offset + size])
+         offset += size
 
       self.lockFlash()
 
@@ -291,7 +317,34 @@
    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 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):
@@ -306,6 +359,16 @@
       x = self.readFlashCr()
       x |= 1 << FLASH_CR_MER
       self.write_debug32(FLASH_F4_CR, x)
+   def setFlashCrPg(self):
+      x = self.readFlashCr()
+      x |= 1 << FLASH_CR_PG
+      self.write_debug32(FLASH_F4_CR, x)
+   def writeFlashCrPsiz(self, n):
+      x = self.readFlashCr()
+      x &= (0x3 << 8)
+      x |= n << 8
+      print('psiz', n)
+      self.write_debug32(FLASH_F4_CR, x)
    def clearFlashCrMer(self):
       x = self.readFlashCr()
       x &= ~(1 << FLASH_CR_MER)
@@ -408,15 +471,17 @@
       print('chip id: 0x{0:X}'.format(i))
    print('cpu: {0}'.format(sl.CpuId))
 
-   print('status: {0}'.format(sl.status()))
+   print('status: {0}'.format(sl.StatusString))
 
    #time.sleep(2.2)
 
    # test registers:
    sl.write_reg(3, 0x1337)
    sl.write_reg(2, 0x1332)
+   sl.write_reg(6, 0x12345)
    assert sl.read_reg(3) == 0x1337
    assert sl.read_reg(2) == 0x1332
+   assert sl.read_reg(6) == 0x12345
 
    sl.exitDebugMode()
    print('mode at end:', sl.CurrentModeString)