summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorVadim Yanitskiy <axilirator@gmail.com>2018-12-23 05:30:21 +0700
committerVadim Yanitskiy <axilirator@gmail.com>2019-01-15 04:26:46 +0700
commit2986a318b12c26904f87f6da40c3dce852de7952 (patch)
tree0e46ccd5fea3fc06a494de3265098c636a55b7eb
parente4e3e6facd1a7e7a81065428a5dba184026652a1 (diff)
layer23/sap_interface.c: reimplement (BT)SAP interface
The (BT)SAP (Bluetooth SIM Access Profile) is a part of Bluetooth specifications, that defines the protocol and procedures that shall be used to access a smart card (usually GSM SIM) via a Bluetooth link. The profile defines two roles: - Server - the side that has direct access to a smart card. It acts as a SIM card reader, which assists the Client in accessing and controlling the smart card. - Client - the side that accesses and controls the smart card inside the Server through the connection with Server. Typical examples of a Server are a simple SIM card holder or a portable phone in the car environment. A typical example of a Client is a car phone, which uses a subscription module in the Server for a connection to the cellular network. OsmocomBB implements the Client role providing abstract SAP interface API to the higher layers. Instead of Bluetooth, a UNIX socket is used to communicate with a Server. The previous implementation of (BT)SAP interface was incomplete and hard to maintain. This change (re)implements it almost from scratch on top of the Osmocom FSM framework. Besides that, the most significant changes are: - The implementation is separated into three parts: - sap_interface.{c|h} - public SAP interface API, - sap_proto.{c|h} - SAP protocol definition, - sap_fsm.{c|h} - SAP FSM implementation. - Both 'sap_message' and 'sap_param' structures follow the SAP message format definition according to 5.1 and 5.2. - The message parsing is done more carefully in order to prevent buffer overflow and NULL-pointer dereference. - Introduced public API for getting / adding message parameters, and checking the ResultCode. - Introduced public API for opening / closing a connection with the server, powering on / off and resetting the SIM card, sending ATR and APDU. - Introduced a call-back for handling the response message. - Card reader state is also a part of the public API. The new implementation was tested against softsim [1]. The only limitation is Server-initiated Release, that allows the Server to 'ask' a Client to release connection as soon as communication with the smart card is finished. This is not implemented (yet), and leads to immediate release. [1] https://git.osmocom.org/softsim/ Change-Id: I77bb108615bb2c94c441568f195b04e0a5421643
-rw-r--r--src/host/layer23/include/osmocom/bb/common/Makefile.am2
-rw-r--r--src/host/layer23/include/osmocom/bb/common/osmocom_data.h9
-rw-r--r--src/host/layer23/include/osmocom/bb/common/sap_fsm.h35
-rw-r--r--src/host/layer23/include/osmocom/bb/common/sap_interface.h29
-rw-r--r--src/host/layer23/include/osmocom/bb/common/sap_proto.h45
-rw-r--r--src/host/layer23/include/osmocom/bb/mobile/subscriber.h2
-rw-r--r--src/host/layer23/src/common/Makefile.am2
-rw-r--r--src/host/layer23/src/common/sap_fsm.c690
-rw-r--r--src/host/layer23/src/common/sap_interface.c561
-rw-r--r--src/host/layer23/src/common/sap_proto.c218
-rw-r--r--src/host/layer23/src/mobile/app_mobile.c3
-rw-r--r--src/host/layer23/src/mobile/subscriber.c49
12 files changed, 1259 insertions, 386 deletions
diff --git a/src/host/layer23/include/osmocom/bb/common/Makefile.am b/src/host/layer23/include/osmocom/bb/common/Makefile.am
index eb1dfb7..d66be98 100644
--- a/src/host/layer23/include/osmocom/bb/common/Makefile.am
+++ b/src/host/layer23/include/osmocom/bb/common/Makefile.am
@@ -1,3 +1,3 @@
noinst_HEADERS = l1ctl.h l1l2_interface.h l23_app.h logging.h \
networks.h gps.h sysinfo.h osmocom_data.h utils.h \
- sap_proto.h sap_interface.h
+ sap_proto.h sap_fsm.h sap_interface.h
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 a3ecc92..ee48d6d 100644
--- a/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
+++ b/src/host/layer23/include/osmocom/bb/common/osmocom_data.h
@@ -23,9 +23,14 @@ struct osmocom_ms;
#include <osmocom/bb/common/l1ctl.h>
struct osmosap_entity {
- sap_cb_t msg_handler;
- uint8_t sap_state;
+ struct osmo_fsm_inst *fi;
uint16_t max_msg_size;
+ uint8_t card_status;
+
+ /* Optional SAP message call-back */
+ sap_msg_cb_t sap_msg_cb;
+ /* Optional response call-back */
+ sap_rsp_cb_t sap_rsp_cb;
};
struct osmol1_entity {
diff --git a/src/host/layer23/include/osmocom/bb/common/sap_fsm.h b/src/host/layer23/include/osmocom/bb/common/sap_fsm.h
new file mode 100644
index 0000000..d79bc1c
--- /dev/null
+++ b/src/host/layer23/include/osmocom/bb/common/sap_fsm.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <osmocom/bb/common/osmocom_data.h>
+
+/* How long should we wait for connection establishment */
+#define SAP_FSM_CONN_EST_TIMEOUT 5
+#define SAP_FSM_CONN_EST_T 0
+
+/* How long should we wait for connection release */
+#define SAP_FSM_CONN_REL_TIMEOUT 3
+#define SAP_FSM_CONN_REL_T 1
+
+/* How long should we wait for request to complete */
+#define SAP_FSM_PROC_REQ_TIMEOUT 5
+#define SAP_FSM_PROC_REQ_T 2
+
+#define SAP_STATE_IS_ACTIVE(state) \
+ (state >= SAP_STATE_WAIT_FOR_CARD)
+
+enum sap_fsm_state {
+ SAP_STATE_NOT_CONNECTED = 0,
+ SAP_STATE_CONNECTING,
+ SAP_STATE_DISCONNECTING, /* Auxiliary state (not from specs) */
+ SAP_STATE_WAIT_FOR_CARD, /* Auxiliary state (not from specs) */
+ SAP_STATE_IDLE,
+ SAP_STATE_PROC_ATR_REQ,
+ SAP_STATE_PROC_APDU_REQ,
+ SAP_STATE_PROC_RESET_REQ,
+ SAP_STATE_PROC_STATUS_REQ,
+ SAP_STATE_PROC_SET_TP_REQ,
+ SAP_STATE_PROC_POWERON_REQ,
+ SAP_STATE_PROC_POWEROFF_REQ,
+};
+
+int sap_fsm_alloc(struct osmocom_ms *ms);
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 96d056b..87a0f85 100644
--- a/src/host/layer23/include/osmocom/bb/common/sap_interface.h
+++ b/src/host/layer23/include/osmocom/bb/common/sap_interface.h
@@ -1,18 +1,23 @@
#pragma once
-typedef int (*sap_cb_t)(struct msgb *msg, struct osmocom_ms *ms);
+#include <stdint.h>
+#include <osmocom/core/msgb.h>
+
+struct osmocom_ms;
+
+typedef int (*sap_msg_cb_t)(struct osmocom_ms *ms, struct msgb *msg);
+typedef int (*sap_rsp_cb_t)(struct osmocom_ms *ms, int res_code,
+ uint8_t res_type, uint16_t param_len, const uint8_t *param_val);
+
+void sap_init(struct osmocom_ms *ms);
int sap_open(struct osmocom_ms *ms);
int sap_close(struct osmocom_ms *ms);
-int sap_send_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t length);
-int sap_register_handler(struct osmocom_ms *ms, sap_cb_t cb);
-int sap_init(struct osmocom_ms *ms);
+int _sap_close_sock(struct osmocom_ms *ms);
+
+int sap_send_reset_req(struct osmocom_ms *ms);
+int sap_send_poweron_req(struct osmocom_ms *ms);
+int sap_send_poweroff_req(struct osmocom_ms *ms);
-enum sap_state {
- SAP_SOCKET_ERROR,
- SAP_NOT_CONNECTED,
- SAP_IDLE,
- SAP_CONNECTION_UNDER_NEGOTIATION,
- SAP_PROCESSING_ATR_REQUEST,
- SAP_PROCESSING_APDU_REQUEST
-};
+int sap_send_atr_req(struct osmocom_ms *ms);
+int sap_send_apdu(struct osmocom_ms *ms, uint8_t *apdu, uint16_t apdu_len);
diff --git a/src/host/layer23/include/osmocom/bb/common/sap_proto.h b/src/host/layer23/include/osmocom/bb/common/sap_proto.h
index 49b30fc..e149f00 100644
--- a/src/host/layer23/include/osmocom/bb/common/sap_proto.h
+++ b/src/host/layer23/include/osmocom/bb/common/sap_proto.h
@@ -3,8 +3,10 @@
#include <stdint.h>
#include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
-/* Table 5.1: Message Overview */
+/* Table 5.1: Message Overview
+ * NOTE: messages are used as events for SAP FSM */
enum sap_msg_type {
SAP_CONNECT_REQ = 0x00,
SAP_CONNECT_RESP = 0x01,
@@ -81,14 +83,39 @@ extern const struct value_string sap_result_names[];
extern const struct value_string sap_card_status_names[];
extern const struct value_string sap_conn_status_names[];
+/* Figure 5.2: Payload Coding */
struct sap_param {
- uint8_t id;
- uint16_t len;
- uint8_t *value;
-};
+ /* Parameter ID, see sap_param_type enum */
+ uint8_t param_id;
+ /* Reserved for further use (shall be set to 0x00) */
+ uint8_t reserved[1];
+ /* Parameter length */
+ uint16_t length;
+ /* Parameter value (and optional padding) */
+ uint8_t value[0];
+} __attribute__((packed));
-struct sap_msg {
- uint8_t id;
+/* Figure 5.1 Message Format */
+struct sap_message {
+ /* Message ID, see sap_msg_type enum */
+ uint8_t msg_id;
+ /* Number of parameters */
uint8_t num_params;
- struct sap_param *params;
-};
+ /* Reserved for further use (shall be set to 0x00) */
+ uint8_t reserved[2];
+ /* Payload, see sap_param struct */
+ uint8_t payload[0];
+} __attribute__((packed));
+
+#define GSM_SAP_LENGTH 300
+#define GSM_SAP_HEADROOM 32
+
+struct msgb *sap_msgb_alloc(uint8_t msg_id);
+struct msgb *sap_msg_parse(const uint8_t *buf, size_t buf_len, int max_msg_size);
+int sap_check_result_code(const struct sap_message *sap_msg);
+
+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_param *sap_get_param(const struct sap_message *sap_msg,
+ enum sap_param_type param_type, uint16_t *param_len);
diff --git a/src/host/layer23/include/osmocom/bb/mobile/subscriber.h b/src/host/layer23/include/osmocom/bb/mobile/subscriber.h
index c747af9..698b0fd 100644
--- a/src/host/layer23/include/osmocom/bb/mobile/subscriber.h
+++ b/src/host/layer23/include/osmocom/bb/mobile/subscriber.h
@@ -90,6 +90,8 @@ int gsm_subscr_init(struct osmocom_ms *ms);
int gsm_subscr_exit(struct osmocom_ms *ms);
int gsm_subscr_testcard(struct osmocom_ms *ms, uint16_t mcc, uint16_t mnc,
uint16_t lac, uint32_t tmsi, uint8_t imsi_attached);
+int gsm_subscr_sap_rsp_cb(struct osmocom_ms *ms, int res_code,
+ uint8_t res_type, uint16_t param_len, const uint8_t *param_val);
int gsm_subscr_sapcard(struct osmocom_ms *ms);
int gsm_subscr_remove_sapcard(struct osmocom_ms *ms);
int gsm_subscr_simcard(struct osmocom_ms *ms);
diff --git a/src/host/layer23/src/common/Makefile.am b/src/host/layer23/src/common/Makefile.am
index 9eed961..a8d9f7e 100644
--- a/src/host/layer23/src/common/Makefile.am
+++ b/src/host/layer23/src/common/Makefile.am
@@ -2,5 +2,5 @@ AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include
AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBGPS_CFLAGS)
noinst_LIBRARIES = liblayer23.a
-liblayer23_a_SOURCES = l1ctl.c l1l2_interface.c sap_proto.c sap_interface.c \
+liblayer23_a_SOURCES = l1ctl.c l1l2_interface.c sap_fsm.c sap_proto.c sap_interface.c \
logging.c networks.c sim.c sysinfo.c gps.c l1ctl_lapdm_glue.c utils.c
diff --git a/src/host/layer23/src/common/sap_fsm.c b/src/host/layer23/src/common/sap_fsm.c
new file mode 100644
index 0000000..9591dab
--- /dev/null
+++ b/src/host/layer23/src/common/sap_fsm.c
@@ -0,0 +1,690 @@
+/*
+ * SAP (SIM Access Profile) FSM definition
+ * based on Bluetooth SAP specification
+ *
+ * (C) 2018-2019 by Vadim Yanitskiy <axilirator@gmail.com>
+ *
+ * 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 <errno.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <arpa/inet.h>
+
+#include <osmocom/core/socket.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/fsm.h>
+
+#include <osmocom/bb/common/osmocom_data.h>
+#include <osmocom/bb/common/logging.h>
+
+#include <osmocom/bb/common/sap_interface.h>
+#include <osmocom/bb/common/sap_proto.h>
+#include <osmocom/bb/common/sap_fsm.h>
+
+/*! Send encoded SAP message to the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \param[in] msg encoded SAP message buffer
+ * \returns 0 in case of success, negative in case of error
+ */
+static int sap_send_msgb(struct osmocom_ms *ms, struct msgb *msg)
+{
+ int rc;
+
+ rc = osmo_wqueue_enqueue(&ms->sap_wq, msg);
+ if (rc) {
+ LOGP(DSAP, LOGL_ERROR, "Failed to enqueue SAP message\n");
+ msgb_free(msg);
+ return rc;
+ }
+
+ return 0;
+}
+
+static void sap_fsm_disconnect(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+ osmo_fsm_inst_term(fi, OSMO_FSM_TERM_REGULAR, NULL);
+}
+
+static void sap_fsm_connect(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+ struct msgb *msg;
+ uint16_t size;
+
+ /* Section 5.1.1, CONNECT_REQ */
+ msg = sap_msgb_alloc(SAP_CONNECT_REQ);
+ if (!msg)
+ return;
+
+ /* Section 4.1.1, start MaxMsgSize negotiation */
+ size = htons(ms->sap_entity.max_msg_size);
+ sap_msgb_add_param(msg, SAP_MAX_MSG_SIZE,
+ sizeof(size), (uint8_t *) &size);
+
+ sap_send_msgb(ms, msg);
+}
+
+static void sap_negotiate_msg_size(struct osmosap_entity *sap,
+ const struct sap_message *sap_msg)
+{
+ uint16_t size, param_len;
+ const char *cause = NULL;
+ struct sap_param *param;
+
+ param = sap_get_param(sap_msg, SAP_MAX_MSG_SIZE, &param_len);
+ if (!param) {
+ cause = "missing expected MaxMsgSize parameter";
+ goto error;
+ }
+ if (param_len != sizeof(size)) {
+ cause = "MaxMsgSize parameter has wrong length";
+ goto error;
+ }
+
+ /* Parse MaxMsgSize suggested by server */
+ size = ntohs((uint16_t *) param->value);
+ if (size > SAP_MAX_MSG_SIZE) {
+ cause = "suggested MaxMsgSize is too big for us";
+ goto error;
+ }
+
+ /* Attempt to initiate connection again */
+ sap->max_msg_size = size;
+ sap_fsm_connect(sap->fi, sap->fi->state);
+ return;
+
+error:
+ LOGP(DSAP, LOGL_ERROR, "MaxMsgSize negotiation failed: %s\n", cause);
+ osmo_fsm_inst_state_chg(sap->fi, SAP_STATE_NOT_CONNECTED, 0, 0);
+}
+
+static void sap_fsm_conn_handler(struct osmo_fsm_inst *fi,
+ uint32_t event, void *data)
+{
+ struct sap_message *sap_msg = (struct sap_message *) data;
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+ struct sap_param *param;
+ uint16_t param_len;
+ uint8_t status;
+
+ /* Section 5.1.2, CONNECT_RESP */
+ param = sap_get_param(sap_msg, SAP_CONNECTION_STATUS, &param_len);
+ if (!param || param_len != sizeof(status)) {
+ LOGP(DSAP, LOGL_ERROR, "Missing mandatory connection status\n");
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);
+ return;
+ }
+
+ /* Parse connection status */
+ status = param->value[0];
+
+ LOGP(DSAP, LOGL_INFO, "SAP connection status (0x%02x): %s\n",
+ status, get_value_string(sap_conn_status_names, status));
+
+ switch ((enum sap_conn_status_type) status) {
+ case SAP_CONN_STATUS_OK_CALL:
+ ms->sap_entity.card_status = SAP_CARD_STATUS_NOT_ACC;
+ /* fall-through */
+ case SAP_CONN_STATUS_OK_READY:
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_WAIT_FOR_CARD, 0, 0);
+ break;
+
+ case SAP_CONN_STATUS_ERROR_SMALL_MSG_SIZE:
+ case SAP_CONN_STATUS_ERROR_CONN:
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);
+ break;
+
+ /* Section 4.1.1, MaxMsgSize negotiation */
+ case SAP_CONN_STATUS_ERROR_MAX_MSG_SIZE:
+ sap_negotiate_msg_size(&ms->sap_entity, sap_msg);
+ break;
+ }
+}
+
+static void sap_fsm_conn_release(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+ struct msgb *msg;
+
+ LOGP(DSAP, LOGL_DEBUG, "Initiating connection release\n");
+
+ /* We don't care about possible allocating / sending errors */
+ msg = sap_msgb_alloc(SAP_DISCONNECT_REQ);
+ if (msg != NULL)
+ sap_send_msgb((struct osmocom_ms *) fi->priv, msg);
+}
+
+static void sap_fsm_release_handler(struct osmo_fsm_inst *fi,
+ uint32_t event, void *data)
+{
+ LOGP(DSAP, LOGL_DEBUG, "Connection release complete\n");
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);
+}
+
+static void sap_fsm_idle_enter(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+
+ switch ((enum sap_fsm_state) prev_state) {
+ case SAP_STATE_CONNECTING:
+ case SAP_STATE_WAIT_FOR_CARD:
+ /* According to 4.1, if a subscription module is inserted
+ * in the Server and powered on (i.e. STATUS_IND message
+ * indicates "Card reset" state), the Client shall request
+ * the ATR of the subscription module. */
+ if (ms->sap_entity.card_status == SAP_CARD_STATUS_RESET)
+ sap_send_atr_req(ms);
+ break;
+ default:
+ /* Do nothing, suppress compiler warning */
+ break;
+ }
+}
+
+static void sap_fsm_idle_handler(struct osmo_fsm_inst *fi,
+ uint32_t event, void *data)
+{
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+ struct msgb *msg = (struct msgb *) data;
+ enum sap_fsm_state state;
+ int rc;
+
+ /* Map event to the corresponding state */
+ switch ((enum sap_msg_type) event) {
+ case SAP_TRANSFER_ATR_REQ:
+ state = SAP_STATE_PROC_ATR_REQ;
+ break;
+ case SAP_TRANSFER_APDU_REQ:
+ state = SAP_STATE_PROC_APDU_REQ;
+ break;
+ case SAP_RESET_SIM_REQ:
+ state = SAP_STATE_PROC_RESET_REQ;
+ break;
+ case SAP_TRANSFER_CARD_READER_STATUS_REQ:
+ state = SAP_STATE_PROC_STATUS_REQ;
+ break;
+ case SAP_SET_TRANSPORT_PROTOCOL_REQ:
+ state = SAP_STATE_PROC_SET_TP_REQ;
+ break;
+ case SAP_POWER_SIM_ON_REQ:
+ state = SAP_STATE_PROC_POWERON_REQ;
+ break;
+ case SAP_POWER_SIM_OFF_REQ:
+ state = SAP_STATE_PROC_POWEROFF_REQ;
+ break;
+ default:
+ /* Shall not happen */
+ OSMO_ASSERT(0);
+ }
+
+ rc = sap_send_msgb(ms, msg);
+ if (rc)
+ return;
+
+ osmo_fsm_inst_state_chg(fi, state,
+ SAP_FSM_PROC_REQ_TIMEOUT, SAP_FSM_PROC_REQ_T);
+}
+
+static void sap_fsm_response_handler(struct osmo_fsm_inst *fi,
+ uint32_t event, void *data)
+{
+ struct sap_message *sap_msg = (struct sap_message *) data;
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+ struct sap_param *param = NULL;
+ uint16_t param_len = 0;
+ int param_id, rc;
+
+ switch ((enum sap_msg_type) event) {
+ /* Both POWER_SIM_OFF_REQ and RESET_SIM_REQ can be sent in nearly
+ * any state, in order to allow the Client to reactivate
+ * a not accessible subscription module card. */
+ case SAP_POWER_SIM_OFF_REQ:
+ case SAP_RESET_SIM_REQ:
+ OSMO_ASSERT(data != NULL);
+ goto request;
+
+ /* Messages without parameters */
+ case SAP_SET_TRANSPORT_PROTOCOL_RESP:
+ case SAP_POWER_SIM_OFF_RESP:
+ case SAP_POWER_SIM_ON_RESP:
+ case SAP_RESET_SIM_RESP:
+ param_id = -1;
+ break;
+
+ case SAP_TRANSFER_CARD_READER_STATUS_RESP:
+ param_id = SAP_CARD_READER_STATUS;
+ break;
+ case SAP_TRANSFER_APDU_RESP:
+ param_id = SAP_RESPONSE_APDU;
+ break;
+ case SAP_TRANSFER_ATR_RESP:
+ param_id = SAP_ATR;
+ break;
+
+ default:
+ /* Shall not happen */
+ OSMO_ASSERT(0);
+ }
+
+ /* We're done with request now */
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);
+
+ /* Check the ResultCode */
+ rc = sap_check_result_code(sap_msg);
+ if (rc != SAP_RESULT_OK_REQ_PROC_CORR) {
+ LOGP(DSAP, LOGL_NOTICE, "Bad ResultCode: '%s'\n",
+ get_value_string(sap_result_names, rc));
+ goto response;
+ }
+
+ if (param_id < 0)
+ goto response;
+
+ param = sap_get_param(sap_msg, param_id, &param_len);
+ if (!param) {
+ LOGP(DSAP, LOGL_ERROR, "Message '%s' missing "
+ "mandatory parameter '%s'\n",
+ get_value_string(sap_msg_names, sap_msg->msg_id),
+ get_value_string(sap_param_names, param_id));
+ rc = -EINVAL;
+ goto response;
+ }
+
+response:
+ /* Poke optional response handler */
+ if (ms->sap_entity.sap_rsp_cb != NULL) {
+ if (param != NULL) {
+ ms->sap_entity.sap_rsp_cb(ms, rc,
+ sap_msg->msg_id, param_len, param->value);
+ } else {
+ ms->sap_entity.sap_rsp_cb(ms, rc,
+ sap_msg->msg_id, 0, NULL);
+ }
+ }
+
+ return;
+
+request:
+ rc = sap_send_msgb(ms, (struct msgb *) data);
+ if (rc)
+ return;
+
+ osmo_fsm_inst_state_chg(fi, event == SAP_RESET_SIM_REQ ?
+ SAP_STATE_PROC_RESET_REQ : SAP_STATE_PROC_POWEROFF_REQ,
+ SAP_FSM_PROC_REQ_TIMEOUT, SAP_FSM_PROC_REQ_T);
+}
+
+/* Generates mask for a single state or event */
+#define S(x) (1 << x)
+
+/* Figure 4.13: Simplified State Machine */
+static const struct osmo_fsm_state sap_fsm_states[] = {
+ [SAP_STATE_NOT_CONNECTED] = {
+ .name = "NOT_CONNECTED",
+ .out_state_mask = S(SAP_STATE_CONNECTING),
+ .onenter = &sap_fsm_disconnect,
+ },
+ [SAP_STATE_CONNECTING] = {
+ .name = "CONNECTING",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_WAIT_FOR_CARD),
+ .in_event_mask = S(SAP_CONNECT_RESP),
+ .onenter = &sap_fsm_connect,
+ .action = &sap_fsm_conn_handler,
+ },
+ /* NOTE: this is a custom state (i.e. not defined by the specs).
+ * We need it in order to do release procedure correctly. */
+ [SAP_STATE_DISCONNECTING] = {
+ .name = "DISCONNECTING",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED),
+ .in_event_mask = S(SAP_DISCONNECT_RESP),
+ .onenter = &sap_fsm_conn_release,
+ .action = &sap_fsm_release_handler,
+ },
+ /* NOTE: this is a custom state (i.e. not defined by the specs).
+ * We need it in order to wait until SIM card becomes available.
+ * SAP_STATUS_IND event is handled by sap_fsm_allstate_action(). */
+ [SAP_STATE_WAIT_FOR_CARD] = {
+ .name = "WAIT_FOR_CARD",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_IDLE),
+ },
+ [SAP_STATE_IDLE] = {
+ .name = "IDLE",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_PROC_APDU_REQ)
+ | S(SAP_STATE_PROC_ATR_REQ)
+ | S(SAP_STATE_PROC_RESET_REQ)
+ | S(SAP_STATE_PROC_STATUS_REQ)
+ | S(SAP_STATE_PROC_SET_TP_REQ)
+ | S(SAP_STATE_PROC_POWERON_REQ)
+ | S(SAP_STATE_PROC_POWEROFF_REQ),
+ .in_event_mask = S(SAP_TRANSFER_ATR_REQ)
+ | S(SAP_TRANSFER_APDU_REQ)
+ | S(SAP_RESET_SIM_REQ)
+ | S(SAP_TRANSFER_CARD_READER_STATUS_REQ)
+ | S(SAP_SET_TRANSPORT_PROTOCOL_REQ)
+ | S(SAP_POWER_SIM_ON_REQ)
+ | S(SAP_POWER_SIM_OFF_REQ),
+ .onenter = &sap_fsm_idle_enter,
+ .action = &sap_fsm_idle_handler,
+ },
+ [SAP_STATE_PROC_ATR_REQ] = {
+ .name = "PROC_ATR_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE)
+ | S(SAP_STATE_PROC_RESET_REQ)
+ | S(SAP_STATE_PROC_POWEROFF_REQ),
+ .in_event_mask = S(SAP_TRANSFER_ATR_RESP)
+ | S(SAP_RESET_SIM_REQ)
+ | S(SAP_POWER_SIM_OFF_REQ),
+ .action = &sap_fsm_response_handler,
+ },
+ [SAP_STATE_PROC_APDU_REQ] = {
+ .name = "PROC_APDU_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE)
+ | S(SAP_STATE_PROC_RESET_REQ)
+ | S(SAP_STATE_PROC_POWEROFF_REQ),
+ .in_event_mask = S(SAP_TRANSFER_APDU_RESP)
+ | S(SAP_RESET_SIM_REQ)
+ | S(SAP_POWER_SIM_OFF_REQ),
+ .action = &sap_fsm_response_handler,
+ },
+ [SAP_STATE_PROC_RESET_REQ] = {
+ .name = "PROC_RESET_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE)
+ | S(SAP_STATE_PROC_POWEROFF_REQ),
+ .in_event_mask = S(SAP_RESET_SIM_RESP)
+ | S(SAP_POWER_SIM_OFF_REQ),
+ .action = &sap_fsm_response_handler,
+ },
+ [SAP_STATE_PROC_STATUS_REQ] = {
+ .name = "PROC_STATUS_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE)
+ | S(SAP_STATE_PROC_RESET_REQ)
+ | S(SAP_STATE_PROC_POWEROFF_REQ),
+ .in_event_mask = S(SAP_TRANSFER_CARD_READER_STATUS_RESP)
+ | S(SAP_RESET_SIM_REQ)
+ | S(SAP_POWER_SIM_OFF_REQ),
+ .action = &sap_fsm_response_handler,
+ },
+ [SAP_STATE_PROC_SET_TP_REQ] = {
+ .name = "PROC_SET_TP_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE),
+ .in_event_mask = S(SAP_SET_TRANSPORT_PROTOCOL_RESP),
+ .action = &sap_fsm_response_handler,
+ },
+ [SAP_STATE_PROC_POWERON_REQ] = {
+ .name = "PROC_POWERON_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE)
+ | S(SAP_STATE_PROC_POWEROFF_REQ),
+ .in_event_mask = S(SAP_POWER_SIM_ON_RESP)
+ | S(SAP_POWER_SIM_OFF_REQ),
+ .action = &sap_fsm_response_handler,
+ },
+ [SAP_STATE_PROC_POWEROFF_REQ] = {
+ .name = "PROC_POWEROFF_REQ",
+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)
+ | S(SAP_STATE_DISCONNECTING)
+ | S(SAP_STATE_WAIT_FOR_CARD)
+ | S(SAP_STATE_IDLE),
+ .in_event_mask = S(SAP_POWER_SIM_OFF_RESP),
+ .action = &sap_fsm_response_handler,
+ },
+};
+
+static void sap_fsm_tear_down(struct osmo_fsm_inst *fi,
+ enum osmo_fsm_term_cause cause)
+{
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+
+ /* Flush buffers, close socket */
+ _sap_close_sock(ms);
+
+ /* Reset SAP state */
+ ms->sap_entity.card_status = SAP_CARD_STATUS_NOT_ACC;
+ ms->sap_entity.max_msg_size = GSM_SAP_LENGTH;
+ ms->sap_entity.fi = NULL;
+}
+
+static void sap_fsm_handle_card_status_ind(struct osmo_fsm_inst *fi,
+ const struct sap_message *sap_msg)
+{
+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;
+ struct sap_param *param;
+ uint16_t param_len;
+ uint8_t status;
+
+ param = sap_get_param(sap_msg, SAP_STATUS_CHANGE, &param_len);
+ if (!param || param_len != sizeof(status)) {
+ LOGP(DSAP, LOGL_ERROR, "Missing mandatory '%s' parameter\n",
+ get_value_string(sap_param_names, SAP_STATUS_CHANGE));
+ return;
+ }
+
+ status = param->value[0];
+
+ if (ms->sap_entity.card_status != status) {
+ LOGP(DSAP, LOGL_NOTICE, "(SIM) card status change '%s' -> '%s'\n",
+ get_value_string(sap_card_status_names, ms->sap_entity.card_status),
+ get_value_string(sap_card_status_names, status));
+ ms->sap_entity.card_status = status;
+ }
+
+ switch ((enum sap_card_status_type) status) {
+ /* SIM card is ready */
+ case SAP_CARD_STATUS_RESET:
+ if (fi->state != SAP_STATE_IDLE)
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);
+ break;
+
+ /* SIM card has recovered after unaccessful state */
+ case SAP_CARD_STATUS_RECOVERED:
+ if (fi->state != SAP_STATE_IDLE)
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);
+ break;
+
+ /* SIM card inserted, we need to power it on */
+ case SAP_CARD_STATUS_INSERTED:
+ if (fi->state != SAP_STATE_IDLE)
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);
+ sap_send_poweron_req(ms);
+ break;
+
+ case SAP_CARD_STATUS_UNKNOWN_ERROR:
+ case SAP_CARD_STATUS_NOT_ACC:
+ case SAP_CARD_STATUS_REMOVED:
+ default: /* Unknown card status */
+ if (fi->state != SAP_STATE_WAIT_FOR_CARD)
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_WAIT_FOR_CARD, 0, 0);
+ break;
+ }
+}
+
+static void sap_fsm_allstate_action(struct osmo_fsm_inst *fi,
+ uint32_t event, void *data)
+{
+ struct sap_message *sap_msg = (struct sap_message *) data;
+
+ switch ((enum sap_msg_type) event) {
+ /* Disconnect indication initiated by the Server.
+ * FIXME: at the moment, immediate release is always assumed,
+ * but ideally we should check type of release (using *data) */
+ case SAP_DISCONNECT_IND:
+ /* This message may arrive in any of the sub-states of
+ * the "Connected" state (i.e. connection shall exist) */
+ if (!SAP_STATE_IS_ACTIVE(fi->state))
+ goto not_peritted;
+
+ sap_msg = NULL;
+ /* fall-through */
+
+ /* Disconnect initiated by the Client */
+ case SAP_DISCONNECT_REQ:
+ /* DISCONNECT_REQ has no parameters, so the caller
+ * shall not allocate the message manually. */
+ OSMO_ASSERT(sap_msg == NULL);
+
+ /* If we have no active connection, tear-down immediately */
+ if (!SAP_STATE_IS_ACTIVE(fi->state)) {
+ osmo_fsm_inst_state_chg(fi,
+ SAP_STATE_NOT_CONNECTED, 0, 0);
+ break;
+ }
+
+ /* Trigger Client-initiated connection release */
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_DISCONNECTING,
+ SAP_FSM_CONN_REL_TIMEOUT, SAP_FSM_CONN_REL_T);
+ break;
+
+ /* SIM status indication (inserted or ejected) */
+ case SAP_STATUS_IND:
+ /* This message may arrive in any of the sub-states of
+ * the "Connected" state (i.e. connection shall exist) */
+ if (!SAP_STATE_IS_ACTIVE(fi->state))
+ goto not_peritted;
+
+ sap_fsm_handle_card_status_ind(fi, sap_msg);
+ break;
+
+ case SAP_ERROR_RESP:
+ LOGP(DSAP, LOGL_NOTICE, "RX Error Response from Server\n");
+
+ if (fi->state == SAP_STATE_CONNECTING) {
+ /* Connection establishment error */
+ osmo_fsm_inst_state_chg(fi,
+ SAP_STATE_NOT_CONNECTED, 0, 0);
+ } else if (fi->state > SAP_STATE_IDLE) {
+ /* Error replaces any Request message */
+ osmo_fsm_inst_state_chg(fi,
+ SAP_STATE_IDLE, 0, 0);
+ } else {
+ /* Should not happen in general */
+ goto not_peritted;
+ }
+ break;
+
+ default:
+ /* Shall not happen */
+ OSMO_ASSERT(0);
+ }
+
+ return;
+
+not_peritted:
+ LOGPFSML(fi, LOGL_NOTICE, "Event '%s' is not "
+ "permitted in state '%s', please fix!\n",
+ osmo_fsm_event_name(fi->fsm, event),
+ osmo_fsm_state_name(fi->fsm, fi->state));
+}
+
+static int sap_fsm_timer_cb(struct osmo_fsm_inst *fi)
+{
+ switch ((enum sap_fsm_state) fi->state) {
+ /* Connection establishment / release timeout */
+ case SAP_STATE_DISCONNECTING:
+ case SAP_STATE_CONNECTING:
+ LOGP(DSAP, LOGL_NOTICE, "Connection timeout\n");
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);
+ break;
+
+ /* Request processing timeout */
+ case SAP_STATE_PROC_ATR_REQ:
+ case SAP_STATE_PROC_APDU_REQ:
+ case SAP_STATE_PROC_RESET_REQ:
+ case SAP_STATE_PROC_STATUS_REQ:
+ case SAP_STATE_PROC_SET_TP_REQ:
+ case SAP_STATE_PROC_POWERON_REQ:
+ case SAP_STATE_PROC_POWEROFF_REQ:
+ LOGP(DSAP, LOGL_NOTICE, "Timeout waiting for '%s' to complete, "
+ "going back to IDLE\n", osmo_fsm_inst_state_name(fi));
+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);
+ break;
+
+ default:
+ LOGP(DSAP, LOGL_ERROR, "Timeout for unhandled state '%s'\n",
+ osmo_fsm_inst_state_name(fi));
+ }
+
+ /* Do not tear-down FSM */
+ return 0;
+}
+
+static struct osmo_fsm sap_fsm_def = {
+ .name = "sap_fsm",
+ .log_subsys = DSAP,
+ .states = sap_fsm_states,
+ .num_states = ARRAY_SIZE(sap_fsm_states),
+ .event_names = sap_msg_names,
+ .cleanup = &sap_fsm_tear_down,
+ .timer_cb = &sap_fsm_timer_cb,
+ .allstate_action = &sap_fsm_allstate_action,
+ .allstate_event_mask = 0
+ | S(SAP_DISCONNECT_REQ)
+ | S(SAP_DISCONNECT_IND)
+ | S(SAP_STATUS_IND)
+ | S(SAP_ERROR_RESP),
+};
+
+/*! Allocate a new SAP state machine for a given ms.
+ * \param[in] ms MS instance associated with SAP FSM
+ * \returns 0 in case of success, negative in case of error
+ */
+int sap_fsm_alloc(struct osmocom_ms *ms)
+{
+ struct osmosap_entity *sap;
+
+ sap = &ms->sap_entity;
+ OSMO_ASSERT(sap->fi == NULL);
+
+ /* Register our FSM (if required) */
+ if (!osmo_fsm_find_by_name(sap_fsm_def.name))
+ OSMO_ASSERT(osmo_fsm_register(&sap_fsm_def) == 0);
+
+ /* Allocate an instance using ms as talloc context */
+ sap->fi = osmo_fsm_inst_alloc(&sap_fsm_def, ms,
+ ms, LOGL_DEBUG, ms->name);
+ if (!sap->fi) {
+ LOGP(DSAP, LOGL_ERROR, "Failed to allocate SAP FSM\n");
+ return -ENOMEM;
+ }
+
+ return 0;
+}
diff --git a/src/host/layer23/src/common/sap_interface.c b/src/host/layer23/src/common/sap_interface.c
index 7b8717d..0eac896 100644
--- a/src/host/layer23/src/common/sap_interface.c
+++ b/src/host/layer23/src/common/sap_interface.c
@@ -4,6 +4,7 @@
* (C) 2010,2018 by Harald Welte <laforge@gnumonks.org>
* (C) 2010 by Andreas Eversberg <jolly@eversberg.eu>
* (C) 2011 by Nico Golde <nico@ngolde.de>
+ * (C) 2018 by Vadim Yanitskiy <axilirator@gmail.com>
*
* All Rights Reserved
*
@@ -23,487 +24,325 @@
*
*/
-#include <osmocom/bb/common/osmocom_data.h>
-#include <osmocom/bb/common/logging.h>
-#include <osmocom/bb/common/sap_interface.h>
-#include <osmocom/bb/common/sap_proto.h>
-
-#include <osmocom/core/utils.h>
-#include <osmocom/core/talloc.h>
-#include <osmocom/core/socket.h>
+#include <unistd.h>
+#include <errno.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>
+#include <osmocom/core/write_queue.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/fsm.h>
-#define GSM_SAP_LENGTH 300
-#define GSM_SAP_HEADROOM 32
+#include <osmocom/bb/common/osmocom_data.h>
+#include <osmocom/bb/common/logging.h>
-static void sap_connect(struct osmocom_ms *ms);
+#include <osmocom/bb/common/sap_interface.h>
+#include <osmocom/bb/common/sap_proto.h>
+#include <osmocom/bb/common/sap_fsm.h>
-static struct msgb *sap_create_msg(uint8_t id, uint8_t num_params, struct sap_param *params)
+/*! Send ATR request to the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \returns 0 in case of success, negative in case of error
+ */
+int sap_send_atr_req(struct osmocom_ms *ms)
{
struct msgb *msg;
- uint8_t *msgp;
- uint8_t i, plen, padding = 0;
-
- msg = msgb_alloc(GSM_SAP_LENGTH, "osmosap");
- if (!msg) {
- LOGP(DSAP, LOGL_ERROR, "Failed to allocate msg.\n");
- return NULL;
- }
+ int rc;
- /* 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);
- }
+ if (!ms->sap_entity.fi) {
+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");
+ return -EAGAIN;
}
- return msg;
-}
-
-static int sap_send(struct osmocom_ms *ms, struct msgb *msg)
-{
- if(ms->sap_entity.sap_state == SAP_NOT_CONNECTED)
- sap_connect(ms);
-
- if (ms->sap_wq.bfd.fd <= 0)
- return -EINVAL;
+ msg = sap_msgb_alloc(SAP_TRANSFER_ATR_REQ);
+ if (!msg)
+ return -ENOMEM;
- if (osmo_wqueue_enqueue(&ms->sap_wq, msg) != 0) {
- LOGP(DSAP, LOGL_ERROR, "Failed to enqueue msg.\n");
+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,
+ SAP_TRANSFER_ATR_REQ, msg);
+ if (rc) {
msgb_free(msg);
- return -1;
+ return rc;
}
return 0;
}
-static int sap_parse_result(struct sap_param *param)
+/*! Send APDU request to the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \param[in] apdu APDU to be send
+ * \param[in] apdu_len length of APDU
+ * \returns 0 in case of success, negative in case of error
+ */
+int sap_send_apdu(struct osmocom_ms *ms, uint8_t *apdu, uint16_t apdu_len)
{
- 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]));
- }
+ struct msgb *msg;
+ int rc;
-#if 0
- /* FIXME: ARRAY_SIZE() is not applicable to
- * extern const struct value_string[] */
- if(param->value[0] > ARRAY_SIZE(sap_result_names)){
- return -1;
+ if (!ms->sap_entity.fi) {
+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");
+ return -EAGAIN;
}
-#endif
- 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;
+ msg = sap_msgb_alloc(SAP_TRANSFER_APDU_REQ);
+ if (!msg)
+ return -ENOMEM;
- return dptr;
-}
+ sap_msgb_add_param(msg, SAP_COMMAND_APDU, apdu_len, apdu);
-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);
+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,
+ SAP_TRANSFER_APDU_REQ, msg);
+ if (rc) {
+ msgb_free(msg);
+ return rc;
}
- talloc_free(msg);
+
+ return 0;
}
-static struct sap_msg *sap_parse_msg(uint8_t *data)
+/*! Send (SIM) reset request to the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \returns 0 in case of success, negative in case of error
+ */
+int sap_send_reset_req(struct osmocom_ms *ms)
{
- struct sap_msg *msg = talloc_zero(NULL, struct sap_msg);
- uint8_t *ptr = data;
- uint8_t i;
+ struct msgb *msg;
+ int rc;
- if(!msg){
- return NULL;
+ if (!ms->sap_entity.fi) {
+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");
+ return -EAGAIN;
}
- 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);
+ msg = sap_msgb_alloc(SAP_RESET_SIM_REQ);
+ if (!msg)
+ return -ENOMEM;
- 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));
+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,
+ SAP_RESET_SIM_REQ, msg);
+ if (rc) {
+ msgb_free(msg);
+ return rc;
}
- return msg;
+ return 0;
}
-static void sap_apdu_resp(struct osmocom_ms *ms, uint8_t *data, uint16_t len)
+/*! Send (SIM) power on request to the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \returns 0 in case of success, negative in case of error
+ */
+int sap_send_poweron_req(struct osmocom_ms *ms)
{
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;
- }
+ int rc;
- apdu = msgb_put(msg, len);
- memcpy(apdu, data, len);
+ if (!ms->sap_entity.fi) {
+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");
+ return -EAGAIN;
+ }
- LOGP(DSAP, LOGL_DEBUG, "Forwarding APDU to SIM handler.\n");
- sim_apdu_resp(ms, msg);
-}
+ msg = sap_msgb_alloc(SAP_POWER_SIM_ON_REQ);
+ if (!msg)
+ return -ENOMEM;
-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;
+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,
+ SAP_POWER_SIM_ON_REQ, msg);
+ if (rc) {
+ msgb_free(msg);
+ return rc;
}
+
return 0;
}
-static void sap_atr(struct osmocom_ms *ms)
+/*! Send (SIM) power off request to the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \returns 0 in case of success, negative in case of error
+ */
+int sap_send_poweroff_req(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;
-
- sap_send(ms, msg);
- ms->sap_entity.sap_state = SAP_PROCESSING_ATR_REQUEST;
-}
+ int rc;
-static void sap_parse_resp(struct osmocom_ms *ms, uint8_t *data, uint16_t len)
-{
- struct sap_msg *msg;
- if(len > ms->sap_entity.max_msg_size){
- LOGP(DSAP, LOGL_ERROR, "Read more data than allowed by max_msg_size, ignoring.\n");
- return;
+ if (!ms->sap_entity.fi) {
+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");
+ return -EAGAIN;
}
- msg = sap_parse_msg(data);
- if (!msg) {
- LOGP(DSAP, LOGL_ERROR, "Failed to parse SAP message\n");
- return;
- }
+ msg = sap_msgb_alloc(SAP_POWER_SIM_OFF_REQ);
+ if (!msg)
+ return -ENOMEM;
- switch(msg->id){
- case SAP_CONNECT_RESP:
- LOGP(DSAP, LOGL_INFO, "Status: %s\n", get_value_string(sap_conn_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_card_status_names,
- msg->params[0].value[0]));
- if(msg->params[0].value[0] != SAP_CARD_STATUS_RESET){
- /* 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);
- break;
- }
- 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);
- break;
- }
- if(msg->num_params != 2){
- LOGP(DSAP, LOGL_ERROR, "wrong number of parameters %u in APDU response\n", msg->num_params);
- break;
- }
- 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;
+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,
+ SAP_POWER_SIM_OFF_REQ, msg);
+ if (rc) {
+ msgb_free(msg);
+ return rc;
}
- sap_msg_free(msg);
+ return 0;
}
-static int sap_read(struct osmo_fd *fd)
+static int sap_read_cb(struct osmo_fd *fd)
{
- struct msgb *msg = NULL;
struct osmocom_ms *ms = (struct osmocom_ms *) fd->data;
- uint8_t *sap_buffer;
+ struct osmosap_entity *sap = &ms->sap_entity;
+ uint8_t buf[GSM_SAP_LENGTH];
+ struct msgb *msg;
ssize_t rc;
- sap_buffer = talloc_zero_size(NULL, ms->sap_entity.max_msg_size);
- if(!sap_buffer){
- LOGP(DSAP, LOGL_ERROR, "Failed to allocate memory\n");
- return -ENOMEM;
- }
+ /* Prevent buffer overflow */
+ OSMO_ASSERT(sap->max_msg_size <= GSM_SAP_LENGTH);
- rc = read(fd->fd, sap_buffer, ms->sap_entity.max_msg_size - 1);
+ rc = read(fd->fd, buf, sap->max_msg_size);
if (rc < 0) {
- LOGP(DSAP, LOGL_ERROR,"SAP socket failed\n");
- msgb_free(msg);
- sap_close(ms);
- return rc;
+ LOGP(DSAP, LOGL_ERROR, "SAP socket failed\n");
+ rc = -EIO;
+ goto conn_error;
}
- if(rc == 0) {
+ if (rc == 0) {
LOGP(DSAP, LOGL_NOTICE, "SAP socket closed by server\n");
- msgb_free(msg);
- sap_close(ms);
- return -ECONNREFUSED;
+ rc = -ECONNREFUSED;
+ goto conn_error;
}
- sap_buffer[rc] = 0;
- LOGP(DSAP, LOGL_INFO, "Received %zd bytes: %s\n", rc, osmo_hexdump(sap_buffer, rc));
+ LOGP(DSAP, LOGL_DEBUG, "RX SAP message '%s' (len=%zd): %s\n",
+ get_value_string(sap_msg_names, buf[0]),
+ rc, osmo_hexdump(buf, rc));
- sap_parse_resp(ms, sap_buffer, rc);
-
- talloc_free(sap_buffer);
+ /* Parse received SAP message and allocate a new msgb */
+ msg = sap_msg_parse(buf, rc, sap->max_msg_size);
+ if (!msg) {
+ LOGP(DSAP, LOGL_ERROR, "Failed to parse SAP message\n");
+ return -EINVAL;
+ }
- if (ms->sap_entity.msg_handler){
- ms->sap_entity.msg_handler(msg, ms);
+ /* Pass parsed message to our FSM using message ID as event */
+ rc = osmo_fsm_inst_dispatch(sap->fi, msg->data[0], msg->data);
+ if (rc) {
+ msgb_free(msg);
+ return rc;
}
+
+ /* Pass to (optional) SAP message handler */
+ if (sap->sap_msg_cb)
+ sap->sap_msg_cb(ms, msg);
+ else
+ msgb_free(msg);
+
return 0;
+
+conn_error:
+ /* Immediately tear-down FSM */
+ osmo_fsm_inst_state_chg(sap->fi, SAP_STATE_NOT_CONNECTED, 0, 0);
+ return rc;
}
-static int sap_write(struct osmo_fd *fd, struct msgb *msg)
+static int sap_write_cb(struct osmo_fd *fd, struct msgb *msg)
{
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: %zd\n", rc);
+ LOGP(DSAP, LOGL_ERROR, "Failed to write data\n");
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;
-
- sap_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;
-
- sap_send(ms, msg);
-
- ms->sap_entity.sap_state = SAP_NOT_CONNECTED;
-}
-
-static int sap_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t len)
-{
- struct msgb *msg;
- struct sap_param params[1];
- int rc;
-
- 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 -EIO;
- }
-
- msg = sap_create_msg(SAP_TRANSFER_APDU_REQ, 1, params);
- if(!msg)
- return -ENOMEM;
-
- rc = sap_send(ms, msg);
- if (rc)
- return rc;
-
- ms->sap_entity.sap_state = SAP_PROCESSING_APDU_REQUEST;
+ LOGP(DSAP, LOGL_DEBUG, "TX SAP message '%s' (len=%u): %s\n",
+ get_value_string(sap_msg_names, msg->data[0]),
+ msg->len, osmo_hexdump(msg->data, msg->len));
return 0;
}
+/*! Establishes SAP connection to the Server,
+ * allocates SAP FSM, and triggers connection procedure.
+ * \param[in] ms MS instance with configured SAP socket path
+ * \returns 0 in case of success, negative in case of error
+ */
int sap_open(struct osmocom_ms *ms)
{
int rc;
+ LOGP(DSAP, LOGL_INFO, "Establishing SAP connection "
+ "(using socket '%s')\n", ms->settings.sap_socket_path);
+
rc = osmo_sock_unix_init_ofd(&ms->sap_wq.bfd, SOCK_STREAM, 0,
ms->settings.sap_socket_path, OSMO_SOCK_F_CONNECT);
if (rc < 0) {
LOGP(DSAP, LOGL_ERROR, "Failed to create unix domain socket %s: %s\n",
ms->settings.sap_socket_path, strerror(-rc));
- ms->sap_entity.sap_state = SAP_SOCKET_ERROR;
return rc;
}
osmo_wqueue_init(&ms->sap_wq, 100);
ms->sap_wq.bfd.data = ms;
- ms->sap_wq.read_cb = sap_read;
- ms->sap_wq.write_cb = sap_write;
+ ms->sap_wq.read_cb = &sap_read_cb;
+ ms->sap_wq.write_cb = &sap_write_cb;
- sap_connect(ms);
+ /* Allocate a SAP FSM for a given ms */
+ rc = sap_fsm_alloc(ms);
+ if (rc) {
+ _sap_close_sock(ms);
+ return rc;
+ }
- return 0;
+ /* Initiate SAP connection with Server */
+ LOGP(DSAP, LOGL_DEBUG, "Connecting to the Server...\n");
+ return osmo_fsm_inst_state_chg(ms->sap_entity.fi, SAP_STATE_CONNECTING,
+ SAP_FSM_CONN_EST_TIMEOUT, SAP_FSM_CONN_EST_T);
}
+/*! Closes SAP connection with the Server.
+ * \param[in] ms MS instance with active SAP connection
+ * \returns 0 in case of success, negative in case of error
+ */
int sap_close(struct osmocom_ms *ms)
{
+ if (ms->sap_entity.fi == NULL) {
+ LOGP(DSAP, LOGL_NOTICE, "No active SAP connection (no FSM)\n");
+ return -EINVAL;
+ }
+
+ LOGP(DSAP, LOGL_INFO, "Closing SAP connection\n");
+ return osmo_fsm_inst_dispatch(ms->sap_entity.fi,
+ SAP_DISCONNECT_REQ, NULL);
+}
+
+/*! Low-level function for closing SAP (socket) connection.
+ * \param[in] ms MS instance with active SAP connection
+ * \returns 0 in case of success, negative in case of error
+ */
+int _sap_close_sock(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);
return 0;
}
-/* same signature as in L1CTL, so it can be called from sim.c */
-int sap_send_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t length)
-{
- //LOGP(DSAP, LOGL_ERROR, "Received the following APDU from sim.c: %s\n" ,
- // osmo_hexdump(data, length));
- return sap_apdu(ms, data, length);
-}
-
-/* register message handler for messages that are sent from L2->L3 */
-int sap_register_handler(struct osmocom_ms *ms, sap_cb_t cb)
-{
- ms->sap_entity.msg_handler = cb;
-
- return 0;
-}
-
-/* init */
-int sap_init(struct osmocom_ms *ms)
+/*! Init SAP client state for a given MS. */
+void sap_init(struct osmocom_ms *ms)
{
struct osmosap_entity *sap = &ms->sap_entity;
LOGP(DSAP, LOGL_INFO, "init SAP client\n");
- sap->sap_state = SAP_NOT_CONNECTED;
- sap->max_msg_size = GSM_SAP_LENGTH;
- return 0;
+ /* Default MaxMsgSize (to be negotiated) */
+ sap->max_msg_size = GSM_SAP_LENGTH;
+ /* SIM card status is not known yet */
+ sap->card_status = SAP_CARD_STATUS_NOT_ACC;
}
-
diff --git a/src/host/layer23/src/common/sap_proto.c b/src/host/layer23/src/common/sap_proto.c
index ef4cb35..c3d202f 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;
+}
diff --git a/src/host/layer23/src/mobile/app_mobile.c b/src/host/layer23/src/mobile/app_mobile.c
index 27e1d9b..d7c65f0 100644
--- a/src/host/layer23/src/mobile/app_mobile.c
+++ b/src/host/layer23/src/mobile/app_mobile.c
@@ -202,6 +202,9 @@ static int mobile_init(struct osmocom_ms *ms)
/* init SAP client before SIM card starts up */
sap_init(ms);
+ /* SAP response call-back */
+ ms->sap_entity.sap_rsp_cb = &gsm_subscr_sap_rsp_cb;
+
gsm_sim_init(ms);
gsm48_cc_init(ms);
gsm480_ss_init(ms);
diff --git a/src/host/layer23/src/mobile/subscriber.c b/src/host/layer23/src/mobile/subscriber.c
index b2be554..e1e6331 100644
--- a/src/host/layer23/src/mobile/subscriber.c
+++ b/src/host/layer23/src/mobile/subscriber.c
@@ -29,6 +29,7 @@
#include <osmocom/bb/common/logging.h>
#include <osmocom/bb/common/osmocom_data.h>
#include <osmocom/bb/common/sap_interface.h>
+#include <osmocom/bb/common/sap_proto.h>
#include <osmocom/bb/common/networks.h>
#include <osmocom/bb/mobile/vty.h>
@@ -1306,3 +1307,51 @@ int gsm_subscr_remove_sapcard(struct osmocom_ms *ms)
{
return sap_close(ms);
}
+
+int gsm_subscr_sap_rsp_cb(struct osmocom_ms *ms, int res_code,
+ uint8_t res_type, uint16_t param_len, const uint8_t *param_val)
+{
+ struct msgb *msg;
+ int rc = 0;
+
+ /* Response parameter is not encoded in case of error */
+ if (res_code != SAP_RESULT_OK_REQ_PROC_CORR)
+ goto ignore_rsp;
+
+ switch (res_type) {
+ case SAP_TRANSFER_APDU_RESP:
+ /* Prevent NULL-pointer dereference */
+ if (!param_len || !param_val) {
+ rc = -EINVAL;
+ goto ignore_rsp;
+ }
+
+ /* FIXME: why do we use this length? */
+ msg = msgb_alloc(GSM_SAP_LENGTH, "sap_apdu");
+ if (!msg) {
+ rc = -ENOMEM;
+ goto ignore_rsp;
+ }
+
+ msg->data = msgb_put(msg, param_len);
+ memcpy(msg->data, param_val, param_len);
+
+ return sim_apdu_resp(ms, msg);
+
+ case SAP_TRANSFER_ATR_RESP:
+ /* TODO: don't read SIM again (if already) */
+ LOGP(DSAP, LOGL_INFO, "SAP card is ready, start reading...\n");
+ return subscr_sim_request(ms);
+
+ default:
+ rc = -ENOTSUP;
+ goto ignore_rsp;
+ }
+
+ return 0;
+
+ignore_rsp:
+ LOGP(DSAP, LOGL_NOTICE, "Ignored SAP response '%s' (code=%d)\n",
+ get_value_string(sap_msg_names, res_type), res_code);
+ return rc;
+}