1 files changed, 218 insertions, 0 deletions

diff --git a/src/host/layer23/src/common/sap_proto.c b/src/host/layer23/src/common/sap_proto.c
index ef4cb350..c3d202f6 100644
--- a/src/host/layer23/src/common/sap_proto.c
+++ b/src/host/layer23/src/common/sap_proto.c
@@ -23,9 +23,19 @@
  *
  */
 
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include <arpa/inet.h>
+
+#include <osmocom/core/logging.h>
 #include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
 
 #include <osmocom/bb/common/sap_proto.h>
+#include <osmocom/bb/common/logging.h>
 
 /* Table 5.1: Message Overview */
 const struct value_string sap_msg_names[] = {
@@ -102,3 +112,211 @@ const struct value_string sap_conn_status_names[] = {
 	{ SAP_CONN_STATUS_OK_CALL,		"OK, ongoing call" },
 	{ 0, NULL }
 };
+
+/*! Allocate a new message buffer with SAP message header.
+ * \param[in] msg_id SAP message identifier
+ * \returns message buffer in case of success, NULL otherwise
+ */
+struct msgb *sap_msgb_alloc(uint8_t msg_id)
+{
+	struct sap_message *sap_msg;
+	struct msgb *msg;
+
+	msg = msgb_alloc(GSM_SAP_LENGTH, "sap_msg");
+	if (!msg) {
+		LOGP(DSAP, LOGL_ERROR, "Failed to allocate SAP message\n");
+		return NULL;
+	}
+
+	sap_msg = (struct sap_message *) msgb_put(msg, sizeof(*sap_msg));
+	sap_msg->msg_id = msg_id;
+
+	return msg;
+}
+
+/*! Add a new parameter to a given SAP message buffer.
+ *  Padding is added automatically, SAP message header
+ *  (number of parameters) is also updated automatically.
+ * \param[in] msg SAP message buffer
+ * \param[in] param_type parameter type (see sap_param_type enum)
+ * \param[in] param_len parameter length
+ * \param[in] param_value pointer to parameter value
+ */
+void sap_msgb_add_param(struct msgb *msg,
+	enum sap_param_type param_type,
+	uint16_t param_len, const uint8_t *param_value)
+{
+	struct sap_message *sap_msg;
+	struct sap_param *param;
+	uint8_t padding;
+	uint8_t *buf;
+
+	/* Update number of parameters */
+	sap_msg = (struct sap_message *) msg->data;
+	sap_msg->num_params++;
+
+	/* Allocate a new parameter */
+	param = (struct sap_param *) msgb_put(msg, sizeof(*param));
+	param->param_id = param_type;
+	param->reserved[0] = 0x00;
+
+	/* Encode parameter value and length */
+	param->length = htons(param_len);
+	buf = msgb_put(msg, param_len);
+	memcpy(buf, param_value, param_len);
+
+	/* Optional padding */
+	padding = 4 - (param_len % 4);
+	if (padding) {
+		buf = msgb_put(msg, padding);
+		memset(buf, 0x00, padding);
+	}
+}
+
+/*! Attempt to find a given parameter in a given SAP message.
+ * \param[in] sap_msg pointer to SAP message header
+ * \param[in] param_type parameter type (see sap_param_type enum)
+ * \param[out] param_len parameter length (if found)
+ * \returns pointer to a given parameter withing the message, NULL otherwise
+ */
+struct sap_param *sap_get_param(const struct sap_message *sap_msg,
+	enum sap_param_type param_type, uint16_t *param_len)
+{
+	const uint8_t *ptr = sap_msg->payload;
+	struct sap_param *param = NULL;
+	uint16_t plen;
+	int i;
+
+	/* We assume that message is parsed already,
+	 * so we don't check for buffer overflows */
+	for (i = 0; i < sap_msg->num_params; i++) {
+		/* Parse one parameter */
+		param = (struct sap_param *) ptr;
+		plen = ntohs(param->length);
+
+		/* Match against a given ID */
+		if (param->param_id == param_type) {
+			if (param_len != NULL)
+				*param_len = plen;
+			return param;
+		}
+
+		/* Shift pointer to the next parameter */
+		ptr += sizeof(*param) + plen;
+		/* Optional padding */
+		ptr += 4 - (plen % 4);
+	}
+
+	return NULL;
+}
+
+/*! Parse SAP message from a given buffer into a new message buffer.
+ * \param[in] buf pointer to a buffer with to be parsed message
+ * \param[in] buf_len length of the buffer
+ * \param[in] max_msg_size max (negotiated) message size
+ * \returns new message buffer with parsed message, NULL otherwise
+ */
+struct msgb *sap_msg_parse(const uint8_t *buf, size_t buf_len, int max_msg_size)
+{
+	const struct sap_message *sap_msg;
+	const uint8_t *ptr;
+	struct msgb *msg;
+	size_t len;
+	int i;
+
+	/* Message header is mandatory */
+	if (buf_len < sizeof(*sap_msg)) {
+		LOGP(DSAP, LOGL_ERROR, "Missing SAP message header\n");
+		return NULL;
+	}
+
+	/* MaxMsgSize limitation (optional) */
+	if (max_msg_size > 0 && buf_len > max_msg_size) {
+		LOGP(DSAP, LOGL_ERROR, "Buffer (len=%zu) is bigger than "
+			"given MaxMsgSize=%d\n", buf_len, max_msg_size);
+		return NULL;
+	}
+
+	sap_msg = (const struct sap_message *) buf;
+	len = buf_len - sizeof(*sap_msg);
+	ptr = sap_msg->payload;
+
+	LOGP(DSAP, LOGL_DEBUG, "SAP message '%s' has %u parameter(s)\n",
+		get_value_string(sap_msg_names, sap_msg->msg_id),
+		sap_msg->num_params);
+
+	for (i = 0; i < sap_msg->num_params; i++) {
+		struct sap_param *param;
+		uint16_t param_len;
+		uint16_t offset;
+
+		/* Prevent buffer overflow */
+		if (len < sizeof(*param))
+			goto malformed;
+
+		/* Parse one parameter */
+		param = (struct sap_param *) ptr;
+		param_len = ntohs(param->length);
+
+		LOGP(DSAP, LOGL_DEBUG, "SAP parameter '%s' (len=%u): %s\n",
+			get_value_string(sap_param_names, param->param_id),
+			param_len, osmo_hexdump(param->value, param_len));
+
+		/* Calculate relative offset */
+		offset  = sizeof(*param) + param_len;
+		offset += 4 - (param_len % 4); /* Optional padding */
+
+		/* Prevent buffer overflow */
+		if (offset > len)
+			goto malformed;
+
+		len -= offset;
+		ptr += offset;
+	}
+
+	/* Allocate a new message buffer */
+	msg = msgb_alloc(GSM_SAP_LENGTH, "sap_msg");
+	if (!msg) {
+		LOGP(DSAP, LOGL_ERROR, "Failed to allocate SAP message\n");
+		return NULL;
+	}
+
+	msg->data = msgb_put(msg, buf_len);
+	memcpy(msg->data, buf, buf_len);
+
+	return msg;
+
+malformed:
+	LOGP(DSAP, LOGL_ERROR, "Malformed SAP message "
+		"(parameter %i/%u)\n", i + 1, sap_msg->num_params);
+	return NULL;
+}
+
+/*! Parse ResultCode from a given SAP message.
+ * \param[in] sap_msg pointer to SAP message header
+ * \returns parsed ResultCode (if found), negative otherwise
+ */
+int sap_check_result_code(const struct sap_message *sap_msg)
+{
+	struct sap_param *param;
+	uint16_t param_len;
+	uint8_t res_code;
+
+	param = sap_get_param(sap_msg, SAP_RESULT_CODE, &param_len);
+	if (!param || param_len != sizeof(res_code)) {
+		LOGP(DSAP, LOGL_ERROR, "Missing mandatory '%s' parameter\n",
+			get_value_string(sap_param_names, SAP_RESULT_CODE));
+		return -EINVAL;
+	}
+
+	res_code = param->value[0];
+	if (res_code >= ARRAY_SIZE(sap_result_names)) {
+		LOGP(DSAP, LOGL_ERROR, "Unknown SAP ResultCode=0x%02x\n", res_code);
+		return -EINVAL;
+	}
+
+	LOGP(DSAP, LOGL_DEBUG, "SAP ResultCode is '%s'\n",
+		get_value_string(sap_result_names, res_code));
+
+	return res_code;
+}