commit 43cb8398960b6df82d775d0bd1c4e5cf5c0ee819
Author: Gianni Tedesco <gianni@scaramanga.co.uk>
Date:   Wed May 11 03:10:21 2011 +0100

    Implement simctl

diff --git a/src/host/layer23/include/osmocom/bb/common/logging.h b/src/host/layer23/include/osmocom/bb/common/logging.h
index 9205fec..84e9234 100644
--- a/src/host/layer23/include/osmocom/bb/common/logging.h
+++ b/src/host/layer23/include/osmocom/bb/common/logging.h
@@ -21,6 +21,7 @@ enum {
 	DSUM,
 	DSIM,
 	DGPS,
+	DSIMCTL,
 };
 
 extern const struct log_info log_info;
diff --git a/src/host/layer23/include/osmocom/bb/common/osmocom_data.h b/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
index 9d8a030..fbcaab7 100644
--- a/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
+++ b/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
@@ -46,11 +46,16 @@ struct rx_meas_stat {
 	int16_t s, rl_fail;
 };
 
+#define USE_SIMCTL 1
+
 /* One Mobilestation for osmocom */
 struct osmocom_ms {
 	struct llist_head entity;
 	char name[32];
 	struct write_queue l2_wq, sap_wq;
+#if USE_SIMCTL
+	struct write_queue simctl_wq;
+#endif
 	uint16_t test_arfcn;
 
 	uint8_t deleting, shutdown, started;
diff --git a/src/host/layer23/include/osmocom/bb/common/simctl_interface.h b/src/host/layer23/include/osmocom/bb/common/simctl_interface.h
new file mode 100644
index 0000000..7e2c2df
--- /dev/null
+++ b/src/host/layer23/include/osmocom/bb/common/simctl_interface.h
@@ -0,0 +1,8 @@
+#ifndef _SIMCTL_INTERFACE_H
+#define _SIMCTL_INTERFACE_H
+
+int simctl_open(struct osmocom_ms *ms, const char *socket_path);
+int simctl_close(struct osmocom_ms *ms);
+int simctl_send(struct osmocom_ms *ms, struct msgb *msg);
+
+#endif /* _SIMCTL_INTERFACE_H */
diff --git a/src/host/layer23/include/osmocom/bb/mobile/settings.h b/src/host/layer23/include/osmocom/bb/mobile/settings.h
index f666f37..24ae1a4 100644
--- a/src/host/layer23/include/osmocom/bb/mobile/settings.h
+++ b/src/host/layer23/include/osmocom/bb/mobile/settings.h
@@ -10,6 +10,7 @@ enum {
 struct gsm_settings {
 	char			layer2_socket_path[128];
 	char			sap_socket_path[128];
+	char			simctl_socket_path[128];
 
 	/* IMEI */
 	char			imei[16];
diff --git a/src/host/layer23/src/common/Makefile.am b/src/host/layer23/src/common/Makefile.am
index f6e3f83..7bf6d65 100644
--- a/src/host/layer23/src/common/Makefile.am
+++ b/src/host/layer23/src/common/Makefile.am
@@ -3,4 +3,4 @@ AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS)
 
 noinst_LIBRARIES = liblayer23.a
 liblayer23_a_SOURCES = l1ctl.c l1l2_interface.c sap_interface.c lapdm.c \
-	logging.c networks.c sim.c sysinfo.c gps.c
+	logging.c networks.c sim.c sysinfo.c gps.c simctl_interface.c
diff --git a/src/host/layer23/src/common/l1ctl.c b/src/host/layer23/src/common/l1ctl.c
index f2714e6..75844e9 100644
--- a/src/host/layer23/src/common/l1ctl.c
+++ b/src/host/layer23/src/common/l1ctl.c
@@ -46,6 +46,7 @@
 #include <osmocom/bb/common/l1l2_interface.h>
 #include <osmocom/bb/common/lapdm.h>
 #include <osmocom/bb/common/logging.h>
+#include <osmocom/bb/common/simctl_interface.h>
 
 static struct msgb *osmo_l1_alloc(uint8_t msg_type)
 {
@@ -595,7 +596,11 @@ int l1ctl_tx_sim_req(struct osmocom_ms *ms, uint8_t *data, uint16_t length)
 	dat = msgb_put(msg, length);
 	memcpy(dat, data, length);
 
+#if USE_SIMCTL
+	return simctl_send(ms, msg);
+#else
 	return osmo_send_l1(ms, msg);
+#endif
 }
 
 /* just forward the SIM response to the SIM handler */
diff --git a/src/host/layer23/src/common/logging.c b/src/host/layer23/src/common/logging.c
index 5283935..ab6ad17 100644
--- a/src/host/layer23/src/common/logging.c
+++ b/src/host/layer23/src/common/logging.c
@@ -120,6 +120,12 @@ static const struct log_info_cat default_categories[] = {
 		.color = "\033[1;35m",
 		.enabled = 1, .loglevel = LOGL_DEBUG,
 	},
+	[DSAP]	= {
+		.name = "DSIMCTL",
+		.description = "SIM Control",
+		.color = "\033[1;31m",
+		.enabled = 1, .loglevel = LOGL_DEBUG,
+	},
 };
 
 const struct log_info log_info = {
diff --git a/src/host/layer23/src/common/simctl_interface.c b/src/host/layer23/src/common/simctl_interface.c
new file mode 100644
index 0000000..584fe13
--- /dev/null
+++ b/src/host/layer23/src/common/simctl_interface.c
@@ -0,0 +1,185 @@
+/* BTSIMCTL socket interface of layer2/3 stack */
+
+/* (C) 2010 by Holger Hans Peter Freyther
+ * (C) 2010 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2010 by Andreas Eversberg <jolly@eversberg.eu>
+ * (c) 2011 by Gianni Tedesco <gianni @
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <osmocom/bb/common/osmocom_data.h>
+#include <osmocom/bb/common/logging.h>
+#include <osmocom/bb/common/simctl_interface.h>
+
+#include <osmocom/core/utils.h>
+
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include <arpa/inet.h>
+
+#define _GNU_SOURCE
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+
+#define MAX_ATR_LEN	64
+#define SIMCTL_MRU	1024 /* more than enough? */
+
+static struct msgb *simctl_alloc(size_t sz)
+{
+	struct msgb *msg = msgb_alloc_headroom(sz, sz, "osmo_simctl");
+	if (!msg) {
+		LOGP(DSIMCTL, LOGL_ERROR, "Failed to allocate memory.\n");
+		return NULL;
+	}
+
+	return msg;
+}
+
+static int simctl_read(struct bsc_fd *fd)
+{
+	struct osmocom_ms *ms = (struct osmocom_ms *) fd->data;
+	struct msgb *msg;
+	uint8_t *data;
+	ssize_t rc;
+
+	msg = simctl_alloc(SIMCTL_MRU);
+	if (!msg)
+		return -ENOMEM;
+
+	data = msgb_push(msg, SIMCTL_MRU);
+
+	rc = read(fd->fd, data, SIMCTL_MRU);
+	if (rc < 0) {
+		fprintf(stderr, "SIMCTL socket failed\n");
+		msgb_free(msg);
+		simctl_close(ms);
+		return rc;
+	}
+	if (rc == 0) {
+		fprintf(stderr, "SIMCTL socket closed by peer\n");
+		msgb_free(msg);
+		simctl_close(ms);
+		return -ECONNREFUSED;
+	}
+
+	msg->len = rc;
+
+	LOGP(DSIMCTL, LOGL_INFO, "Received %zd bytes: %s\n", rc,
+		hexdump(data, rc));
+
+	sim_apdu_resp(ms, msg);
+	return 0;
+}
+
+static int simctl_write(struct bsc_fd *fd, struct msgb *msg)
+{
+	ssize_t rc;
+
+	if (fd->fd <= 0)
+		return -EINVAL;
+
+	rc = write(fd->fd, msg->data, msg->len);
+	if (rc < 0 || rc != msg->len) {
+		LOGP(DSIMCTL, LOGL_ERROR, "Failed to write data: rc: %d\n", rc);
+		return rc;
+	}
+
+	return 0;
+}
+
+int simctl_open(struct osmocom_ms *ms, const char *socket_path)
+{
+	int rc;
+	struct sockaddr_un local;
+
+	ms->simctl_wq.bfd.fd = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+	if (ms->simctl_wq.bfd.fd < 0) {
+		fprintf(stderr, "Failed to create unix domain socket.\n");
+		return ms->simctl_wq.bfd.fd;
+	}
+
+	local.sun_family = AF_UNIX;
+	strncpy(local.sun_path, socket_path, sizeof(local.sun_path));
+	local.sun_path[sizeof(local.sun_path) - 1] = '\0';
+
+	rc = connect(ms->simctl_wq.bfd.fd, (struct sockaddr *) &local,
+		     sizeof(local.sun_family) + strlen(local.sun_path));
+	if (rc < 0) {
+		fprintf(stderr, "Failed to connect to '%s'.\n", local.sun_path);
+		fprintf(stderr, "%s\n", strerror(errno));
+		close(ms->simctl_wq.bfd.fd);
+		return rc;
+	}
+
+	write_queue_init(&ms->simctl_wq, 100);
+	ms->simctl_wq.bfd.data = ms;
+	ms->simctl_wq.bfd.when = BSC_FD_READ;
+	ms->simctl_wq.read_cb = simctl_read;
+	ms->simctl_wq.write_cb = simctl_write;
+
+	rc = bsc_register_fd(&ms->simctl_wq.bfd);
+	if (rc != 0) {
+		fprintf(stderr, "Failed to register fd.\n");
+		return rc;
+	}
+
+	return 0;
+}
+
+int simctl_close(struct osmocom_ms *ms)
+{
+	if (ms->simctl_wq.bfd.fd <= 0)
+		return -EINVAL;
+
+	close(ms->simctl_wq.bfd.fd);
+	ms->simctl_wq.bfd.fd = -1;
+	bsc_unregister_fd(&ms->simctl_wq.bfd);
+
+	return 0;
+}
+
+int simctl_send(struct osmocom_ms *ms, struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h;
+
+	if (ms->simctl_wq.bfd.fd <= 0)
+		return -EINVAL;
+
+	DEBUGP(DSIMCTL, "Sending: '%s'\n", hexdump(msg->data, msg->len));
+
+	l1h = (struct l1ctl_hdr *) msg->l1h;
+	if (l1h->msg_type != L1CTL_SIM_REQ ){
+		LOGP(DSIMCTL, LOGL_ERROR, "Message type != L1CTL_SIM_REQ\n");
+		return -EINVAL;
+	}
+
+	/* pull the L1 header from the msgb */
+	msgb_pull(msg, sizeof(struct l1ctl_hdr));
+
+	if (write_queue_enqueue(&ms->simctl_wq, msg) != 0) {
+		LOGP(DSIMCTL, LOGL_ERROR, "Failed to enqueue msg.\n");
+		msgb_free(msg);
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/src/host/layer23/src/mobile/app_mobile.c b/src/host/layer23/src/mobile/app_mobile.c
index 33fdde6..d516fd1 100644
--- a/src/host/layer23/src/mobile/app_mobile.c
+++ b/src/host/layer23/src/mobile/app_mobile.c
@@ -187,6 +187,16 @@ int mobile_init(struct osmocom_ms *ms)
 	}
 #endif
 
+#if USE_SIMCTL
+	rc = simctl_open(ms, ms->settings.simctl_socket_path);
+	if (rc < 0) {
+		fprintf(stderr, "Failed during simctl_open(), no SIM reader\n");
+		ms->simctl_wq.bfd.fd = -1;
+		mobile_exit(ms, 1);
+		return rc;
+	}
+#endif
+
 	if (mncc_recv_app)
 		ms->cclayer.mncc_recv = mncc_recv_app;
 	else if (ms->settings.ch_cap == GSM_CAP_SDCCH)
diff --git a/src/host/layer23/src/mobile/settings.c b/src/host/layer23/src/mobile/settings.c
index db22fd9..8d2c3a5 100644
--- a/src/host/layer23/src/mobile/settings.c
+++ b/src/host/layer23/src/mobile/settings.c
@@ -30,6 +30,7 @@
 
 static char *layer2_socket_path = "/tmp/osmocom_l2";
 static char *sap_socket_path = "/tmp/osmocom_sap";
+static char *simctl_socket_path = "/tmp/osmocom_simctl";
 
 int gsm_settings_init(struct osmocom_ms *ms)
 {
@@ -38,6 +39,7 @@ int gsm_settings_init(struct osmocom_ms *ms)
 
 	strcpy(set->layer2_socket_path, layer2_socket_path);
 	strcpy(set->sap_socket_path, sap_socket_path);
+	strcpy(set->simctl_socket_path, simctl_socket_path);
 
 	/* IMEI */
 	sprintf(set->imei,   "000000000000000");
diff --git a/src/host/layer23/src/mobile/vty_interface.c b/src/host/layer23/src/mobile/vty_interface.c
index 418c3f6..3d41a13 100644
--- a/src/host/layer23/src/mobile/vty_interface.c
+++ b/src/host/layer23/src/mobile/vty_interface.c
@@ -1057,6 +1057,7 @@ static void config_write_ms(struct vty *vty, struct osmocom_ms *ms)
 	vty_out(vty, " layer2-socket %s%s", set->layer2_socket_path,
 		VTY_NEWLINE);
 	vty_out(vty, " sap-socket %s%s", set->sap_socket_path, VTY_NEWLINE);
+	vty_out(vty, " simctl-socket %s%s", set->simctl_socket_path, VTY_NEWLINE);
 	switch(set->sim_type) {
 		case GSM_SIM_TYPE_NONE:
 		vty_out(vty, " sim none%s", VTY_NEWLINE);
@@ -1260,6 +1261,20 @@ DEFUN(cfg_ms_sap, cfg_ms_sap_cmd, "sap-socket PATH",
 	return CMD_SUCCESS;
 }
 
+DEFUN(cfg_ms_simctl, cfg_ms_simctl_cmd, "simctl-socket PATH",
+	"Define socket path to connect to SIM reader\n"
+	"Unix socket, default '/tmp/osmocom_simctl'")
+{
+	struct osmocom_ms *ms = vty->index;
+	struct gsm_settings *set = &ms->settings;
+
+	strncpy(set->simctl_socket_path, argv[0],
+		sizeof(set->simctl_socket_path) - 1);
+
+	vty_restart(vty, ms);
+	return CMD_SUCCESS;
+}
+
 DEFUN(cfg_ms_sim, cfg_ms_sim_cmd, "sim (none|reader|test)",
 	"Set SIM card type when powering on\nNo SIM interted\n"
 	"Use SIM from reader\nTest SIM inserted")
@@ -2324,6 +2339,7 @@ int ms_vty_init(void)
 	install_element(MS_NODE, &cfg_ms_show_this_cmd);
 	install_element(MS_NODE, &cfg_ms_layer2_cmd);
 	install_element(MS_NODE, &cfg_ms_sap_cmd);
+	install_element(MS_NODE, &cfg_ms_simctl_cmd);
 	install_element(MS_NODE, &cfg_ms_sim_cmd);
 	install_element(MS_NODE, &cfg_ms_mode_cmd);
 	install_element(MS_NODE, &cfg_ms_imei_cmd);
