csp_rdp_queue_rx_add return success or error

src/csp_rdp.c

@@ -201,7 +201,9 @@ static int csp_rdp_send_eack(csp_conn_t * conn) {
 		}
 
 		/* Requeue */
-		csp_rdp_queue_rx_add(conn, packet);
+		if (csp_rdp_queue_rx_add(conn, packet) != 0){
+			csp_rdp_error("RDP %p: Packet lost in csp_rdp_send_eack \n", conn);
+		}
 	}
 
 	return csp_rdp_send_cmp(conn, packet_eack, RDP_ACK | RDP_EAK, conn->rdp.snd_nxt, conn->rdp.rcv_cur);
@@ -282,7 +284,9 @@ static inline void csp_rdp_rx_queue_flush(csp_conn_t * conn) {
 
 			/* Otherwise, requeue */
 		} else {
-			csp_rdp_queue_rx_add(conn, packet);
+			if (csp_rdp_queue_rx_add(conn, packet) != 0){
+				csp_rdp_error("RDP %p: Packet lost in csp_rdp_rx_queue_flush \n", conn);
+			}
 		}
 	}
 }
@@ -300,7 +304,9 @@ static inline bool csp_rdp_seq_in_rx_queue(csp_conn_t * conn, uint16_t seq_nr) {
 			break;
 		}
 
-		csp_rdp_queue_rx_add(conn, packet);
+		if (csp_rdp_queue_rx_add(conn, packet) != 0){
+			csp_rdp_error("RDP %p: Packet lost in csp_rdp_seq_in_rx_queue \n", conn);
+		}
 
 		rdp_header_t * header = csp_rdp_header_ref((csp_packet_t *)packet);
 		csp_rdp_protocol("RDP %p: RX Queue exists matching Element, seq %u\n", conn, header->seq_nr);
@@ -317,10 +323,11 @@ static inline bool csp_rdp_seq_in_rx_queue(csp_conn_t * conn, uint16_t seq_nr) {
 
 static inline int csp_rdp_rx_queue_add(csp_conn_t * conn, csp_packet_t * packet, uint16_t seq_nr) {
 
-	if (csp_rdp_seq_in_rx_queue(conn, seq_nr))
+	if (csp_rdp_seq_in_rx_queue(conn, seq_nr)){
+		csp_rdp_protocol("RDP %p: Duplicate sequence number\n", conn);
 		return -1;
-	csp_rdp_queue_rx_add(conn, packet);
-	return 0;
+	}
+	return csp_rdp_queue_rx_add(conn, packet);
 }
 
 static void csp_rdp_flush_eack(csp_conn_t * conn, csp_packet_t * eack_packet) {
@@ -735,8 +742,8 @@ bool csp_rdp_new_packet(csp_conn_t * conn, csp_packet_t * packet) {
 
 			/* If message is not in sequence, send EACK and store packet */
 			if (rx_header->seq_nr != (uint16_t)(conn->rdp.rcv_cur + 1)) {
-				if (csp_rdp_rx_queue_add(conn, packet, rx_header->seq_nr) != 0) {
-					csp_rdp_protocol("RDP %p: Duplicate sequence number\n", conn);
+				if (csp_rdp_rx_queue_add(conn, packet, rx_header->seq_nr) != 0) {					
+					csp_rdp_protocol("RDP %p: Fail to add packet to RX queue\n", conn);
 					csp_rdp_check_ack(conn);
 					goto discard_open;
 				}

src/csp_rdp_queue.c

@@ -104,11 +104,13 @@ int csp_rdp_queue_rx_size(void) {
     return csp_queue_size(rx_queue);
 }
 
-void csp_rdp_queue_rx_add(csp_conn_t * conn, csp_packet_t * packet) {
+int csp_rdp_queue_rx_add(csp_conn_t * conn, csp_packet_t * packet) {
     packet->conn = conn;
     if (csp_queue_enqueue(rx_queue, &packet, 0) != CSP_QUEUE_OK) {
         csp_buffer_free(packet);
+        return -1;
     }
+    return 0;
 }
 
 csp_packet_t * csp_rdp_queue_rx_get(csp_conn_t * conn) {
@@ -122,7 +124,9 @@ csp_packet_t * csp_rdp_queue_rx_get(csp_conn_t * conn) {
         if (packet->conn == conn) {
             return packet;
         } else {
-            csp_rdp_queue_rx_add(conn, packet);
+            if (csp_rdp_queue_rx_add(conn, packet) != 0){
+			    csp_rdp_error("RDP %p: Packet lost in csp_rdp_queue_rx_get \n", conn);
+		    }
         }
     }
     return NULL;

src/csp_rdp_queue.h

@@ -10,5 +10,5 @@ void csp_rdp_queue_tx_add(csp_conn_t * conn, csp_packet_t * packet);
 csp_packet_t * csp_rdp_queue_tx_get(csp_conn_t * conn);
 
 int csp_rdp_queue_rx_size(void);
-void csp_rdp_queue_rx_add(csp_conn_t * conn, csp_packet_t * packet);
+int csp_rdp_queue_rx_add(csp_conn_t * conn, csp_packet_t * packet);
 csp_packet_t * csp_rdp_queue_rx_get(csp_conn_t * conn);