diff --git a/src/DAP.c b/src/DAP.c
index 6460dc3..6eb16d1 100644
--- a/src/DAP.c
+++ b/src/DAP.c
@@ -80,7 +80,7 @@ static uint8_t DAP_Info(uint8_t id, uint8_t *info)
 {
 	uint8_t length = 0;
 
-	DEBUG("DAP_Info: %02X\n", id);
+	DEBUGF("DAP_Info: %02X\n", id);
 
 	switch (id)
 	{
@@ -167,11 +167,11 @@ static uint32_t DAP_LED(uint8_t *request, uint8_t *response)
 	switch (*request)
 	{
 		case DAP_LED_DEBUGGER_CONNECTED:
-			DEBUG("DAP_LED: CONNECTED %02X\n", (*(request+1) & 1));
+			DEBUGF("DAP_LED: CONNECTED %02X\n", (*(request+1) & 1));
 			LED_CONNECTED_OUT((*(request+1) & 1));
 			break;
 		case DAP_LED_TARGET_RUNNING:
-			DEBUG("DAP_LED: RUNNING   %02X\n", (*(request+1) & 1));
+			DEBUGF("DAP_LED: RUNNING   %02X\n", (*(request+1) & 1));
 			LED_RUNNING_OUT((*(request+1) & 1));
 			break;
 		default:
@@ -251,7 +251,7 @@ static uint32_t DAP_ResetTarget(uint8_t *response)
 {
 	*(response + 1) = RESET_TARGET();
 	*(response + 0) = DAP_OK;
-	DEBUG("DAP_RESET: %02X\n", *(response + 1));
+	DEBUGF("DAP_RESET: %02X\n", *(response + 1));
 	return (2);
 }
 
@@ -274,7 +274,7 @@ static uint32_t DAP_SWJ_Pins(uint8_t *request, uint8_t *response)
 				(*(request + 4) << 16) |
 				(*(request + 5) << 24);
 
-	DEBUG("DAP_SWJ_Pins: %04X %04X %04X", select, value, wait);
+	DEBUGF("DAP_SWJ_Pins: %04X %04X %04X", select, value, wait);
 
 	if (select & (1 << DAP_SWJ_SWCLK_TCK))
 	{
@@ -350,7 +350,7 @@ static uint32_t DAP_SWJ_Pins(uint8_t *request, uint8_t *response)
 			(PIN_nTRST_IN()     << DAP_SWJ_nTRST)     |
 			(PIN_nRESET_IN()    << DAP_SWJ_nRESET);
 
-	DEBUG(" %02X\n", (uint8_t)value);
+	DEBUGF(" %02X\n", (uint8_t)value);
 	*response = (uint8_t)value;
 	return (1);
 }
@@ -372,7 +372,7 @@ static uint32_t DAP_SWJ_Clock(uint8_t *request, uint8_t *response)
 			(*(request+2) << 16) |
 			(*(request+3) << 24);
 
-	DEBUG("DAP_SWJ_Clock: %u\n", clock);
+	DEBUGF("DAP_SWJ_Clock: %u\n", clock);
 
 	if (clock == 0)
 	{
@@ -403,7 +403,7 @@ static uint32_t DAP_SWJ_Clock(uint8_t *request, uint8_t *response)
 		DAP_Data.clock_delay = delay;
 		int success = ftdi_set_baudrate (&__ftdic, clock/2);
 		if( success < 0){
-			DEBUG("Could not set baudrate: %s\n",ftdi_get_error_string(&__ftdic));
+			DEBUGF("Could not set baudrate: %s\n",ftdi_get_error_string(&__ftdic));
 		}
 	}
 
@@ -426,7 +426,7 @@ static uint32_t DAP_SWJ_Sequence(uint8_t *request, uint8_t *response)
 	count = *request++;
 	if (count == 0) count = 256;
 
-	DEBUG("DAP_SWJ_Sequence: %u\n", count);
+	DEBUGF("DAP_SWJ_Sequence: %u\n", count);
 
 	SWJ_Sequence(count, request);
 
@@ -449,7 +449,7 @@ static uint32_t DAP_SWD_Configure(uint8_t *request, uint8_t *response)
 	DAP_Data.swd_conf.turnaround  = (value & 0x03) + 1;
 	DAP_Data.swd_conf.data_phase  = (value & 0x04) ? 1 : 0;
 	
-	DEBUG("DAP_SWD_Configure: %d %d\n",
+	DEBUGF("DAP_SWD_Configure: %d %d\n",
 		DAP_Data.swd_conf.turnaround,
 		DAP_Data.swd_conf.data_phase
 	);
@@ -484,7 +484,7 @@ static uint32_t DAP_SWD_Abort(uint8_t *request, uint8_t *response)
 			(*(request+3) << 16) |
 			(*(request+4) << 24);
 
-	DEBUG("%04X\n", data);
+	DEBUGF("%04X\n", data);
 	// Write Abort register
 	SWD_Transfer(DP_ABORT, &data);
 	*response = DAP_OK;
@@ -604,7 +604,7 @@ err:	DEBUG("ERROR\n");
 	*(response+3) = (uint8_t)(data >> 16);
 	*(response+4) = (uint8_t)(data >> 24);
 
-	DEBUG("%04X\n", data);
+	DEBUGF("%04X\n", data);
 	
 	return (1+4);
 }
@@ -645,7 +645,7 @@ err:	DEBUG("ERROR\n");
 	JTAG_WriteAbort(data);
 	*response = DAP_OK;
 
-	DEBUG("%04X\n", data);
+	DEBUGF("%04X\n", data);
 
 	return (1);
 }
@@ -661,7 +661,7 @@ static uint32_t DAP_TransferConfigure(uint8_t *request, uint8_t *response)
 	DAP_Data.transfer.idle_cycles = *(request+0);
 	DAP_Data.transfer.retry_count = *(request+1) | (*(request+2) << 8);
 	DAP_Data.transfer.match_retry = *(request+3) | (*(request+4) << 8);
-	DEBUG("DAP_TransferConfigure: %d %d %d\n",
+	DEBUGF("DAP_TransferConfigure: %d %d %d\n",
 		DAP_Data.transfer.idle_cycles,
 		DAP_Data.transfer.retry_count,
 		DAP_Data.transfer.match_retry
@@ -703,7 +703,7 @@ static uint32_t DAP_SWD_Transfer(uint8_t *request, uint8_t *response)
 	request++;            // Ignore DAP index
 
 	request_count = *request++;
-	DEBUG("DAP_SWD_Transfer: %d\n", request_count);
+	DEBUGF("DAP_SWD_Transfer: %d\n", request_count);
 
 	while (request_count--)
 	{
@@ -1320,7 +1320,7 @@ end:
 #if 0
 __weak uint32_t DAP_ProcessVendorCommand(uint8_t *request, uint8_t *response)
 {
-	DEBUG("DAP_ProcessVendorCommand:\n");
+	DEBUGF("DAP_ProcessVendorCommand:\n");
 	*response = ID_DAP_Invalid;
 	return (1);
 }
diff --git a/src/DAP.h b/src/DAP.h
index 509a2e3..744a135 100644
--- a/src/DAP.h
+++ b/src/DAP.h
@@ -205,6 +205,7 @@ extern void     DAP_Setup (void);
 // Configurable delay for clock generation
 #define DELAY_SLOW_CYCLES       1       // Number of cycles for one iteration
 static __forceinline void PIN_DELAY_SLOW (uint32_t delay) {
+	(void)delay;
 	//Delayus(delay);
 	//printf("delay %d\n",delay);
 	FTDI_SUBMIT();
diff --git a/src/DAP_config.h b/src/DAP_config.h
index d6152ef..e108a76 100644
--- a/src/DAP_config.h
+++ b/src/DAP_config.h
@@ -176,11 +176,11 @@ static __inline void FTDI_BITBANG_SETUP() {
 	/* Enable bitbang mode */
 	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));
+		DEBUGF("Could not change to async. bitbang: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 	success = ftdi_write_data(&__ftdic,&out_state,1);
 	if( success < 0){
-		DEBUG("Could not write pins: %s\n",ftdi_get_error_string(&__ftdic));
+		DEBUGF("Could not write pins: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 	/*ftdi_read_data_set_chunksize(&__ftdic,1);
 	ftdi_write_data_set_chunksize(&__ftdic,1);*/
@@ -190,7 +190,7 @@ static __inline void FTDI_BITBANG_SETUP() {
 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));
+		DEBUGF("Could not write pins: %s\n",ftdi_get_error_string(&__ftdic));
 		exit(1);
 	}
 }
@@ -198,7 +198,7 @@ static __forceinline void FTDI_WRITE_OUT_READ_MODE( ) {
 static __forceinline void FTDI_WRITE_OUT( ) {
 	int success;
 	if( out_state_buff_pos==0 ){
-		DEBUG("buffer is empty %d \n",out_state_buff_pos);
+		DEBUGF("buffer is empty %d \n",out_state_buff_pos);
 		return;
 	}
 	FTDI_WRITE_OUT_READ_MODE( );
@@ -209,7 +209,7 @@ static __forceinline void FTDI_WRITE_OUT( ) {
 	while(out_state_buff_pos!=0) {
 		success = ftdi_read_data(&__ftdic,in_buff,out_state_buff_pos);
 		if( success < 0 ) {
-			DEBUG("Could not read pins: %s\n",ftdi_get_error_string(&__ftdic));
+			DEBUGF("Could not read pins: %s\n",ftdi_get_error_string(&__ftdic));
 			exit(1);
 		}
 		out_state_buff_pos -= success;
@@ -234,11 +234,11 @@ static __inline void FTDI_BEGIN_READ(){
 
 	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));
+		DEBUGF("Could not change to sync. bitbang: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 	success = ftdi_usb_purge_rx_buffer(&__ftdic);
 	if( success<0 ){
-		DEBUG("Could not flush buffer: %s\n",ftdi_get_error_string(&__ftdic));
+		DEBUGF("Could not flush buffer: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 
 	//memset ( pin_read_buff, 0, BUFFER_SIZE*sizeof(int) );
@@ -255,13 +255,13 @@ static __inline void FTDI_END_READ(){
     while( remaining ){
         success = ftdi_read_data(&__ftdic,ptr,remaining);
         if( success<0 ){
-            DEBUG("Error while read: %s\n",ftdi_get_error_string(&__ftdic));
+            DEBUGF("Error while read: %s\n",ftdi_get_error_string(&__ftdic));
             exit(1);
         }
         ptr+=success;
         remaining-=success;
     }
-    DEBUG("Read %d/%d bytes\n",(int)(ptr-in_buff),out_state_buff_pos);
+    DEBUGF("Read %d/%d bytes\n",(int)(ptr-in_buff),out_state_buff_pos);
 	out_state_buff_pos = 0;
 
 	for(i=0;i<pin_read_pos;i++){
@@ -276,7 +276,7 @@ static __inline void FTDI_END_READ(){
 
 	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));
+		DEBUGF("Could not change to async. bitbang: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 }
 
@@ -326,7 +326,7 @@ static __inline void DAP_SETUP (void) {
 	/* 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));
+		DEBUGF("Can't open device [%x:%x]: %s\n",VID,PID,ftdi_get_error_string(&__ftdic));
 		exit(1);
 	}
 
@@ -336,7 +336,7 @@ static __inline void DAP_SETUP (void) {
 
 	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));
+		DEBUGF("Could not set baudrate: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 	/*uint8_t foo;
     success = ftdi_read_data(&__ftdic,&foo,1);*/
@@ -374,11 +374,11 @@ static __forceinline uint32_t PIN_IN  ( uint8_t pin ) {
 	if( dir_state & pin ){
 		//dir_state &= ~pin;
 		//ftdi_set_bitmode(&__ftdic, dir_state, BITMODE_SYNCBB );
-		DEBUG("Pin %s is not an input pin!\n", pin_to_str(pin));
+		DEBUGF("Pin %s is not an input pin!\n", pin_to_str(pin));
 	}
 	int success = ftdi_read_pins(&__ftdic,&c);
 	if(success < 0){
-		DEBUG("Could not read pin: %s\n",ftdi_get_error_string(&__ftdic));
+		DEBUGF("Could not read pin: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 	return (c & pin)!=0 ? 1 : 0;
 }
@@ -387,7 +387,7 @@ 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_SYNCBB );
-		DEBUG("Pin %s is not an output pin!\n", pin_to_str(pin));
+		DEBUGF("Pin %s is not an output pin!\n", pin_to_str(pin));
 		return;
 	}
 	/*if(bit & ~1){
@@ -573,7 +573,7 @@ static __forceinline void     PIN_SWDIO_OUT_ENABLE  (void) {
 	dir_state |= TMS_SWDIO;
 	int success = ftdi_set_bitmode(&__ftdic, dir_state, __ftdic.bitbang_mode  );
 	if( success<0 ){
-		DEBUG("Could not change SWDIO to out: %s\n",ftdi_get_error_string(&__ftdic));
+		DEBUGF("Could not change SWDIO to out: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 }
 
@@ -588,7 +588,7 @@ static __forceinline void     PIN_SWDIO_OUT_DISABLE (void) {
 
 	int success = ftdi_set_bitmode(&__ftdic, dir_state, __ftdic.bitbang_mode  );
 	if( success<0 ){
-		DEBUG("Could not change SWDIO to in: %s\n",ftdi_get_error_string(&__ftdic));
+		DEBUGF("Could not change SWDIO to in: %s\n",ftdi_get_error_string(&__ftdic));
 	}
 }
 
diff --git a/src/Debug.h b/src/Debug.h
index b000b0b..e133e2d 100644
--- a/src/Debug.h
+++ b/src/Debug.h
@@ -12,9 +12,10 @@
 
 #define DEBUG_OUT
 #ifdef DEBUG_OUT
-#  define DEBUG(format, ...) fprintf (stderr, format, ##__VA_ARGS__)
+#  define DEBUGF(format, ...) fprintf (stderr, format, ##__VA_ARGS__)
+#  define DEBUG(str) fprintf (stderr, str)
 #else
-#  define DEBUG(format, ...)
+#  define DEBUGF(format, ...)
 #endif
 
 #endif //SRC_DEBUG_H_
diff --git a/src/JTAG_DP.c b/src/JTAG_DP.c
index 336ca3b..c23d384 100644
--- a/src/JTAG_DP.c
+++ b/src/JTAG_DP.c
@@ -241,13 +241,13 @@ exit:
 
 #undef  PIN_DELAY
 #define PIN_DELAY() PIN_DELAY_FAST()
-JTAG_IR_Function(Fast);
-JTAG_TransferFunction(Fast);
+JTAG_IR_Function(Fast)
+JTAG_TransferFunction(Fast)
 
 #undef  PIN_DELAY
 #define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
-JTAG_IR_Function(Slow);
-JTAG_TransferFunction(Slow);
+JTAG_IR_Function(Slow)
+JTAG_TransferFunction(Slow)
 
 
 // JTAG Read IDCODE register
diff --git a/src/SW_DP.c b/src/SW_DP.c
index 8ed9d38..bbe643c 100644
--- a/src/SW_DP.c
+++ b/src/SW_DP.c
@@ -70,7 +70,7 @@ void SWJ_Sequence (uint32_t count, uint8_t *data)
 		if (n == 0)
 		{
 			val = *data++;
-			DEBUG(" %02X", val);
+			DEBUGF(" %02X", val);
 			n = 8;
 		}
 		if (val & 1)
@@ -133,6 +133,7 @@ uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {
   }                                                                             \
                                                                                 \
   /* Acknowledge response */                                                    \
+  ack = 0;                                                                      \
   SW_READ_BIT(&ack,1);                                                          \
   SW_READ_BIT(NULL,0);                                                          \
   SW_READ_BIT(NULL,0);                                                          \
@@ -222,11 +223,11 @@ uint8_t SWD_Transfer##speed (uint8_t request, uint32_t *data) {
 
 #undef  PIN_DELAY
 #define PIN_DELAY() PIN_DELAY_FAST()
-SWD_TransferFunction(Fast);
+SWD_TransferFunction(Fast)
 
 #undef  PIN_DELAY
 #define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
-SWD_TransferFunction(Slow);
+SWD_TransferFunction(Slow)
 
 
 // SWD Transfer I/O
diff --git a/src/socket_ipc.c b/src/socket_ipc.c
index f97c4e9..3bfcab2 100644
--- a/src/socket_ipc.c
+++ b/src/socket_ipc.c
@@ -58,21 +58,21 @@ void init_socket(void) {
 
 	sockfd = socket(AF_INET, SOCK_STREAM, 0);
 	if (sockfd < 0) {
-	   DEBUG("ERROR opening socket: %s\n", strerror(errno));
+	   DEBUGF("ERROR opening socket: %s\n", strerror(errno));
        exit(1);
 	}
-	bzero((char *) &serv_addr, sizeof(serv_addr));
+	memset((char *) &serv_addr, 0, sizeof(serv_addr));
 	portno = 4777;
 	serv_addr.sin_family = AF_INET;
 	serv_addr.sin_addr.s_addr = INADDR_ANY;
 	serv_addr.sin_port = htons(portno);
 	int yes = 1;
 	if ( setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int)) < 0){
-		DEBUG("ERROR on setting socket option: %s\n", strerror(errno));
+		DEBUGF("ERROR on setting socket option: %s\n", strerror(errno));
 		exit(1);
 	}
 	if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0){
-		DEBUG("ERROR on binding: %s\n", strerror(errno));
+		DEBUGF("ERROR on binding: %s\n", strerror(errno));
 		exit(1);
 	}
 	listen(sockfd,5);
@@ -99,7 +99,7 @@ void wait_for_client(void) {
 	clilen = sizeof(cli_addr);
 	csockfd = accept(sockfd, (struct sockaddr*)&cli_addr, (socklen_t*)&clilen);
 	if (csockfd < 0){
-		DEBUG("ERROR on accept: %s\n", strerror(errno));
+		DEBUGF("ERROR on accept: %s\n", strerror(errno));
 		exit(1);
 	}
 
@@ -109,7 +109,7 @@ void wait_for_client(void) {
 		flags = 0;
 	}
 	if( fcntl(csockfd, F_SETFL, flags | O_NONBLOCK) < 0 ){
-		DEBUG("ERROR seting non-blocking: %s\n", strerror(errno));
+		DEBUGF("ERROR seting non-blocking: %s\n", strerror(errno));
 	}
 	printf("connected\n");
 }
@@ -120,7 +120,7 @@ int write_response() {
 	if ((response_tail != response_head) || response_full) {
 		int n = write(csockfd,response_buff[response_tail],DAP_PACKET_SIZE);
 		if (n != DAP_PACKET_SIZE){
-			DEBUG("ERROR writing to socket: %s\n", strerror(errno));
+			DEBUGF("ERROR writing to socket: %s\n", strerror(errno));
 			close_socket();
 		}
 
@@ -180,7 +180,7 @@ void process_data(void) {
 		switch( errno ) {
 			case EAGAIN: break;
 			default:
-				DEBUG("A problem occured: %d %d %s\n", n, errno,strerror(errno) );
+				DEBUGF("A problem occured: %d %d %s\n", n, errno,strerror(errno) );
 				DEBUG("Closing connection!\n");
 				close_socket(csockfd);
 				break;
@@ -211,7 +211,7 @@ void process_data(void) {
 	}
 	if(should_close) {
 		close_socket();
+		should_close = 0;
 	}
-	should_close = 0;
 }