qmi: wds: add utility to parse Data System Status tlv

2 files changed, 55 insertions, 0 deletions

diff --git a/drivers/qmimodem/wds.c b/drivers/qmimodem/wds.c
index d126f4712..77e22f443 100644
--- a/drivers/qmimodem/wds.c
+++ b/drivers/qmimodem/wds.c
@@ -5,6 +5,11 @@
  * SPDX-License-Identifier: LGPL-2.1-or-later
  */
 
+#include <stdint.h>
+#include <stddef.h>
+
+#include <ell/ell.h>
+
 #include "src/common.h"
 
 #include "wds.h"
@@ -37,3 +42,36 @@ int qmi_wds_pdp_type_from_ofono(enum ofono_gprs_proto proto)
 
 	return -ENOENT;
 }
+
+int qmi_wds_parse_data_system_status(const void *dss, uint16_t len)
+{
+	const size_t network_info_size = sizeof(uint8_t) + 2 * sizeof(uint32_t);
+	uint8_t num_networks;
+	uint8_t network;
+	uint32_t rat_mask;
+
+	if (len < 2 * sizeof(uint8_t))
+		return -EBADMSG;
+
+	/* uint8_t preferred network type followed by number of network infos */
+	num_networks = l_get_u8(dss + 1);
+
+	len -= 2 * sizeof(uint8_t);
+	dss += 2 * sizeof(uint8_t);
+
+	if (len != num_networks * network_info_size)
+		return -EBADMSG;
+
+	while (len >= network_info_size) {
+		network = l_get_u8(dss);
+		rat_mask = l_get_le32(dss + 1);
+
+		if (network == QMI_WDS_PROFILE_TYPE_3GPP)
+			return rat_mask;
+
+		len -= network_info_size;
+		dss += network_info_size;
+	}
+
+	return -ENOENT;
+}
diff --git a/drivers/qmimodem/wds.h b/drivers/qmimodem/wds.h
index d896fd8cc..94f1c028b 100644
--- a/drivers/qmimodem/wds.h
+++ b/drivers/qmimodem/wds.h
@@ -68,6 +68,21 @@ enum qmi_wds_profile_family {
 	QMI_WDS_PROFILE_FAMILY_TETHERED =	0x01,
 };
 
+enum qmi_wds_3gpp_rat {
+	QMI_WDS_3GPP_RAT_WCDMA			= 0x01,
+	QMI_WDS_RAT_3GPP_GPRS			= 0x02,
+	QMI_WDS_RAT_3GPP_HSDPA			= 0x04,
+	QMI_WDS_RAT_3GPP_HSUPA			= 0x08,
+	QMI_WDS_RAT_3GPP_EDGE			= 0x10,
+	QMI_WDS_RAT_3GPP_LTE			= 0x20,
+	QMI_WDS_RAT_3GPP_HSDPAPLUS		= 0x40,
+	QMI_WDS_RAT_3GPP_DCHSDPAPLUS		= 0x80,
+	QMI_WDS_RAT_3GPP_64QAM			= 0x100,
+	QMI_WDS_RAT_3GPP_TDSCDMA		= 0x200,
+	QMI_WDS_RAT_3GPP_5GNR			= 0x400,
+	QMI_WDS_RAT_3GPP_NULL_BEARER		= 0x8000,
+};
+
 enum qmi_wds_command {
 	QMI_WDS_RESET					= 0x00,
 	QMI_WDS_EVENT_REPORT				= 0x01,
@@ -111,3 +126,5 @@ enum qmi_wds_command {
 
 int qmi_wds_auth_from_ofono(enum ofono_gprs_auth_method method);
 int qmi_wds_pdp_type_from_ofono(enum ofono_gprs_proto proto);
+
+int qmi_wds_parse_data_system_status(const void *dss, uint16_t len);