osmo-map-masq/c_src/sccp_node.c

/* cnode_s.c */

#include <osmocore/msgb.h>
#include <osmocom/sccp/sccp.h>
#include <osmocom/sccp/sccp_types.h>

#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <unistd.h>

#include "erl_interface.h"
#include "ei.h"

#define MIN(a, b) ((a) < (b) ? (a) : (b))

#define UDT_SPLIT_CMD	"udt_split_cmd"
#define UDT_SPLIT_RES	"udt_split_res"
#define UDT_SPLIT_ERR	"udt_split_err"

#define UDT_WRAP_CMD	"udt_wrap_cmd"
#define UDT_WRAP_RES	"udt_wrap_res"
#define UDT_WRAP_ERR	"udt_wrap_err"

struct osmo_erl_addr {
	uint8_t sccp_ssn;
	int use_poi;
	uint8_t poi[2];

	uint8_t gti_len;
	uint8_t gti[32];
	uint8_t gti_ind;
} __attribute__((packed));

struct osmo_erl_udt {
	struct osmo_erl_addr called;
	struct osmo_erl_addr calling;
} __attribute__((packed));

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");
		return;
	}

	erl_addr->sccp_ssn = addr->ssn;

	erl_addr->use_poi = addr->address.point_code_indicator;
	erl_addr->poi[0] = addr->poi[0];
	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
 * let it parse, and then return the data and the header (as binary)
 * to the caller...
 */
static int split_udt(int fd, ETERM *fromp, ETERM *argp)
{
	struct msgb *msg = NULL;
	ETERM *resp, *hdr, *data;

	if (!ERL_IS_BINARY(argp)) {
		printf("Data is not binary.\n");
		return -1;
	}

	if (ERL_BIN_SIZE(argp) == 0) {
		printf("Empty input.\n");
		goto error;
	} else {
		struct sccp_parse_result result;
		struct osmo_erl_udt sccp_hdr;

		msg = msgb_alloc(ERL_BIN_SIZE(argp), "split udt");
		msg->l2h = msgb_put(msg, ERL_BIN_SIZE(argp));
		memcpy(msg->l2h, ERL_BIN_PTR(argp), msgb_l2len(msg));

		memset(&result, 0, sizeof(result));
		if (sccp_parse_header(msg, &result) == 0) {
			convert_addr(&sccp_hdr.called, &result.called);
			convert_addr(&sccp_hdr.calling, &result.calling);
			hdr = erl_mk_binary((const char *) &sccp_hdr, sizeof(sccp_hdr));
			data = erl_mk_binary((const char *) msg->l3h, result.data_len);
		} else {
			printf("Failed to parse the header.\n");
			goto error;
		}
	}

	resp = erl_format("{~a, {~w, ~w}}", UDT_SPLIT_RES, hdr, data);
	erl_send(fd, fromp, resp);
	erl_free_term(resp);
	erl_free_term(hdr);
	erl_free_term(data);

	if (msg)
		msgb_free(msg);

	return 0;

error:
	resp = erl_format("{~a, 23}", UDT_SPLIT_ERR);
	erl_send(fd, fromp, resp);
	erl_free_term(resp);
	return -1;
}

/**
 * Create a SCCP UDT message from the information we get from
 * the arg..
 */
static int wrap_udt(int fd, ETERM *fromp, ETERM *argp)
{
	struct msgb *msg;
	struct sockaddr_sccp called_sock, calling_sock;
	const struct osmo_erl_udt *sccp_hdr;
	ETERM *resp, *data;

	ETERM *hdr, *payload;

	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 error;
	}

	data = erl_mk_binary((const char * )msg->l2h, msgb_l2len(msg));
	resp = erl_format("{~a, ~w}", UDT_WRAP_RES, data);
	erl_send(fd, fromp, resp);

	erl_free_term(resp);
	erl_free_term(data);
	erl_free_term(hdr);
	erl_free_term(payload);
	if (msg)
		msgb_free(msg);

	return 0;
error:
	resp = erl_format("{~a, 23}", UDT_WRAP_ERR);
	erl_send(fd, fromp, resp);
	erl_free_term(resp);
	erl_free_term(hdr);
	erl_free_term(payload);

	return -1;
}

static int create_listen_socket(int port)
{
	int listen_fd;
	struct sockaddr_in addr;
	int on = 1, rc;

	listen_fd = socket(AF_INET, SOCK_STREAM, 0);
	if (listen_fd < 0)
		return (-1);

	setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));

	memset((void*) &addr, 0, (size_t) sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_port = htons(port);
	addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

	rc = bind(listen_fd, (struct sockaddr*) &addr, sizeof(addr));
	if (rc < 0)
		return -1;

	rc = listen(listen_fd, 5);
	return listen_fd;
}

static int init_erlang(int id, int port, char *cookie)
{
	int fd;

	erl_init(NULL, 0);

	if (erl_connect_init(id, cookie, 0) == -1)
		erl_err_quit("Failed to init.");

	fd = create_listen_socket(port);
	if (fd < 0)
		erl_err_quit("Failed to create socket.");

	if (erl_publish(port) == -1)
		erl_err_quit("Failed to publish.");

	return fd;
}

static int handle_command(int fd) {
	unsigned char buf[4096];              /* Buffer for incoming message */
	ErlMessage emsg;                         /* Incoming message */

	ETERM *fromp, *tuplep, *fnp, *argp;
	int got;                                 /* Result of receive */
	int exit = 0;

	got = erl_receive_msg(fd, buf, sizeof(buf), &emsg);
	if (got == ERL_TICK) {
		/* ignore */
	} else if (got == ERL_ERROR) {
		exit = 1;
	} else if (emsg.type == ERL_REG_SEND) {
		fromp = erl_element(2, emsg.msg);
		tuplep = erl_element(3, emsg.msg);
		fnp = erl_element(1, tuplep);
		argp = erl_element(2, tuplep);

		if (ERL_IS_ATOM(fnp)) {
			if (strcmp(ERL_ATOM_PTR(fnp), UDT_SPLIT_CMD) == 0) {
				split_udt(fd, fromp, argp);
			} else if (strcmp(ERL_ATOM_PTR(fnp), UDT_WRAP_CMD) == 0) {
				wrap_udt(fd, fromp, argp);
			} else {
				printf("unknown command '%s'\n", ERL_ATOM_PTR(fnp));
			}
		}

		erl_free_term(emsg.from); erl_free_term(emsg.msg);
		erl_free_term(fromp); erl_free_term(tuplep);
		erl_free_term(fnp); erl_free_term(argp);
	}

	return exit;
}

int main(int argc, char **argv) {
	ErlConnect conn;                         /* Connection data */
	int port = 2342;
	int server_fd;
	int fd;
	int exit;
	int quit = 0;

	server_fd = init_erlang(port, port, "123");
	if (server_fd < 0)
		erl_err_quit("Failed to listen.");

	while (!quit) {
		printf("Waiting for a connection.\n");
		fd = erl_accept(server_fd, &conn);
		if (fd == ERL_ERROR)
			continue;

		do {
			exit = handle_command(fd);
		} while (exit == 0);

		close(fd);
	}

	return 0;
}