--- 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()
+

--- a/python/usb.py	Sat Jan 05 00:06:27 2013 +0100
+++ b/python/usb.py	Sat Jan 05 19:07:14 2013 +0100
@@ -1,12 +1,12 @@
-from ctypes import Structure, POINTER, CDLL
-from ctypes import c_uint16, c_uint8, c_int, c_ssize_t
-from ctypes import byref
+from ctypes import Structure, POINTER, CDLL, CFUNCTYPE
+from ctypes import c_uint16, c_uint8, c_int, c_uint, c_ssize_t, c_void_p
+from ctypes import byref, create_string_buffer
 
 # libusb wrapper:
 libusb = CDLL('libusb-1.0.so')
 
 # helper:
-def buildfunc(name, argtypes, restype):
+def buildfunc(name, argtypes, restype=c_int):
    f = getattr(libusb, name)
    f.argtypes = argtypes
    f.restype = restype
@@ -34,6 +34,7 @@
          CANCELLED=3, STALL=4, NO_DEVICE=5, OVERFLOW=6)
 
 # types
+c_int_p = POINTER(c_int)
 class libusb_context(Structure):
    pass
 libusb_context_p = POINTER(libusb_context)
@@ -69,6 +70,27 @@
               ]
 libusb_device_descriptor_p = POINTER(libusb_device_descriptor)
 
+"""
+class libusb_transfer(Structure):
+   pass
+libusb_transfer_p = POINTER(libusb_transfer)
+libusb_transfer_cb_fn = CFUNCTYPE(None, libusb_transfer_p)
+libusb_transfer._fields_ = [
+      ('dev_handle', libusb_device_handle_p),
+      ('flags', c_uint8),
+      ('endpoint', c_uchar),
+      ('type', c_uchar),
+      ('timeout', c_uint),
+      ('status', c_int), # enum libusb_transfer_status
+      ('length', c_int),
+      ('actual_length', c_int),
+      ('callback', libusb_transfer_cb_fn),
+      ('userdata', c_void_p),
+      ('buffer', c_void_p),
+      ('num_iso_packets', c_int),
+      ('iso_packet_desc', libusb_iso_packet_descriptor)
+   ]
+"""
 # functions
 buildfunc('libusb_init', [libusb_context_p_p], c_int)
 
@@ -77,18 +99,21 @@
 buildfunc('libusb_free_device_list', [libusb_device_p_p, c_int], None)
 buildfunc('libusb_get_bus_number', [libusb_device_p], c_uint8)
 buildfunc('libusb_get_device_address', [libusb_device_p], c_uint8)
-buildfunc('libusb_get_device_speed', [libusb_device_p], c_int)
+buildfunc('libusb_get_device_speed', [libusb_device_p])
 buildfunc('libusb_unref_device', [libusb_device_p], None)
-buildfunc('libusb_open', [libusb_device_p, libusb_device_handle_p_p], c_int)
+buildfunc('libusb_open', [libusb_device_p, libusb_device_handle_p_p])
 buildfunc('libusb_close', [libusb_device_handle_p], None)
-buildfunc('libusb_get_configuration', \
-   [libusb_device_handle_p, POINTER(c_int)], c_int)
-buildfunc('libusb_set_configuration', [libusb_device_handle_p, c_int], c_int)
-buildfunc('libusb_claim_interface', [libusb_device_handle_p, c_int], c_int)
+buildfunc('libusb_get_configuration',[libusb_device_handle_p,POINTER(c_int)])
+buildfunc('libusb_set_configuration', [libusb_device_handle_p, c_int])
+buildfunc('libusb_claim_interface', [libusb_device_handle_p, c_int])
 
 
 buildfunc('libusb_get_device_descriptor',\
-   [libusb_device_p, libusb_device_descriptor_p], c_int)
+   [libusb_device_p, libusb_device_descriptor_p])
+
+# synchronous functions:
+buildfunc('libusb_bulk_transfer', [libusb_device_handle_p, c_uint8, \
+   c_void_p, c_int, c_int_p, c_uint])
 
 # pythonic API:
 
@@ -159,6 +184,10 @@
          self.ProductId, self.Speed)
       return r2
 
+USB_ENDPOINT_DIR_MASK = 0x80
+USB_ENDPOINT_IN = 0x80
+USB_ENDPOINT_OUT = 0x0
+
 class UsbDeviceHandle:
    """ Handle to a detected usb device """
    def __init__(self, device, handle_p):
@@ -176,7 +205,35 @@
    def claimInterface(self, interface_number):
       r = libusb_claim_interface(self.handle_p, interface_number)
       if r != 0: raise UsbError('Error claiming interface', r)
-
+   def bulkWrite(self, endpoint, data, timeout=0):
+      """ Synchronous bulk write """
+      assert type(data) is bytes
+      # assure the endpoint indicates the correct:
+      endpoint = (endpoint & (~USB_ENDPOINT_DIR_MASK)) | USB_ENDPOINT_OUT
+      buf = create_string_buffer(data)
+      transferred = c_int()
+      r = libusb_bulk_transfer(self.handle_p, endpoint, buf, len(data), \
+         byref(transferred), timeout)
+      if r != 0:
+         raise UsbError('Bulk write failed', r)
+      if transferred.value != len(data):
+         raise UsbError('Not all {0} transferred {1}'.format(len(data), \
+            transferred.value))
+   def bulkRead(self, endpoint, numbytes, timeout=0):
+      """ Synchronous bulk read """
+      # assure the endpoint indicates the correct:
+      endpoint = (endpoint & (~USB_ENDPOINT_DIR_MASK)) | USB_ENDPOINT_IN
+      buf = create_string_buffer(numbytes)
+      transferred = c_int()
+      r = libusb_bulk_transfer(self.handle_p, endpoint, buf, numbytes, \
+         byref(transferred), timeout)
+      if r != 0:
+         raise UsbError('Bulk read failed', r)
+      if transferred.value != numbytes:
+         raise UsbError('Not all {0} transferred {1}'.format(numbytes, \
+            transferred.value))
+      data = buf.raw[0:numbytes]
+      return data
 
 class UsbTransfer:
    def __init__(self):