diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c index 44a7fc69f..c426f87a1 100644 --- a/src/jtag/hla/hla_interface.c +++ b/src/jtag/hla/hla_interface.c @@ -37,7 +37,7 @@ #include -static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, NULL, 0}, 0, 0 }; +static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, NULL, 0, -1}, 0, 0 }; int hl_interface_open(enum hl_transports tr) { @@ -148,20 +148,31 @@ int hl_interface_init_reset(void) return ERROR_OK; } -static int dummy_khz(int khz, int *jtag_speed) +static int hl_interface_khz(int khz, int *jtag_speed) { - *jtag_speed = khz; + *jtag_speed = hl_if.layout->api->speed(hl_if.handle, khz, true); return ERROR_OK; } -static int dummy_speed_div(int speed, int *khz) +static int hl_interface_speed_div(int speed, int *khz) { *khz = speed; return ERROR_OK; } -static int dummy_speed(int speed) +static int hl_interface_speed(int speed) { + if (hl_if.layout->api->speed == NULL) + return ERROR_OK; + + if (hl_if.handle == NULL) { + /* pass speed as initial param as interface not open yet */ + hl_if.param.initial_interface_speed = speed; + return ERROR_OK; + } + + hl_if.layout->api->speed(hl_if.handle, speed, false); + return ERROR_OK; } @@ -340,7 +351,7 @@ struct jtag_interface hl_interface = { .init = hl_interface_init, .quit = hl_interface_quit, .execute_queue = hl_interface_execute_queue, - .speed = &dummy_speed, - .khz = &dummy_khz, - .speed_div = &dummy_speed_div, + .speed = &hl_interface_speed, + .khz = &hl_interface_khz, + .speed_div = &hl_interface_speed_div, }; diff --git a/src/jtag/hla/hla_interface.h b/src/jtag/hla/hla_interface.h index cb961bdea..aac1be30a 100644 --- a/src/jtag/hla/hla_interface.h +++ b/src/jtag/hla/hla_interface.h @@ -50,6 +50,8 @@ struct hl_interface_param_s { FILE *trace_f; /** Trace module source clock rate */ uint32_t trace_source_hz; + /** Initial interface clock clock speed */ + int initial_interface_speed; }; struct hl_interface_s { diff --git a/src/jtag/hla/hla_layout.h b/src/jtag/hla/hla_layout.h index df93cb69d..e989f6659 100644 --- a/src/jtag/hla/hla_layout.h +++ b/src/jtag/hla/hla_layout.h @@ -78,6 +78,8 @@ struct hl_layout_api_s { /** */ int (*custom_command) (void *handle, const char *command); /** */ + int (*speed)(void *handle, int khz, bool query); + /** */ enum target_state (*state) (void *fd); };