- corrected rounding in ft2232_khz

[openocd.git] / src / jtag / ft2232.c

diff --git a/src/jtag/ft2232.c b/src/jtag/ft2232.c
index 20824bbbd51e1675f25d3ed3c8bc3cbbfffae125..0b2e79c990280937d6ebf86b2e4a1b9f689bf704 100644 (file)
--- a/src/jtag/ft2232.c
+++ b/src/jtag/ft2232.c
@@ -46,9 +46,6 @@
 #include <ftdi.h>
 #endif
 
-#include <sys/time.h>
-#include <time.h>
-
 /* enable this to debug io latency
  */
 #if 0
@@ -64,6 +61,8 @@
 int ft2232_execute_queue(void);
 
 int ft2232_speed(int speed);
+int ft2232_speed_div(int speed, int *khz);
+int ft2232_khz(int khz, int *jtag_speed);
 int ft2232_register_commands(struct command_context_s *cmd_ctx);
 int ft2232_init(void);
 int ft2232_quit(void);
@@ -160,6 +159,9 @@ jtag_interface_t ft2232_interface =
        .execute_queue = ft2232_execute_queue,
        
        .speed = ft2232_speed,
+  .speed_div = ft2232_speed_div,
+  .khz = ft2232_khz,
+  
        .register_commands = ft2232_register_commands,
        .init = ft2232_init,
        .quit = ft2232_quit,
@@ -259,11 +261,53 @@ int ft2232_speed(int speed)
                return retval;
        }
 
-       jtag_speed = speed;
-       
        return ERROR_OK;
 }
 
+int ft2232_speed_div(int speed, int *khz)
+{
+  /*
+   * Take a look in the FT2232 manual, 
+   * AN2232C-01 Command Processor for
+   * MPSSE and MCU Host Bus. Chapter 3.8
+   */
+  *khz = 6000 / (1+speed);
+  
+       return ERROR_OK;
+}
+
+int ft2232_khz(int khz, int *jtag_speed)
+{
+  /*
+   * Take a look in the FT2232 manual, 
+   * AN2232C-01 Command Processor for
+   * MPSSE and MCU Host Bus. Chapter 3.8
+   *
+   * We will calc here with a multiplier
+   * of 10 for better rounding later.
+   */
+   
+  /* Calc speed, (6000 / khz) - 1 */
+  /* Use 65000 for better rounding */ 
+  *jtag_speed = (60000 / khz) - 10;
+  
+  /* Add 0.9 for rounding */
+  *jtag_speed += 9;
+  
+  /* Calc real speed */
+  *jtag_speed = *jtag_speed / 10;
+  
+  /* Check if speed is greater than 0 */
+  if (*jtag_speed < 0)
+  {
+    *jtag_speed = 0;
+  }    
+  
+       return ERROR_OK;
+}
+
+
+
 int ft2232_register_commands(struct command_context_s *cmd_ctx)
 {
        register_command(cmd_ctx, NULL, "ft2232_device_desc", ft2232_handle_device_desc_command,
@@ -455,11 +499,11 @@ void ft2232_add_pathmove(pathmove_command_t *cmd)
        state_count = 0;
        while (num_states)
        {
-               tms_byte = 0x0;
                int bit_count = 0;
                
                int num_states_batch = num_states > 7 ? 7 : num_states;
 
+               tms_byte = 0x0;
                /* command "Clock Data to TMS/CS Pin (no Read)" */
                BUFFER_ADD = 0x4b;
                /* number of states remaining */