diff --git a/drivers/char/diag/diagfwd_glink.c b/drivers/char/diag/diagfwd_glink.c
index fea1b74aacae..a2ffabe43c86 100644
--- a/drivers/char/diag/diagfwd_glink.c
+++ b/drivers/char/diag/diagfwd_glink.c
@@ -413,19 +413,16 @@ static int  diag_glink_write(void *ctxt, unsigned char *buf, int len)
 		return -ENODEV;
 	}
 
-	err = wait_event_interruptible(glink_info->wait_q,
-				   atomic_read(&glink_info->tx_intent_ready));
-	if (err) {
-		diagfwd_write_buffer_done(glink_info->fwd_ctxt, buf);
-		return -ERESTARTSYS;
-	}
-
-	atomic_dec(&glink_info->tx_intent_ready);
-	err = glink_tx(glink_info->hdl, glink_info, buf, len, tx_flags);
-	if (!err) {
-		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to glink, len: %d\n",
-				 glink_info->name, len);
-	}
+	if (atomic_read(&glink_info->tx_intent_ready)) {
+		atomic_dec(&glink_info->tx_intent_ready);
+		err = glink_tx(glink_info->hdl, glink_info, buf, len, tx_flags);
+		if (!err) {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+				"%s wrote to glink, len: %d\n",
+				glink_info->name, len);
+		}
+	} else
+		err = -ENOMEM;
 
 	return err;
 
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 066890aebf39..22b9e05086bd 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -751,7 +751,9 @@ int diagfwd_write(uint8_t peripheral, uint8_t type, void *buf, int len)
 
 	if (!err)
 		fwd_info->write_bytes += len;
-
+	else
+		if (fwd_info->transport == TRANSPORT_GLINK)
+			diagfwd_write_buffer_done(fwd_info, buf_ptr);
 	return err;
 }