drivers: wifi: esp: rework +IPD handling

Split +IPD message handling into smaller logical functions, so it is
much easier to follow code and further improve it. Make sure to do as
much parsing work as possible, before accessing esp_socket object. This
will allow to introduce thread-safety locking just in the critical parts
in subsequent commits.

Prefer early return from function over goto to return instruction,
whenever cleanup code is not needed.

Signed-off-by: Marcin Niestroj <[email protected]>

Author: mniestroj

drivers/wifi/esp/esp.c

@@ -442,70 +442,101 @@ struct net_pkt *esp_prepare_pkt(struct esp_data *dev, struct net_buf *src,
  */
 #define MIN_IPD_LEN (sizeof("+IPD,I,0E") - 1)
 #define MAX_IPD_LEN (sizeof("+IPD,I,4294967295E") - 1)
-MODEM_CMD_DIRECT_DEFINE(on_cmd_ipd)
-{
-	char *endptr, end, ipd_buf[MAX_IPD_LEN + 1];
-	int data_offset, data_len, ret;
-	size_t match_len, frags_len;
-	struct esp_socket *sock;
-	struct esp_data *dev;
-	struct net_pkt *pkt;
-	uint8_t link_id;
 
-	dev = CONTAINER_OF(data, struct esp_data, cmd_handler_data);
+static int cmd_ipd_parse_hdr(struct net_buf *buf, uint16_t len,
+			     uint8_t *link_id,
+			     int *data_offset, int *data_len,
+			     char *end)
+{
+	char *endptr, ipd_buf[MAX_IPD_LEN + 1];
+	size_t frags_len;
+	size_t match_len;
 
-	frags_len = net_buf_frags_len(data->rx_buf);
+	frags_len = net_buf_frags_len(buf);
 
 	/* Wait until minimum cmd length is available */
 	if (frags_len < MIN_IPD_LEN) {
-		ret = -EAGAIN;
-		goto out;
+		return -EAGAIN;
 	}
 
 	match_len = net_buf_linearize(ipd_buf, MAX_IPD_LEN,
-				      data->rx_buf, 0, MAX_IPD_LEN);
+				      buf, 0, MAX_IPD_LEN);
 
 	ipd_buf[match_len] = 0;
 	if (ipd_buf[len] != ',' || ipd_buf[len + 2] != ',') {
 		LOG_ERR("Invalid IPD: %s", log_strdup(ipd_buf));
-		ret = len;
-		goto out;
+		return -EBADMSG;
 	}
 
-	link_id = ipd_buf[len + 1] - '0';
-	sock = esp_socket_from_link_id(dev, link_id);
-	if (sock == NULL) {
-		LOG_ERR("No socket for link %d", link_id);
-		ret = len;
-		goto out;
+	*link_id = ipd_buf[len + 1] - '0';
+
+	*data_len = strtol(&ipd_buf[len + 3], &endptr, 10);
+
+	if (endptr == &ipd_buf[len + 3] ||
+	    (*endptr == 0 && match_len >= MAX_IPD_LEN)) {
+		LOG_ERR("Invalid IPD len: %s", log_strdup(ipd_buf));
+		return -EBADMSG;
+	} else if (*endptr == 0) {
+		return -EAGAIN;
 	}
 
+	*end = *endptr;
+	*data_offset = (endptr - ipd_buf) + 1;
+
+	return 0;
+}
+
+static int cmd_ipd_check_hdr_end(struct esp_socket *sock, char actual)
+{
+	char expected;
+
 	/* When using passive mode, the +IPD command ends with \r\n */
 	if (ESP_PROTO_PASSIVE(sock->ip_proto)) {
-		end = '\r';
+		expected = '\r';
 	} else {
-		end = ':';
+		expected = ':';
 	}
 
-	data_len = strtol(&ipd_buf[len + 3], &endptr, 10);
-	if (endptr == &ipd_buf[len + 3] ||
-	    (*endptr == 0 && match_len >= MAX_IPD_LEN)) {
-		/* Invalid */
-		LOG_ERR("Invalid IPD len: %s", log_strdup(ipd_buf));
-		ret = len;
-		goto out;
-	} else if (*endptr == 0) {
-		ret = -EAGAIN;
-		goto out;
-	} else if (*endptr != end) {
-		LOG_ERR("Invalid cmd end 0x%02x, expected 0x%02x", *endptr,
-			end);
-		ret = len;
-		goto out;
+	if (expected != actual) {
+		LOG_ERR("Invalid cmd end 0x%02x, expected 0x%02x", actual,
+			expected);
+		return -EBADMSG;
 	}
 
-	*endptr = 0;
-	data_offset = strlen(ipd_buf) + 1;
+	return 0;
+}
+
+MODEM_CMD_DIRECT_DEFINE(on_cmd_ipd)
+{
+	struct esp_data *dev = CONTAINER_OF(data, struct esp_data,
+					    cmd_handler_data);
+	struct esp_socket *sock;
+	int data_offset, data_len;
+	struct net_pkt *pkt;
+	uint8_t link_id;
+	char cmd_end;
+	int err;
+
+	err = cmd_ipd_parse_hdr(data->rx_buf, len, &link_id, &data_offset,
+				&data_len, &cmd_end);
+	if (err) {
+		if (err == -EAGAIN) {
+			return -EAGAIN;
+		}
+
+		return len;
+	}
+
+	sock = esp_socket_from_link_id(dev, link_id);
+	if (!sock) {
+		LOG_ERR("No socket for link %d", link_id);
+		return len;
+	}
+
+	err = cmd_ipd_check_hdr_end(sock, cmd_end);
+	if (err) {
+		return len;
+	}
 
 	/*
 	 * When using passive TCP, the data itself is not included in the +IPD
@@ -514,22 +545,18 @@ MODEM_CMD_DIRECT_DEFINE(on_cmd_ipd)
 	if (ESP_PROTO_PASSIVE(sock->ip_proto)) {
 		sock->bytes_avail = data_len;
 		k_work_submit_to_queue(&dev->workq, &sock->recvdata_work);
-		ret = data_offset;
-		goto out;
+		return data_offset;
 	}
 
 	/* Do we have the whole message? */
-	if (data_offset + data_len > frags_len) {
-		ret = -EAGAIN;
-		goto out;
+	if (data_offset + data_len > net_buf_frags_len(data->rx_buf)) {
+		return -EAGAIN;
 	}
 
-	ret = data_offset + data_len; /* Skip */
-
 	if ((sock->flags & (ESP_SOCK_CONNECTED | ESP_SOCK_CLOSE_PENDING)) !=
 							ESP_SOCK_CONNECTED) {
-		LOG_DBG("Received data on closed link %d", link_id);
-		goto out;
+		LOG_DBG("Received data on closed link %d", sock->link_id);
+		return data_offset + data_len;
 	}
 
 	pkt = esp_prepare_pkt(dev, data->rx_buf, data_offset, data_len);
@@ -546,8 +573,7 @@ MODEM_CMD_DIRECT_DEFINE(on_cmd_ipd)
 submit_work:
 	k_work_submit_to_queue(&dev->workq, &sock->recv_work);
 
-out:
-	return ret;
+	return data_offset + data_len;
 }
 
 MODEM_CMD_DEFINE(on_cmd_busy_sending)