--- a/python/stlink.py	Mon Apr 22 23:54:54 2013 +0200
+++ b/python/stlink.py	Sat May 04 12:07:17 2013 +0200
@@ -3,6 +3,13 @@
 from devices import Interface, STLinkException, registerInterface
 import adi
 
+"""
+   More or less copied from:
+     https://github.com/texane/stlink
+   Tracing from:
+     https://github.com/obe1line/stlink-trace
+
+"""
 ST_VID, STLINK2_PID = 0x0483, 0x3748
 
 def checkDevice(device):
@@ -39,6 +46,7 @@
 
 JTAG_WRITEDEBUG_32BIT = 0x35
 JTAG_READDEBUG_32BIT = 0x36
+TRACE_GET_BYTE_COUNT = 0x42
 
 # cortex M3
 CM3_REG_CPUID = 0xE000ED00
@@ -172,19 +180,55 @@
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, DEBUG_FORCEDEBUG
       self.send_recv(cmd, 2)
+   
+   # Tracing:
+   def traceEnable(self):
+      self.write_debug32(0xE000EDF0, 0xA05F0003)
 
-   def traceEnable(self):
+      # Enable TRCENA:
       DEMCR = 0xE000EDFC
       v = self.read_debug32(DEMCR)
       v |= (1 << 24)
       self.write_debug32(DEMCR, v)
 
+      # ?? Enable write??
+      self.write_debug32(0xE0002000, 0x2) #
+
+      # DBGMCU_CR:
+      self.write_debug32(0xE0042004, 0x27) # Enable trace in async mode
+
+      # TPIU config:
+      self.write_debug32(0xE0040004, 0x00000001) # current port size register --> 1 == port size = 1
+      self.write_debug32(0xE0040010, 0x23) # random clock divider??
+      self.write_debug32(0xE00400F0, 0x2) # selected pin protocol (2 == NRZ)
+      self.write_debug32(0xE0040304, 0x100) # continuous formatting
+      
+      # ITM config:
+      self.write_debug32(0xE0000FB0, 0xC5ACCE55) # Unlock write access to ITM
+      self.write_debug32(0xE0000F80, 0x00010005) # ITM Enable, sync enable, ATB=1
+      self.write_debug32(0xE0000E00, 0xFFFFFFFF) # Enable all trace ports in ITM
+      self.write_debug32(0xE0000E40, 0x0000000F) # Set privilege mask for all 32 ports.
+   def writePort0(self, v32):
+      self.write_debug32(0xE0000000, v32)
+   def getTraceByteCount(self):
+      cmd = bytearray(16)
+      cmd[0:2] = DEBUG_COMMAND, 0x42
+      reply = self.send_recv(cmd, 2)
+      return struct.unpack('<H', reply[0:2])[0]
+   def readTraceData(self):
+      bsize = self.getTraceByteCount()
+      if bsize > 0:
+         td = self.recv_ep3(bsize)
+         print(td)
+      else:
+         print('no trace data')
+
    # Helper 1 functions:
    def write_debug32(self, address, value):
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, JTAG_WRITEDEBUG_32BIT
       cmd[2:10] = struct.pack('<II', address, value)
-      self.send_recv(cmd, 2)
+      r = self.send_recv(cmd, 2)
    def read_debug32(self, address):
       cmd = bytearray(16)
       cmd[0:2] = DEBUG_COMMAND, JTAG_READDEBUG_32BIT
@@ -231,6 +275,8 @@
       self.devHandle.bulkWrite(2, tx) # write to endpoint 2
       if rxsize > 0:
          return self.devHandle.bulkRead(1, rxsize) # read from endpoint 1
+   def recv_ep3(self, rxsize):
+      return self.devHandle.bulkRead(3, rxsize)
 
 if __name__ == '__main__':
    # Test program
@@ -263,6 +309,14 @@
    regs = sl.read_all_regs()
    for i in range(len(regs)):
       print('R{0}=0x{1:X}'.format(i, regs[i]))
+
+   print('tracing')
+   sl.traceEnable()
+   sl.run()
+   sl.writePort0(0x1337) # For test
+   time.sleep(0.1)
+   td = sl.readTraceData()
+   print('trace data:', td)
    
    # Test CoreSight registers:
    idr4 = sl.read_debug32(0xE0041fd0)