- reformat src/jtag/bitq.c (thanks to Pavel Chromy)

- fix multiple reads from FT2232 into same buffer location (thanks to Magnus Lundin)
- retry JTAG chain validation (thanks to Magnus Lundin)
- reworked GDB packet input handling (thanks to Pavel Chromy)
- output error message when setting a watchpoint failed
- removed duplicate out-of-bounds check in at91sam7.c (thanks to Pavel Chromy)


git-svn-id: svn://svn.berlios.de/openocd/trunk@181 b42882b7-edfa-0310-969c-e2dbd0fdcd60

9 files changed, 300 insertions, 308 deletions

diff --git a/src/flash/at91sam7.c b/src/flash/at91sam7.c
index 7b9fb659..0d97c34c 100644
--- a/src/flash/at91sam7.c
+++ b/src/flash/at91sam7.c
@@ -675,9 +675,6 @@ int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 	}
 	
-	if (offset + count > bank->size)
-		return ERROR_FLASH_DST_OUT_OF_BANK;
-	
 	if (at91sam7_info->cidr_arch == 0)
 		return ERROR_FLASH_BANK_NOT_PROBED;
 
@@ -688,7 +685,7 @@ int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 	
 	/* Configure the flash controller timing */	
 	at91sam7_read_clock_info(bank);	
