sccp_node: First implementation of wrap_udt...

Given a header from split_udt and a payload a new udt
messages will be built and returned to the caller.

1 files changed, 57 insertions, 4 deletions

diff --git a/src/sccp_node.c b/src/sccp_node.c
index 59b673f..848aa2d 100644
--- a/src/sccp_node.c
+++ b/src/sccp_node.c
@@ -2,6 +2,7 @@
 
 #include <osmocore/msgb.h>
 #include <sccp/sccp.h>
+#include <sccp/sccp_types.h>
 
 #include <stdio.h>
 #include <sys/types.h>
@@ -13,13 +14,16 @@
 #include "erl_interface.h"
 #include "ei.h"
 
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+
 struct osmo_erl_addr {
 	uint8_t sccp_ssn;
 	int use_poi;
 	uint8_t poi[2];
 
-	int gti_len;
+	uint8_t gti_len;
 	uint8_t gti[32];
+	uint8_t gti_ind;
 } __attribute__((packed));
 
 struct osmo_erl_udt {
@@ -27,7 +31,7 @@ struct osmo_erl_udt {
 	struct osmo_erl_addr calling;
 } __attribute__((packed));
 
-static void convert_addr(struct osmo_erl_addr *erl_addr, struct sccp_address *addr)
+static void convert_addr(struct osmo_erl_addr *erl_addr, const struct sccp_address *addr)
 {
 	if (addr->gti_len > sizeof(erl_addr->gti)) {
 		printf("FAILED to copy the GTI... increase the size limit.\n");
@@ -41,9 +45,26 @@ static void convert_addr(struct osmo_erl_addr *erl_addr, struct sccp_address *ad
 	erl_addr->poi[1] = addr->poi[1];
 
 	erl_addr->gti_len = addr->gti_len;
+	erl_addr->gti_ind = addr->address.global_title_indicator;
 	memcpy(&erl_addr->gti, addr->gti_data, addr->gti_len);
 }
 
+static void copy_addr(struct sockaddr_sccp *addr, const struct osmo_erl_addr *sock)
+{
+	memset(addr, 0, sizeof(*addr));
+
+	addr->sccp_family = 0;
+	addr->sccp_ssn = sock->sccp_ssn;
+
+	addr->use_poi = sock->use_poi;
+	addr->poi[0] = sock->poi[0];
+	addr->poi[1] = sock->poi[1];
+
+	addr->gti = (uint8_t *) &sock->gti[0];
+	addr->gti_len = MIN(sock->gti_len, sizeof(sock->gti));
+	addr->gti_ind = sock->gti_ind;
+}
+
 /*
  * Split the SCCP UDT message into header and data... We assume
  * that ETERM is binary data, we will copy it to to a msgb, then
@@ -106,11 +127,43 @@ send_empty:
  */
 static int wrap_udt(int fd, ETERM *fromp, ETERM *argp)
 {
-	ETERM *resp;
+	struct msgb *msg;
+	struct sockaddr_sccp called_sock, calling_sock;
+	const struct osmo_erl_udt *sccp_hdr;
+	ETERM *resp, *data;
+
+	ETERM *hdr, *payload;
 
-	resp = erl_format("{wrap_udt, ~i}", 10);
+	hdr = erl_element(1, argp);
+	if (ERL_BIN_SIZE(hdr) != sizeof(struct osmo_erl_udt)) {
+		printf("The header should have the right size...\n");
+		erl_free_term(hdr);
+		return -1;
+	}
+
+	sccp_hdr = (const struct osmo_erl_udt *) ERL_BIN_PTR(hdr);
+	copy_addr(&called_sock, &sccp_hdr->called);
+	copy_addr(&calling_sock, &sccp_hdr->calling);
+	payload = erl_element(2, argp);
+
+	msg = sccp_create_udt(0, &calling_sock, &called_sock,
+			      ERL_BIN_PTR(payload), ERL_BIN_SIZE(payload));
+	if (!msg) {
+		printf("Failed to create a UDT packet...\n");
+		goto free_data;
+	}
+
+	data = erl_mk_binary((const char * )msg->l2h, msgb_l2len(msg));
+	resp = erl_format("{wrap_udt, ~w}", data);
 	erl_send(fd, fromp, resp);
+
+free_data:
 	erl_free_term(resp);
+	erl_free_term(data);
+	erl_free_term(hdr);
+	erl_free_term(payload);
+	if (msg)
+		msgb_free(msg);
 	return 0;
 }