--- a/python/stlink.py	Sat Jan 05 00:06:27 2013 +0100
+++ b/python/stlink.py	Sat Jan 05 19:07:14 2013 +0100
@@ -1,3 +1,4 @@
+import struct, time
 from usb import UsbContext
 
 class STLinkException(Exception):
@@ -8,11 +9,28 @@
    STLINK2_PID=0x3748
    return device.VendorId == ST_VID and device.ProductId == STLINK2_PID
 
-XFER_TO_DEV=0
-XFER_FROM_DEV=0x80
+DFU_MODE, MASS_MODE, DEBUG_MODE = range(3)
+# Commands:
+GET_VERSION = 0xf1
+DEBUG_COMMAND = 0xf2
+DFU_COMMAND = 0xf3
+GET_CURRENT_MODE = 0xf5
+
+# dfu commands:
+DFU_EXIT = 0x7
 
-class STLink(object):
-   DFU_MODE = 222
+# debug commands:
+DEBUG_ENTER = 0x20
+DEBUG_EXIT = 0x21
+DEBUG_ENTER_SWD = 0xa3
+DEBUG_GETSTATUS = 0x01
+
+JTAG_READDEBUG_32BIT = 0x36
+
+# cortex M3
+CM3_REG_CPUID = 0xE000ED00
+
+class STLink:
    def __init__(self):
       self.context = UsbContext()
    def open(self):
@@ -23,22 +41,129 @@
       if len(stlink2s) > 1:
          print('More then one stlink2 found, picking first one')
       stlink2 = stlink2s[0]
-      dev = stlink2.open()
-      if dev.Configuration != 1:
-         dev.Configuration = 1
-      dev.claimInterface(0)
+      self.devHandle = stlink2.open()
+      if self.devHandle.Configuration != 1:
+         self.devHandle.Configuration = 1
+      self.devHandle.claimInterface(0)
+   def close(self):
+      pass
    def getCurrentMode(self):
-      print('get cur mode')
-      rep_len = 2
-      self.fillCommand(self, XFER_FROM_DEV, rep_len)
-      size = self.send_recv(1, cmd, data)
-      return self.q_buf[0]
+      cmd = bytearray(16)
+      cmd[0] = GET_CURRENT_MODE
+      reply = self.send_recv(cmd, 2) # Expect 2 bytes back
+      return reply[0]
    CurrentMode = property(getCurrentMode)
-   def fillCommand(self, di, rl):
-      bytes(b'USBC')
-      pass
-   def reset(self):
-      pass
-   def send_recv(self, txbuf, rxbuf):
-      pass
+   @property
+   def CurrentModeString(self):
+      modes = {DFU_MODE: 'dfu', MASS_MODE: 'massmode', DEBUG_MODE:'debug'}
+      return modes[self.CurrentMode]
+   def exitDfuMode(self):
+      cmd = bytearray(16)
+      cmd[0] = DFU_COMMAND
+      cmd[1] = DFU_EXIT
+      self.send_recv(cmd)
+   def enterSwdMode(self):
+      cmd = bytearray(16)
+      cmd[0] = DEBUG_COMMAND
+      cmd[1] = DEBUG_ENTER
+      cmd[2] = DEBUG_ENTER_SWD
+      self.send_recv(cmd)
+   def exitDebugMode(self):
+      cmd = bytearray(16)
+      cmd[0] = DEBUG_COMMAND
+      cmd[1] = DEBUG_EXIT
+      self.send_recv(cmd)
+      
+   def getVersion(self):
+      cmd = bytearray(16)
+      cmd[0] = GET_VERSION
+      data = self.send_recv(cmd, 6) # Expect 6 bytes back
+      # Parse 6 bytes into various versions:
+      b0, b1, b2, b3, b4, b5 = data
+      stlink_v = b0 >> 4
+      jtag_v = ((b0 & 0xf) << 2) | (b1 >> 6)
+      swim_v = b1 & 0x3f
+      vid = (b3 << 8) | b2
+      pid = (b5 << 8) | b4
+
+      return 'stlink={0} jtag={1} swim={2} vid:pid={3:04X}:{4:04X}'.format(\
+         stlink_v, jtag_v, swim_v, vid, pid)
+   Version = property(getVersion)
+   
+   @property
+   def ChipId(self):
+      return self.read_debug32(0xE0042000)
+   @property
+   def CpuId(self):
+      u32 = self.read_debug32(CM3_REG_CPUID)
+      implementer_id = (u32 >> 24) & 0x7f
+      variant = (u32 >> 20) & 0xf
+      part = (u32 >> 4) & 0xfff
+      revision = u32 & 0xf
+      return implementer_id, variant, part, revision
+
+   def status(self):
+      cmd = bytearray(16)
+      cmd[0] = DEBUG_COMMAND
+      cmd[1] = DEBUG_GETSTATUS
+      reply = self.send_recv(cmd, 2)
+      return reply[0]
 
+   def step(self):
+      cmd = bytearray(16)
+      cmd[0] = DEBUG_COMMAND
+      cmd[1] = DEBUG_STEPCORE
+      self.send_recv(cmd, 2)
+   def run(self):
+      cmd = bytearray(16)
+      cmd[0] = DEBUG_COMMAND
+      cmd[1] = DEBUG_RUNCORE
+      self.send_recv(cmd, 2)
+      
+   # Helper 1 functions:
+   def read_debug32(self, address):
+      cmd = bytearray(16)
+      cmd[0] = DEBUG_COMMAND
+      cmd[1] = JTAG_READDEBUG_32BIT
+      cmd[2:6] = struct.pack('<I', address) # pack into u32 little endian
+      reply = self.send_recv(cmd, 8)
+      return struct.unpack('<I', reply[4:8])[0]
+
+   # Helper 2 functions:
+   def send_recv(self, tx, rxsize=0):
+      """ Helper function that transmits and receives data. """
+      # TODO: we could use here the non-blocking libusb api.
+      tx = bytes(tx)
+      self.devHandle.bulkWrite(2, tx) # write to endpoint 2
+      if rxsize > 0:
+         return self.devHandle.bulkRead(1, rxsize) # read from endpoint 1
+
+knownChipIds = {0x1: 'x'}
+
+if __name__ == '__main__':
+   # Test program
+   sl = STLink()
+   sl.open()
+   print('version:', sl.Version)
+   print('mode before doing anything:', sl.CurrentModeString)
+   if sl.CurrentMode == DFU_MODE:
+      sl.exitDfuMode()
+   sl.enterSwdMode()
+   print('mode after entering swd mode:', sl.CurrentModeString)
+
+   i = sl.ChipId
+   if i in knownChipIds:
+      print('chip id: 0x{0:X} -> {1}'.format(i, knownChipIds[i]))
+   else:
+      print('chip id: 0x{0:X}'.format(i))
+   print('cpu: {0}'.format(sl.CpuId))
+
+   print('status: {0}'.format(sl.status()))
+
+   time.sleep(2.2)
+
+   sl.exitDebugMode()
+   print('mode at end:', sl.CurrentModeString)
+
+   sl.close()
+