-       at91sam7_set_flash_mode(bank,FMR_TIMING_FLASH);
+	at91sam7_set_flash_mode(bank,FMR_TIMING_FLASH);
 
 	for (pagen=first_page; pagen<last_page; pagen++) {
 		if (bytes_remaining<dst_min_alignment) 
diff --git a/src/jtag/bitq.c b/src/jtag/bitq.c
index 03f9645d..05f4c925 100644
--- a/src/jtag/bitq.c
+++ b/src/jtag/bitq.c
@@ -55,326 +55,326 @@ void bitq_in_proc(void)
 {
 	/* static information preserved between calls to increase performance */
 	static u8 *in_buff; /* pointer to buffer for scanned data */
-  static int in_idx; /* index of byte being scanned */
-  static u8 in_mask; /* mask of next bit to be scanned */
-
-  scan_field_t *field;
-  int tdo;
-  
-  int result;
-
-  /* loop through the queue */
-  while (bitq_in_state.cmd) {
-    /* only JTAG_SCAN command may return data */
-    if (bitq_in_state.cmd->type==JTAG_SCAN) {
-      /* loop through the fields */
-      while (bitq_in_state.field_idx<bitq_in_state.cmd->cmd.scan->num_fields) {
-
-        field=&bitq_in_state.cmd->cmd.scan->fields[bitq_in_state.field_idx];
-        if (field->in_check_value || field->in_value || field->in_handler) {
-
-          if (bitq_in_state.bit_pos==0) {
-            /* initialize field scanning */
-            in_mask=0x01;
-            in_idx=0;
-            if (field->in_value) in_buff=field->in_value;
-            else {
-              /* buffer reallocation needed? */
-              if (field->num_bits>bitq_in_bufsize*8) {
-                /* buffer previously allocated? */
-                if (bitq_in_buffer!=NULL) {
-                  /* free it */
-                  free(bitq_in_buffer);
-                  bitq_in_buffer=NULL;
-                }
-                /* double the buffer size until it fits */
-                while (field->num_bits>bitq_in_bufsize*8) bitq_in_bufsize*=2;
-              }
-              /* if necessary, allocate buffer and check for malloc error */ 
-              if (bitq_in_buffer==NULL && (bitq_in_buffer=malloc(bitq_in_bufsize))==NULL) {
-                ERROR("malloc error");
-                exit(-1);                
-              }
-              in_buff=(void *)bitq_in_buffer;
-            }
-          }
-
-          /* field scanning */
-          while (bitq_in_state.bit_pos<field->num_bits) {
-            if ((tdo=bitq_interface->in())<0) {
+	static int in_idx; /* index of byte being scanned */
+	static u8 in_mask; /* mask of next bit to be scanned */
+
+	scan_field_t *field;
+	int tdo;
+	
+	int result;
+
+	/* loop through the queue */
+	while (bitq_in_state.cmd) {
+		/* only JTAG_SCAN command may return data */
+		if (bitq_in_state.cmd->type==JTAG_SCAN) {
+			/* loop through the fields */
+			while (bitq_in_state.field_idx<bitq_in_state.cmd->cmd.scan->num_fields) {
+
+				field=&bitq_in_state.cmd->cmd.scan->fields[bitq_in_state.field_idx];
+				if (field->in_check_value || field->in_value || field->in_handler) {
+
+					if (bitq_in_state.bit_pos==0) {
+						/* initialize field scanning */
+						in_mask=0x01;
+						in_idx=0;
+						if (field->in_value) in_buff=field->in_value;
+						else {
+							/* buffer reallocation needed? */
+							if (field->num_bits>bitq_in_bufsize*8) {
+								/* buffer previously allocated? */
+								if (bitq_in_buffer!=NULL) {
+									/* free it */
+									free(bitq_in_buffer);
+									bitq_in_buffer=NULL;
+								}
+								/* double the buffer size until it fits */
+								while (field->num_bits>bitq_in_bufsize*8) bitq_in_bufsize*=2;
+							}
+							/* if necessary, allocate buffer and check for malloc error */ 
+							if (bitq_in_buffer==NULL && (bitq_in_buffer=malloc(bitq_in_bufsize))==NULL) {
+								ERROR("malloc error");
+								exit(-1);                
+							}
+							in_buff=(void *)bitq_in_buffer;
+						}
+					}
+
+					/* field scanning */
+					while (bitq_in_state.bit_pos<field->num_bits) {
+						if ((tdo=bitq_interface->in())<0) {
 #ifdef _DEBUG_JTAG_IO_
-              DEBUG("bitq in EOF");
+							DEBUG("bitq in EOF");
 #endif
-              return;
-            }
-            if (in_mask==0x01) in_buff[in_idx]=0;
-            if (tdo) in_buff[in_idx]|=in_mask;
-            if (in_mask==0x80) {
-              in_mask=0x01;
-              in_idx++;
-            }
-            else in_mask<<=1;
-            bitq_in_state.bit_pos++;
-          }
-          
-          if (field->in_check_value) {
-            /* match scanned in value */
-            for (in_idx=0; in_idx*8<field->num_bits; in_idx++) {
-              if (field->in_check_mask) in_mask=field->in_check_mask[in_idx];
-              else in_mask=0xff;
-              if (field->num_bits-in_idx*8<8) in_mask>>=8-(field->num_bits-in_idx*8);
-              if (field->in_check_value[in_idx]&in_mask!=in_buff[in_idx]&in_mask) {
-      					char *captured_char = buf_to_str(in_buff, (field->num_bits > 64) ? 64 : field->num_bits, 16);
-      					char *in_check_value_char = buf_to_str(field->in_check_value, (field->num_bits > 64) ? 64 : field->num_bits, 16);
-      					char *in_check_mask_char = buf_to_str(field->in_check_mask, (field->num_bits > 64) ? 64 : field->num_bits, 16);
-      					/* TODO: error reporting */
-      					WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);
-      					bitq_in_state.status=ERROR_JTAG_QUEUE_FAILED;
-      					free(captured_char);
-      					free(in_check_value_char);
-      					free(in_check_mask_char);
-                break; /* leave the comparison loop upon first mismatch */      					
-              }
-            }
-          }
-
-          if (field->in_handler && bitq_in_state.status==ERROR_OK) {
-            bitq_in_state.status=(*field->in_handler)(in_buff, field->in_handler_priv);
-          }
-
-        }
-        
-        bitq_in_state.field_idx++; /* advance to next field */
-        bitq_in_state.bit_pos=0; /* start next field from the first bit */
-      }
-
-    }
-    bitq_in_state.cmd=bitq_in_state.cmd->next; /* advance to next command */
-    bitq_in_state.field_idx=0; /* preselect first field */
-  }
+							return;
+						}
+						if (in_mask==0x01) in_buff[in_idx]=0;
+						if (tdo) in_buff[in_idx]|=in_mask;
+						if (in_mask==0x80) {
+							in_mask=0x01;
+							in_idx++;
+						}
+						else in_mask<<=1;
+						bitq_in_state.bit_pos++;
+					}
+					
+					if (field->in_check_value) {
+						/* match scanned in value */
+						for (in_idx=0; in_idx*8<field->num_bits; in_idx++) {
+							if (field->in_check_mask) in_mask=field->in_check_mask[in_idx];
+							else in_mask=0xff;
+							if (field->num_bits-in_idx*8<8) in_mask>>=8-(field->num_bits-in_idx*8);
+							if (field->in_check_value[in_idx]&in_mask!=in_buff[in_idx]&in_mask) {
+								char *captured_char = buf_to_str(in_buff, (field->num_bits > 64) ? 64 : field->num_bits, 16);
+								char *in_check_value_char = buf_to_str(field->in_check_value, (field->num_bits > 64) ? 64 : field->num_bits, 16);
+								char *in_check_mask_char = buf_to_str(field->in_check_mask, (field->num_bits > 64) ? 64 : field->num_bits, 16);
+								/* TODO: error reporting */
+								WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);
+								bitq_in_state.status=ERROR_JTAG_QUEUE_FAILED;
+								free(captured_char);
+								free(in_check_value_char);
+								free(in_check_mask_char);
+								break; /* leave the comparison loop upon first mismatch */      					
+							}
+						}
+					}
+
+					if (field->in_handler && bitq_in_state.status==ERROR_OK) {
+						bitq_in_state.status=(*field->in_handler)(in_buff, field->in_handler_priv);
+					}
+
+				}
+				
+				bitq_in_state.field_idx++; /* advance to next field */
+				bitq_in_state.bit_pos=0; /* start next field from the first bit */
+			}
+
+		}
+		bitq_in_state.cmd=bitq_in_state.cmd->next; /* advance to next command */
+		bitq_in_state.field_idx=0; /* preselect first field */
+	}
 }
 
 
 
 void bitq_io(int tms, int tdi, int tdo_req)
 {
-  bitq_interface->out(tms, tdi, tdo_req);
-  /* check and process the input queue */ 
-  if (bitq_interface->in_rdy()) bitq_in_proc();
+	bitq_interface->out(tms, tdi, tdo_req);
+	/* check and process the input queue */ 
+	if (bitq_interface->in_rdy()) bitq_in_proc();
 }
 
 
 void bitq_end_state(enum tap_state state)
 {
-  if (state==-1) return;
-  if (tap_move_map[state]==-1) {
+	if (state==-1) return;
+	if (tap_move_map[state]==-1) {
 		ERROR("BUG: %i is not a valid end state", state);
 		exit(-1);
 	}
-  end_state = state;
+	end_state = state;
 }
 
 
 void bitq_state_move(enum tap_state new_state)
 {
-  int i=0;
-  u8 tms_scan;
-
-  if (tap_move_map[cur_state]==-1 || tap_move_map[new_state]==-1) {
-    ERROR("TAP move from or to unstable state");
-    exit(-1);
-  }
-  
-  tms_scan=TAP_MOVE(cur_state, new_state);
-
-  for (i=0; i<7; i++) {
-    bitq_io(tms_scan&1, 0, 0);
-    tms_scan>>=1;
-  }
-
-  cur_state = new_state;
+	int i=0;
+	u8 tms_scan;
+
+	if (tap_move_map[cur_state]==-1 || tap_move_map[new_state]==-1) {
+		ERROR("TAP move from or to unstable state");
+		exit(-1);
+	}
+	
+	tms_scan=TAP_MOVE(cur_state, new_state);
+
+	for (i=0; i<7; i++) {
+		bitq_io(tms_scan&1, 0, 0);
+		tms_scan>>=1;
+	}
+
+	cur_state = new_state;
 }
 
 
 void bitq_path_move(pathmove_command_t *cmd)
 {
-  int i;
+	int i;
 
-  for (i=0; i<=cmd->num_states; i++) {
-    if (tap_transitions[cur_state].low == cmd->path[i]) bitq_io(0, 0, 0);
-    else if (tap_transitions[cur_state].high == cmd->path[i]) bitq_io(1, 0, 0);
-    else {
-      ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[i]]);
-      exit(-1);
-    }
+	for (i=0; i<=cmd->num_states; i++) {
+		if (tap_transitions[cur_state].low == cmd->path[i]) bitq_io(0, 0, 0);
+		else if (tap_transitions[cur_state].high == cmd->path[i]) bitq_io(1, 0, 0);
+		else {
+			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[i]]);
+			exit(-1);
+		}
 
