--- a/py_avrjtag/jtagdev.py	Sun Feb 22 22:43:26 2009 +0800
+++ b/py_avrjtag/jtagdev.py	Tue Feb 24 13:32:04 2009 +0800
@@ -2,14 +2,16 @@
 import cmd_proto
 
 class jtagdev(object):
-    ST_REST = 0
+    ST_RESET = 0
     ST_IDLE = 1
     ST_SHIFT_DR = 2
     ST_SHIFT_IR = 3
+    ST_EXIT1_DR = 4
+    ST_EXIT1_IR = 5
     
     def __init__(self, fo):
         self.fo = fo
-        self.state = self.IDLE
+        self.state = self.ST_IDLE
         self.seq = 0
         pass
 
@@ -34,21 +36,41 @@
     def wait_reply(self, tmo=None):
         import select
         
-        if tmo is None:
-            select.select((self.fo,), (), ())
-        else:
-            select.select((self.fo,), (), (), tmo)
+        frame = ''
+        while True:
+            if tmo == None:
+                rlist, wlist, xlist = select.select((self.fo,), (), ())
+            else:
+                rlist, wlist, xlist = select.select((self.fo,), (), (), tmo)
+                pass
+            if not rlist:
+                break
+            frame = frame + self.fo.read()
+            if len(frame) >= 4 and \
+                    len(frame) >= (ord(frame[3]) +
+                                   cmd_proto.cmd.FRAME_OVERHEAD):
+                    break
+            tmo = 0.05
             pass
-        frame = self.fo.read()
+        
+        if not frame:
+            return None
+        
         cmd = cmd_proto.cmd()
-        cmd.from_frame(frame)
+        r = cmd.from_frame(frame)
+        if r:
+            return None
         return cmd
 
     def idle(self):
         self.seq = (self.seq + 1) % 256
         
-        ptn, nbits = tms_ptns.TMS_IDLE_SEQ
-        self.send_tms(ptn, nbits)
+        if self.state == self.ST_RESET:
+            ptn, nbits = tms_ptns.TMS_RESET_2_IDLE
+        else:
+            ptn, nbits = tms_ptns.TMS_IDLE_SEQ
+            pass
+        self._send_tms(ptn, nbits)
         
         self.state = self.ST_IDLE
         pass
@@ -58,11 +80,11 @@
             return
         
         if self.state == self.ST_IDLE:
-            ptn, ptn_nbits = tms_ptns.TMS_CAP_IR
-            self.send_tms(ptn, nbits)
-        elif self.state == self.ST_SHIFT_DR:
-            ptn, ptn_nbits = tms_ptns.TMS_SHIFT_DR_2_CAP_IR
-            self.send_tms(ptn, nbits)
+            ptn, ptn_nbits = tms_ptns.TMS_SHIFT_IR
+            self._send_tms(ptn, ptn_nbits)
+        elif self.state == self.ST_EXIT1_DR:
+            ptn, ptn_nbits = tms_ptns.TMS_EXIT1_DR_2_SHIFT_IR
+            self._send_tms(ptn, ptn_nbits)
         else:
             raise RuntimeError, 'Transite to shift IR state from invalid state'
         self.state = self.ST_SHIFT_IR
@@ -73,11 +95,11 @@
             return
         
         if self.state == self.ST_IDLE:
-            ptn, ptn_nbits = tms_ptns.TMS_CAP_DR
-            self.send_tms(ptn, nbits)
-        elif self.state == self.ST_SHIFT_IR:
-            ptn, ptn_nbits = tms_ptns.TMS_SHIFT_IR_2_CAP_DR
-            self.send_tms(ptn, nbits)
+            ptn, ptn_nbits = tms_ptns.TMS_SHIFT_DR
+            self._send_tms(ptn, ptn_nbits)
+        elif self.state == self.ST_EXIT1_IR:
+            ptn, ptn_nbits = tms_ptns.TMS_EXIT1_IR_2_SHIFT_DR
+            self._send_tms(ptn, ptn_nbits)
         else:
             raise RuntimeError, 'Transite to shift DR state from invalid state'
             pass
@@ -92,6 +114,7 @@
 
         self._send_nbits_data_cmd(cmd_proto.CPCMD_SHIFT_TDI,
                                   nbits, data)
+        self.state = self.ST_EXIT1_IR
         pass
 
     def shift_DR(self, data, nbits):
@@ -102,6 +125,7 @@
 
         self._send_nbits_data_cmd(cmd_proto.CPCMD_SHIFT_TDI,
                                   nbits, data)
+        self.state = self.ST_EXIT1_DR
         pass
 
     def shift_DR_n_out(self, data, nbits):
@@ -112,6 +136,7 @@
 
         self._send_nbits_data_cmd(cmd_proto.CPCMD_SHIFT_TDI_TDO,
                                   nbits, data)
+        self.state = self.ST_EXIT1_DR
         pass
 
     def reset(self):
@@ -121,7 +146,7 @@
     pass
 
 def _extract_nbits_data(nbits_data):
-    nbits = ord(nbits_data[0]) | (ord(nbits_data[1] << 8))
+    nbits = ord(nbits_data[0]) | (ord(nbits_data[1]) << 8)
     return nbits, nbits_data[2:]
 
 def _get_bit(data, bit_idx):
@@ -133,13 +158,24 @@
     dev.go_shift_IR()
     reply = dev.wait_reply()
     if reply.code != cmd_proto.CPCMD_ACK:
-        raise RutimeError, 'invliad reply code 0x%02x' % (reply.code)
+        raise RuntimeError, 'invalid reply code 0x%02x' % (reply.code)
+    
+    dev.shift_IR('\x02', 4)
+    reply = dev.wait_reply()
+    if reply.code != cmd_proto.CPCMD_ACK:
+        raise RuntimeError, 'invalid reply code 0x%02x' % (reply.code)
 
     dev.go_shift_DR()
-    dev.shift_DR('\xff' * 253, 253 * 8)
     reply = dev.wait_reply()
+    if reply.code != cmd_proto.CPCMD_ACK:
+        raise RuntimeError, 'invalid reply code 0x%02x' % (reply.code)
+    
+    dev.shift_DR_n_out('\xff' * 252, 252 * 8)
+    reply = dev.wait_reply()
+    if not reply:
+        raise RuntimeError, 'Invalid replied message'
     if reply.code != cmd_proto.CPCMD_DATA:
-        raise RutimeError, 'invliad reply code 0x%02x' % (reply.code)
+        raise RuntimeError, 'invalid replied code 0x%02x %s' % (reply.code, repr(reply))
 
     nbits, data = _extract_nbits_data(reply.data)