diff --git a/libfreerdp/core/connection.c b/libfreerdp/core/connection.c
index 266a52935..cdbeb1c3b 100644
--- a/libfreerdp/core/connection.c
+++ b/libfreerdp/core/connection.c
@@ -731,7 +731,8 @@ static BOOL rdp_client_establish_keys(rdpRdp* rdp)
 		goto end;
 	}
 
-	rdp_write_header(rdp, s, length, MCS_GLOBAL_CHANNEL_ID);
+	if (!rdp_write_header(rdp, s, length, MCS_GLOBAL_CHANNEL_ID))
+		goto end;
 	rdp_write_security_header(s, SEC_EXCHANGE_PKT | SEC_LICENSE_ENCRYPT_SC);
 	length = key_len + 8;
 	Stream_Write_UINT32(s, length);
@@ -1042,6 +1043,9 @@ BOOL rdp_client_connect_auto_detect(rdpRdp* rdp, wStream* s)
 	UINT16 length;
 	UINT16 channelId;
 
+	WINPR_ASSERT(rdp);
+	WINPR_ASSERT(rdp->mcs);
+
 	/* If the MCS message channel has been joined... */
 	if (rdp->mcs->messageChannelId != 0)
 	{
@@ -1097,6 +1101,7 @@ int rdp_client_connect_license(rdpRdp* rdp, wStream* s)
 
 	if ((securityFlags & SEC_LICENSE_PKT) == 0)
 		return -1;
+
 	status = license_recv(rdp->license, s);
 
 	if (status < 0)
diff --git a/libfreerdp/core/rdp.c b/libfreerdp/core/rdp.c
index 6946c4ab3..8017d3a54 100644
--- a/libfreerdp/core/rdp.c
+++ b/libfreerdp/core/rdp.c
@@ -564,14 +564,14 @@ BOOL rdp_read_header(rdpRdp* rdp, wStream* s, UINT16* length, UINT16* channelId)
  * @param channel_id channel id
  */
 
-void rdp_write_header(rdpRdp* rdp, wStream* s, UINT16 length, UINT16 channelId)
+BOOL rdp_write_header(rdpRdp* rdp, wStream* s, UINT16 length, UINT16 channelId)
 {
-	int body_length;
 	DomainMCSPDU MCSPDU;
 
 	WINPR_ASSERT(rdp);
 	WINPR_ASSERT(rdp->settings);
 	WINPR_ASSERT(s);
+	WINPR_ASSERT(length >= RDP_PACKET_HEADER_MAX_LENGTH);
 
 	MCSPDU = (rdp->settings->ServerMode) ? DomainMCSPDU_SendDataIndication
 	                                     : DomainMCSPDU_SendDataRequest;
@@ -579,18 +579,22 @@ void rdp_write_header(rdpRdp* rdp, wStream* s, UINT16 length, UINT16 channelId)
 	if ((rdp->sec_flags & SEC_ENCRYPT) &&
 	    (rdp->settings->EncryptionMethods == ENCRYPTION_METHOD_FIPS))
 	{
-		int pad;
-		body_length = length - RDP_PACKET_HEADER_MAX_LENGTH - 16;
-		pad = 8 - (body_length % 8);
+		const UINT16 body_length = length - RDP_PACKET_HEADER_MAX_LENGTH;
+		const UINT16 pad = 8 - (body_length % 8);
 
 		if (pad != 8)
 			length += pad;
 	}
 
-	mcs_write_domain_mcspdu_header(s, MCSPDU, length, 0);
-	per_write_integer16(s, rdp->mcs->userId, MCS_BASE_CHANNEL_ID); /* initiator */
-	per_write_integer16(s, channelId, 0);                          /* channelId */
-	Stream_Write_UINT8(s, 0x70);                                   /* dataPriority + segmentation */
+	if (!mcs_write_domain_mcspdu_header(s, MCSPDU, length, 0))
+		return FALSE;
+	if (!per_write_integer16(s, rdp->mcs->userId, MCS_BASE_CHANNEL_ID)) /* initiator */
+		return FALSE;
+	if (!per_write_integer16(s, channelId, 0)) /* channelId */
+		return FALSE;
+	if (!Stream_EnsureRemainingCapacity(s, 3))
+		return FALSE;
+	Stream_Write_UINT8(s, 0x70); /* dataPriority + segmentation */
 	/*
 	 * We always encode length in two bytes, even though we could use
 	 * only one byte if length <= 0x7F. It is just easier that way,
@@ -599,6 +603,7 @@ void rdp_write_header(rdpRdp* rdp, wStream* s, UINT16 length, UINT16 channelId)
 	 */
 	length = (length - RDP_PACKET_HEADER_MAX_LENGTH) | 0x8000;
 	Stream_Write_UINT16_BE(s, length); /* userData (OCTET_STRING) */
+	return TRUE;
 }
 
 static BOOL rdp_security_stream_out(rdpRdp* rdp, wStream* s, int length, UINT32 sec_flags,
@@ -811,11 +816,8 @@ BOOL rdp_send_message_channel_pdu(rdpRdp* rdp, wStream* s, UINT16 sec_flags)
 	UINT16 length;
 	UINT32 pad;
 
-	if (!s)
-		return FALSE;
-
-	if (!rdp)
-		goto fail;
+	WINPR_ASSERT(rdp);
+	WINPR_ASSERT(s);
 
 	length = Stream_GetPosition(s);
 	Stream_SetPosition(s, 0);
@@ -1044,8 +1046,7 @@ int rdp_recv_data_pdu(rdpRdp* rdp, wStream* s)
 		case DATA_PDU_TYPE_SYNCHRONIZE:
 			if (!rdp_recv_server_synchronize_pdu(rdp, cs))
 			{
-				WLog_ERR(TAG,
-				         "DATA_PDU_TYPE_SYNCHRONIZE - rdp_recv_server_synchronize_pdu() failed");
+				WLog_ERR(TAG, "DATA_PDU_TYPE_SYNCHRONIZE - rdp_recv_synchronize_pdu() failed");
 				goto out_fail;
 			}
 
@@ -2099,9 +2100,7 @@ BOOL rdp_reset(rdpRdp* rdp)
 		goto fail;
 
 	rdp->errorInfo = 0;
-	rdp_finalize_reset_flags(rdp, TRUE);
-
-	rc = TRUE;
+	rc = rdp_finalize_reset_flags(rdp, TRUE);
 
 fail:
 	return rc;
diff --git a/libfreerdp/core/rdp.h b/libfreerdp/core/rdp.h
index 114b0732c..11ae4e69c 100644
--- a/libfreerdp/core/rdp.h
+++ b/libfreerdp/core/rdp.h
@@ -219,7 +219,7 @@ FREERDP_LOCAL wStream* rdp_send_stream_init(rdpRdp* rdp);
 FREERDP_LOCAL wStream* rdp_send_stream_pdu_init(rdpRdp* rdp);
 
 FREERDP_LOCAL BOOL rdp_read_header(rdpRdp* rdp, wStream* s, UINT16* length, UINT16* channel_id);
-FREERDP_LOCAL void rdp_write_header(rdpRdp* rdp, wStream* s, UINT16 length, UINT16 channel_id);
+FREERDP_LOCAL BOOL rdp_write_header(rdpRdp* rdp, wStream* s, UINT16 length, UINT16 channel_id);
 
 FREERDP_LOCAL BOOL rdp_send_pdu(rdpRdp* rdp, wStream* s, UINT16 type, UINT16 channel_id);