#include <stdbool.h>
 
-/** Delay value for SCAN operations with less than maximum TCK frequency */
-u8 delay_scan = 0;
+/** Delay value for SCAN_IN operations with less than maximum TCK frequency */
+u8 delay_scan_in = 0;
 
-/** Delay value for CLOCK_TCK operations */
+/** Delay value for SCAN_OUT operations with less than maximum TCK frequency */
+u8 delay_scan_out = 0;
+
+/** Delay value for SCAN_IO operations with less than maximum TCK frequency */
+u8 delay_scan_io = 0;
+
+/** Delay value for CLOCK_TCK operations with less than maximum frequency */
 u8 delay_tck = 0;
 
 /** Delay value for CLOCK_TMS operations with less than maximum frequency */
  * Dummy data is shifted into the JTAG chain via TDI, TDO data is sampled and
  * stored in the EP2 IN buffer.
  *
+ * Maximum achievable TCK frequency is 182 kHz for ULINK clocked at 24 MHz.
+ *
  * @param out_offset offset in OUT2BUF where payload data starts
  */
 void jtag_scan_in(u8 out_offset, u8 in_offset)
 
     for (j = 0; j < 8; j++) {
       OUTB = outb_buffer; /* TCK changes here */
-      OUTB = (outb_buffer | PIN_TCK);
       tdo_data = tdo_data >> 1;
+      OUTB = (outb_buffer | PIN_TCK);
 
       if (GET_TDO()) {
         tdo_data |= 0x80;
     }
 
     OUTB = outb_buffer; /* TCK change here */
-    OUTB = (outb_buffer | PIN_TCK);
     tdo_data = tdo_data >> 1;
+    OUTB = (outb_buffer | PIN_TCK);
 
     if (GET_TDO()) {
       tdo_data |= 0x80;
   }
 }
 
+/**
+ * Perform JTAG SCAN-IN operation at variable TCK frequency.
+ *
+ * Dummy data is shifted into the JTAG chain via TDI, TDO data is sampled and
+ * stored in the EP2 IN buffer.
+ *
+ * Maximum achievable TCK frequency is 113 kHz for ULINK clocked at 24 MHz.
+ *
+ * @param out_offset offset in OUT2BUF where payload data starts
+ */
+void jtag_slow_scan_in(u8 out_offset, u8 in_offset)
+{
+  u8 scan_size_bytes, bits_last_byte;
+  u8 tms_count_start, tms_count_end;
+  u8 tms_sequence_start, tms_sequence_end;
+  u8 tdo_data, i, j, k;
+
+  u8 outb_buffer;
+
+  /* Get parameters from OUT2BUF */
+  scan_size_bytes = OUT2BUF[out_offset];
+  bits_last_byte = OUT2BUF[out_offset + 1];
+  tms_count_start = (OUT2BUF[out_offset + 2] >> 4) & 0x0F;
+  tms_count_end = OUT2BUF[out_offset + 2] & 0x0F;
+  tms_sequence_start = OUT2BUF[out_offset + 3];
+  tms_sequence_end = OUT2BUF[out_offset + 4];
+
+  if (tms_count_start > 0) {
+    jtag_slow_clock_tms(tms_count_start, tms_sequence_start);
+  }
+
+  outb_buffer = OUTB & ~(PIN_TDI | PIN_TCK | PIN_TMS);
+
+  /* Shift all bytes except the last byte */
+  for (i = 0; i < scan_size_bytes - 1; i++) {
+    tdo_data = 0;
+
+    for (j = 0; j < 8; j++) {
+      OUTB = outb_buffer; /* TCK changes here */
+      for (k = 0; k < delay_scan_in; k++);
+      tdo_data = tdo_data >> 1;
+
+      OUTB = (outb_buffer | PIN_TCK);
+      for (k = 0; k < delay_scan_in; k++);
+
+      if (GET_TDO()) {
+        tdo_data |= 0x80;
+      }
+    }
+
+    /* Copy TDO data to IN2BUF */
+    IN2BUF[i + in_offset] = tdo_data;
+  }
+
+  tdo_data = 0;
+
+  /* Shift the last byte */
+  for (j = 0; j < bits_last_byte; j++) {
+    /* Assert TMS signal if requested and this is the last bit */
+    if ((j == bits_last_byte - 1) && (tms_count_end > 0)) {
+      outb_buffer |= PIN_TMS;
+      tms_count_end--;
+      tms_sequence_end = tms_sequence_end >> 1;
+    }
+
+    OUTB = outb_buffer; /* TCK change here */
+    for (k = 0; k < delay_scan_in; k++);
+    tdo_data = tdo_data >> 1;
+
+    OUTB = (outb_buffer | PIN_TCK);
+    for (k = 0; k < delay_scan_in; k++);
+
+    if (GET_TDO()) {
+      tdo_data |= 0x80;
+    }
+  }
+  tdo_data = tdo_data >> (8 - bits_last_byte);
+
+  /* Copy TDO data to IN2BUF */
+  IN2BUF[i + in_offset] = tdo_data;
+
+  /* Move to correct end state */
+  if (tms_count_end > 0) {
+    jtag_slow_clock_tms(tms_count_end, tms_sequence_end);
+  }
+}
+
 /**
  * Perform JTAG SCAN-OUT operation at maximum TCK frequency.
  *
  * data is not sampled.
  * The TAP-FSM state is alyways left in the PAUSE-DR/PAUSE-IR state.
  *
+ * Maximum achievable TCK frequency is 142 kHz for ULINK clocked at 24 MHz.
+ *
  * @param out_offset offset in OUT2BUF where payload data starts
  */
 void jtag_scan_out(u8 out_offset)
   }
 }
 
