diff --git a/bsp/rp2040/m_isp/mehfet_hw.c b/bsp/rp2040/m_isp/mehfet_hw.c
index 040ab2c..d3fc70b 100644
--- a/bsp/rp2040/m_isp/mehfet_hw.c
+++ b/bsp/rp2040/m_isp/mehfet_hw.c
@@ -56,8 +56,12 @@ void mehfet_hw_disconnect(void) {
     sbw_deinit();
 }
 
-void mehfet_hw_reset_target(void) {
-
+void mehfet_hw_set_clkspeed(bool fast) {
+    if (fast) {
+        sbw_set_freq(true, 350e3);
+    } else {
+        sbw_set_freq(false, 50e3);
+    }
 }
 uint8_t mehfet_hw_get_old_lines(void) {
     return (sbw_get_last_tclk() ? 1 : 0)
@@ -111,12 +115,6 @@ enum mehfet_resettap_status mehfet_hw_reset_tap(enum mehfet_resettap_flags flags
         }
     }
 
-    if (flags & mehfet_rsttap_highspeed) {
-        sbw_set_freq(true, 350e3);
-    } else {
-        sbw_set_freq(false, 50e3);
-    }
-
     return rv;
 }
 
diff --git a/src/m_isp/mehfet.c b/src/m_isp/mehfet.c
index a439157..59df6d5 100644
--- a/src/m_isp/mehfet.c
+++ b/src/m_isp/mehfet.c
@@ -47,19 +47,19 @@ static uint8_t read_byte(void) {
     rxpos = 0;
 
     // empty tinyusb internal buffer
-    if (tud_vendor_n_mounted(VND_N_CFG)) {
-        while (tud_vendor_n_available(VND_N_CFG)) {
-            tud_vendor_n_read(VND_N_CFG, rx_buf, sizeof rx_buf);
+    if (tud_vendor_n_mounted(VND_N_MEHFET)) {
+        while (tud_vendor_n_available(VND_N_MEHFET)) {
+            tud_vendor_n_read(VND_N_MEHFET, rx_buf, sizeof rx_buf);
         }
     }
 }*/
 static void write_flush(void) {
     // TODO: is this needed?
-    while (tud_vendor_n_write_available(VND_N_CFG) < txpos) {
+    while (tud_vendor_n_write_available(VND_N_MEHFET) < txpos) {
         thread_yield();
     }
 
-    tud_vendor_n_write(VND_N_CFG, tx_buf, txpos);
+    tud_vendor_n_write(VND_N_MEHFET, tx_buf, txpos);
     txpos = 0;
 }
 static void write_byte(uint8_t v) {
@@ -79,15 +79,19 @@ static struct cmdlen read_cmd_len(void) {
             lastbyte = cmd;
     uint32_t l = 0;
 
+    //printf("cmd=%02x\n", cmd);
+
     for (size_t i = 0; (i < 4) && (lastbyte & 0x80); ++i) {
         lastbyte = read_byte();
+        //printf("lenbyte=%02x\n");
 
         uint8_t mask = (i == 3) ? 0xff : 0x7f;
         l |= (lastbyte & mask) << (i * 7);
     }
 
+    //printf("len=0x%x\n");
     plpos = 0;
-    return (struct cmdlen){ .len = l, .cmd = cmd };
+    return (struct cmdlen){ .len = l, .cmd = cmd & 0x7f };
 }
 
 static inline uint8_t read_pl(void) {
@@ -153,6 +157,7 @@ void mehfet_task(void) {
 
     switch (cmdhdr.cmd) {
     case mehfet_info:
+        //printf("in info cmd\n");
         if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Info takes no parameters");
         else {
             // TODO: add flag once Loop has been implemented
@@ -179,6 +184,7 @@ void mehfet_task(void) {
 
             memcpy(&buf[8], name, bufsize - 8);
 
+            //printf("infocmd done\n");
             write_resp(mehfet_ok, bufsize, buf);
         }
         break;
@@ -270,11 +276,11 @@ void mehfet_task(void) {
             write_resp(mehfet_ok, 0, NULL);
         }
         break;
-    case mehfet_reset_target:
-        if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "ResetTarget takes no parameters");
+    case mehfet_set_clkspeed:
+        if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "SetClkSpeed takes one parameter byte");
         else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
         else {
-            mehfet_hw_reset_target();
+            mehfet_hw_set_clkspeed(read_pl() != 0);
             write_resp(mehfet_ok, 0, NULL);
         }
         break;
diff --git a/src/m_isp/mehfet.h b/src/m_isp/mehfet.h
index 7572dc1..74b36f6 100644
--- a/src/m_isp/mehfet.h
+++ b/src/m_isp/mehfet.h
@@ -18,7 +18,7 @@ enum mehfet_cmd {
     mehfet_connect       = 0x03,
     mehfet_disconnect    = 0x04,
     mehfet_delay         = 0x05,
-    mehfet_reset_target  = 0x06,
+    mehfet_set_clkspeed  = 0x06,
     mehfet_get_old_lines = 0x07,
     mehfet_tdio_seq      = 0x08,
     mehfet_tms_seq       = 0x09,
@@ -90,7 +90,7 @@ void mehfet_hw_delay_us(uint32_t t);
 void mehfet_hw_timer_start(bool us, uint32_t to_reach);
 bool mehfet_hw_timer_reached(void);
 
-void mehfet_hw_reset_target(void);
+void mehfet_hw_set_clkspeed(bool fast);
 uint8_t mehfet_hw_get_old_lines(void);
 
 void mehfet_hw_tdio_seq(uint32_t ncyc, bool tmslvl, const uint8_t* tdi, uint8_t* tdo);