diff --git a/cpp/hal/gpiobus.cpp b/cpp/hal/gpiobus.cpp
index 1f467bfa..4b4ead70 100644
--- a/cpp/hal/gpiobus.cpp
+++ b/cpp/hal/gpiobus.cpp
@@ -38,11 +38,10 @@ bool GPIOBUS::Init(mode_e mode)
 //---------------------------------------------------------------------------
 int GPIOBUS::CommandHandShake(vector<uint8_t> &buf)
 {
-    GPIO_FUNCTION_TRACE
     // Only works in TARGET mode
-    if (actmode != mode_e::TARGET) {
-        return 0;
-    }
+	assert(actmode == mode_e::TARGET);
+
+	GPIO_FUNCTION_TRACE
 
     DisableIRQ();
 
@@ -321,12 +320,6 @@ int GPIOBUS::SendHandShake(uint8_t *buf, int count, int delay_after_bytes)
         phase_t phase = GetPhase();
 
         for (i = 0; i < count; i++) {
-            if (i == delay_after_bytes) {
-                spdlog::trace("DELAYING for " + to_string(SCSI_DELAY_SEND_DATA_DAYNAPORT_US) + " after " +
-                		to_string(delay_after_bytes) + " bytes");
-                SysTimer::SleepUsec(SCSI_DELAY_SEND_DATA_DAYNAPORT_US);
-            }
-
             // Set the DATA signals
             SetDAT(*buf);
 
@@ -338,6 +331,11 @@ int GPIOBUS::SendHandShake(uint8_t *buf, int count, int delay_after_bytes)
                 break;
             }
 
+           	// Signal the last MESSAGE OUT byte
+            if (phase == phase_t::msgout && i == count - 1) {
+            	SetATN(false);
+            }
+
             // Phase error
             Acquire();
             if (GetPhase() != phase) {
@@ -389,7 +387,6 @@ bool GPIOBUS::PollSelectEvent()
     return false;
 #else
     GPIO_FUNCTION_TRACE
-    spdlog::trace(__PRETTY_FUNCTION__);
     errno = 0;
 
     if (epoll_event epev; epoll_wait(epfd, &epev, 1, -1) <= 0) {