*   Copyright (C) 2008 by Spencer Oliver                                  *
  *   spen@spen-soft.co.uk                                                  *
  *                                                                         *
+ *   Copyright (C) 2009 by Oyvind Harboe                                   *
+ *   oyvind.harboe@zylin.com                                               *
+ *                                                                                                                                                *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
  *   the Free Software Foundation; either version 2 of the License, or     *
        int retval;
        u32 ctrlstat;
 
-       keep_alive();
+       /* too expensive to call keep_alive() here */
 
        /* Danger!!!! BROKEN!!!! */
        scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 
        swjdp->ack = swjdp->ack & 0x7;
 
-       long long then=timeval_ms();
-       while (swjdp->ack != 2)
+       if (swjdp->ack != 2)
        {
-               if (swjdp->ack == 1)
+               long long then=timeval_ms();
+               while (swjdp->ack != 2)
                {
-                       if ((timeval_ms()-then) > 1000)
+                       if (swjdp->ack == 1)
                        {
-                               LOG_WARNING("Timeout (1000ms) waiting for ACK = OK/FAULT in SWJDP transaction");
+                               if ((timeval_ms()-then) > 1000)
+                               {
+                                       LOG_WARNING("Timeout (1000ms) waiting for ACK = OK/FAULT in SWJDP transaction");
+                                       return ERROR_JTAG_DEVICE_ERROR;
+                               }
+                       }
+                       else
+                       {
+                               LOG_WARNING("Invalid ACK in SWJDP transaction");
                                return ERROR_JTAG_DEVICE_ERROR;
                        }
-               }
-               else
-               {
-                       LOG_WARNING("Invalid ACK in SWJDP transaction");
-                       return ERROR_JTAG_DEVICE_ERROR;
-               }
 
-               scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-               if ((retval=jtag_execute_queue())!=ERROR_OK)
-                       return retval;
-               swjdp->ack = swjdp->ack & 0x7;
+                       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
+                       if ((retval=jtag_execute_queue())!=ERROR_OK)
+                               return retval;
+                       swjdp->ack = swjdp->ack & 0x7;
+               }
+       } else
+       {
+               /* common code path avoids fn to timeval_ms() */
        }
 
        /* Check for STICKYERR and STICKYORUN */
 
        return target_wait_state(target, TARGET_HALTED, ms);
 }
 
+/* wait for target state to change. The trick here is to have a low
+ * latency for short waits and not to suck up all the CPU time
+ * on longer waits.
+ *
+ * After 500ms, keep_alive() is invoked
+ */
 int target_wait_state(target_t *target, enum target_state state, int ms)
 {
        int retval;
-       struct timeval timeout, now;
+       long long then=0, cur;
        int once=1;
-       gettimeofday(&timeout, NULL);
-       timeval_add_time(&timeout, 0, ms * 1000);
 
        for (;;)
        {
                if ((retval=target_poll(target))!=ERROR_OK)
                        return retval;
-               keep_alive();
                if (target->state == state)
                {
                        break;
                }
+               cur = timeval_ms();
                if (once)
                {
                        once=0;
+                       then = timeval_ms();
                        LOG_DEBUG("waiting for target %s...",
                                Jim_Nvp_value2name_simple(nvp_target_state,state)->name);
                }
 
-               gettimeofday(&now, NULL);
-               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
+               if (cur-then>500)
+               {
+                       keep_alive();
+               }
+
+               if ((cur-then)>ms)
                {
                        LOG_ERROR("timed out while waiting for target %s",
                                Jim_Nvp_value2name_simple(nvp_target_state,state)->name);