-    cur_state = cmd->path[i];
-  }
+		cur_state = cmd->path[i];
+	}
 
-  end_state = cur_state;
+	end_state = cur_state;
 }
 
 
 void bitq_runtest(int num_cycles)
 {
-  int i;
+	int i;
 
-  /* only do a state_move when we're not already in RTI */
-  if (cur_state != TAP_RTI) bitq_state_move(TAP_RTI);
+	/* only do a state_move when we're not already in RTI */
+	if (cur_state != TAP_RTI) bitq_state_move(TAP_RTI);
 
-  /* execute num_cycles */
-  for (i = 0; i < num_cycles; i++)
-    bitq_io(0, 0, 0);
+	/* execute num_cycles */
+	for (i = 0; i < num_cycles; i++)
+		bitq_io(0, 0, 0);
 
-  /* finish in end_state */
-  if (cur_state != end_state) bitq_state_move(end_state);
+	/* finish in end_state */
+	if (cur_state != end_state) bitq_state_move(end_state);
 }
 
 
 void bitq_scan_field(scan_field_t *field, int pause)
 {
-  int bit_cnt;
-  int tdo_req;
-
-  u8 *out_ptr;
-  u8 out_mask;
-  
-  if (field->in_check_value || field->in_value || field->in_handler) tdo_req=1;
-  else tdo_req=0;
-
-  if (field->out_value==NULL) {
-    /* just send zeros and request data from TDO */
-    for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--)
-      bitq_io(0, 0, tdo_req);
-    bitq_io(pause, 0, tdo_req);
-  }
-  else {
-    /* send data, and optionally request TDO */
-    out_mask=0x01;
-    out_ptr=field->out_value;
-    for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--) {
-      bitq_io(0, ((*out_ptr)&out_mask)!=0, tdo_req);
-      if (out_mask==0x80) {
-        out_mask=0x01;
-        out_ptr++;
-      }
-      else out_mask<<=1;
-    }
-    bitq_io(pause, ((*out_ptr)&out_mask)!=0, tdo_req);
-  }
-
-  if (pause) {
-    bitq_io(0,0,0);
-    if (cur_state==TAP_SI) cur_state=TAP_PI;
-    else if (cur_state==TAP_SD) cur_state=TAP_PD;
-  }
+	int bit_cnt;
+	int tdo_req;
+
+	u8 *out_ptr;
+	u8 out_mask;
+	
+	if (field->in_check_value || field->in_value || field->in_handler) tdo_req=1;
+	else tdo_req=0;
+
+	if (field->out_value==NULL) {
+		/* just send zeros and request data from TDO */
+		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--)
+			bitq_io(0, 0, tdo_req);
+		bitq_io(pause, 0, tdo_req);
+	}
+	else {
+		/* send data, and optionally request TDO */
+		out_mask=0x01;
+		out_ptr=field->out_value;
+		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--) {
+			bitq_io(0, ((*out_ptr)&out_mask)!=0, tdo_req);
+			if (out_mask==0x80) {
+				out_mask=0x01;
+				out_ptr++;
+			}
+			else out_mask<<=1;
+		}
+		bitq_io(pause, ((*out_ptr)&out_mask)!=0, tdo_req);
+	}
+
+	if (pause) {
+		bitq_io(0,0,0);
+		if (cur_state==TAP_SI) cur_state=TAP_PI;
+		else if (cur_state==TAP_SD) cur_state=TAP_PD;
+	}
 }
 
 
 void bitq_scan(scan_command_t *cmd)
 {
-  int i;
+	int i;
 
-  if (cmd->ir_scan) bitq_state_move(TAP_SI);
-  else bitq_state_move(TAP_SD);
+	if (cmd->ir_scan) bitq_state_move(TAP_SI);
+	else bitq_state_move(TAP_SD);
 
-  for (i=0; i < cmd->num_fields-1; i++)
-    bitq_scan_field(&cmd->fields[i], 0);
-  bitq_scan_field(&cmd->fields[i], 1);
+	for (i=0; i < cmd->num_fields-1; i++)
+		bitq_scan_field(&cmd->fields[i], 0);
+	bitq_scan_field(&cmd->fields[i], 1);
 }
 
 
 int bitq_execute_queue(void)
 {
-  jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
-  
-  bitq_in_state.cmd = jtag_command_queue;
-  bitq_in_state.field_idx = 0;
-  bitq_in_state.bit_pos = 0;
-  bitq_in_state.status = ERROR_OK;
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	
+	bitq_in_state.cmd = jtag_command_queue;
+	bitq_in_state.field_idx = 0;
+	bitq_in_state.bit_pos = 0;
+	bitq_in_state.status = ERROR_OK;
 
-  while (cmd) {
+	while (cmd) {
 
-    switch (cmd->type) {
+		switch (cmd->type) {
 
-      case JTAG_END_STATE:
+			case JTAG_END_STATE:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
 #endif
-        bitq_end_state(cmd->cmd.end_state->end_state);
-        break;
+				bitq_end_state(cmd->cmd.end_state->end_state);
+				break;
 
-      case JTAG_RESET:
+			case JTAG_RESET:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 #endif
 				bitq_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-        if (bitq_interface->in_rdy()) bitq_in_proc();
-        break;
+				if (bitq_interface->in_rdy()) bitq_in_proc();
+				break;
 
-      case JTAG_RUNTEST:
+			case JTAG_RUNTEST:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
 #endif
-        bitq_end_state(cmd->cmd.runtest->end_state);
-        bitq_runtest(cmd->cmd.runtest->num_cycles);
-        break;
+				bitq_end_state(cmd->cmd.runtest->end_state);
+				bitq_runtest(cmd->cmd.runtest->num_cycles);
+				break;
 
-      case JTAG_STATEMOVE:
+			case JTAG_STATEMOVE:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
 #endif
-        bitq_end_state(cmd->cmd.statemove->end_state);
-        bitq_state_move(end_state); /* uncoditional TAP move */
-        break;
+				bitq_end_state(cmd->cmd.statemove->end_state);
+				bitq_state_move(end_state); /* uncoditional TAP move */
+				break;
 
-      case JTAG_PATHMOVE:
+			case JTAG_PATHMOVE:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
 #endif
-        bitq_path_move(cmd->cmd.pathmove);
-        break;
+				bitq_path_move(cmd->cmd.pathmove);
+				break;
 
-      case JTAG_SCAN:
+			case JTAG_SCAN:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("scan end in %i", cmd->cmd.scan->end_state);
-        if (cmd->cmd.scan->ir_scan) DEBUG("scan ir");
-        else DEBUG("scan dr");
+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
+				if (cmd->cmd.scan->ir_scan) DEBUG("scan ir");
+				else DEBUG("scan dr");
 #endif
-        bitq_end_state(cmd->cmd.scan->end_state);
-        bitq_scan(cmd->cmd.scan);
-        if (cur_state != end_state) bitq_state_move(end_state);
-        break;
+				bitq_end_state(cmd->cmd.scan->end_state);
+				bitq_scan(cmd->cmd.scan);
+				if (cur_state != end_state) bitq_state_move(end_state);
+				break;
 
-      case JTAG_SLEEP:
+			case JTAG_SLEEP:
 #ifdef _DEBUG_JTAG_IO_
-        DEBUG("sleep %i", cmd->cmd.sleep->us);
+				DEBUG("sleep %i", cmd->cmd.sleep->us);
 #endif
 				bitq_interface->sleep(cmd->cmd.sleep->us);
-        if (bitq_interface->in_rdy()) bitq_in_proc();
-        break;
-
-      default:
-        ERROR("BUG: unknown JTAG command type encountered");
-        exit(-1);
-    }
-
-    cmd = cmd->next;
-  }
-
-  bitq_interface->flush();
-  bitq_in_proc();
-
-  if (bitq_in_state.cmd) {
-    ERROR("missing data from bitq interface");
-    return ERROR_JTAG_QUEUE_FAILED;
-  }
-  if (bitq_interface->in()>=0) {
-    ERROR("extra data from bitq interface");
-    return ERROR_JTAG_QUEUE_FAILED;
-  }
-
-  return bitq_in_state.status;
+				if (bitq_interface->in_rdy()) bitq_in_proc();
+				break;
+
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+
+		cmd = cmd->next;
+	}
+
+	bitq_interface->flush();
+	bitq_in_proc();
+
+	if (bitq_in_state.cmd) {
+		ERROR("missing data from bitq interface");
+		return ERROR_JTAG_QUEUE_FAILED;
+	}
+	if (bitq_interface->in()>=0) {
+		ERROR("extra data from bitq interface");
+		return ERROR_JTAG_QUEUE_FAILED;
+	}
+
+	return bitq_in_state.status;
 }
 
 
diff --git a/src/jtag/ft2232.c b/src/jtag/ft2232.c
index a6dbadc6..44b0e6a8 100644
--- a/src/jtag/ft2232.c
+++ b/src/jtag/ft2232.c
@@ -205,7 +205,8 @@ int ft2232_read(u8* buf, int size, u32* bytes_read)
 
 	while ((*bytes_read < size) && timeout--)
 	{
-		if ((status = FT_Read(ftdih, buf, size, &dw_bytes_read)) != FT_OK)
+		if ((status = FT_Read(ftdih, buf + *bytes_read, size - 
+			*bytes_read, &dw_bytes_read)) != FT_OK)		
 		{
 			*bytes_read = 0; 
 			ERROR("FT_Read returned: %lu", status);
diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
index 1db8247e..c1c52fe4 100644
--- a/src/jtag/jtag.c
+++ b/src/jtag/jtag.c
@@ -1345,7 +1345,7 @@ int jtag_validate_chain()
 			char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
 			ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
 			free(cbuf);
-			exit(-1);
+			return ERROR_JTAG_INIT_FAILED;
 		}
 		chain_pos += device->ir_length;
 		device = device->next;
@@ -1356,7 +1356,7 @@ int jtag_validate_chain()
 		char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
 		ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
 		free(cbuf);
-		exit(-1);
+		return ERROR_JTAG_INIT_FAILED;
 	}
 	
 	free(ir_test);
@@ -1402,7 +1402,7 @@ int jtag_register_commands(struct command_context_s *cmd_ctx)
 
 int jtag_init(struct command_context_s *cmd_ctx)
 {
-	int i;
+	int i, validate_tries = 0;
 	
 	DEBUG("-");
 
@@ -1433,11 +1433,20 @@ int jtag_init(struct command_context_s *cmd_ctx)
 				
 				jtag_add_statemove(TAP_TLR);
 				jtag_execute_queue();
-				
+
+				while (jtag_validate_chain() != ERROR_OK)
+				{
+					validate_tries++;
+					if (validate_tries > 5)
+					{
+						ERROR("Could not validate JTAG chain, exit");
+						exit(-1);
+					}
+					usleep(10000);
+				}
+
 				jtag_examine_chain();
 				
-				jtag_validate_chain();
-				
 				return ERROR_OK;
 			}
 		}
