SAP client - revisited

Submitted by Tomcsányi, Domonkos on April 23, 2015, 4:01 p.m.

Details

Message ID 1015750550.17043.1429804875628.JavaMail.zimbra@tomcsanyi.net
State New
Headers show

Commit Message

Tomcsányi, Domonkos April 23, 2015, 4:01 p.m.
Hi everyone,

I'm following up on this conversation:
http://comments.gmane.org/gmane.comp.mobile.osmocom.baseband.devel/1796

It seems like there was no movement in this topic in the last couple of years, so I decided to go ahead and integrate Nico's SAP client into the current master branch and created a patch from it.
I tested it (with Kevin's softSIM and a pcsc reader), it worked for me, however it is currently quite ugly: it kind of hand crafts the msgb structure (in l1ctl.c, patch line 121), sorry for that, but I kept getting extra bytes stuffed into the msg that was passed to the sap_interface so I decided to manually go around the problem.
I'm of course open to any suggestions to get it cleaner, and then if you think and decide so it could be merged into the master (as far as I see.

One thing however that I think is strange, and worth mentioning: I'm not sure why Nico decided to implement the switch between phone and SAP-client inside of l1ctl.c, for me it would feel better to do it in sim.c (since sim.c deals with SIM activities, l1ctl should deal only with L1 stuff...also the current SAP client calls back to sim.c, but receives data from l1ctl - little bit confusing), but I left it as is because of not knowing exactly the thoughts behind it.

Regards,
Domi

Comments

Nico Golde April 24, 2015, 2:41 p.m.
Hi,
* Tomcsányi, Domonkos <domi@tomcsanyi.net> [2015-04-23 18:12]:
[...] 
> It seems like there was no movement in this topic in the last couple of years, so I decided to go ahead and integrate Nico's SAP client into the current master branch and created a patch from it.

Cool, great work!

> One thing however that I think is strange, and worth mentioning: I'm not 
> sure why Nico decided to implement the switch between phone and SAP-client 
> inside of l1ctl.c, for me it would feel better to do it in sim.c (since 
> sim.c deals with SIM activities, l1ctl should deal only with L1 stuff...also 
> the current SAP client calls back to sim.c, but receives data from l1ctl - 
> little bit confusing), but I left it as is because of not knowing exactly 
> the thoughts behind it.

I don't remember details to be honest as this is too long ago, but I think 
there was no real design decision behind this other than that the code that is 
sending traffic to the SIM was already in l1ctl and I just added the switch...

Cheers,
Nico

Patch hide | download patch | download mbox

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 ab7c250..593b884 100644
--- a/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
+++ b/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
@@ -24,6 +24,8 @@  struct osmocom_ms;
 
 struct osmosap_entity {
 	osmosap_cb_t msg_handler;
+        uint8_t sap_state;
+        uint16_t max_msg_size;
 };
 
 struct osmol1_entity {
diff --git a/src/host/layer23/include/osmocom/bb/common/sap_interface.h b/src/host/layer23/include/osmocom/bb/common/sap_interface.h
index f2f577a..95d7d51 100644
--- a/src/host/layer23/include/osmocom/bb/common/sap_interface.h
+++ b/src/host/layer23/include/osmocom/bb/common/sap_interface.h
@@ -5,7 +5,70 @@  typedef int (*osmosap_cb_t)(struct msgb *msg, struct osmocom_ms *ms);
 
 int sap_open(struct osmocom_ms *ms, const char *socket_path);
 int sap_close(struct osmocom_ms *ms);
-int osmosap_send(struct osmocom_ms *ms, struct msgb *msg);
+int osmosap_send_apdu(struct osmocom_ms *ms, struct msgb *msg);
 int osmosap_register_handler(struct osmocom_ms *ms, osmosap_cb_t cb);
+int osmosap_sapsocket(struct osmocom_ms *ms, const char *path);
+int osmosap_init(struct osmocom_ms *ms);
+
+enum osmosap_state {
+	SAP_NOT_CONNECTED,
+	SAP_IDLE,
+	SAP_CONNECTION_UNDER_NEGOTIATION,
+	SAP_PROCESSING_ATR_REQUEST,
+	SAP_PROCESSING_APDU_REQUEST
+};
+
+/* BTSAP 1.13 */
+enum osmosap_msg_type {
+	SAP_CONNECT_REQ = 0x00,
+	SAP_CONNECT_RESP = 0x01,
+	SAP_DISCONNECT_REQ = 0x02,
+	SAP_DISCONNECT_RESP = 0x03,
+	SAP_DISCONNECT_IND = 0x04,
+	SAP_TRANSFER_APDU_REQ = 0x05,
+	SAP_TRANSFER_APDU_RESP = 0x06,
+	SAP_TRANSFER_ATR_REQ = 0x07,
+	SAP_TRANSFER_ATR_RESP = 0x08,
+	SAP_POWER_SIM_OFF_REQ = 0x09,
+	SAP_POWER_SIM_OFF_RESP = 0x0A,
+	SAP_POWER_SIM_ON_REQ = 0x0B,
+	SAP_POWER_SIM_ON_RESP = 0x0C,
+	SAP_RESET_SIM_REQ = 0x0D,
+	SAP_RESET_SIM_RESP = 0x0E,
+	SAP_TRANSFER_CARD_READER_STATUS_REQ = 0x0F,
+	SAP_TRANSFER_CARD_READER_STATUS_RESP = 0x10,
+	SAP_STATUS_IND = 0x11,
+	SAP_ERROR_RESP = 0x12,
+	SAP_SET_TRANSPORT_PROTOCOL_REQ = 0x13,
+	SAP_SET_TRANSPORT_PROTOCOL_RESP = 0x14
+};
+
+/* BTSAP 5.2 */
+enum osmosap_param_type {
+	SAP_MAX_MSG_SIZE = 0x00,
+	SAP_CONNECTION_STATUS = 0x01,
+	SAP_RESULT_CODE = 0x02,
+	SAP_DISCONNECTION_TYPE = 0x03,
+	SAP_COMMAND_APDU = 0x04,
+	SAP_COMMAND_APDU_7816 = 0x10,
+	SAP_RESPONSE_APDU = 0x05,
+	SAP_ATR = 0x06,
+	SAP_CARD_READER_STATUS = 0x07,
+	SAP_STATUS_CHANGE = 0x08,
+	SAP_TRANSPORT_PROTOCOL = 0x09
+};
+
+struct sap_param {
+	uint8_t id;
+	uint16_t len;
+	uint8_t *value;
+};
+
+struct sap_msg {
+	uint8_t id;
+	uint8_t num_params;
+	struct sap_param *params;
+};
+
 
 #endif /* _SAP_INTERFACE_H */
diff --git a/src/host/layer23/src/common/l1ctl.c b/src/host/layer23/src/common/l1ctl.c
index c75872e..470ad20 100644
--- a/src/host/layer23/src/common/l1ctl.c
+++ b/src/host/layer23/src/common/l1ctl.c
@@ -632,8 +632,12 @@  int l1ctl_tx_echo_req(struct osmocom_ms *ms, unsigned int len)
 
 int l1ctl_tx_sim_req(struct osmocom_ms *ms, uint8_t *data, uint16_t length)
 {
+	/* adding SAP client support */
+
 	struct msgb *msg;
 	uint8_t *dat;
+	uint8_t *real_apdu = malloc(length*sizeof(uint8_t));
+	memcpy(real_apdu, data, length);
 
 	if (length <= sizeof(apdu_data)) {
 		memcpy(apdu_data, data, length);
@@ -644,11 +648,20 @@  int l1ctl_tx_sim_req(struct osmocom_ms *ms, uint8_t *data, uint16_t length)
 	msg = osmo_l1_alloc(L1CTL_SIM_REQ);
 	if (!msg)
 		return -1;
-
 	dat = msgb_put(msg, length);
 	memcpy(dat, data, length);
 
-	return osmo_send_l1(ms, msg);
+        if(ms->settings.sap_socket_path[0] == 0) {
+		LOGP(DL1C, LOGL_INFO, "Using built-in SIM reader\n");
+                return osmo_send_l1(ms, msg);
+	}
+        else  {
+		memcpy(msg->data, data, length);
+		msg->len = length;
+		LOGP(DL1C, LOGL_INFO, "Received APDU: %s\n", osmo_hexdump(msg->data, msg->len));
+		LOGP(DL1C, LOGL_INFO, "Using SAP backend\n");
+                return osmosap_send_apdu(ms, msg);
+	}
 }
 
 /* just forward the SIM response to the SIM handler */
diff --git a/src/host/layer23/src/common/sap_interface.c b/src/host/layer23/src/common/sap_interface.c
index 1dad748..16988ab 100644
--- a/src/host/layer23/src/common/sap_interface.c
+++ b/src/host/layer23/src/common/sap_interface.c
@@ -3,6 +3,7 @@ 
 /* (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 Nico Golde <nico@ngolde.de>
  *
  * All Rights Reserved
  *
@@ -27,6 +28,7 @@ 
 #include <osmocom/bb/common/sap_interface.h>
 
 #include <osmocom/core/utils.h>
+#include <osmocom/core/talloc.h>
 
 #include <sys/socket.h>
 #include <sys/un.h>
@@ -42,72 +44,463 @@ 
 #define GSM_SAP_LENGTH 300
 #define GSM_SAP_HEADROOM 32
 
-static int sap_read(struct osmo_fd *fd)
+static void sap_connect(struct osmocom_ms *ms);
+
+static const struct value_string sap_param_names[] = {
+	{SAP_MAX_MSG_SIZE,			"MaxMsgSize"},
+	{SAP_CONNECTION_STATUS, 	"ConnectionStatus"},
+	{SAP_RESULT_CODE,			"ResultCode"},
+	{SAP_DISCONNECTION_TYPE,	"DisconnectionType"},
+	{SAP_COMMAND_APDU,			"CommandAPDU"},
+	{SAP_COMMAND_APDU_7816,		"CommandAPDU7816"},
+	{SAP_RESPONSE_APDU,			"ResponseAPDU"},
+	{SAP_ATR,					"ATR"},
+	{SAP_CARD_READER_STATUS,	"CardReaderStatus"},
+	{SAP_STATUS_CHANGE,			"StatusChange"},
+	{SAP_TRANSPORT_PROTOCOL,	"TransportProtocol"}
+};
+
+static const struct value_string sap_msg_names[] = {
+	{SAP_CONNECT_REQ,						"CONNECT_REQ"},
+	{SAP_CONNECT_RESP,						"CONNECT_RESP"},
+	{SAP_DISCONNECT_REQ,					"DISCONNECT_REQ"},
+	{SAP_DISCONNECT_RESP,					"DISCONNECT_RESP"},
+	{SAP_DISCONNECT_IND,					"DISCONNECT_IND"},
+	{SAP_TRANSFER_APDU_REQ,					"TRANSFER_APDU_REQ"},
+	{SAP_TRANSFER_APDU_RESP,				"TRANSFER_APDU_RESP"},
+	{SAP_TRANSFER_ATR_REQ,					"TRANSFER_ATR_REQ"},
+	{SAP_TRANSFER_ATR_RESP,					"TRANSFER_ATR_RESP"},
+	{SAP_POWER_SIM_OFF_REQ,					"POWER_SIM_OFF_REQ"},
+	{SAP_POWER_SIM_OFF_RESP,				"POWER_SIM_OFF_RESP"},
+	{SAP_POWER_SIM_ON_REQ,					"POWER_SIM_ON_REQ"},
+	{SAP_POWER_SIM_ON_RESP,					"POWER_SIM_ON_RESP"},
+	{SAP_RESET_SIM_REQ,						"RESET_SIM_REQ"},
+	{SAP_RESET_SIM_RESP,					"RESET_SIM_RESP"},
+	{SAP_TRANSFER_CARD_READER_STATUS_REQ,	"TRANSFER_CARD_READER_STATUS_REQ"},
+	{SAP_TRANSFER_CARD_READER_STATUS_RESP,	"TRANSFER_CARD_READER_STATUS_RESP"},
+	{SAP_STATUS_IND,						"STATUS_IND"},
+	{SAP_ERROR_RESP,						"ERROR_RESP"},
+	{SAP_SET_TRANSPORT_PROTOCOL_REQ,		"SET_TRANSPORT_PROTOCOL_REQ"},
+	{SAP_SET_TRANSPORT_PROTOCOL_RESP,		"SET_TRANSPORT_PROTOCOL_RESP"}
+};
+
+/* BTSAP table 5.18 */
+static const struct value_string sap_result_names[] = {
+	{0, "OK, request processed correctly"},
+	{1, "Error, no reason defined"},
+	{2, "Error, card not accessible"},
+	{3, "Error, card (already) powered off"},
+	{4, "Error, card removed"},
+	{5, "Error, card already powered on"},
+	{6, "Error, data not available"},
+	{7, "Error, not supported"}
+};
+
+static const struct value_string sap_status_change_names[] = {
+	{0, "Unknown Error"},
+	{1, "Card reset"},
+	{2, "Card not accessible"},
+	{3, "Card removed"},
+	{4, "Card inserted"},
+	{5, "Card recovered"},
+};
+
+static const struct value_string sap_status_names[] = {
+	{0, "OK, Server can fulfill requirements"},
+	{1, "Error, Server unable to establish connection"},
+	{2, "Error, Server does not support maximum message size"},
+	{3, "Error, maximum message size by Client is too small"},
+	{4, "OK, ongoing call"}
+};
+
+static struct msgb *sap_create_msg(uint8_t id, uint8_t num_params, struct sap_param *params)
 {
 	struct msgb *msg;
-	uint16_t len;
-	int rc;
-	struct osmocom_ms *ms = (struct osmocom_ms *) fd->data;
+	uint8_t *msgp;
+	uint8_t i, plen, padding = 0;
 
-	msg = msgb_alloc_headroom(GSM_SAP_LENGTH+GSM_SAP_HEADROOM, GSM_SAP_HEADROOM, "Layer2");
+	msg = msgb_alloc(GSM_SAP_LENGTH, "osmosap");
 	if (!msg) {
 		LOGP(DSAP, LOGL_ERROR, "Failed to allocate msg.\n");
+		return NULL;
+	}
+
+	/* BTSAP 5.1 */
+	msgb_put_u8(msg, id);
+	msgb_put_u8(msg, num_params);
+	msgb_put_u16(msg, 0);
+
+	for(i=0; i<num_params; i++){
+		plen = params[i].len;
+		msgb_put_u8(msg, params[i].id);
+		msgb_put_u8(msg, 0);
+		msgb_put_u16(msg, plen);
+		if(plen % 4){
+			padding = 4 - (plen % 4);
+		}
+		msgp = msgb_put(msg, plen + padding);
+		memcpy(msgp, params[i].value, plen);
+
+		if(padding){
+			memset(msgp + plen, 0, padding);
+		}
+	}
+
+	return msg;
+}
+
+static int osmosap_send(struct osmocom_ms *ms, struct msgb *msg)
+{
+	if(ms->sap_entity.sap_state == SAP_NOT_CONNECTED && !ms->sap_entity.sap_state == SAP_CONNECTION_UNDER_NEGOTIATION)
+		sap_connect(ms);
+
+	if (ms->sap_wq.bfd.fd <= 0)
+		return -EINVAL;
+
+	if (osmo_wqueue_enqueue(&ms->sap_wq, msg) != 0) {
+		LOGP(DSAP, LOGL_ERROR, "Failed to enqueue msg.\n");
+		msgb_free(msg);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int sap_parse_result(struct sap_param *param)
+{
+	if(param->id != SAP_RESULT_CODE){
+		LOGP(DSAP, LOGL_INFO, "> Parameter id: %u no valid result type\n", param->id);
+		return -1;
+	} else {
+		LOGP(DSAP, LOGL_INFO, "> RESULT CODE: %s\n",
+				get_value_string(sap_result_names, param->value[0]));
+	}
+
+	if(param->value[0] > sizeof(sap_result_names)/sizeof(struct value_string)){
+		return -1;
+	}
+
+	return 0;
+}
+
+static uint8_t *sap_get_param(uint8_t *data, struct sap_param *param)
+{
+	uint8_t *dptr = data;
+	uint8_t padlen;
+
+	param->id = *dptr++;
+	/* skip reserved byte */
+	dptr++;
+	param->len = *dptr << 8;
+	dptr++;
+	param->len |= *dptr++;
+	param->value = talloc_zero_size(NULL, param->len);
+	memcpy(param->value, dptr, param->len);
+
+	/* skip parameter and padding and return pointer to next parameter */
+	dptr += param->len;
+	if(param->len % 4){
+		padlen = (4 - param->len % 4);
+	} else {
+		padlen = 0;
+	}
+	dptr += padlen;
+
+	return dptr;
+}
+
+static void sap_msg_free(struct sap_msg *msg)
+{
+	uint8_t i;
+	for(i=0; i<msg->num_params; i++){
+		talloc_free(msg->params[i].value);
+		talloc_free(msg->params);
+	}
+	talloc_free(msg);
+}
+
+static struct sap_msg *sap_parse_msg(uint8_t *data)
+{
+	struct sap_msg *msg = talloc_zero(NULL, struct sap_msg);
+	uint8_t *ptr = data;
+	uint8_t i;
+
+	if(!msg){
+		return NULL;
+	}
+
+	msg->id = *ptr++;
+	LOGP(DSAP, LOGL_INFO, "> %s \n", get_value_string(sap_msg_names, msg->id));
+
+	msg->num_params = *ptr++;
+	/* skip two reserved null bytes, BTSAP 5.1 */
+	ptr += 2;
+        
+	msg->params = talloc_zero_size(NULL, sizeof(struct sap_param) * msg->num_params);
+
+	for(i=0; i<msg->num_params; i++){
+		ptr = sap_get_param(ptr, &msg->params[i]);
+		LOGP(DSAP, LOGL_INFO, "> %s %s\n",
+				get_value_string(sap_param_names, msg->params[i].id),
+				osmo_hexdump(msg->params[i].value, msg->params[i].len));
+	}
+
+	return msg;
+}
+
+static void sap_apdu_resp(struct osmocom_ms *ms, uint8_t *data, uint16_t len)
+{
+	struct msgb *msg;
+	uint8_t *apdu;
+	msg = msgb_alloc(GSM_SAP_LENGTH, "osmosap");
+	if(!msg){
+		LOGP(DSAP, LOGL_ERROR, "Failed to allocate memory.\n");
+		return;
+	}
+
+	apdu = msgb_put(msg, len);
+	memcpy(apdu, data, len);
+
+	LOGP(DSAP, LOGL_DEBUG, "Forwarding APDU to SIM handler.\n");
+	sim_apdu_resp(ms, msg);
+}
+
+static int sap_adapt_msg_size(struct osmocom_ms *ms, struct sap_param *param)
+{
+	uint16_t size;
+	size = (param->value[0] << 8) | param->value[1];
+	if(size != ms->sap_entity.max_msg_size && size > 0){
+		LOGP(DSAP, LOGL_NOTICE, "Server can not handle max_msg_size, adapting.\n");
+		ms->sap_entity.max_msg_size = size;
+		return -1;
+	}
+	return 0;
+}
+
+static void sap_atr(struct osmocom_ms *ms)
+{
+	struct msgb *msg;
+	if(ms->sap_entity.sap_state != SAP_IDLE){
+		LOGP(DSAP, LOGL_ERROR, "Attempting to send ATR request while not being idle.\n");
+		return;
+	}
+
+	msg = sap_create_msg(SAP_TRANSFER_ATR_REQ, 0, NULL);
+	if(!msg)
+		return;
+
+	osmosap_send(ms, msg);
+	ms->sap_entity.sap_state = SAP_PROCESSING_ATR_REQUEST;
+}
+
+static void sap_parse_resp(struct osmocom_ms *ms, uint8_t *data, uint16_t len)
+{
+	struct sap_msg *msg = NULL;
+	if(len > ms->sap_entity.max_msg_size){
+		LOGP(DSAP, LOGL_ERROR, "Read more data than allowed by max_msg_size, ignoring.\n");
+		return;
+	}
+
+	msg = sap_parse_msg(data);
+	if(!msg){
+		sap_msg_free(msg);
+		return;
+	}
+
+	switch(msg->id){
+	case SAP_CONNECT_RESP:
+		LOGP(DSAP, LOGL_INFO, "Status: %s\n", get_value_string(sap_status_names, msg->params[0].value[0]));
+		if(msg->params[0].value[0] == 0){
+			ms->sap_entity.sap_state = SAP_IDLE;
+		}
+		if(msg->num_params == 2 && msg->params[1].len == 2){
+			if(sap_adapt_msg_size(ms, &msg->params[1]) < 0) {
+				ms->sap_entity.sap_state = SAP_NOT_CONNECTED;
+			} else {
+				sap_atr(ms);
+			}
+		}
+		break;
+	case SAP_DISCONNECT_RESP:
+		ms->sap_entity.sap_state = SAP_NOT_CONNECTED;
+		break;
+	case SAP_STATUS_IND:
+		LOGP(DSAP, LOGL_INFO, "New card state: %s\n", get_value_string(sap_status_change_names,
+					msg->params[0].value[0]));
+		if(msg->params[0].value[0] != 1){
+			/* TODO: handle case in which the card is not ready yet */
+		}
+		break;
+	case SAP_TRANSFER_ATR_RESP:
+		if(ms->sap_entity.sap_state != SAP_PROCESSING_ATR_REQUEST){
+			LOGP(DSAP, LOGL_ERROR, "got ATR resp in state: %u\n", ms->sap_entity.sap_state);
+			return;
+		}
+		if(msg->num_params >= 2){
+			LOGP(DSAP, LOGL_INFO, "ATR: %s\n", osmo_hexdump(msg->params[1].value, msg->params[1].len));
+		}
+		ms->sap_entity.sap_state = SAP_IDLE;
+		break;
+	case SAP_TRANSFER_APDU_RESP:
+		if(ms->sap_entity.sap_state != SAP_PROCESSING_APDU_REQUEST){
+			LOGP(DSAP, LOGL_ERROR, "got APDU resp in state: %u\n", ms->sap_entity.sap_state);
+			return;
+		}
+		if(msg->num_params != 2){
+			LOGP(DSAP, LOGL_ERROR, "wrong number of parameters %u in APDU response\n", msg->num_params);
+			return;
+		}
+		ms->sap_entity.sap_state = SAP_IDLE;
+		if(sap_parse_result(&msg->params[0]) == 0){
+			/* back apdu resp to layer23 */
+			sap_apdu_resp(ms, msg->params[1].value, msg->params[1].len);
+			LOGP(DSAP, LOGL_INFO, "sap_apdu_resp called, sending data back to layer23\n");
+		}
+		break;
+	case SAP_ERROR_RESP:
+		if(ms->sap_entity.sap_state == SAP_CONNECTION_UNDER_NEGOTIATION){
+			ms->sap_entity.sap_state = SAP_NOT_CONNECTED;
+		} else {
+			ms->sap_entity.sap_state = SAP_IDLE;
+		}
+		break;
+	default:
+		LOGP(DSAP, LOGL_ERROR, "got unknown or not implemented SAP msgid: %u\n", msg->id);
+		break;
+	}
+}
+
+static int sap_read(struct osmo_fd *fd)
+{
+	struct msgb *msg = NULL;
+	struct osmocom_ms *ms = (struct osmocom_ms *) fd->data;
+	uint8_t *sap_buffer;
+	ssize_t rc;
+
+	sap_buffer = talloc_zero_size(NULL, ms->sap_entity.max_msg_size);
+	if(!sap_buffer){
+		fprintf(stderr, "Failed to allocate memory\n");
 		return -ENOMEM;
 	}
 
-	rc = read(fd->fd, &len, sizeof(len));
-	if (rc < sizeof(len)) {
+	rc = read(fd->fd, sap_buffer, ms->sap_entity.max_msg_size - 1);
+	if (rc < 0) {
 		fprintf(stderr, "SAP socket failed\n");
 		msgb_free(msg);
-		if (rc >= 0)
-			rc = -EIO;
 		sap_close(ms);
 		return rc;
 	}
-
-	len = ntohs(len);
-	if (len > GSM_SAP_LENGTH) {
-		LOGP(DSAP, LOGL_ERROR, "Length is too big: %u\n", len);
+	if(rc == 0) {
+		fprintf(stderr, "SAP socket closed by server\n");
 		msgb_free(msg);
-		return -EINVAL;
+		sap_close(ms);
+		return -ECONNREFUSED;
 	}
 
+	sap_buffer[rc] = 0;
+	LOGP(DSAP, LOGL_INFO, "Received %zd bytes: %s\n", rc, osmo_hexdump(sap_buffer, rc));
 
-	msg->l1h = msgb_put(msg, len);
-	rc = read(fd->fd, msg->l1h, msgb_l1len(msg));
-	if (rc != msgb_l1len(msg)) {
-		LOGP(DSAP, LOGL_ERROR, "Can not read data: len=%d rc=%d "
-		     "errno=%d\n", len, rc, errno);
-		msgb_free(msg);
-		return rc;
-	}
+	sap_parse_resp(ms, sap_buffer, rc);
 
-	if (ms->sap_entity.msg_handler)
-		ms->sap_entity.msg_handler(msg, ms);
+	talloc_free(sap_buffer);
 
+	if (ms->sap_entity.msg_handler){
+		ms->sap_entity.msg_handler(msg, ms);
+	}
 	return 0;
 }
 
 static int sap_write(struct osmo_fd *fd, struct msgb *msg)
 {
-	int rc;
+	ssize_t rc;
 
 	if (fd->fd <= 0)
 		return -EINVAL;
 
+	LOGP(DSAP, LOGL_INFO, "< %s\n", osmo_hexdump(msg->data, msg->len));
 	rc = write(fd->fd, msg->data, msg->len);
 	if (rc != msg->len) {
-		LOGP(DSAP, LOGL_ERROR, "Failed to write data: rc: %d\n", rc);
+		LOGP(DSAP, LOGL_ERROR, "Failed to write data: rc: %zd\n", rc);
 		return rc;
 	}
 
 	return 0;
 }
 
+static void sap_connect(struct osmocom_ms *ms)
+{
+	uint8_t buffer[3];
+	struct msgb *msg;
+	uint16_t size = ms->sap_entity.max_msg_size;
+	struct sap_param params[1];
+
+	params[0].id = SAP_MAX_MSG_SIZE;
+	params[0].len = 2;
+
+	if(ms->sap_entity.sap_state != SAP_NOT_CONNECTED) {
+		LOGP(DSAP, LOGL_ERROR, "Attempting to connect while there is an active connection.\n");
+		return;
+	}
+
+	buffer[0] = (size >> 8) & 0xFF;
+	buffer[1] = size & 0xFF;
+	buffer[2] = 0;
+	params[0].value = buffer;
+
+	msg = sap_create_msg(SAP_CONNECT_REQ, 1, params);
+	if(!msg)
+		return;
+
+	osmosap_send(ms, msg);
+
+	ms->sap_entity.sap_state = SAP_CONNECTION_UNDER_NEGOTIATION;
+}
+
+static void sap_disconnect(struct osmocom_ms *ms)
+{
+	struct msgb *msg;
+	if(ms->sap_entity.sap_state != SAP_NOT_CONNECTED && ms->sap_entity.sap_state != SAP_CONNECTION_UNDER_NEGOTIATION){
+		LOGP(DSAP, LOGL_ERROR, "Attempting to disconnect while no active connection.\n");
+		return;
+	}
+
+	msg = sap_create_msg(SAP_DISCONNECT_REQ, 0, NULL);
+	if(!msg)
+		return;
+
+	osmosap_send(ms, msg);
+
+	ms->sap_entity.sap_state = SAP_NOT_CONNECTED;
+}
+
+static void sap_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t len)
+{
+	struct msgb *msg;
+	struct sap_param params[1];
+
+	params[0].id = SAP_COMMAND_APDU;
+	params[0].len = len;
+	params[0].value = data;
+
+	if(ms->sap_entity.sap_state != SAP_IDLE){
+		LOGP(DSAP, LOGL_ERROR, "Attempting to send APDU request while not being idle.\n");
+		return;
+	}
+
+	msg = sap_create_msg(SAP_TRANSFER_APDU_REQ, 1, params);
+	if(!msg)
+		return;
+
+	osmosap_send(ms, msg);
+
+	ms->sap_entity.sap_state = SAP_PROCESSING_APDU_REQUEST;
+}
+
 int sap_open(struct osmocom_ms *ms, const char *socket_path)
 {
-	int rc;
+	ssize_t rc;
 	struct sockaddr_un local;
+	struct gsm_settings *set = &ms->settings;
 
 	ms->sap_wq.bfd.fd = socket(AF_UNIX, SOCK_STREAM, 0);
 	if (ms->sap_wq.bfd.fd < 0) {
@@ -119,10 +512,10 @@  int sap_open(struct osmocom_ms *ms, const char *socket_path)
 	strncpy(local.sun_path, socket_path, sizeof(local.sun_path));
 	local.sun_path[sizeof(local.sun_path) - 1] = '\0';
 
-	rc = connect(ms->sap_wq.bfd.fd, (struct sockaddr *) &local,
-		     sizeof(local.sun_family) + strlen(local.sun_path));
+	rc = connect(ms->sap_wq.bfd.fd, (struct sockaddr *) &local, sizeof(local));
 	if (rc < 0) {
-		fprintf(stderr, "Failed to connect to '%s'.\n", local.sun_path);
+		fprintf(stderr, "Failed to connect to '%s'\n", local.sun_path);
+		set->sap_socket_path[0] = 0;
 		close(ms->sap_wq.bfd.fd);
 		return rc;
 	}
@@ -139,6 +532,8 @@  int sap_open(struct osmocom_ms *ms, const char *socket_path)
 		return rc;
 	}
 
+	sap_connect(ms);
+
 	return 0;
 }
 
@@ -147,43 +542,60 @@  int sap_close(struct osmocom_ms *ms)
 	if (ms->sap_wq.bfd.fd <= 0)
 		return -EINVAL;
 
+	sap_disconnect(ms);
 	close(ms->sap_wq.bfd.fd);
 	ms->sap_wq.bfd.fd = -1;
 	osmo_fd_unregister(&ms->sap_wq.bfd);
-	osmo_wqueue_clear(&ms->sap_wq);
+        osmo_wqueue_clear(&ms->sap_wq);
 
 	return 0;
 }
 
-int osmosap_send(struct osmocom_ms *ms, struct msgb *msg)
+int osmosap_send_apdu(struct osmocom_ms *ms, struct msgb *msg)
 {
-	uint16_t *len;
+	LOGP(DSAP, LOGL_ERROR, "Received the following APDU from layer23: %s\n" ,
+	     osmo_hexdump(msg->data, msg->len));
+	sap_apdu(ms, msg->data, msg->len);
 
-	if (ms->sap_wq.bfd.fd <= 0)
-		return -EINVAL;
+	return 0;
+}
 
-	DEBUGP(DSAP, "Sending: '%s'\n", osmo_hexdump(msg->data, msg->len));
+/* register message handler for messages that are sent from L2->L3 */
+int osmosap_register_handler(struct osmocom_ms *ms, osmosap_cb_t cb)
+{
+	ms->sap_entity.msg_handler = cb;
 
-	if (msg->l1h != msg->data)
-		LOGP(DSAP, LOGL_ERROR, "Message SAP header != Message Data\n");
-	
-	/* prepend 16bit length before sending */
-	len = (uint16_t *) msgb_push(msg, sizeof(*len));
-	*len = htons(msg->len - sizeof(*len));
+	return 0;
+}
 
-	if (osmo_wqueue_enqueue(&ms->sap_wq, msg) != 0) {
-		LOGP(DSAP, LOGL_ERROR, "Failed to enqueue msg.\n");
-		msgb_free(msg);
-		return -1;
-	}
+int osmosap_sapsocket(struct osmocom_ms *ms, const char *path)
+{
+	struct gsm_settings *set = &ms->settings;
+	memset(set->sap_socket_path, 0, sizeof(set->sap_socket_path));
+	strncpy(set->sap_socket_path, path, sizeof(set->sap_socket_path) - 1);
 
 	return 0;
 }
 
-/* register message handler for messages that are sent from L2->L3 */
-int osmosap_register_handler(struct osmocom_ms *ms, osmosap_cb_t cb)
+/* init */
+int osmosap_init(struct osmocom_ms *ms)
 {
-	ms->sap_entity.msg_handler = cb;
+	struct osmosap_entity *sap = &ms->sap_entity;
+	int rc;
+
+	sap->sap_state = SAP_NOT_CONNECTED;
+	sap->max_msg_size = GSM_SAP_LENGTH;
+
+	LOGP(DSAP, LOGL_INFO, "init SAP client\n");
+
+	if(ms->settings.sap_socket_path){
+		rc = sap_open(ms, ms->settings.sap_socket_path);
+		if (rc < 0) {
+			fprintf(stderr, "Failed during sap_open(), no SAP based SIM reader\n");
+			ms->sap_wq.bfd.fd = -1;
+			return rc;
+		}
+	}
 
 	return 0;
 }
diff --git a/src/host/layer23/src/common/sim.c b/src/host/layer23/src/common/sim.c
index 8c89cf0..1c26d7b 100644
--- a/src/host/layer23/src/common/sim.c
+++ b/src/host/layer23/src/common/sim.c
@@ -185,6 +185,7 @@  static int sim_apdu_send(struct osmocom_ms *ms, uint8_t *data, uint16_t length)
 {
 	LOGP(DSIM, LOGL_INFO, "sending APDU (class 0x%02x, ins 0x%02x)\n",
 		data[0], data[1]);
+	LOGP(DSIM, LOGL_INFO, "%s\n", osmo_hexdump(data, length));
 	l1ctl_tx_sim_req(ms, data, length);
 	return 0;
 }
@@ -946,7 +947,7 @@  int sim_apdu_resp(struct osmocom_ms *ms, struct msgb *msg)
 	case GSM1111_STAT_DL_ERROR:
 	case GSM1111_STAT_RESPONSE:
 	case GSM1111_STAT_RESPONSE_TOO:
-		LOGP(DSIM, LOGL_INFO, "command successfull\n");
+		LOGP(DSIM, LOGL_INFO, "command successful\n");
 		break;
 	default:
 		LOGP(DSIM, LOGL_INFO, "command failed\n");
diff --git a/src/host/layer23/src/mobile/app_mobile.c b/src/host/layer23/src/mobile/app_mobile.c
index 9f28b0a..b473d4d 100644
--- a/src/host/layer23/src/mobile/app_mobile.c
+++ b/src/host/layer23/src/mobile/app_mobile.c
@@ -37,6 +37,9 @@ 
 #include <osmocom/bb/mobile/app_mobile.h>
 #include <osmocom/bb/mobile/mncc.h>
 #include <osmocom/bb/mobile/voice.h>
+/*adding SAP SIM reading */
+#include <osmocom/bb/common/sap_interface.h>
+
 #include <osmocom/vty/telnet_interface.h>
 
 #include <osmocom/core/msgb.h>
@@ -190,6 +193,8 @@  int mobile_init(struct osmocom_ms *ms)
 		T200_ACCH;
 	ms->lapdm_channel.lapdm_acch.datalink[DL_SAPI3].dl.t200_usec = 0;
 	lapdm_channel_set_l1(&ms->lapdm_channel, l1ctl_ph_prim_cb, ms);
+/* init SAP client before SIM card starts up */
+	osmosap_init(ms);
 
 	gsm_sim_init(ms);
 	gsm48_cc_init(ms);
diff --git a/src/host/layer23/src/mobile/main.c b/src/host/layer23/src/mobile/main.c
index 8bcecba..a6dd082 100644
--- a/src/host/layer23/src/mobile/main.c
+++ b/src/host/layer23/src/mobile/main.c
@@ -69,7 +69,7 @@  int mobile_exit(struct osmocom_ms *ms, int force);
 
 
 const char *debug_default =
-	"DCS:DNB:DPLMN:DRR:DMM:DSIM:DCC:DMNCC:DSS:DLSMS:DPAG:DSUM";
+	"DCS:DNB:DPLMN:DRR:DMM:DSIM:DCC:DMNCC:DSS:DLSMS:DPAG:DSUM:DSAP";
 
 const char *openbsc_copyright =
 	"Copyright (C) 2008-2010 ...\n"
diff --git a/src/target/firmware/Makefile b/src/target/firmware/Makefile
index 42f7ad4..b816061 100644
--- a/src/target/firmware/Makefile
+++ b/src/target/firmware/Makefile
@@ -127,7 +127,7 @@  INCLUDES=-Iinclude/ -I../../../include -I../../shared/libosmocore/include
 #
 
 # Uncomment this line if you want to enable Tx (Transmit) Support.
-#CFLAGS += -DCONFIG_TX_ENABLE
+CFLAGS += -DCONFIG_TX_ENABLE
 
 # Uncomment this line if you want to write to flash.
 #CFLAGS += -DCONFIG_FLASH_WRITE