+/**
+ * Perform JTAG SCAN-OUT operation at maximum TCK frequency.
+ *
+ * Data stored in EP2 OUT buffer is shifted into the JTAG chain via TDI, TDO
+ * data is not sampled.
+ * The TAP-FSM state is alyways left in the PAUSE-DR/PAUSE-IR state.
+ *
+ * Maximum achievable TCK frequency is 97 kHz for ULINK clocked at 24 MHz.
+ *
+ * @param out_offset offset in OUT2BUF where payload data starts
+ */
+void jtag_slow_scan_out(u8 out_offset)
+{
+  u8 scan_size_bytes, bits_last_byte;
+  u8 tms_count_start, tms_count_end;
+  u8 tms_sequence_start, tms_sequence_end;
+  u8 tdi_data, i, j, k;
+
+  u8 outb_buffer;
+
+  /* Get parameters from OUT2BUF */
+  scan_size_bytes = OUT2BUF[out_offset];
+  bits_last_byte = OUT2BUF[out_offset + 1];
+  tms_count_start = (OUT2BUF[out_offset + 2] >> 4) & 0x0F;
+  tms_count_end = OUT2BUF[out_offset + 2] & 0x0F;
+  tms_sequence_start = OUT2BUF[out_offset + 3];
+  tms_sequence_end = OUT2BUF[out_offset + 4];
+
+  if (tms_count_start > 0) {
+    jtag_slow_clock_tms(tms_count_start, tms_sequence_start);
+  }
+
+  outb_buffer = OUTB & ~(PIN_TCK | PIN_TMS);
+
+  /* Shift all bytes except the last byte */
+  for (i = 0; i < scan_size_bytes - 1; i++) {
+    tdi_data = OUT2BUF[i + out_offset + 5];
+
+    for (j = 0; j < 8; j++) {
+      if (tdi_data & 0x01) {
+        outb_buffer |= PIN_TDI;
+      }
+      else {
+        outb_buffer &= ~PIN_TDI;
+      }
+
+      OUTB = outb_buffer; /* TDI and TCK change here */
+      for (k = 0; k < delay_scan_out; k++);
+      tdi_data = tdi_data >> 1;
+
+      OUTB = (outb_buffer | PIN_TCK);
+      for (k = 0; k < delay_scan_out; k++);
+    }
+  }
+
+  tdi_data = OUT2BUF[i + out_offset + 5];
+
+  /* Shift the last byte */
+  for (j = 0; j < bits_last_byte; j++) {
+    if (tdi_data & 0x01) {
+      outb_buffer |= PIN_TDI;
+    }
+    else {
+      outb_buffer &= ~PIN_TDI;
+    }
+
+    /* Assert TMS signal if requested and this is the last bit */
+    if ((j == bits_last_byte - 1) && (tms_count_end > 0)) {
+      outb_buffer |= PIN_TMS;
+      tms_count_end--;
+      tms_sequence_end = tms_sequence_end >> 1;
+    }
+
+    OUTB = outb_buffer; /* TDI and TCK change here */
+    for (k = 0; k < delay_scan_out; k++);
+    tdi_data = tdi_data >> 1;
+
+    OUTB = (outb_buffer | PIN_TCK);
+    for (k = 0; k < delay_scan_out; k++);
+  }
+
+  /* Move to correct end state */
+  if (tms_count_end > 0) {
+    jtag_slow_clock_tms(tms_count_end, tms_sequence_end);
+  }
+}
+
 /**
  * Perform bidirectional JTAG SCAN operation at maximum TCK frequency.
  *
  * data is sampled and stored in the EP2 IN buffer.
  * The TAP-FSM state is alyways left in the PAUSE-DR/PAUSE-IR state.
  *
+ * Maximum achievable TCK frequency is 100 kHz for ULINK clocked at 24 MHz.
+ *
  * @param out_offset offset in OUT2BUF where payload data starts
  */
 void jtag_scan_io(u8 out_offset, u8 in_offset)
   }
 }
 