diff --git a/src/jtag/jtag.h b/src/jtag/jtag.h
index 470ba93e..1b03d615 100644
--- a/src/jtag/jtag.h
+++ b/src/jtag/jtag.h
@@ -25,7 +25,7 @@
 
 #include "command.h"
 
-#if 1
+#if 0
 #define _DEBUG_JTAG_IO_
 #endif
 
diff --git a/src/jtag/presto.c b/src/jtag/presto.c
index a6c048ad..fe54b131 100644
--- a/src/jtag/presto.c
+++ b/src/jtag/presto.c
@@ -166,7 +166,7 @@ int presto_open(char *req_serial)
 		if (FT_GetDeviceInfo(presto->handle,&device,&vidpid,presto->serial,devname,NULL)==FT_OK)
 		{
 			if (vidpid==PRESTO_VID_PID && (req_serial==NULL || !strcmp(presto->serial,req_serial)))
-			  break;
+				break;
 		}
 		FT_Close(presto->handle);
 		presto->handle=INVALID_HANDLE_VALUE;
diff --git a/src/openocd.c b/src/openocd.c
index 33f876b7..600225b9 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -18,7 +18,7 @@
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
 
-#define OPENOCD_VERSION "Open On-Chip Debugger (2007-07-15 13:15 CEST)"
+#define OPENOCD_VERSION "Open On-Chip Debugger (2007-07-25 12:00 CEST)"
 
 #ifdef HAVE_CONFIG_H
 #include "config.h"
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 34b66050..19c6a233 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -264,63 +264,31 @@ int gdb_get_packet(connection_t *connection, char *buffer, int *len)
 		{
 			if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
 				return retval;
-			
-			if( !first_char ) {
-				packet_type = character;
-				first_char = 1;	
-			}
-			
-			if( packet_type == 'X' )
+				
+			if (character == '#') break;
+
+			if (character == '}')
 			{
-				switch (character)
-				{
-					case '#':
-						break;
-					case 0x7d:
-						/* data transmitted in binary mode (X packet)
-						* uses 0x7d as escape character */
-						my_checksum += character & 0xff;
-						gdb_get_char(connection, &character);
-						my_checksum += character & 0xff;
-						buffer[count++] = (character ^ 0x20) & 0xff;
-						if (count > *len)
-						{
-							ERROR("packet buffer too small");
-							return ERROR_GDB_BUFFER_TOO_SMALL;
-						}
-						break;
-					default:
-						buffer[count++] = character & 0xff;
-						my_checksum += character & 0xff;
-						if (count > *len)
-						{
-							ERROR("packet buffer too small");
-							return ERROR_GDB_BUFFER_TOO_SMALL;
-						}
-						break;
-				}
+				/* data transmitted in binary mode (X packet)
+				* uses 0x7d as escape character */
+				my_checksum += character & 0xff;
+				if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+					return retval;
+				my_checksum += character & 0xff;
+				buffer[count++] = (character ^ 0x20) & 0xff;
 			}
 			else
 			{
-				switch (character)
-				{
-					case '#':
-						break;
-					case 0x3:
-						gdb_con->ctrl_c = 1;
-						break;
-					default:
-						buffer[count++] = character & 0xff;
-						my_checksum += character & 0xff;
-						if (count > *len)
-						{
-							ERROR("packet buffer too small");
-							return ERROR_GDB_BUFFER_TOO_SMALL;
-						}
-						break;
-				}
+				my_checksum += character & 0xff;
+				buffer[count++] = character & 0xff;
+			}
+
+			if (count > *len)
+			{
+				ERROR("packet buffer too small");
+				return ERROR_GDB_BUFFER_TOO_SMALL;
 			}
-		} while (character != '#');
+		} while (1);
 
 		*len = count;
 		
diff --git a/src/target/target.c b/src/target/target.c
index 07c450cd..10d430b9 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1862,6 +1862,7 @@ int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args
 int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	target_t *target = get_current_target(cmd_ctx);
+	int retval;
 
 	if (argc == 0)
 	{
@@ -1905,7 +1906,23 @@ int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args,
 		{
 			data_mask = strtoul(args[4], NULL, 0);
 		}
-		watchpoint_add(target, strtoul(args[0], NULL, 0), strtoul(args[1], NULL, 0), type, data_value, data_mask);
+		
+		if ((retval = watchpoint_add(target, strtoul(args[0], NULL, 0),
+				strtoul(args[1], NULL, 0), type, data_value, data_mask)) != ERROR_OK)
+		{
+			switch (retval)
+			{
+				case ERROR_TARGET_NOT_HALTED:
+					command_print(cmd_ctx, "target must be halted to set watchpoints");
+					break;
+				case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+					command_print(cmd_ctx, "no more watchpoints available");
+					break;
+				default:
+					command_print(cmd_ctx, "unknown error, watchpoint not set");
+					break;
+			}	
+		}
 	}
 	else
 	{