diff --git a/src/DAP_config.h b/src/DAP_config.h
index 342089b..f21e224 100644
--- a/src/DAP_config.h
+++ b/src/DAP_config.h
@@ -38,6 +38,7 @@ Provides definitions about:
 #include "ftdi_vars.h"
 #include "device.h"                             // Debug Unit Cortex-M Processor Header File
 #include "Debug.h"
+#include "Timer.h"
 #include <stdint.h>
 #include <stdlib.h>
 
@@ -138,29 +139,29 @@ of the same I/O port. The following SWDIO I/O Pin functions are provided:
 
 // Configure DAP I/O pins ------------------------------
 
-#define TMS_SWDIO   DCD
-#define TCK_SWCLK   RI
+#define TMS_SWDIO   RI
+#define TCK_SWCLK   TXD
 #define TDO_SWO     RXD
-#define TDI         RTS
+#define TDI         DCD
 #define nRESET      DTR
-#define nTRST       TXD
+#define nTRST       RTS
 #define LED_CONN    CTS
 #define LED_RUN     DSR
 
 static __inline const char* pin_to_str(uint8_t pin){
 	switch(pin){
 	case TMS_SWDIO:
-		return "TMS_SWDIO(DCD)";
+		return "TMS_SWDIO(RI)";
 	case TCK_SWCLK:
-		return "TCK_SWCLK(RI)";
+		return "TCK_SWCLK(TXD)";
 	case TDO_SWO:
 		return "TDO_SWO(RXD)";
 	case TDI:
-		return "TDI(RTS)";
+		return "TDI(DCD)";
 	case nRESET:
 	return "nRESET(DTR)";
 	case nTRST:
-		return "nTRST(TXD)";
+		return "nTRST(RTS)";
 	case LED_CONN:
 		return "LED_CONN(CTS)";
 	case LED_RUN:
@@ -173,7 +174,7 @@ static __inline const char* pin_to_str(uint8_t pin){
 static __inline void FTDI_BITBANG_SETUP() {
 	int success;
 	/* Enable bitbang mode */
-	success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_BITBANG  );
+	success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB  );
 	if( success<0 ){
 		DEBUG("Could not change to async. bitbang: %s\n",ftdi_get_error_string(&__ftdic));
 	}
@@ -185,52 +186,38 @@ static __inline void FTDI_BITBANG_SETUP() {
 	ftdi_write_data_set_chunksize(&__ftdic,1);*/
 }
 
-/** Setup of the Debug Unit I/O pins and LEDs (called when Debug Unit is initialized).
-This function performs the initialization of the CMSIS-DAP Hardware I/O Pins and the
-Status LEDs. In detail the operation of Hardware I/O and LED pins are enabled and set:
- - I/O clock system enabled.
- - all I/O pins: input buffer enabled, output pins are set to HighZ mode.
- - for nTRST, nRESET a weak pull-up (if available) is enabled.
- - LED output pins are enabled and LEDs are turned off.
-*/
-static __inline void DAP_SETUP (void) {
-    /* Initialize context for subsequent function calls */
-	ftdi_init(&__ftdic);
-	int success;
-	/* Open FTDI device based on FT232R vendor & product IDs */
-	success = ftdi_usb_open(&__ftdic, VID, PID);
-	if(success < 0) {
-		DEBUG("Can't open device [%x:%x]: %s\n",VID,PID,ftdi_get_error_string(&__ftdic));
-		exit(1);
-	}
-
-	dir_state = LED_CONN|LED_RUN;
-	out_state = 0xFF & ~(LED_CONN|LED_RUN);
-	FTDI_BITBANG_SETUP();
 
-	success = ftdi_set_baudrate (&__ftdic, DAP_DEFAULT_SWJ_CLOCK/2);
-	if( success < 0){
-		DEBUG("Could not set baudrate: %s\n",ftdi_get_error_string(&__ftdic));
+static __forceinline void FTDI_WRITE_OUT_READ_MODE( ) {
+	int success = ftdi_write_data(&__ftdic,out_state_buff,out_state_buff_pos);
+	if( success<0 ){
+		DEBUG("Could not write pins: %s\n",ftdi_get_error_string(&__ftdic));
+		exit(1);
 	}
-	uint8_t foo;
-    success = ftdi_read_data(&__ftdic,&foo,1);
 }
 
-
 static __forceinline void FTDI_WRITE_OUT( ) {
 	int success;
 	if( out_state_buff_pos==0 ){
 		DEBUG("buffer is empty %d \n",out_state_buff_pos);
 		return;
 	}
-	success = ftdi_write_data(&__ftdic,out_state_buff,out_state_buff_pos);
-	if( success<0 ){
-		DEBUG("Could not write pins: %s\n",ftdi_get_error_string(&__ftdic));
+	FTDI_WRITE_OUT_READ_MODE( );
+
+	/* every generates the same amout of reads */
+
+	success = 0;
+	while(success == 0) {
+		success = ftdi_read_data(&__ftdic,in_buff,out_state_buff_pos);
+	}
+	if( success != out_state_buff_pos ){
+		DEBUG("Could not read pins: %s\n",ftdi_get_error_string(&__ftdic));
 		exit(1);
 	}
-	out_state_buff_pos=0;
+
+	out_state_buff_pos = 0;
 }
 
+
 static __forceinline void FTDI_SUBMIT( ) {
 	out_state_buff[out_state_buff_pos] = out_state;
 	out_state_buff_pos++;
@@ -240,39 +227,13 @@ static __forceinline void FTDI_SUBMIT( ) {
 	}
 }
 
-/** Setup JTAG I/O pins: TCK, TMS, TDI, TDO, nTRST, and nRESET.
-Configures the DAP Hardware I/O pins for JTAG mode:
- - TCK, TMS, TDI, nTRST, nRESET to output mode and set to high level.
- - TDO to input mode.
-*/ 
-static __inline void PORT_JTAG_SETUP (void) {
-    /* Initialize context for subsequent function calls */
-	dir_state = LED_CONN|LED_RUN|TCK_SWCLK|TMS_SWDIO|TDI|nRESET;;
-	out_state = TCK_SWCLK | TMS_SWDIO | TDI | nTRST | nRESET;
-	FTDI_BITBANG_SETUP();
-}
-
-
-/** Setup SWD I/O pins: SWCLK, SWDIO, and nRESET.
-Configures the DAP Hardware I/O pins for Serial Wire Debug (SWD) mode:
- - SWCLK, SWDIO, nRESET to output mode and set to default high level.
- - TDI, TMS, nTRST to HighZ mode (pins are unused in SWD mode).
-*/
-static __inline void PORT_SWD_SETUP (void) {
-    /* Initialize context for subsequent function calls */
-	dir_state = LED_CONN|LED_RUN|TCK_SWCLK|TMS_SWDIO|nRESET;
-	out_state = TCK_SWCLK | TMS_SWDIO | nRESET;
-	FTDI_BITBANG_SETUP();
-}
-
 static __inline void FTDI_BEGIN_READ(){
 	/* submit signal change buffer */
 	FTDI_SUBMIT();
 	FTDI_WRITE_OUT();
 	/* flush read buffer */
-	int success;
 
-	success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
+	int success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
 	if( success<0 ){
 		DEBUG("Could not change to sync. bitbang: %s\n",ftdi_get_error_string(&__ftdic));
 	}
@@ -280,6 +241,7 @@ static __inline void FTDI_BEGIN_READ(){
 	if( success<0 ){
 		DEBUG("Could not flush buffer: %s\n",ftdi_get_error_string(&__ftdic));
 	}
+
 	//memset ( pin_read_buff, 0, BUFFER_SIZE*sizeof(int) );
 	is_in_read_mode = 1;
 }
@@ -287,9 +249,11 @@ static __inline void FTDI_BEGIN_READ(){
 static __inline void FTDI_END_READ(){
 	int success, remaining;
 	uint32_t read_size;
+
 	read_size = remaining = out_state_buff_pos;
-	FTDI_SUBMIT();
-	FTDI_WRITE_OUT();
+	FTDI_WRITE_OUT_READ_MODE();
+	out_state_buff_pos = 0;
+
     success = 0;
     uint8_t *ptr = in_buff;
     while( remaining ){
@@ -303,30 +267,128 @@ static __inline void FTDI_END_READ(){
         printf("try %d\n", success);
     }
     printf("Read %d/%d bytes\n",(int)(ptr-in_buff),read_size);
-	if( read_size!=success ){
-		DEBUG("Not sufficient bytes read: %s\n",ftdi_get_error_string(&__ftdic));
-	}else{
-		DEBUG("Read %d bytes\n",read_size);
-        int i;
-        for(i=0;i<read_size;i++){
-            printf("%x ",in_buff[i] & dir_state);
-        }
-        printf("\n");
+
+	printf("%x\n",dir_state);
+	int i;
+	for(i=0;i<success;i++){
+		printf("%x ",in_buff[i]>>7);
 	}
+	printf("\n");
 
-	success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_BITBANG );
+	for(i=0;i<pin_read_pos;i++){
+		uint8_t is_set = (in_buff[pin_read_buff[i].pos] & pin_read_buff[i].read_bitmask) != 0;
+		printf("%d ",is_set);
+		if( is_set ) {
+			(*pin_read_buff[i].ptr) |= pin_read_buff[i].write_bitmask;
+		}else{
+			(*pin_read_buff[i].ptr) &= ~pin_read_buff[i].write_bitmask;
+		}
+	}
+	printf("\n");
+	pin_read_pos = is_in_read_mode = 0;
+
+	success = ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
 	if( success<0 ){
 		DEBUG("Could not change to async. bitbang: %s\n",ftdi_get_error_string(&__ftdic));
 	}
+}
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Config_Initialization_gr CMSIS-DAP Initialization
+\ingroup DAP_ConfigIO_gr
+@{
+
+CMSIS-DAP Hardware I/O and LED Pins are initialized with the function \ref DAP_SETUP.
+*/
 
+static void     PIN_nRESET_OUT (uint32_t bit);
+/** Reset Target Device with custom specific I/O pin or command sequence.
+This function allows the optional implementation of a device specific reset sequence.
+It is called when the command \ref DAP_ResetTarget and is for example required
+when a device needs a time-critical unlock sequence that enables the debug port.
+\return 0 = no device specific reset sequence is implemented.\n
+        1 = a device specific reset sequence is implemented.
+*/
+static __inline uint32_t RESET_TARGET (void) {
+  //return (0);              // change to '1' when a device reset sequence is implemented
+	PIN_nRESET_OUT(1);
+	FTDI_SUBMIT();
+	FTDI_WRITE_OUT();
+
+	Delayms(10);
+
+	PIN_nRESET_OUT(0);
+	FTDI_SUBMIT();
+	FTDI_WRITE_OUT();
+	return 1;
+}
+
+/** Setup of the Debug Unit I/O pins and LEDs (called when Debug Unit is initialized).
+This function performs the initialization of the CMSIS-DAP Hardware I/O Pins and the
+Status LEDs. In detail the operation of Hardware I/O and LED pins are enabled and set:
+ - I/O clock system enabled.
+ - all I/O pins: input buffer enabled, output pins are set to HighZ mode.
+ - for nTRST, nRESET a weak pull-up (if available) is enabled.
+ - LED output pins are enabled and LEDs are turned off.
+*/
+static __inline void DAP_SETUP (void) {
+    /* Initialize context for subsequent function calls */
+	ftdi_init(&__ftdic);
+	int success;
+	/* Open FTDI device based on FT232R vendor & product IDs */
+	success = ftdi_usb_open(&__ftdic, VID, PID);
+	if(success < 0) {
+		DEBUG("Can't open device [%x:%x]: %s\n",VID,PID,ftdi_get_error_string(&__ftdic));
+		exit(1);
+	}
+
+	dir_state = LED_CONN|LED_RUN|TCK_SWCLK|nRESET;
+	out_state = 0xFF;// & ~(LED_CONN|LED_RUN);
+	FTDI_BITBANG_SETUP();
+
+	success = ftdi_set_baudrate (&__ftdic, DAP_DEFAULT_SWJ_CLOCK/2);
+	if( success < 0){
+		DEBUG("Could not set baudrate: %s\n",ftdi_get_error_string(&__ftdic));
+	}
+	/*uint8_t foo;
+    success = ftdi_read_data(&__ftdic,&foo,1);*/
+
+    RESET_TARGET();
+}
+
+///@}
+
+/** Setup JTAG I/O pins: TCK, TMS, TDI, TDO, nTRST, and nRESET.
+Configures the DAP Hardware I/O pins for JTAG mode:
+ - TCK, TMS, TDI, nTRST, nRESET to output mode and set to high level.
+ - TDO to input mode.
+*/ 
+static __inline void PORT_JTAG_SETUP (void) {
+    /* Initialize context for subsequent function calls */
+	dir_state = LED_CONN|LED_RUN|TCK_SWCLK|TMS_SWDIO|TDI|nRESET;;
+	out_state = TCK_SWCLK | TMS_SWDIO | TDI | nTRST | nRESET;
+	FTDI_BITBANG_SETUP();
+}
+
+
+/** Setup SWD I/O pins: SWCLK, SWDIO, and nRESET.
+Configures the DAP Hardware I/O pins for Serial Wire Debug (SWD) mode:
+ - SWCLK, SWDIO, nRESET to output mode and set to default high level.
+ - TDI, TMS, nTRST to HighZ mode (pins are unused in SWD mode).
+*/
+static __inline void PORT_SWD_SETUP (void) {
+    /* Initialize context for subsequent function calls */
+	dir_state = LED_CONN|LED_RUN|TCK_SWCLK|TMS_SWDIO|nRESET;
+	out_state = TCK_SWCLK | TMS_SWDIO | nRESET;
+	FTDI_BITBANG_SETUP();
 }
 
- 
 static __forceinline uint32_t PIN_IN  ( uint8_t pin ) {
 	uint8_t c;
 	if( dir_state & pin ){
 		//dir_state &= ~pin;
-		//ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_BITBANG );
+		//ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
 		DEBUG("Pin %s is not an input pin!\n", pin_to_str(pin));
 	}
 	int success = ftdi_read_pins(&__ftdic,&c);
@@ -339,11 +401,14 @@ static __forceinline uint32_t PIN_IN  ( uint8_t pin ) {
 static __forceinline void PIN_SET  ( uint8_t pin, uint32_t bit ) {
 	if( !(dir_state & pin) ){
 		//dir_state |= pin;
-		//ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_BITBANG );
+		//ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
 		DEBUG("Pin %s is not an output pin!\n", pin_to_str(pin));
 		return;
 	}
-	if(bit){
+	/*if(bit & ~1){
+		DEBUG("Garbage bits set %d!\n",bit);
+	}*/
+	if((bit & 1)){
 		out_state |= pin;
 	}else{
 		out_state &= ~pin;
@@ -476,7 +541,8 @@ static __forceinline void PIN_SWDIO_IN      (uint8_t *ptr,uint8_t bit){
 
 	/* correlate the read with the output buffer */
 	pin_read_buff[pin_read_pos].pos = out_state_buff_pos;
-	pin_read_buff[pin_read_pos].bitmask = pin_read_mask;
+	pin_read_buff[pin_read_pos].write_bitmask = pin_read_mask;
+	pin_read_buff[pin_read_pos].read_bitmask = TMS_SWDIO;
 
 	pin_read_mask = pin_read_mask<<1;
 	if( !pin_read_mask ){
@@ -490,8 +556,6 @@ static __forceinline void PIN_SWDIO_IN      (uint8_t *ptr,uint8_t bit){
 	if( pin_read_pos==BUFFER_SIZE ){
 		DEBUG("read buffer overflow!\n");
 	}
-
-
 }
 
 /** SWDIO I/O pin: Set Output (used in SWD mode only).
@@ -633,27 +697,4 @@ static __inline void LED_RUNNING_OUT (uint32_t bit) {
 ///@}
 
 
-//**************************************************************************************************
-/** 
-\defgroup DAP_Config_Initialization_gr CMSIS-DAP Initialization
-\ingroup DAP_ConfigIO_gr
-@{
-
-CMSIS-DAP Hardware I/O and LED Pins are initialized with the function \ref DAP_SETUP.
-*/
-
-/** Reset Target Device with custom specific I/O pin or command sequence.
-This function allows the optional implementation of a device specific reset sequence.
-It is called when the command \ref DAP_ResetTarget and is for example required 
-when a device needs a time-critical unlock sequence that enables the debug port.
-\return 0 = no device specific reset sequence is implemented.\n
-        1 = a device specific reset sequence is implemented.
-*/
-static __inline uint32_t RESET_TARGET (void) {
-  return (0);              // change to '1' when a device reset sequence is implemented
-}
-
-///@}
-
-
 #endif /* __DAP_CONFIG_H__ */
diff --git a/src/SW_DP.c b/src/SW_DP.c
index 3b09144..728f0fc 100644
--- a/src/SW_DP.c
+++ b/src/SW_DP.c
@@ -36,6 +36,7 @@
   PIN_DELAY()
 
 #define SW_WRITE_BIT(bit)               \
+	printf("swdio=%d\n",bit);\
   PIN_SWDIO_OUT(bit);                   \
   PIN_SWCLK_CLR();                      \
   PIN_DELAY();                          \
@@ -98,14 +99,14 @@ void SWJ_Sequence (uint32_t count, uint8_t *data)
 //   data:    DATA[31:0]
 //   return:  ACK[2:0]
 #define SWD_TransferFunction(speed)     /**/                                    \
-uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {                \
-  uint8_t ack;                                                                 \
+uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {                 \
+  uint8_t ack;                                                                  \
   uint32_t bit;                                                                 \
   uint8_t val[4];                                                               \
   uint32_t parity;                                                              \
                                                                                 \
   uint32_t n;                                                                   \
-                                                                                \
+                       printf("req %d\n",request);                                                         \
   /* Packet Request */                                                          \
   parity = 0;                                                                   \
   SW_WRITE_BIT(1);                      /* Start Bit */                         \
@@ -119,12 +120,14 @@ uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {                \
   SW_WRITE_BIT(bit);                    /* A2 Bit */                            \
   parity += bit;                                                                \
   bit = request >> 3;                                                           \
+  SW_WRITE_BIT(bit);                    /* A3 Bit */                            \
   parity += bit;                                                                \
   SW_WRITE_BIT(parity);                 /* Parity Bit */                        \
   SW_WRITE_BIT(0);                      /* Stop Bit */                          \
   SW_WRITE_BIT(1);                      /* Park Bit */                          \
                                                                                 \
   /* Turnaround */                                                              \
+  FTDI_BEGIN_READ();                                                            \
   PIN_SWDIO_OUT_DISABLE();                                                      \
   for (n = DAP_Data.swd_conf.turnaround; n; n--) {                              \
     SW_CLOCK_CYCLE();                                                           \
diff --git a/src/ftdi_vars.c b/src/ftdi_vars.c
index 0b85c3e..a0b0cb9 100644
--- a/src/ftdi_vars.c
+++ b/src/ftdi_vars.c
@@ -21,7 +21,7 @@ int out_state_buff_pos = 0;
 uint8_t in_buff[BUFFER_SIZE];
 uint8_t *read_slot;
 
-red_pos_t pin_read_buff[BUFFER_SIZE];
+read_pos_t pin_read_buff[BUFFER_SIZE];
 int pin_read_pos = 0;
 uint8_t pin_read_mask = 1;
 
diff --git a/src/ftdi_vars.h b/src/ftdi_vars.h
index b256d0b..9653396 100644
--- a/src/ftdi_vars.h
+++ b/src/ftdi_vars.h
@@ -23,9 +23,10 @@
 
 typedef struct {
 	uint8_t* ptr;
-	uint8_t bitmask;
+	uint8_t write_bitmask;
+	uint8_t read_bitmask;
 	int pos;
-} red_pos_t;
+} read_pos_t;
 
 
 extern struct ftdi_context __ftdic;
@@ -40,7 +41,7 @@ extern int out_state_buff_pos;
 extern uint8_t in_buff[BUFFER_SIZE];
 extern uint8_t *read_slot;
 
-extern red_pos_t pin_read_buff[BUFFER_SIZE];
+extern read_pos_t pin_read_buff[BUFFER_SIZE];
 extern int pin_read_pos;
 
 extern uint8_t pin_read_mask;