<p>Vadim Yanitskiy <strong>merged</strong> this change.</p><p><a href="https://gerrit.osmocom.org/12441">View Change</a></p><div style="white-space:pre-wrap">Approvals:
Jenkins Builder: Verified
Harald Welte: Looks good to me, approved
</div><pre style="font-family: monospace,monospace; white-space: pre-wrap;">layer23/sap_interface.c: reimplement (BT)SAP interface<br><br>The (BT)SAP (Bluetooth SIM Access Profile) is a part of Bluetooth<br>specifications, that defines the protocol and procedures that<br>shall be used to access a smart card (usually GSM SIM) via<br>a Bluetooth link.<br><br>The profile defines two roles:<br><br> - Server - the side that has direct access to a smart card.<br> It acts as a SIM card reader, which assists the Client<br> in accessing and controlling the smart card.<br><br> - Client - the side that accesses and controls the smart card<br> inside the Server through the connection with Server.<br><br>Typical examples of a Server are a simple SIM card holder or<br>a portable phone in the car environment. A typical example of<br>a Client is a car phone, which uses a subscription module in<br>the Server for a connection to the cellular network.<br><br>OsmocomBB implements the Client role providing abstract SAP<br>interface API to the higher layers. Instead of Bluetooth,<br>a UNIX socket is used to communicate with a Server.<br><br>The previous implementation of (BT)SAP interface was incomplete<br>and hard to maintain. This change (re)implements it almost from<br>scratch on top of the Osmocom FSM framework.<br><br>Besides that, the most significant changes are:<br><br> - The implementation is separated into three parts:<br> - sap_interface.{c|h} - public SAP interface API,<br> - sap_proto.{c|h} - SAP protocol definition,<br> - sap_fsm.{c|h} - SAP FSM implementation.<br><br> - Both 'sap_message' and 'sap_param' structures follow the<br> SAP message format definition according to 5.1 and 5.2.<br><br> - The message parsing is done more carefully in order to<br> prevent buffer overflow and NULL-pointer dereference.<br><br> - Introduced public API for getting / adding message<br> parameters, and checking the ResultCode.<br><br> - Introduced public API for opening / closing a connection<br> with the server, powering on / off and resetting the SIM<br> card, sending ATR and APDU.<br><br> - Introduced a call-back for handling the response message.<br><br> - Card reader state is also a part of the public API.<br><br>The new implementation was tested against softsim [1]. The<br>only limitation is Server-initiated Release, that allows the<br>Server to 'ask' a Client to release connection as soon as<br>communication with the smart card is finished. This is not<br>implemented (yet), and leads to immediate release.<br><br>[1] https://git.osmocom.org/softsim/<br><br>Change-Id: I77bb108615bb2c94c441568f195b04e0a5421643<br>---<br>M src/host/layer23/include/osmocom/bb/common/Makefile.am<br>M src/host/layer23/include/osmocom/bb/common/osmocom_data.h<br>A src/host/layer23/include/osmocom/bb/common/sap_fsm.h<br>M src/host/layer23/include/osmocom/bb/common/sap_interface.h<br>M src/host/layer23/include/osmocom/bb/common/sap_proto.h<br>M src/host/layer23/include/osmocom/bb/mobile/subscriber.h<br>M src/host/layer23/src/common/Makefile.am<br>A src/host/layer23/src/common/sap_fsm.c<br>M src/host/layer23/src/common/sap_interface.c<br>M src/host/layer23/src/common/sap_proto.c<br>M src/host/layer23/src/mobile/app_mobile.c<br>M src/host/layer23/src/mobile/subscriber.c<br>12 files changed, 1,312 insertions(+), 439 deletions(-)<br><br></pre><pre style="font-family: monospace,monospace; white-space: pre-wrap;"><span>diff --git a/src/host/layer23/include/osmocom/bb/common/Makefile.am b/src/host/layer23/include/osmocom/bb/common/Makefile.am</span><br><span>index eb1dfb7..d66be98 100644</span><br><span>--- a/src/host/layer23/include/osmocom/bb/common/Makefile.am</span><br><span>+++ b/src/host/layer23/include/osmocom/bb/common/Makefile.am</span><br><span>@@ -1,3 +1,3 @@</span><br><span> noinst_HEADERS = l1ctl.h l1l2_interface.h l23_app.h logging.h \</span><br><span> networks.h gps.h sysinfo.h osmocom_data.h utils.h \</span><br><span style="color: hsl(0, 100%, 40%);">- sap_proto.h sap_interface.h</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_proto.h sap_fsm.h sap_interface.h</span><br><span>diff --git a/src/host/layer23/include/osmocom/bb/common/osmocom_data.h b/src/host/layer23/include/osmocom/bb/common/osmocom_data.h</span><br><span>index a3ecc92..ee48d6d 100644</span><br><span>--- a/src/host/layer23/include/osmocom/bb/common/osmocom_data.h</span><br><span>+++ b/src/host/layer23/include/osmocom/bb/common/osmocom_data.h</span><br><span>@@ -23,9 +23,14 @@</span><br><span> #include <osmocom/bb/common/l1ctl.h></span><br><span> </span><br><span> struct osmosap_entity {</span><br><span style="color: hsl(0, 100%, 40%);">- sap_cb_t msg_handler;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t sap_state;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmo_fsm_inst *fi;</span><br><span> uint16_t max_msg_size;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t card_status;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Optional SAP message call-back */</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg_cb_t sap_msg_cb;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Optional response call-back */</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_rsp_cb_t sap_rsp_cb;</span><br><span> };</span><br><span> </span><br><span> struct osmol1_entity {</span><br><span>diff --git a/src/host/layer23/include/osmocom/bb/common/sap_fsm.h b/src/host/layer23/include/osmocom/bb/common/sap_fsm.h</span><br><span>new file mode 100644</span><br><span>index 0000000..d79bc1c</span><br><span>--- /dev/null</span><br><span>+++ b/src/host/layer23/include/osmocom/bb/common/sap_fsm.h</span><br><span>@@ -0,0 +1,35 @@</span><br><span style="color: hsl(120, 100%, 40%);">+#pragma once</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/osmocom_data.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/* How long should we wait for connection establishment */</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_FSM_CONN_EST_TIMEOUT 5</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_FSM_CONN_EST_T 0</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/* How long should we wait for connection release */</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_FSM_CONN_REL_TIMEOUT 3</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_FSM_CONN_REL_T 1</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/* How long should we wait for request to complete */</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_FSM_PROC_REQ_TIMEOUT 5</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_FSM_PROC_REQ_T 2</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#define SAP_STATE_IS_ACTIVE(state) \</span><br><span style="color: hsl(120, 100%, 40%);">+ (state >= SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+enum sap_fsm_state {</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_NOT_CONNECTED = 0,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_CONNECTING,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_DISCONNECTING, /* Auxiliary state (not from specs) */</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_WAIT_FOR_CARD, /* Auxiliary state (not from specs) */</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_IDLE,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_ATR_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_APDU_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_RESET_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_STATUS_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_SET_TP_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_POWERON_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_POWEROFF_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+};</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_fsm_alloc(struct osmocom_ms *ms);</span><br><span>diff --git a/src/host/layer23/include/osmocom/bb/common/sap_interface.h b/src/host/layer23/include/osmocom/bb/common/sap_interface.h</span><br><span>index 96d056b..87a0f85 100644</span><br><span>--- a/src/host/layer23/include/osmocom/bb/common/sap_interface.h</span><br><span>+++ b/src/host/layer23/include/osmocom/bb/common/sap_interface.h</span><br><span>@@ -1,18 +1,23 @@</span><br><span> #pragma once</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-typedef int (*sap_cb_t)(struct msgb *msg, struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+#include <stdint.h></span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/msgb.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+struct osmocom_ms;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+typedef int (*sap_msg_cb_t)(struct osmocom_ms *ms, struct msgb *msg);</span><br><span style="color: hsl(120, 100%, 40%);">+typedef int (*sap_rsp_cb_t)(struct osmocom_ms *ms, int res_code,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t res_type, uint16_t param_len, const uint8_t *param_val);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+void sap_init(struct osmocom_ms *ms);</span><br><span> int sap_open(struct osmocom_ms *ms);</span><br><span> int sap_close(struct osmocom_ms *ms);</span><br><span style="color: hsl(0, 100%, 40%);">-int sap_send_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t length);</span><br><span style="color: hsl(0, 100%, 40%);">-int sap_register_handler(struct osmocom_ms *ms, sap_cb_t cb);</span><br><span style="color: hsl(0, 100%, 40%);">-int sap_init(struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+int _sap_close_sock(struct osmocom_ms *ms);</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-enum sap_state {</span><br><span style="color: hsl(0, 100%, 40%);">- SAP_SOCKET_ERROR,</span><br><span style="color: hsl(0, 100%, 40%);">- SAP_NOT_CONNECTED,</span><br><span style="color: hsl(0, 100%, 40%);">- SAP_IDLE,</span><br><span style="color: hsl(0, 100%, 40%);">- SAP_CONNECTION_UNDER_NEGOTIATION,</span><br><span style="color: hsl(0, 100%, 40%);">- SAP_PROCESSING_ATR_REQUEST,</span><br><span style="color: hsl(0, 100%, 40%);">- SAP_PROCESSING_APDU_REQUEST</span><br><span style="color: hsl(0, 100%, 40%);">-};</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_reset_req(struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_poweron_req(struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_poweroff_req(struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_atr_req(struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_apdu(struct osmocom_ms *ms, uint8_t *apdu, uint16_t apdu_len);</span><br><span>diff --git a/src/host/layer23/include/osmocom/bb/common/sap_proto.h b/src/host/layer23/include/osmocom/bb/common/sap_proto.h</span><br><span>index 49b30fc..e149f00 100644</span><br><span>--- a/src/host/layer23/include/osmocom/bb/common/sap_proto.h</span><br><span>+++ b/src/host/layer23/include/osmocom/bb/common/sap_proto.h</span><br><span>@@ -3,8 +3,10 @@</span><br><span> #include <stdint.h></span><br><span> </span><br><span> #include <osmocom/core/utils.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/msgb.h></span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-/* Table 5.1: Message Overview */</span><br><span style="color: hsl(120, 100%, 40%);">+/* Table 5.1: Message Overview</span><br><span style="color: hsl(120, 100%, 40%);">+ * NOTE: messages are used as events for SAP FSM */</span><br><span> enum sap_msg_type {</span><br><span> SAP_CONNECT_REQ = 0x00,</span><br><span> SAP_CONNECT_RESP = 0x01,</span><br><span>@@ -81,14 +83,39 @@</span><br><span> extern const struct value_string sap_card_status_names[];</span><br><span> extern const struct value_string sap_conn_status_names[];</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+/* Figure 5.2: Payload Coding */</span><br><span> struct sap_param {</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t id;</span><br><span style="color: hsl(0, 100%, 40%);">- uint16_t len;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t *value;</span><br><span style="color: hsl(0, 100%, 40%);">-};</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parameter ID, see sap_param_type enum */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t param_id;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Reserved for further use (shall be set to 0x00) */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t reserved[1];</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parameter length */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t length;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parameter value (and optional padding) */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t value[0];</span><br><span style="color: hsl(120, 100%, 40%);">+} __attribute__((packed));</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-struct sap_msg {</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t id;</span><br><span style="color: hsl(120, 100%, 40%);">+/* Figure 5.1 Message Format */</span><br><span style="color: hsl(120, 100%, 40%);">+struct sap_message {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Message ID, see sap_msg_type enum */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t msg_id;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Number of parameters */</span><br><span> uint8_t num_params;</span><br><span style="color: hsl(0, 100%, 40%);">- struct sap_param *params;</span><br><span style="color: hsl(0, 100%, 40%);">-};</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Reserved for further use (shall be set to 0x00) */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t reserved[2];</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Payload, see sap_param struct */</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t payload[0];</span><br><span style="color: hsl(120, 100%, 40%);">+} __attribute__((packed));</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#define GSM_SAP_LENGTH 300</span><br><span style="color: hsl(120, 100%, 40%);">+#define GSM_SAP_HEADROOM 32</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+struct msgb *sap_msgb_alloc(uint8_t msg_id);</span><br><span style="color: hsl(120, 100%, 40%);">+struct msgb *sap_msg_parse(const uint8_t *buf, size_t buf_len, int max_msg_size);</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_check_result_code(const struct sap_message *sap_msg);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+void sap_msgb_add_param(struct msgb *msg,</span><br><span style="color: hsl(120, 100%, 40%);">+ enum sap_param_type param_type,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len, const uint8_t *param_value);</span><br><span style="color: hsl(120, 100%, 40%);">+struct sap_param *sap_get_param(const struct sap_message *sap_msg,</span><br><span style="color: hsl(120, 100%, 40%);">+ enum sap_param_type param_type, uint16_t *param_len);</span><br><span>diff --git a/src/host/layer23/include/osmocom/bb/mobile/subscriber.h b/src/host/layer23/include/osmocom/bb/mobile/subscriber.h</span><br><span>index c747af9..698b0fd 100644</span><br><span>--- a/src/host/layer23/include/osmocom/bb/mobile/subscriber.h</span><br><span>+++ b/src/host/layer23/include/osmocom/bb/mobile/subscriber.h</span><br><span>@@ -90,6 +90,8 @@</span><br><span> int gsm_subscr_exit(struct osmocom_ms *ms);</span><br><span> int gsm_subscr_testcard(struct osmocom_ms *ms, uint16_t mcc, uint16_t mnc,</span><br><span> uint16_t lac, uint32_t tmsi, uint8_t imsi_attached);</span><br><span style="color: hsl(120, 100%, 40%);">+int gsm_subscr_sap_rsp_cb(struct osmocom_ms *ms, int res_code,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t res_type, uint16_t param_len, const uint8_t *param_val);</span><br><span> int gsm_subscr_sapcard(struct osmocom_ms *ms);</span><br><span> int gsm_subscr_remove_sapcard(struct osmocom_ms *ms);</span><br><span> int gsm_subscr_simcard(struct osmocom_ms *ms);</span><br><span>diff --git a/src/host/layer23/src/common/Makefile.am b/src/host/layer23/src/common/Makefile.am</span><br><span>index 9eed961..a8d9f7e 100644</span><br><span>--- a/src/host/layer23/src/common/Makefile.am</span><br><span>+++ b/src/host/layer23/src/common/Makefile.am</span><br><span>@@ -2,5 +2,5 @@</span><br><span> AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBGPS_CFLAGS)</span><br><span> </span><br><span> noinst_LIBRARIES = liblayer23.a</span><br><span style="color: hsl(0, 100%, 40%);">-liblayer23_a_SOURCES = l1ctl.c l1l2_interface.c sap_proto.c sap_interface.c \</span><br><span style="color: hsl(120, 100%, 40%);">+liblayer23_a_SOURCES = l1ctl.c l1l2_interface.c sap_fsm.c sap_proto.c sap_interface.c \</span><br><span> logging.c networks.c sim.c sysinfo.c gps.c l1ctl_lapdm_glue.c utils.c</span><br><span>diff --git a/src/host/layer23/src/common/sap_fsm.c b/src/host/layer23/src/common/sap_fsm.c</span><br><span>new file mode 100644</span><br><span>index 0000000..9591dab</span><br><span>--- /dev/null</span><br><span>+++ b/src/host/layer23/src/common/sap_fsm.c</span><br><span>@@ -0,0 +1,690 @@</span><br><span style="color: hsl(120, 100%, 40%);">+/*</span><br><span style="color: hsl(120, 100%, 40%);">+ * SAP (SIM Access Profile) FSM definition</span><br><span style="color: hsl(120, 100%, 40%);">+ * based on Bluetooth SAP specification</span><br><span style="color: hsl(120, 100%, 40%);">+ *</span><br><span style="color: hsl(120, 100%, 40%);">+ * (C) 2018-2019 by Vadim Yanitskiy <axilirator@gmail.com></span><br><span style="color: hsl(120, 100%, 40%);">+ *</span><br><span style="color: hsl(120, 100%, 40%);">+ * All Rights Reserved</span><br><span style="color: hsl(120, 100%, 40%);">+ *</span><br><span style="color: hsl(120, 100%, 40%);">+ * This program is free software; you can redistribute it and/or modify</span><br><span style="color: hsl(120, 100%, 40%);">+ * it under the terms of the GNU General Public License as published by</span><br><span style="color: hsl(120, 100%, 40%);">+ * the Free Software Foundation; either version 2 of the License, or</span><br><span style="color: hsl(120, 100%, 40%);">+ * (at your option) any later version.</span><br><span style="color: hsl(120, 100%, 40%);">+ *</span><br><span style="color: hsl(120, 100%, 40%);">+ * This program is distributed in the hope that it will be useful,</span><br><span style="color: hsl(120, 100%, 40%);">+ * but WITHOUT ANY WARRANTY; without even the implied warranty of</span><br><span style="color: hsl(120, 100%, 40%);">+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the</span><br><span style="color: hsl(120, 100%, 40%);">+ * GNU General Public License for more details.</span><br><span style="color: hsl(120, 100%, 40%);">+ *</span><br><span style="color: hsl(120, 100%, 40%);">+ * You should have received a copy of the GNU General Public License along</span><br><span style="color: hsl(120, 100%, 40%);">+ * with this program; if not, write to the Free Software Foundation, Inc.,</span><br><span style="color: hsl(120, 100%, 40%);">+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.</span><br><span style="color: hsl(120, 100%, 40%);">+ *</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <errno.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <stdlib.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <unistd.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <arpa/inet.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/socket.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/utils.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/fsm.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/osmocom_data.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/logging.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_interface.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_proto.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_fsm.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Send encoded SAP message to the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] msg encoded SAP message buffer</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+static int sap_send_msgb(struct osmocom_ms *ms, struct msgb *msg)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_wqueue_enqueue(&ms->sap_wq, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Failed to enqueue SAP message\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_disconnect(struct osmo_fsm_inst *fi, uint32_t prev_state)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_term(fi, OSMO_FSM_TERM_REGULAR, NULL);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_connect(struct osmo_fsm_inst *fi, uint32_t prev_state)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t size;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Section 5.1.1, CONNECT_REQ */</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_CONNECT_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg)</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Section 4.1.1, start MaxMsgSize negotiation */</span><br><span style="color: hsl(120, 100%, 40%);">+ size = htons(ms->sap_entity.max_msg_size);</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msgb_add_param(msg, SAP_MAX_MSG_SIZE,</span><br><span style="color: hsl(120, 100%, 40%);">+ sizeof(size), (uint8_t *) &size);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_send_msgb(ms, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_negotiate_msg_size(struct osmosap_entity *sap,</span><br><span style="color: hsl(120, 100%, 40%);">+ const struct sap_message *sap_msg)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t size, param_len;</span><br><span style="color: hsl(120, 100%, 40%);">+ const char *cause = NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ param = sap_get_param(sap_msg, SAP_MAX_MSG_SIZE, ¶m_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!param) {</span><br><span style="color: hsl(120, 100%, 40%);">+ cause = "missing expected MaxMsgSize parameter";</span><br><span style="color: hsl(120, 100%, 40%);">+ goto error;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ if (param_len != sizeof(size)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ cause = "MaxMsgSize parameter has wrong length";</span><br><span style="color: hsl(120, 100%, 40%);">+ goto error;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parse MaxMsgSize suggested by server */</span><br><span style="color: hsl(120, 100%, 40%);">+ size = ntohs((uint16_t *) param->value);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (size > SAP_MAX_MSG_SIZE) {</span><br><span style="color: hsl(120, 100%, 40%);">+ cause = "suggested MaxMsgSize is too big for us";</span><br><span style="color: hsl(120, 100%, 40%);">+ goto error;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Attempt to initiate connection again */</span><br><span style="color: hsl(120, 100%, 40%);">+ sap->max_msg_size = size;</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_fsm_connect(sap->fi, sap->fi->state);</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+error:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "MaxMsgSize negotiation failed: %s\n", cause);</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(sap->fi, SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_conn_handler(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint32_t event, void *data)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_message *sap_msg = (struct sap_message *) data;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t status;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Section 5.1.2, CONNECT_RESP */</span><br><span style="color: hsl(120, 100%, 40%);">+ param = sap_get_param(sap_msg, SAP_CONNECTION_STATUS, ¶m_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!param || param_len != sizeof(status)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Missing mandatory connection status\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parse connection status */</span><br><span style="color: hsl(120, 100%, 40%);">+ status = param->value[0];</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_INFO, "SAP connection status (0x%02x): %s\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ status, get_value_string(sap_conn_status_names, status));</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_conn_status_type) status) {</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CONN_STATUS_OK_CALL:</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.card_status = SAP_CARD_STATUS_NOT_ACC;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* fall-through */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CONN_STATUS_OK_READY:</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_WAIT_FOR_CARD, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CONN_STATUS_ERROR_SMALL_MSG_SIZE:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CONN_STATUS_ERROR_CONN:</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Section 4.1.1, MaxMsgSize negotiation */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CONN_STATUS_ERROR_MAX_MSG_SIZE:</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_negotiate_msg_size(&ms->sap_entity, sap_msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_conn_release(struct osmo_fsm_inst *fi, uint32_t prev_state)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "Initiating connection release\n");</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* We don't care about possible allocating / sending errors */</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_DISCONNECT_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (msg != NULL)</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_send_msgb((struct osmocom_ms *) fi->priv, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_release_handler(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint32_t event, void *data)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "Connection release complete\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_idle_enter(struct osmo_fsm_inst *fi, uint32_t prev_state)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_fsm_state) prev_state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_CONNECTING:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_WAIT_FOR_CARD:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* According to 4.1, if a subscription module is inserted</span><br><span style="color: hsl(120, 100%, 40%);">+ * in the Server and powered on (i.e. STATUS_IND message</span><br><span style="color: hsl(120, 100%, 40%);">+ * indicates "Card reset" state), the Client shall request</span><br><span style="color: hsl(120, 100%, 40%);">+ * the ATR of the subscription module. */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ms->sap_entity.card_status == SAP_CARD_STATUS_RESET)</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_send_atr_req(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Do nothing, suppress compiler warning */</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_idle_handler(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint32_t event, void *data)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg = (struct msgb *) data;</span><br><span style="color: hsl(120, 100%, 40%);">+ enum sap_fsm_state state;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Map event to the corresponding state */</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_msg_type) event) {</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_ATR_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_ATR_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_APDU_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_APDU_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_RESET_SIM_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_RESET_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_CARD_READER_STATUS_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_STATUS_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_SET_TRANSPORT_PROTOCOL_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_SET_TP_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_POWER_SIM_ON_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_POWERON_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_POWER_SIM_OFF_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ state = SAP_STATE_PROC_POWEROFF_REQ;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Shall not happen */</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(0);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = sap_send_msgb(ms, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc)</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, state,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_FSM_PROC_REQ_TIMEOUT, SAP_FSM_PROC_REQ_T);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_response_handler(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint32_t event, void *data)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_message *sap_msg = (struct sap_message *) data;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param = NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len = 0;</span><br><span style="color: hsl(120, 100%, 40%);">+ int param_id, rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_msg_type) event) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Both POWER_SIM_OFF_REQ and RESET_SIM_REQ can be sent in nearly</span><br><span style="color: hsl(120, 100%, 40%);">+ * any state, in order to allow the Client to reactivate</span><br><span style="color: hsl(120, 100%, 40%);">+ * a not accessible subscription module card. */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_POWER_SIM_OFF_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_RESET_SIM_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(data != NULL);</span><br><span style="color: hsl(120, 100%, 40%);">+ goto request;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Messages without parameters */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_SET_TRANSPORT_PROTOCOL_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_POWER_SIM_OFF_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_POWER_SIM_ON_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_RESET_SIM_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ param_id = -1;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_CARD_READER_STATUS_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ param_id = SAP_CARD_READER_STATUS;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_APDU_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ param_id = SAP_RESPONSE_APDU;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_ATR_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ param_id = SAP_ATR;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Shall not happen */</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(0);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* We're done with request now */</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Check the ResultCode */</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = sap_check_result_code(sap_msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc != SAP_RESULT_OK_REQ_PROC_CORR) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "Bad ResultCode: '%s'\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_result_names, rc));</span><br><span style="color: hsl(120, 100%, 40%);">+ goto response;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (param_id < 0)</span><br><span style="color: hsl(120, 100%, 40%);">+ goto response;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ param = sap_get_param(sap_msg, param_id, ¶m_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!param) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Message '%s' missing "</span><br><span style="color: hsl(120, 100%, 40%);">+ "mandatory parameter '%s'\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_msg_names, sap_msg->msg_id),</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_param_names, param_id));</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = -EINVAL;</span><br><span style="color: hsl(120, 100%, 40%);">+ goto response;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+response:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Poke optional response handler */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ms->sap_entity.sap_rsp_cb != NULL) {</span><br><span style="color: hsl(120, 100%, 40%);">+ if (param != NULL) {</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.sap_rsp_cb(ms, rc,</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg->msg_id, param_len, param->value);</span><br><span style="color: hsl(120, 100%, 40%);">+ } else {</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.sap_rsp_cb(ms, rc,</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg->msg_id, 0, NULL);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+request:</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = sap_send_msgb(ms, (struct msgb *) data);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc)</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, event == SAP_RESET_SIM_REQ ?</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_PROC_RESET_REQ : SAP_STATE_PROC_POWEROFF_REQ,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_FSM_PROC_REQ_TIMEOUT, SAP_FSM_PROC_REQ_T);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/* Generates mask for a single state or event */</span><br><span style="color: hsl(120, 100%, 40%);">+#define S(x) (1 << x)</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/* Figure 4.13: Simplified State Machine */</span><br><span style="color: hsl(120, 100%, 40%);">+static const struct osmo_fsm_state sap_fsm_states[] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_NOT_CONNECTED] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "NOT_CONNECTED",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_CONNECTING),</span><br><span style="color: hsl(120, 100%, 40%);">+ .onenter = &sap_fsm_disconnect,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_CONNECTING] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "CONNECTING",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_CONNECT_RESP),</span><br><span style="color: hsl(120, 100%, 40%);">+ .onenter = &sap_fsm_connect,</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_conn_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ /* NOTE: this is a custom state (i.e. not defined by the specs).</span><br><span style="color: hsl(120, 100%, 40%);">+ * We need it in order to do release procedure correctly. */</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_DISCONNECTING] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "DISCONNECTING",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_DISCONNECT_RESP),</span><br><span style="color: hsl(120, 100%, 40%);">+ .onenter = &sap_fsm_conn_release,</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_release_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ /* NOTE: this is a custom state (i.e. not defined by the specs).</span><br><span style="color: hsl(120, 100%, 40%);">+ * We need it in order to wait until SIM card becomes available.</span><br><span style="color: hsl(120, 100%, 40%);">+ * SAP_STATUS_IND event is handled by sap_fsm_allstate_action(). */</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_WAIT_FOR_CARD] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "WAIT_FOR_CARD",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE),</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_IDLE] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "IDLE",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_APDU_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_ATR_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_RESET_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_STATUS_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_SET_TP_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWERON_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWEROFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_TRANSFER_ATR_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_TRANSFER_APDU_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_RESET_SIM_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_TRANSFER_CARD_READER_STATUS_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_SET_TRANSPORT_PROTOCOL_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_ON_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_OFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .onenter = &sap_fsm_idle_enter,</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_idle_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_ATR_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_ATR_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_RESET_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWEROFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_TRANSFER_ATR_RESP)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_RESET_SIM_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_OFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_APDU_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_APDU_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_RESET_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWEROFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_TRANSFER_APDU_RESP)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_RESET_SIM_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_OFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_RESET_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_RESET_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWEROFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_RESET_SIM_RESP)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_OFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_STATUS_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_STATUS_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_RESET_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWEROFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_TRANSFER_CARD_READER_STATUS_RESP)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_RESET_SIM_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_OFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_SET_TP_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_SET_TP_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_SET_TRANSPORT_PROTOCOL_RESP),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_POWERON_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_POWERON_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_PROC_POWEROFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_POWER_SIM_ON_RESP)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_POWER_SIM_OFF_REQ),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+ [SAP_STATE_PROC_POWEROFF_REQ] = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "PROC_POWEROFF_REQ",</span><br><span style="color: hsl(120, 100%, 40%);">+ .out_state_mask = S(SAP_STATE_NOT_CONNECTED)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_DISCONNECTING)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATE_IDLE),</span><br><span style="color: hsl(120, 100%, 40%);">+ .in_event_mask = S(SAP_POWER_SIM_OFF_RESP),</span><br><span style="color: hsl(120, 100%, 40%);">+ .action = &sap_fsm_response_handler,</span><br><span style="color: hsl(120, 100%, 40%);">+ },</span><br><span style="color: hsl(120, 100%, 40%);">+};</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_tear_down(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ enum osmo_fsm_term_cause cause)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Flush buffers, close socket */</span><br><span style="color: hsl(120, 100%, 40%);">+ _sap_close_sock(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Reset SAP state */</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.card_status = SAP_CARD_STATUS_NOT_ACC;</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.max_msg_size = GSM_SAP_LENGTH;</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.fi = NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_handle_card_status_ind(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ const struct sap_message *sap_msg)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fi->priv;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t status;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ param = sap_get_param(sap_msg, SAP_STATUS_CHANGE, ¶m_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!param || param_len != sizeof(status)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Missing mandatory '%s' parameter\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_param_names, SAP_STATUS_CHANGE));</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ status = param->value[0];</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ms->sap_entity.card_status != status) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "(SIM) card status change '%s' -> '%s'\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_card_status_names, ms->sap_entity.card_status),</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_card_status_names, status));</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.card_status = status;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_card_status_type) status) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* SIM card is ready */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CARD_STATUS_RESET:</span><br><span style="color: hsl(120, 100%, 40%);">+ if (fi->state != SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* SIM card has recovered after unaccessful state */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CARD_STATUS_RECOVERED:</span><br><span style="color: hsl(120, 100%, 40%);">+ if (fi->state != SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* SIM card inserted, we need to power it on */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CARD_STATUS_INSERTED:</span><br><span style="color: hsl(120, 100%, 40%);">+ if (fi->state != SAP_STATE_IDLE)</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_send_poweron_req(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CARD_STATUS_UNKNOWN_ERROR:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CARD_STATUS_NOT_ACC:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_CARD_STATUS_REMOVED:</span><br><span style="color: hsl(120, 100%, 40%);">+ default: /* Unknown card status */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (fi->state != SAP_STATE_WAIT_FOR_CARD)</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_WAIT_FOR_CARD, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void sap_fsm_allstate_action(struct osmo_fsm_inst *fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint32_t event, void *data)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_message *sap_msg = (struct sap_message *) data;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_msg_type) event) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Disconnect indication initiated by the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * FIXME: at the moment, immediate release is always assumed,</span><br><span style="color: hsl(120, 100%, 40%);">+ * but ideally we should check type of release (using *data) */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_DISCONNECT_IND:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* This message may arrive in any of the sub-states of</span><br><span style="color: hsl(120, 100%, 40%);">+ * the "Connected" state (i.e. connection shall exist) */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!SAP_STATE_IS_ACTIVE(fi->state))</span><br><span style="color: hsl(120, 100%, 40%);">+ goto not_peritted;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg = NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* fall-through */</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Disconnect initiated by the Client */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_DISCONNECT_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* DISCONNECT_REQ has no parameters, so the caller</span><br><span style="color: hsl(120, 100%, 40%);">+ * shall not allocate the message manually. */</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(sap_msg == NULL);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* If we have no active connection, tear-down immediately */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!SAP_STATE_IS_ACTIVE(fi->state)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Trigger Client-initiated connection release */</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_DISCONNECTING,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_FSM_CONN_REL_TIMEOUT, SAP_FSM_CONN_REL_T);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* SIM status indication (inserted or ejected) */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATUS_IND:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* This message may arrive in any of the sub-states of</span><br><span style="color: hsl(120, 100%, 40%);">+ * the "Connected" state (i.e. connection shall exist) */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!SAP_STATE_IS_ACTIVE(fi->state))</span><br><span style="color: hsl(120, 100%, 40%);">+ goto not_peritted;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_fsm_handle_card_status_ind(fi, sap_msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_ERROR_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "RX Error Response from Server\n");</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (fi->state == SAP_STATE_CONNECTING) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Connection establishment error */</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ } else if (fi->state > SAP_STATE_IDLE) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Error replaces any Request message */</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_STATE_IDLE, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ } else {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Should not happen in general */</span><br><span style="color: hsl(120, 100%, 40%);">+ goto not_peritted;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Shall not happen */</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(0);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+not_peritted:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGPFSML(fi, LOGL_NOTICE, "Event '%s' is not "</span><br><span style="color: hsl(120, 100%, 40%);">+ "permitted in state '%s', please fix!\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_event_name(fi->fsm, event),</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_state_name(fi->fsm, fi->state));</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static int sap_fsm_timer_cb(struct osmo_fsm_inst *fi)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ switch ((enum sap_fsm_state) fi->state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Connection establishment / release timeout */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_DISCONNECTING:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_CONNECTING:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "Connection timeout\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Request processing timeout */</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_ATR_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_APDU_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_RESET_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_STATUS_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_SET_TP_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_POWERON_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_STATE_PROC_POWEROFF_REQ:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "Timeout waiting for '%s' to complete, "</span><br><span style="color: hsl(120, 100%, 40%);">+ "going back to IDLE\n", osmo_fsm_inst_state_name(fi));</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(fi, SAP_STATE_IDLE, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Timeout for unhandled state '%s'\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_name(fi));</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Do not tear-down FSM */</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static struct osmo_fsm sap_fsm_def = {</span><br><span style="color: hsl(120, 100%, 40%);">+ .name = "sap_fsm",</span><br><span style="color: hsl(120, 100%, 40%);">+ .log_subsys = DSAP,</span><br><span style="color: hsl(120, 100%, 40%);">+ .states = sap_fsm_states,</span><br><span style="color: hsl(120, 100%, 40%);">+ .num_states = ARRAY_SIZE(sap_fsm_states),</span><br><span style="color: hsl(120, 100%, 40%);">+ .event_names = sap_msg_names,</span><br><span style="color: hsl(120, 100%, 40%);">+ .cleanup = &sap_fsm_tear_down,</span><br><span style="color: hsl(120, 100%, 40%);">+ .timer_cb = &sap_fsm_timer_cb,</span><br><span style="color: hsl(120, 100%, 40%);">+ .allstate_action = &sap_fsm_allstate_action,</span><br><span style="color: hsl(120, 100%, 40%);">+ .allstate_event_mask = 0</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_DISCONNECT_REQ)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_DISCONNECT_IND)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_STATUS_IND)</span><br><span style="color: hsl(120, 100%, 40%);">+ | S(SAP_ERROR_RESP),</span><br><span style="color: hsl(120, 100%, 40%);">+};</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Allocate a new SAP state machine for a given ms.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance associated with SAP FSM</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_fsm_alloc(struct osmocom_ms *ms)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmosap_entity *sap;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap = &ms->sap_entity;</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(sap->fi == NULL);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Register our FSM (if required) */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!osmo_fsm_find_by_name(sap_fsm_def.name))</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(osmo_fsm_register(&sap_fsm_def) == 0);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Allocate an instance using ms as talloc context */</span><br><span style="color: hsl(120, 100%, 40%);">+ sap->fi = osmo_fsm_inst_alloc(&sap_fsm_def, ms,</span><br><span style="color: hsl(120, 100%, 40%);">+ ms, LOGL_DEBUG, ms->name);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!sap->fi) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Failed to allocate SAP FSM\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -ENOMEM;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span>diff --git a/src/host/layer23/src/common/sap_interface.c b/src/host/layer23/src/common/sap_interface.c</span><br><span>index 7b8717d..0eac896 100644</span><br><span>--- a/src/host/layer23/src/common/sap_interface.c</span><br><span>+++ b/src/host/layer23/src/common/sap_interface.c</span><br><span>@@ -4,6 +4,7 @@</span><br><span> * (C) 2010,2018 by Harald Welte <laforge@gnumonks.org></span><br><span> * (C) 2010 by Andreas Eversberg <jolly@eversberg.eu></span><br><span> * (C) 2011 by Nico Golde <nico@ngolde.de></span><br><span style="color: hsl(120, 100%, 40%);">+ * (C) 2018 by Vadim Yanitskiy <axilirator@gmail.com></span><br><span> *</span><br><span> * All Rights Reserved</span><br><span> *</span><br><span>@@ -23,487 +24,325 @@</span><br><span> *</span><br><span> */</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/bb/common/osmocom_data.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/bb/common/logging.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/bb/common/sap_interface.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/bb/common/sap_proto.h></span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/core/utils.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/core/talloc.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <osmocom/core/socket.h></span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-#include <sys/socket.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <sys/un.h></span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-#include <arpa/inet.h></span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-#define _GNU_SOURCE</span><br><span> #include <unistd.h></span><br><span> #include <errno.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <string.h></span><br><span style="color: hsl(0, 100%, 40%);">-#include <stdlib.h></span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-#define GSM_SAP_LENGTH 300</span><br><span style="color: hsl(0, 100%, 40%);">-#define GSM_SAP_HEADROOM 32</span><br><span style="color: hsl(120, 100%, 40%);">+#include <sys/socket.h></span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_connect(struct osmocom_ms *ms);</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/write_queue.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/logging.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/socket.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/utils.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/msgb.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/fsm.h></span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-static struct msgb *sap_create_msg(uint8_t id, uint8_t num_params, struct sap_param *params)</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/osmocom_data.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/logging.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_interface.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_proto.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_fsm.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Send ATR request to the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_atr_req(struct osmocom_ms *ms)</span><br><span> {</span><br><span> struct msgb *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t *msgp;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t i, plen, padding = 0;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- msg = msgb_alloc(GSM_SAP_LENGTH, "osmosap");</span><br><span style="color: hsl(0, 100%, 40%);">- if (!msg) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Failed to allocate msg.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!ms->sap_entity.fi) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EAGAIN;</span><br><span> }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- /* BTSAP 5.1 */</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_put_u8(msg, id);</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_put_u8(msg, num_params);</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_put_u16(msg, 0);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- for(i=0; i<num_params; i++){</span><br><span style="color: hsl(0, 100%, 40%);">- plen = params[i].len;</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_put_u8(msg, params[i].id);</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_put_u8(msg, 0);</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_put_u16(msg, plen);</span><br><span style="color: hsl(0, 100%, 40%);">- if(plen % 4){</span><br><span style="color: hsl(0, 100%, 40%);">- padding = 4 - (plen % 4);</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- msgp = msgb_put(msg, plen + padding);</span><br><span style="color: hsl(0, 100%, 40%);">- memcpy(msgp, params[i].value, plen);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if(padding){</span><br><span style="color: hsl(0, 100%, 40%);">- memset(msgp + plen, 0, padding);</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return msg;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static int sap_send(struct osmocom_ms *ms, struct msgb *msg)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state == SAP_NOT_CONNECTED)</span><br><span style="color: hsl(0, 100%, 40%);">- sap_connect(ms);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if (ms->sap_wq.bfd.fd <= 0)</span><br><span style="color: hsl(0, 100%, 40%);">- return -EINVAL;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if (osmo_wqueue_enqueue(&ms->sap_wq, msg) != 0) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Failed to enqueue msg.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_free(msg);</span><br><span style="color: hsl(0, 100%, 40%);">- return -1;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static int sap_parse_result(struct sap_param *param)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- if(param->id != SAP_RESULT_CODE){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "> Parameter id: %u no valid result type\n", param->id);</span><br><span style="color: hsl(0, 100%, 40%);">- return -1;</span><br><span style="color: hsl(0, 100%, 40%);">- } else {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "> RESULT CODE: %s\n",</span><br><span style="color: hsl(0, 100%, 40%);">- get_value_string(sap_result_names, param->value[0]));</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-#if 0</span><br><span style="color: hsl(0, 100%, 40%);">- /* FIXME: ARRAY_SIZE() is not applicable to</span><br><span style="color: hsl(0, 100%, 40%);">- * extern const struct value_string[] */</span><br><span style="color: hsl(0, 100%, 40%);">- if(param->value[0] > ARRAY_SIZE(sap_result_names)){</span><br><span style="color: hsl(0, 100%, 40%);">- return -1;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-#endif</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static uint8_t *sap_get_param(uint8_t *data, struct sap_param *param)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t *dptr = data;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t padlen;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- param->id = *dptr++;</span><br><span style="color: hsl(0, 100%, 40%);">- /* skip reserved byte */</span><br><span style="color: hsl(0, 100%, 40%);">- dptr++;</span><br><span style="color: hsl(0, 100%, 40%);">- param->len = *dptr << 8;</span><br><span style="color: hsl(0, 100%, 40%);">- dptr++;</span><br><span style="color: hsl(0, 100%, 40%);">- param->len |= *dptr++;</span><br><span style="color: hsl(0, 100%, 40%);">- param->value = talloc_zero_size(NULL, param->len);</span><br><span style="color: hsl(0, 100%, 40%);">- memcpy(param->value, dptr, param->len);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- /* skip parameter and padding and return pointer to next parameter */</span><br><span style="color: hsl(0, 100%, 40%);">- dptr += param->len;</span><br><span style="color: hsl(0, 100%, 40%);">- if(param->len % 4){</span><br><span style="color: hsl(0, 100%, 40%);">- padlen = (4 - param->len % 4);</span><br><span style="color: hsl(0, 100%, 40%);">- } else {</span><br><span style="color: hsl(0, 100%, 40%);">- padlen = 0;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- dptr += padlen;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return dptr;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_msg_free(struct sap_msg *msg)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t i;</span><br><span style="color: hsl(0, 100%, 40%);">- for(i=0; i<msg->num_params; i++){</span><br><span style="color: hsl(0, 100%, 40%);">- talloc_free(msg->params[i].value);</span><br><span style="color: hsl(0, 100%, 40%);">- talloc_free(msg->params);</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- talloc_free(msg);</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static struct sap_msg *sap_parse_msg(uint8_t *data)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct sap_msg *msg = talloc_zero(NULL, struct sap_msg);</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t *ptr = data;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t i;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if(!msg){</span><br><span style="color: hsl(0, 100%, 40%);">- return NULL;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg->id = *ptr++;</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "> %s \n", get_value_string(sap_msg_names, msg->id));</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg->num_params = *ptr++;</span><br><span style="color: hsl(0, 100%, 40%);">- /* skip two reserved null bytes, BTSAP 5.1 */</span><br><span style="color: hsl(0, 100%, 40%);">- ptr += 2;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg->params = talloc_zero_size(NULL, sizeof(struct sap_param) * msg->num_params);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- for(i=0; i<msg->num_params; i++){</span><br><span style="color: hsl(0, 100%, 40%);">- ptr = sap_get_param(ptr, &msg->params[i]);</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "> %s %s\n",</span><br><span style="color: hsl(0, 100%, 40%);">- get_value_string(sap_param_names, msg->params[i].id),</span><br><span style="color: hsl(0, 100%, 40%);">- osmo_hexdump(msg->params[i].value, msg->params[i].len));</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return msg;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_apdu_resp(struct osmocom_ms *ms, uint8_t *data, uint16_t len)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct msgb *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t *apdu;</span><br><span style="color: hsl(0, 100%, 40%);">- msg = msgb_alloc(GSM_SAP_LENGTH, "osmosap");</span><br><span style="color: hsl(0, 100%, 40%);">- if(!msg){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Failed to allocate memory.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- apdu = msgb_put(msg, len);</span><br><span style="color: hsl(0, 100%, 40%);">- memcpy(apdu, data, len);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_DEBUG, "Forwarding APDU to SIM handler.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- sim_apdu_resp(ms, msg);</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static int sap_adapt_msg_size(struct osmocom_ms *ms, struct sap_param *param)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- uint16_t size;</span><br><span style="color: hsl(0, 100%, 40%);">- size = (param->value[0] << 8) | param->value[1];</span><br><span style="color: hsl(0, 100%, 40%);">- if(size != ms->sap_entity.max_msg_size && size > 0){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_NOTICE, "Server can not handle max_msg_size, adapting.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.max_msg_size = size;</span><br><span style="color: hsl(0, 100%, 40%);">- return -1;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_atr(struct osmocom_ms *ms)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct msgb *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state != SAP_IDLE){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Attempting to send ATR request while not being idle.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg = sap_create_msg(SAP_TRANSFER_ATR_REQ, 0, NULL);</span><br><span style="color: hsl(0, 100%, 40%);">- if(!msg)</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- sap_send(ms, msg);</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_PROCESSING_ATR_REQUEST;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_parse_resp(struct osmocom_ms *ms, uint8_t *data, uint16_t len)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct sap_msg *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- if(len > ms->sap_entity.max_msg_size){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Read more data than allowed by max_msg_size, ignoring.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg = sap_parse_msg(data);</span><br><span style="color: hsl(0, 100%, 40%);">- if (!msg) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Failed to parse SAP message\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- switch(msg->id){</span><br><span style="color: hsl(0, 100%, 40%);">- case SAP_CONNECT_RESP:</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "Status: %s\n", get_value_string(sap_conn_status_names, msg->params[0].value[0]));</span><br><span style="color: hsl(0, 100%, 40%);">- if(msg->params[0].value[0] == 0){</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_IDLE;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- if(msg->num_params == 2 && msg->params[1].len == 2){</span><br><span style="color: hsl(0, 100%, 40%);">- if(sap_adapt_msg_size(ms, &msg->params[1]) < 0) {</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_NOT_CONNECTED;</span><br><span style="color: hsl(0, 100%, 40%);">- } else {</span><br><span style="color: hsl(0, 100%, 40%);">- sap_atr(ms);</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- case SAP_DISCONNECT_RESP:</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_NOT_CONNECTED;</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- case SAP_STATUS_IND:</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "New card state: %s\n", get_value_string(sap_card_status_names,</span><br><span style="color: hsl(0, 100%, 40%);">- msg->params[0].value[0]));</span><br><span style="color: hsl(0, 100%, 40%);">- if(msg->params[0].value[0] != SAP_CARD_STATUS_RESET){</span><br><span style="color: hsl(0, 100%, 40%);">- /* TODO: handle case in which the card is not ready yet */</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- case SAP_TRANSFER_ATR_RESP:</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state != SAP_PROCESSING_ATR_REQUEST){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "got ATR resp in state: %u\n", ms->sap_entity.sap_state);</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- if(msg->num_params >= 2){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "ATR: %s\n", osmo_hexdump(msg->params[1].value, msg->params[1].len));</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_IDLE;</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- case SAP_TRANSFER_APDU_RESP:</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state != SAP_PROCESSING_APDU_REQUEST){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "got APDU resp in state: %u\n", ms->sap_entity.sap_state);</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- if(msg->num_params != 2){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "wrong number of parameters %u in APDU response\n", msg->num_params);</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_IDLE;</span><br><span style="color: hsl(0, 100%, 40%);">- if(sap_parse_result(&msg->params[0]) == 0){</span><br><span style="color: hsl(0, 100%, 40%);">- /* back apdu resp to layer23 */</span><br><span style="color: hsl(0, 100%, 40%);">- sap_apdu_resp(ms, msg->params[1].value, msg->params[1].len);</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "sap_apdu_resp called, sending data back to layer23\n");</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- case SAP_ERROR_RESP:</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state == SAP_CONNECTION_UNDER_NEGOTIATION){</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_NOT_CONNECTED;</span><br><span style="color: hsl(0, 100%, 40%);">- } else {</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_IDLE;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- default:</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "got unknown or not implemented SAP msgid: %u\n", msg->id);</span><br><span style="color: hsl(0, 100%, 40%);">- break;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- sap_msg_free(msg);</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static int sap_read(struct osmo_fd *fd)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct msgb *msg = NULL;</span><br><span style="color: hsl(0, 100%, 40%);">- struct osmocom_ms *ms = (struct osmocom_ms *) fd->data;</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t *sap_buffer;</span><br><span style="color: hsl(0, 100%, 40%);">- ssize_t rc;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- sap_buffer = talloc_zero_size(NULL, ms->sap_entity.max_msg_size);</span><br><span style="color: hsl(0, 100%, 40%);">- if(!sap_buffer){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Failed to allocate memory\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_TRANSFER_ATR_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg)</span><br><span> return -ENOMEM;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- rc = read(fd->fd, sap_buffer, ms->sap_entity.max_msg_size - 1);</span><br><span style="color: hsl(0, 100%, 40%);">- if (rc < 0) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR,"SAP socket failed\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_TRANSFER_ATR_REQ, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span> msgb_free(msg);</span><br><span style="color: hsl(0, 100%, 40%);">- sap_close(ms);</span><br><span> return rc;</span><br><span> }</span><br><span style="color: hsl(0, 100%, 40%);">- if(rc == 0) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_NOTICE, "SAP socket closed by server\n");</span><br><span style="color: hsl(0, 100%, 40%);">- msgb_free(msg);</span><br><span style="color: hsl(0, 100%, 40%);">- sap_close(ms);</span><br><span style="color: hsl(0, 100%, 40%);">- return -ECONNREFUSED;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- sap_buffer[rc] = 0;</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "Received %zd bytes: %s\n", rc, osmo_hexdump(sap_buffer, rc));</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- sap_parse_resp(ms, sap_buffer, rc);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- talloc_free(sap_buffer);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if (ms->sap_entity.msg_handler){</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.msg_handler(msg, ms);</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span> return 0;</span><br><span> }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-static int sap_write(struct osmo_fd *fd, struct msgb *msg)</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Send APDU request to the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] apdu APDU to be send</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] apdu_len length of APDU</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_apdu(struct osmocom_ms *ms, uint8_t *apdu, uint16_t apdu_len)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!ms->sap_entity.fi) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EAGAIN;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_TRANSFER_APDU_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg)</span><br><span style="color: hsl(120, 100%, 40%);">+ return -ENOMEM;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msgb_add_param(msg, SAP_COMMAND_APDU, apdu_len, apdu);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_TRANSFER_APDU_REQ, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Send (SIM) reset request to the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_reset_req(struct osmocom_ms *ms)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!ms->sap_entity.fi) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EAGAIN;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_RESET_SIM_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg)</span><br><span style="color: hsl(120, 100%, 40%);">+ return -ENOMEM;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_RESET_SIM_REQ, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Send (SIM) power on request to the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_poweron_req(struct osmocom_ms *ms)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!ms->sap_entity.fi) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EAGAIN;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_POWER_SIM_ON_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg)</span><br><span style="color: hsl(120, 100%, 40%);">+ return -ENOMEM;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_POWER_SIM_ON_REQ, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Send (SIM) power off request to the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_send_poweroff_req(struct osmocom_ms *ms)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!ms->sap_entity.fi) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "SAP interface is not connected\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EAGAIN;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msgb_alloc(SAP_POWER_SIM_OFF_REQ);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg)</span><br><span style="color: hsl(120, 100%, 40%);">+ return -ENOMEM;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_fsm_inst_dispatch(ms->sap_entity.fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_POWER_SIM_OFF_REQ, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static int sap_read_cb(struct osmo_fd *fd)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmocom_ms *ms = (struct osmocom_ms *) fd->data;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct osmosap_entity *sap = &ms->sap_entity;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t buf[GSM_SAP_LENGTH];</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ ssize_t rc;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Prevent buffer overflow */</span><br><span style="color: hsl(120, 100%, 40%);">+ OSMO_ASSERT(sap->max_msg_size <= GSM_SAP_LENGTH);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = read(fd->fd, buf, sap->max_msg_size);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc < 0) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "SAP socket failed\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = -EIO;</span><br><span style="color: hsl(120, 100%, 40%);">+ goto conn_error;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc == 0) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "SAP socket closed by server\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = -ECONNREFUSED;</span><br><span style="color: hsl(120, 100%, 40%);">+ goto conn_error;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "RX SAP message '%s' (len=%zd): %s\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_msg_names, buf[0]),</span><br><span style="color: hsl(120, 100%, 40%);">+ rc, osmo_hexdump(buf, rc));</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parse received SAP message and allocate a new msgb */</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = sap_msg_parse(buf, rc, sap->max_msg_size);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Failed to parse SAP message\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EINVAL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Pass parsed message to our FSM using message ID as event */</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = osmo_fsm_inst_dispatch(sap->fi, msg->data[0], msg->data);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Pass to (optional) SAP message handler */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (sap->sap_msg_cb)</span><br><span style="color: hsl(120, 100%, 40%);">+ sap->sap_msg_cb(ms, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ else</span><br><span style="color: hsl(120, 100%, 40%);">+ msgb_free(msg);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+conn_error:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Immediately tear-down FSM */</span><br><span style="color: hsl(120, 100%, 40%);">+ osmo_fsm_inst_state_chg(sap->fi, SAP_STATE_NOT_CONNECTED, 0, 0);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static int sap_write_cb(struct osmo_fd *fd, struct msgb *msg)</span><br><span> {</span><br><span> ssize_t rc;</span><br><span> </span><br><span> if (fd->fd <= 0)</span><br><span> return -EINVAL;</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_INFO, "< %s\n", osmo_hexdump(msg->data, msg->len));</span><br><span> rc = write(fd->fd, msg->data, msg->len);</span><br><span> if (rc != msg->len) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Failed to write data: rc: %zd\n", rc);</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Failed to write data\n");</span><br><span> return rc;</span><br><span> }</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "TX SAP message '%s' (len=%u): %s\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_msg_names, msg->data[0]),</span><br><span style="color: hsl(120, 100%, 40%);">+ msg->len, osmo_hexdump(msg->data, msg->len));</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span> return 0;</span><br><span> }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_connect(struct osmocom_ms *ms)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- uint8_t buffer[3];</span><br><span style="color: hsl(0, 100%, 40%);">- struct msgb *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- uint16_t size = ms->sap_entity.max_msg_size;</span><br><span style="color: hsl(0, 100%, 40%);">- struct sap_param params[1];</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- params[0].id = SAP_MAX_MSG_SIZE;</span><br><span style="color: hsl(0, 100%, 40%);">- params[0].len = 2;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state != SAP_NOT_CONNECTED) {</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Attempting to connect while there is an active connection.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- buffer[0] = (size >> 8) & 0xFF;</span><br><span style="color: hsl(0, 100%, 40%);">- buffer[1] = size & 0xFF;</span><br><span style="color: hsl(0, 100%, 40%);">- buffer[2] = 0;</span><br><span style="color: hsl(0, 100%, 40%);">- params[0].value = buffer;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg = sap_create_msg(SAP_CONNECT_REQ, 1, params);</span><br><span style="color: hsl(0, 100%, 40%);">- if(!msg)</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- sap_send(ms, msg);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_CONNECTION_UNDER_NEGOTIATION;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static void sap_disconnect(struct osmocom_ms *ms)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct msgb *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state != SAP_NOT_CONNECTED && ms->sap_entity.sap_state != SAP_CONNECTION_UNDER_NEGOTIATION){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Attempting to disconnect while no active connection.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg = sap_create_msg(SAP_DISCONNECT_REQ, 0, NULL);</span><br><span style="color: hsl(0, 100%, 40%);">- if(!msg)</span><br><span style="color: hsl(0, 100%, 40%);">- return;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- sap_send(ms, msg);</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_NOT_CONNECTED;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-static int sap_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t len)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- struct msgb *msg;</span><br><span style="color: hsl(0, 100%, 40%);">- struct sap_param params[1];</span><br><span style="color: hsl(0, 100%, 40%);">- int rc;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- params[0].id = SAP_COMMAND_APDU;</span><br><span style="color: hsl(0, 100%, 40%);">- params[0].len = len;</span><br><span style="color: hsl(0, 100%, 40%);">- params[0].value = data;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- if(ms->sap_entity.sap_state != SAP_IDLE){</span><br><span style="color: hsl(0, 100%, 40%);">- LOGP(DSAP, LOGL_ERROR, "Attempting to send APDU request while not being idle.\n");</span><br><span style="color: hsl(0, 100%, 40%);">- return -EIO;</span><br><span style="color: hsl(0, 100%, 40%);">- }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- msg = sap_create_msg(SAP_TRANSFER_APDU_REQ, 1, params);</span><br><span style="color: hsl(0, 100%, 40%);">- if(!msg)</span><br><span style="color: hsl(0, 100%, 40%);">- return -ENOMEM;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- rc = sap_send(ms, msg);</span><br><span style="color: hsl(0, 100%, 40%);">- if (rc)</span><br><span style="color: hsl(0, 100%, 40%);">- return rc;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_PROCESSING_APDU_REQUEST;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Establishes SAP connection to the Server,</span><br><span style="color: hsl(120, 100%, 40%);">+ * allocates SAP FSM, and triggers connection procedure.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with configured SAP socket path</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span> int sap_open(struct osmocom_ms *ms)</span><br><span> {</span><br><span> int rc;</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_INFO, "Establishing SAP connection "</span><br><span style="color: hsl(120, 100%, 40%);">+ "(using socket '%s')\n", ms->settings.sap_socket_path);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span> rc = osmo_sock_unix_init_ofd(&ms->sap_wq.bfd, SOCK_STREAM, 0,</span><br><span> ms->settings.sap_socket_path, OSMO_SOCK_F_CONNECT);</span><br><span> if (rc < 0) {</span><br><span> LOGP(DSAP, LOGL_ERROR, "Failed to create unix domain socket %s: %s\n",</span><br><span> ms->settings.sap_socket_path, strerror(-rc));</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.sap_state = SAP_SOCKET_ERROR;</span><br><span> return rc;</span><br><span> }</span><br><span> </span><br><span> osmo_wqueue_init(&ms->sap_wq, 100);</span><br><span> ms->sap_wq.bfd.data = ms;</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_wq.read_cb = sap_read;</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_wq.write_cb = sap_write;</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_wq.read_cb = &sap_read_cb;</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_wq.write_cb = &sap_write_cb;</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- sap_connect(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Allocate a SAP FSM for a given ms */</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = sap_fsm_alloc(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (rc) {</span><br><span style="color: hsl(120, 100%, 40%);">+ _sap_close_sock(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Initiate SAP connection with Server */</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "Connecting to the Server...\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return osmo_fsm_inst_state_chg(ms->sap_entity.fi, SAP_STATE_CONNECTING,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_FSM_CONN_EST_TIMEOUT, SAP_FSM_CONN_EST_T);</span><br><span> }</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+/*! Closes SAP connection with the Server.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span> int sap_close(struct osmocom_ms *ms)</span><br><span> {</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ms->sap_entity.fi == NULL) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "No active SAP connection (no FSM)\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EINVAL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_INFO, "Closing SAP connection\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return osmo_fsm_inst_dispatch(ms->sap_entity.fi,</span><br><span style="color: hsl(120, 100%, 40%);">+ SAP_DISCONNECT_REQ, NULL);</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Low-level function for closing SAP (socket) connection.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] ms MS instance with active SAP connection</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns 0 in case of success, negative in case of error</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int _sap_close_sock(struct osmocom_ms *ms)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span> if (ms->sap_wq.bfd.fd <= 0)</span><br><span> return -EINVAL;</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">- sap_disconnect(ms);</span><br><span> close(ms->sap_wq.bfd.fd);</span><br><span> ms->sap_wq.bfd.fd = -1;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span> osmo_fd_unregister(&ms->sap_wq.bfd);</span><br><span> osmo_wqueue_clear(&ms->sap_wq);</span><br><span> </span><br><span> return 0;</span><br><span> }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-/* same signature as in L1CTL, so it can be called from sim.c */</span><br><span style="color: hsl(0, 100%, 40%);">-int sap_send_apdu(struct osmocom_ms *ms, uint8_t *data, uint16_t length)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- //LOGP(DSAP, LOGL_ERROR, "Received the following APDU from sim.c: %s\n" ,</span><br><span style="color: hsl(0, 100%, 40%);">- // osmo_hexdump(data, length));</span><br><span style="color: hsl(0, 100%, 40%);">- return sap_apdu(ms, data, length);</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-/* register message handler for messages that are sent from L2->L3 */</span><br><span style="color: hsl(0, 100%, 40%);">-int sap_register_handler(struct osmocom_ms *ms, sap_cb_t cb)</span><br><span style="color: hsl(0, 100%, 40%);">-{</span><br><span style="color: hsl(0, 100%, 40%);">- ms->sap_entity.msg_handler = cb;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(0, 100%, 40%);">-}</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">-/* init */</span><br><span style="color: hsl(0, 100%, 40%);">-int sap_init(struct osmocom_ms *ms)</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Init SAP client state for a given MS. */</span><br><span style="color: hsl(120, 100%, 40%);">+void sap_init(struct osmocom_ms *ms)</span><br><span> {</span><br><span> struct osmosap_entity *sap = &ms->sap_entity;</span><br><span> </span><br><span> LOGP(DSAP, LOGL_INFO, "init SAP client\n");</span><br><span style="color: hsl(0, 100%, 40%);">- sap->sap_state = SAP_NOT_CONNECTED;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Default MaxMsgSize (to be negotiated) */</span><br><span> sap->max_msg_size = GSM_SAP_LENGTH;</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span style="color: hsl(0, 100%, 40%);">- return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* SIM card status is not known yet */</span><br><span style="color: hsl(120, 100%, 40%);">+ sap->card_status = SAP_CARD_STATUS_NOT_ACC;</span><br><span> }</span><br><span style="color: hsl(0, 100%, 40%);">-</span><br><span>diff --git a/src/host/layer23/src/common/sap_proto.c b/src/host/layer23/src/common/sap_proto.c</span><br><span>index ef4cb35..c3d202f 100644</span><br><span>--- a/src/host/layer23/src/common/sap_proto.c</span><br><span>+++ b/src/host/layer23/src/common/sap_proto.c</span><br><span>@@ -23,9 +23,19 @@</span><br><span> *</span><br><span> */</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+#include <string.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <stdlib.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <stdint.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <errno.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <arpa/inet.h></span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/logging.h></span><br><span> #include <osmocom/core/utils.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/core/msgb.h></span><br><span> </span><br><span> #include <osmocom/bb/common/sap_proto.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/logging.h></span><br><span> </span><br><span> /* Table 5.1: Message Overview */</span><br><span> const struct value_string sap_msg_names[] = {</span><br><span>@@ -102,3 +112,211 @@</span><br><span> { SAP_CONN_STATUS_OK_CALL, "OK, ongoing call" },</span><br><span> { 0, NULL }</span><br><span> };</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Allocate a new message buffer with SAP message header.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] msg_id SAP message identifier</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns message buffer in case of success, NULL otherwise</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+struct msgb *sap_msgb_alloc(uint8_t msg_id)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_message *sap_msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = msgb_alloc(GSM_SAP_LENGTH, "sap_msg");</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Failed to allocate SAP message\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg = (struct sap_message *) msgb_put(msg, sizeof(*sap_msg));</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg->msg_id = msg_id;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return msg;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Add a new parameter to a given SAP message buffer.</span><br><span style="color: hsl(120, 100%, 40%);">+ * Padding is added automatically, SAP message header</span><br><span style="color: hsl(120, 100%, 40%);">+ * (number of parameters) is also updated automatically.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] msg SAP message buffer</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] param_type parameter type (see sap_param_type enum)</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] param_len parameter length</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] param_value pointer to parameter value</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+void sap_msgb_add_param(struct msgb *msg,</span><br><span style="color: hsl(120, 100%, 40%);">+ enum sap_param_type param_type,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len, const uint8_t *param_value)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_message *sap_msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t padding;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t *buf;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Update number of parameters */</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg = (struct sap_message *) msg->data;</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg->num_params++;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Allocate a new parameter */</span><br><span style="color: hsl(120, 100%, 40%);">+ param = (struct sap_param *) msgb_put(msg, sizeof(*param));</span><br><span style="color: hsl(120, 100%, 40%);">+ param->param_id = param_type;</span><br><span style="color: hsl(120, 100%, 40%);">+ param->reserved[0] = 0x00;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Encode parameter value and length */</span><br><span style="color: hsl(120, 100%, 40%);">+ param->length = htons(param_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ buf = msgb_put(msg, param_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ memcpy(buf, param_value, param_len);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Optional padding */</span><br><span style="color: hsl(120, 100%, 40%);">+ padding = 4 - (param_len % 4);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (padding) {</span><br><span style="color: hsl(120, 100%, 40%);">+ buf = msgb_put(msg, padding);</span><br><span style="color: hsl(120, 100%, 40%);">+ memset(buf, 0x00, padding);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Attempt to find a given parameter in a given SAP message.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] sap_msg pointer to SAP message header</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] param_type parameter type (see sap_param_type enum)</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[out] param_len parameter length (if found)</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns pointer to a given parameter withing the message, NULL otherwise</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+struct sap_param *sap_get_param(const struct sap_message *sap_msg,</span><br><span style="color: hsl(120, 100%, 40%);">+ enum sap_param_type param_type, uint16_t *param_len)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ const uint8_t *ptr = sap_msg->payload;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param = NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t plen;</span><br><span style="color: hsl(120, 100%, 40%);">+ int i;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* We assume that message is parsed already,</span><br><span style="color: hsl(120, 100%, 40%);">+ * so we don't check for buffer overflows */</span><br><span style="color: hsl(120, 100%, 40%);">+ for (i = 0; i < sap_msg->num_params; i++) {</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parse one parameter */</span><br><span style="color: hsl(120, 100%, 40%);">+ param = (struct sap_param *) ptr;</span><br><span style="color: hsl(120, 100%, 40%);">+ plen = ntohs(param->length);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Match against a given ID */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (param->param_id == param_type) {</span><br><span style="color: hsl(120, 100%, 40%);">+ if (param_len != NULL)</span><br><span style="color: hsl(120, 100%, 40%);">+ *param_len = plen;</span><br><span style="color: hsl(120, 100%, 40%);">+ return param;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Shift pointer to the next parameter */</span><br><span style="color: hsl(120, 100%, 40%);">+ ptr += sizeof(*param) + plen;</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Optional padding */</span><br><span style="color: hsl(120, 100%, 40%);">+ ptr += 4 - (plen % 4);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Parse SAP message from a given buffer into a new message buffer.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] buf pointer to a buffer with to be parsed message</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] buf_len length of the buffer</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] max_msg_size max (negotiated) message size</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns new message buffer with parsed message, NULL otherwise</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+struct msgb *sap_msg_parse(const uint8_t *buf, size_t buf_len, int max_msg_size)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ const struct sap_message *sap_msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ const uint8_t *ptr;</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ size_t len;</span><br><span style="color: hsl(120, 100%, 40%);">+ int i;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Message header is mandatory */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (buf_len < sizeof(*sap_msg)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Missing SAP message header\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* MaxMsgSize limitation (optional) */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (max_msg_size > 0 && buf_len > max_msg_size) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Buffer (len=%zu) is bigger than "</span><br><span style="color: hsl(120, 100%, 40%);">+ "given MaxMsgSize=%d\n", buf_len, max_msg_size);</span><br><span style="color: hsl(120, 100%, 40%);">+ return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg = (const struct sap_message *) buf;</span><br><span style="color: hsl(120, 100%, 40%);">+ len = buf_len - sizeof(*sap_msg);</span><br><span style="color: hsl(120, 100%, 40%);">+ ptr = sap_msg->payload;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "SAP message '%s' has %u parameter(s)\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_msg_names, sap_msg->msg_id),</span><br><span style="color: hsl(120, 100%, 40%);">+ sap_msg->num_params);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ for (i = 0; i < sap_msg->num_params; i++) {</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t offset;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Prevent buffer overflow */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (len < sizeof(*param))</span><br><span style="color: hsl(120, 100%, 40%);">+ goto malformed;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Parse one parameter */</span><br><span style="color: hsl(120, 100%, 40%);">+ param = (struct sap_param *) ptr;</span><br><span style="color: hsl(120, 100%, 40%);">+ param_len = ntohs(param->length);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "SAP parameter '%s' (len=%u): %s\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_param_names, param->param_id),</span><br><span style="color: hsl(120, 100%, 40%);">+ param_len, osmo_hexdump(param->value, param_len));</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Calculate relative offset */</span><br><span style="color: hsl(120, 100%, 40%);">+ offset = sizeof(*param) + param_len;</span><br><span style="color: hsl(120, 100%, 40%);">+ offset += 4 - (param_len % 4); /* Optional padding */</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Prevent buffer overflow */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (offset > len)</span><br><span style="color: hsl(120, 100%, 40%);">+ goto malformed;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ len -= offset;</span><br><span style="color: hsl(120, 100%, 40%);">+ ptr += offset;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Allocate a new message buffer */</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = msgb_alloc(GSM_SAP_LENGTH, "sap_msg");</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Failed to allocate SAP message\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg->data = msgb_put(msg, buf_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ memcpy(msg->data, buf, buf_len);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return msg;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+malformed:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Malformed SAP message "</span><br><span style="color: hsl(120, 100%, 40%);">+ "(parameter %i/%u)\n", i + 1, sap_msg->num_params);</span><br><span style="color: hsl(120, 100%, 40%);">+ return NULL;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Parse ResultCode from a given SAP message.</span><br><span style="color: hsl(120, 100%, 40%);">+ * \param[in] sap_msg pointer to SAP message header</span><br><span style="color: hsl(120, 100%, 40%);">+ * \returns parsed ResultCode (if found), negative otherwise</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+int sap_check_result_code(const struct sap_message *sap_msg)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct sap_param *param;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t param_len;</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t res_code;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ param = sap_get_param(sap_msg, SAP_RESULT_CODE, ¶m_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!param || param_len != sizeof(res_code)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Missing mandatory '%s' parameter\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_param_names, SAP_RESULT_CODE));</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EINVAL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ res_code = param->value[0];</span><br><span style="color: hsl(120, 100%, 40%);">+ if (res_code >= ARRAY_SIZE(sap_result_names)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_ERROR, "Unknown SAP ResultCode=0x%02x\n", res_code);</span><br><span style="color: hsl(120, 100%, 40%);">+ return -EINVAL;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_DEBUG, "SAP ResultCode is '%s'\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_result_names, res_code));</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return res_code;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span>diff --git a/src/host/layer23/src/mobile/app_mobile.c b/src/host/layer23/src/mobile/app_mobile.c</span><br><span>index 27e1d9b..d7c65f0 100644</span><br><span>--- a/src/host/layer23/src/mobile/app_mobile.c</span><br><span>+++ b/src/host/layer23/src/mobile/app_mobile.c</span><br><span>@@ -202,6 +202,9 @@</span><br><span> /* init SAP client before SIM card starts up */</span><br><span> sap_init(ms);</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+ /* SAP response call-back */</span><br><span style="color: hsl(120, 100%, 40%);">+ ms->sap_entity.sap_rsp_cb = &gsm_subscr_sap_rsp_cb;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span> gsm_sim_init(ms);</span><br><span> gsm48_cc_init(ms);</span><br><span> gsm480_ss_init(ms);</span><br><span>diff --git a/src/host/layer23/src/mobile/subscriber.c b/src/host/layer23/src/mobile/subscriber.c</span><br><span>index b2be554..e1e6331 100644</span><br><span>--- a/src/host/layer23/src/mobile/subscriber.c</span><br><span>+++ b/src/host/layer23/src/mobile/subscriber.c</span><br><span>@@ -29,6 +29,7 @@</span><br><span> #include <osmocom/bb/common/logging.h></span><br><span> #include <osmocom/bb/common/osmocom_data.h></span><br><span> #include <osmocom/bb/common/sap_interface.h></span><br><span style="color: hsl(120, 100%, 40%);">+#include <osmocom/bb/common/sap_proto.h></span><br><span> #include <osmocom/bb/common/networks.h></span><br><span> #include <osmocom/bb/mobile/vty.h></span><br><span> </span><br><span>@@ -1306,3 +1307,51 @@</span><br><span> {</span><br><span> return sap_close(ms);</span><br><span> }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+int gsm_subscr_sap_rsp_cb(struct osmocom_ms *ms, int res_code,</span><br><span style="color: hsl(120, 100%, 40%);">+ uint8_t res_type, uint16_t param_len, const uint8_t *param_val)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ struct msgb *msg;</span><br><span style="color: hsl(120, 100%, 40%);">+ int rc = 0;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Response parameter is not encoded in case of error */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (res_code != SAP_RESULT_OK_REQ_PROC_CORR)</span><br><span style="color: hsl(120, 100%, 40%);">+ goto ignore_rsp;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ switch (res_type) {</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_APDU_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* Prevent NULL-pointer dereference */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!param_len || !param_val) {</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = -EINVAL;</span><br><span style="color: hsl(120, 100%, 40%);">+ goto ignore_rsp;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* FIXME: why do we use this length? */</span><br><span style="color: hsl(120, 100%, 40%);">+ msg = msgb_alloc(GSM_SAP_LENGTH, "sap_apdu");</span><br><span style="color: hsl(120, 100%, 40%);">+ if (!msg) {</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = -ENOMEM;</span><br><span style="color: hsl(120, 100%, 40%);">+ goto ignore_rsp;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ msg->data = msgb_put(msg, param_len);</span><br><span style="color: hsl(120, 100%, 40%);">+ memcpy(msg->data, param_val, param_len);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return sim_apdu_resp(ms, msg);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ case SAP_TRANSFER_ATR_RESP:</span><br><span style="color: hsl(120, 100%, 40%);">+ /* TODO: don't read SIM again (if already) */</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_INFO, "SAP card is ready, start reading...\n");</span><br><span style="color: hsl(120, 100%, 40%);">+ return subscr_sim_request(ms);</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ rc = -ENOTSUP;</span><br><span style="color: hsl(120, 100%, 40%);">+ goto ignore_rsp;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ return 0;</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ignore_rsp:</span><br><span style="color: hsl(120, 100%, 40%);">+ LOGP(DSAP, LOGL_NOTICE, "Ignored SAP response '%s' (code=%d)\n",</span><br><span style="color: hsl(120, 100%, 40%);">+ get_value_string(sap_msg_names, res_type), res_code);</span><br><span style="color: hsl(120, 100%, 40%);">+ return rc;</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span></span><br></pre><p>To view, visit <a href="https://gerrit.osmocom.org/12441">change 12441</a>. To unsubscribe, or for help writing mail filters, visit <a href="https://gerrit.osmocom.org/settings">settings</a>.</p><div itemscope itemtype="http://schema.org/EmailMessage"><div itemscope itemprop="action" itemtype="http://schema.org/ViewAction"><link itemprop="url" href="https://gerrit.osmocom.org/12441"/><meta itemprop="name" content="View Change"/></div></div>
<div style="display:none"> Gerrit-Project: osmocom-bb </div>
<div style="display:none"> Gerrit-Branch: master </div>
<div style="display:none"> Gerrit-MessageType: merged </div>
<div style="display:none"> Gerrit-Change-Id: I77bb108615bb2c94c441568f195b04e0a5421643 </div>
<div style="display:none"> Gerrit-Change-Number: 12441 </div>
<div style="display:none"> Gerrit-PatchSet: 9 </div>
<div style="display:none"> Gerrit-Owner: Vadim Yanitskiy <axilirator@gmail.com> </div>
<div style="display:none"> Gerrit-Reviewer: Harald Welte <laforge@gnumonks.org> </div>
<div style="display:none"> Gerrit-Reviewer: Jenkins Builder (1000002) </div>
<div style="display:none"> Gerrit-Reviewer: Kévin Redon <kredon@sysmocom.de> </div>
<div style="display:none"> Gerrit-Reviewer: Vadim Yanitskiy <axilirator@gmail.com> </div>
<div style="display:none"> Gerrit-CC: Max <msuraev@sysmocom.de> </div>