+/**
+ * Perform bidirectional JTAG SCAN operation at maximum TCK frequency.
+ *
+ * Data stored in EP2 OUT buffer is shifted into the JTAG chain via TDI, TDO
+ * data is sampled and stored in the EP2 IN buffer.
+ * The TAP-FSM state is alyways left in the PAUSE-DR/PAUSE-IR state.
+ *
+ * Maximum achievable TCK frequency is 78 kHz for ULINK clocked at 24 MHz.
+ *
+ * @param out_offset offset in OUT2BUF where payload data starts
+ */
+void jtag_slow_scan_io(u8 out_offset, u8 in_offset)
+{
+  u8 scan_size_bytes, bits_last_byte;
+  u8 tms_count_start, tms_count_end;
+  u8 tms_sequence_start, tms_sequence_end;
+  u8 tdi_data, tdo_data, i, j, k;
+
+  u8 outb_buffer;
+
+  /* Get parameters from OUT2BUF */
+  scan_size_bytes = OUT2BUF[out_offset];
+  bits_last_byte = OUT2BUF[out_offset + 1];
+  tms_count_start = (OUT2BUF[out_offset + 2] >> 4) & 0x0F;
+  tms_count_end = OUT2BUF[out_offset + 2] & 0x0F;
+  tms_sequence_start = OUT2BUF[out_offset + 3];
+  tms_sequence_end = OUT2BUF[out_offset + 4];
+
+  if (tms_count_start > 0) {
+    jtag_slow_clock_tms(tms_count_start, tms_sequence_start);
+  }
+
+  outb_buffer = OUTB & ~(PIN_TCK | PIN_TMS);
+
+  /* Shift all bytes except the last byte */
+  for (i = 0; i < scan_size_bytes - 1; i++) {
+    tdi_data = OUT2BUF[i + out_offset + 5];
+    tdo_data = 0;
+
+    for (j = 0; j < 8; j++) {
+      if (tdi_data & 0x01) {
+        outb_buffer |= PIN_TDI;
+      }
+      else {
+        outb_buffer &= ~PIN_TDI;
+      }
+
+      OUTB = outb_buffer; /* TDI and TCK change here */
+      for (k = 0; k < delay_scan_io; k++);
+      tdi_data = tdi_data >> 1;
+
+      OUTB = (outb_buffer | PIN_TCK);
+      for (k = 0; k < delay_scan_io; k++);
+      tdo_data = tdo_data >> 1;
+
+      if (GET_TDO()) {
+        tdo_data |= 0x80;
+      }
+    }
+
+    /* Copy TDO data to IN2BUF */
+    IN2BUF[i + in_offset] = tdo_data;
+  }
+
+  tdi_data = OUT2BUF[i + out_offset + 5];
+  tdo_data = 0;
+
+  /* Shift the last byte */
+  for (j = 0; j < bits_last_byte; j++) {
+    if (tdi_data & 0x01) {
+      outb_buffer |= PIN_TDI;
+    }
+    else {
+      outb_buffer &= ~PIN_TDI;
+    }
+
+    /* Assert TMS signal if requested and this is the last bit */
+    if ((j == bits_last_byte - 1) && (tms_count_end > 0)) {
+      outb_buffer |= PIN_TMS;
+      tms_count_end--;
+      tms_sequence_end = tms_sequence_end >> 1;
+    }
+
+    OUTB = outb_buffer; /* TDI and TCK change here */
+    for (k = 0; k < delay_scan_io; k++);
+    tdi_data = tdi_data >> 1;
+
+    OUTB = (outb_buffer | PIN_TCK);
+    for (k = 0; k < delay_scan_io; k++);
+    tdo_data = tdo_data >> 1;
+
+    if (GET_TDO()) {
+      tdo_data |= 0x80;
+    }
+  }
+  tdo_data = tdo_data >> (8 - bits_last_byte);
+
+  /* Copy TDO data to IN2BUF */
+  IN2BUF[i + in_offset] = tdo_data;
+
+  /* Move to correct end state */
+  if (tms_count_end > 0) {
+    jtag_slow_clock_tms(tms_count_end, tms_sequence_end);
+  }
+}
+
 /**
  * Generate TCK clock cycles.
  *
+ * Maximum achievable TCK frequency is 375 kHz for ULINK clocked at 24 MHz.
+ *
  * @param count number of TCK clock cyclces to generate.
  */
 void jtag_clock_tck(u16 count)
 {
   u16 i;
-  u8 j;
+  u8 outb_buffer = OUTB & ~(PIN_TCK);
 
   for ( i = 0; i < count; i++ ) {
-    SET_TCK_LOW();
-    for(j = 0; j < delay_tck; j++);
+    OUTB = outb_buffer;
+    OUTB = outb_buffer | PIN_TCK;
+  }
+}
+
+/**
+ * Generate TCK clock cycles at variable frequency.
+ *
+ * Maximum achieveable TCK frequency is 166.6 kHz for ULINK clocked at 24 MHz.
+ *
+ * @param count number of TCK clock cyclces to generate.
+ */
+void jtag_slow_clock_tck(u16 count)
+{
+  u16 i;
+  u8 j;
+  u8 outb_buffer = OUTB & ~(PIN_TCK);
 
-    SET_TCK_HIGH();
-    for(j = 0; j < delay_tck; j++);
+  for ( i = 0; i < count; i++ ) {
+    OUTB = outb_buffer;
+    for (j = 0; j < delay_tck; j++);
+    OUTB = outb_buffer | PIN_TCK;
+    for (j = 0; j < delay_tck; j++);
   }
 }
 
 /**
- * Perform TAP-FSM state transitions at maximum TCK frequency.
+ * Perform TAP FSM state transitions at maximum TCK frequency.
+ *
+ * Maximum achievable TCK frequency is 176 kHz for ULINK clocked at 24 MHz.
  *
  * @param count the number of state transitions to perform.
  * @param sequence the TMS pin levels for each state transition, starting with
  */
 void jtag_clock_tms(u8 count, u8 sequence)
 {
-  volatile u8 outb_buffer;
+  u8 outb_buffer = OUTB & ~(PIN_TCK);
   u8 i;
 
-  outb_buffer = OUTB & ~(PIN_TCK);
-
   for ( i = 0; i < count; i++ ) {
     /* Set TMS pin according to sequence parameter */
     if ( sequence & 0x1 ) {
 /**
  * Perform TAP-FSM state transitions at less than maximum TCK frequency.
  *
+ * Maximum achievable TCK frequency is 117 kHz for ULINK clocked at 24 MHz.
+ *
  * @param count the number of state transitions to perform.
  * @param sequence the TMS pin levels for each state transition, starting with
  *  the least-significant bit.
  */
 void jtag_slow_clock_tms(u8 count, u8 sequence)
 {
+  u8 outb_buffer = OUTB & ~(PIN_TCK);
+  u8 i, j;
+
+  for (i = 0; i < count; i++) {
+    /* Set TMS pin according to sequence parameter */
+    if ( sequence & 0x1 ) {
+      outb_buffer |= PIN_TMS;
+    }
+    else {
+      outb_buffer &= ~PIN_TMS;
+    }
 
+    OUTB = outb_buffer;
+    for (j = 0; j < delay_tms; j++);
+    sequence = sequence >> 1;
+    OUTB = outb_buffer | PIN_TCK;
+    for (j = 0; j < delay_tms; j++);
+  }
 }
 
 /**
 /**
  * Configure TCK delay parameters.
  *
- * @param scan number of delay cycles in shift operations.
+ * @param scan_in number of delay cycles in scan_in operations.
+ * @param scan_out number of delay cycles in scan_out operations.
+ * @param scan_io number of delay cycles in scan_io operations.
  * @param tck number of delay cycles in clock_tck operations.
  * @param tms number of delay cycles in clock_tms operations.
  */
-void jtag_configure_tck_delay(u8 scan, u8 tck, u8 tms)
+void jtag_configure_tck_delay(u8 scan_in, u8 scan_out, u8 scan_io, u8 tck,
+    u8 tms)
 {
-  delay_scan = scan;
+  delay_scan_in = scan_in;
+  delay_scan_out = scan_out;
+  delay_scan_io = scan_io;
   delay_tck = tck;
   delay_tms = tms;
 }