diff --git a/usb.v b/usb.v
index 765d300..76d332f 100644
--- a/usb.v
+++ b/usb.v
@@ -171,10 +171,12 @@ always @(posedge clk_48 or negedge rst_n) begin
                     if (recv_pid[1:0] == pt_token) begin
                         state <= st_data;
                     end else begin
-                        if (recv_pid[1:0] == pt_data && !recv_pid[2] && token_active)
+                        if (recv_pid[1:0] == pt_data && !recv_pid[2] && token_active) begin
+                            handshake_latch <= handshake;
                             state <= recv_pid[3] == data_toggle? st_data: st_send_ack;
-                        else
+                        end else begin
                             state <= st_err;
+                        end
                     end
                 end
             end
@@ -211,9 +213,8 @@ always @(posedge clk_48 or negedge rst_n) begin
                         end
                         pt_data: begin
                             if (recv_queue_1_valid && recv_crc16_ok) begin
-                                if (handshake == hs_ack || handshake == hs_none)
+                                if (handshake_latch == hs_ack || handshake_latch == hs_none)
                                     success <= 1'b1;
-                                handshake_latch <= handshake;
                                 state <= st_send_handshake;
                             end
                         end
@@ -230,7 +231,7 @@ always @(posedge clk_48 or negedge rst_n) begin
                                 state <= st_err;
                         end
                         pt_data: begin
-                            if (recv_queue_1_valid)
+                            if (recv_queue_1_valid && (handshake_latch == hs_ack || handshake_latch == hs_none))
                                 data_strobe <= 1'b1;
                         end
                         default: begin
diff --git a/usb_ep.v b/usb_ep.v
index 5397916..967013e 100644
--- a/usb_ep.v
+++ b/usb_ep.v
@@ -14,7 +14,7 @@ module usb_ep(
     input ctrl_dir_in,
     output reg[15:0] ctrl_rd_data,
     input[15:0] ctrl_wr_data,
-    input ctrl_wr_strobe
+    input[1:0] ctrl_wr_en
     );
 
 localparam
@@ -89,20 +89,23 @@ always @(posedge clk) begin
         end
     end
 
-    if (ctrl_wr_strobe && ctrl_dir_in) begin
+    if (ctrl_wr_en[1] && ctrl_dir_in) begin
         ep_in_cnt <= ctrl_wr_data[14:8];
+    end
+
+    if (ctrl_wr_en[0] && ctrl_dir_in) begin
         if (ctrl_wr_data[7])
             ep_in_toggle <= 1'b0;
         if (ctrl_wr_data[6])
             ep_in_toggle <= 1'b1;
         ep_in_stall <= ctrl_wr_data[4];
-        if (ctrl_wr_data[15] || ctrl_wr_data[1])
+        if (ctrl_wr_data[1])
             ep_in_full <= 1'b0;
-        if (ctrl_wr_data[14] || ctrl_wr_data[0])
+        if (ctrl_wr_data[0])
             ep_in_full <= 1'b1;
     end
 
-    if (ctrl_wr_strobe && !ctrl_dir_in) begin
+    if (ctrl_wr_en[0] && !ctrl_dir_in) begin
         if (ctrl_wr_data[7])
             ep_out_toggle <= 1'b0;
         if (ctrl_wr_data[6])
@@ -110,9 +113,9 @@ always @(posedge clk) begin
         ep_out_stall <= ctrl_wr_data[4];
         if (ctrl_wr_data[3])
             ep_setup <= 1'b0;
-        if (ctrl_wr_data[15] || ctrl_wr_data[1])
+        if (ctrl_wr_data[1])
             ep_out_full <= 1'b0;
-        if (ctrl_wr_data[14] || ctrl_wr_data[0])
+        if (ctrl_wr_data[0])
             ep_out_full <= 1'b1;
     end
 end