| |
| /* simtrace2-protocol - USB protocol library code for SIMtrace2 |
| * |
| * (C) 2016-2019 by Harald Welte <hwelte@hmw-consulting.de> |
| * (C) 2018, sysmocom -s.f.m.c. GmbH, Author: Kevin Redon <kredon@sysmocom.de> |
| * |
| * This program is free software; you can redistribute it and/or |
| * modify it under the terms of the GNU General Public License |
| * as published by the Free Software Foundation; either version 2 |
| * of the License, or (at your option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU General Public License for more details. |
| * |
| * You should have received a copy of the GNU General Public License |
| * along with this program; if not, write to the Free Software |
| * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
| */ |
| |
| #include <errno.h> |
| #include <unistd.h> |
| #include <stdio.h> |
| #include <stdlib.h> |
| #include <string.h> |
| #include <stdint.h> |
| #include <signal.h> |
| #include <time.h> |
| #define _GNU_SOURCE |
| #include <getopt.h> |
| |
| #include <sys/time.h> |
| #include <sys/types.h> |
| #include <sys/socket.h> |
| #include <netinet/in.h> |
| #include <arpa/inet.h> |
| |
| #include <libusb.h> |
| |
| //#include <osmocom/simtrace2/libusb_util.h> |
| #include <osmocom/simtrace2/simtrace_prot.h> |
| #include <osmocom/simtrace2/simtrace2_api.h> |
| //#include "apdu_dispatch.h" |
| //#include "simtrace2-discovery.h" |
| |
| #include <osmocom/core/utils.h> |
| #include <osmocom/core/socket.h> |
| #include <osmocom/core/msgb.h> |
| #include <osmocom/sim/class_tables.h> |
| #include <osmocom/sim/sim.h> |
| |
| /*********************************************************************** |
| * SIMTRACE core protocol |
| ***********************************************************************/ |
| |
| /*! \brief allocate a message buffer for simtrace use */ |
| static struct msgb *st_msgb_alloc(void) |
| { |
| return msgb_alloc_headroom(1024+32, 32, "SIMtrace"); |
| } |
| |
| #if 0 |
| static void apdu_out_cb(uint8_t *buf, unsigned int len, void *user_data) |
| { |
| printf("APDU: %s\n", osmo_hexdump(buf, len)); |
| gsmtap_send_sim(buf, len); |
| } |
| #endif |
| |
| /*! \brief Transmit a given command to the SIMtrace2 device */ |
| int st_transp_tx_msg(struct st_transport *transp, struct msgb *msg) |
| { |
| int rc; |
| |
| printf("<- %s\n", msgb_hexdump(msg)); |
| |
| if (transp->udp_fd < 0) { |
| int xfer_len; |
| |
| rc = libusb_bulk_transfer(transp->usb_devh, transp->usb_ep.out, |
| msgb_data(msg), msgb_length(msg), |
| &xfer_len, 100000); |
| } else { |
| rc = write(transp->udp_fd, msgb_data(msg), msgb_length(msg)); |
| } |
| |
| msgb_free(msg); |
| return rc; |
| } |
| |
| static struct simtrace_msg_hdr *st_push_hdr(struct msgb *msg, uint8_t msg_class, uint8_t msg_type, |
| uint8_t slot_nr) |
| { |
| struct simtrace_msg_hdr *sh; |
| |
| sh = (struct simtrace_msg_hdr *) msgb_push(msg, sizeof(*sh)); |
| memset(sh, 0, sizeof(*sh)); |
| sh->msg_class = msg_class; |
| sh->msg_type = msg_type; |
| sh->slot_nr = slot_nr; |
| sh->msg_len = msgb_length(msg); |
| |
| return sh; |
| } |
| |
| /* transmit a given message to a specified slot. Expects all headers |
| * present before calling the function */ |
| int st_slot_tx_msg(struct st_slot *slot, struct msgb *msg, |
| uint8_t msg_class, uint8_t msg_type) |
| { |
| st_push_hdr(msg, msg_class, msg_type, slot->slot_nr); |
| |
| return st_transp_tx_msg(slot->transp, msg); |
| } |
| |
| /*********************************************************************** |
| * Card Emulation protocol |
| ***********************************************************************/ |
| |
| |
| /*! \brief Request the SIMtrace2 to generate a card-insert signal */ |
| int cardem_request_card_insert(struct cardem_inst *ci, bool inserted) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct cardemu_usb_msg_cardinsert *cins; |
| |
| cins = (struct cardemu_usb_msg_cardinsert *) msgb_put(msg, sizeof(*cins)); |
| memset(cins, 0, sizeof(*cins)); |
| if (inserted) |
| cins->card_insert = 1; |
| |
| return st_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_CARDINSERT); |
| } |
| |
| /*! \brief Request the SIMtrace2 to transmit a Procedure Byte, then Rx */ |
| int cardem_request_pb_and_rx(struct cardem_inst *ci, uint8_t pb, uint8_t le) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct cardemu_usb_msg_tx_data *txd; |
| txd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*txd)); |
| |
| printf("<= %s(%02x, %d)\n", __func__, pb, le); |
| |
| memset(txd, 0, sizeof(*txd)); |
| txd->data_len = 1; |
| txd->flags = CEMU_DATA_F_PB_AND_RX; |
| /* one data byte */ |
| msgb_put_u8(msg, pb); |
| |
| return st_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA); |
| } |
| |
| /*! \brief Request the SIMtrace2 to transmit a Procedure Byte, then Tx */ |
| int cardem_request_pb_and_tx(struct cardem_inst *ci, uint8_t pb, |
| const uint8_t *data, uint16_t data_len_in) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct cardemu_usb_msg_tx_data *txd; |
| uint8_t *cur; |
| |
| txd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*txd)); |
| |
| printf("<= %s(%02x, %s, %d)\n", __func__, pb, |
| osmo_hexdump(data, data_len_in), data_len_in); |
| |
| memset(txd, 0, sizeof(*txd)); |
| txd->data_len = 1 + data_len_in; |
| txd->flags = CEMU_DATA_F_PB_AND_TX; |
| /* procedure byte */ |
| msgb_put_u8(msg, pb); |
| /* data */ |
| cur = msgb_put(msg, data_len_in); |
| memcpy(cur, data, data_len_in); |
| |
| return st_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA); |
| } |
| |
| /*! \brief Request the SIMtrace2 to send a Status Word */ |
| int cardem_request_sw_tx(struct cardem_inst *ci, const uint8_t *sw) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct cardemu_usb_msg_tx_data *txd; |
| uint8_t *cur; |
| |
| txd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*txd)); |
| |
| printf("<= %s(%02x %02x)\n", __func__, sw[0], sw[1]); |
| |
| memset(txd, 0, sizeof(*txd)); |
| txd->data_len = 2; |
| txd->flags = CEMU_DATA_F_PB_AND_TX | CEMU_DATA_F_FINAL; |
| cur = msgb_put(msg, 2); |
| cur[0] = sw[0]; |
| cur[1] = sw[1]; |
| |
| return st_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA); |
| } |
| |
| int cardem_request_set_atr(struct cardem_inst *ci, const uint8_t *atr, unsigned int atr_len) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct cardemu_usb_msg_set_atr *satr; |
| uint8_t *cur; |
| |
| satr = (struct cardemu_usb_msg_set_atr *) msgb_put(msg, sizeof(*satr)); |
| |
| printf("<= %s(%s)\n", __func__, osmo_hexdump(atr, atr_len)); |
| |
| memset(satr, 0, sizeof(*satr)); |
| satr->atr_len = atr_len; |
| cur = msgb_put(msg, atr_len); |
| memcpy(cur, atr, atr_len); |
| |
| return st_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_SET_ATR); |
| } |
| |
| /*********************************************************************** |
| * Modem Control protocol |
| ***********************************************************************/ |
| |
| static int _modem_reset(struct st_slot *slot, uint8_t asserted, uint16_t pulse_ms) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct st_modem_reset *sr ; |
| |
| sr = (struct st_modem_reset *) msgb_put(msg, sizeof(*sr)); |
| sr->asserted = asserted; |
| sr->pulse_duration_msec = pulse_ms; |
| |
| return st_slot_tx_msg(slot, msg, SIMTRACE_MSGC_MODEM, SIMTRACE_MSGT_DT_MODEM_RESET); |
| } |
| |
| /*! \brief pulse the RESET line of the modem for \a duration_ms milli-seconds*/ |
| int st_modem_reset_pulse(struct st_slot *slot, uint16_t duration_ms) |
| { |
| return _modem_reset(slot, 2, duration_ms); |
| } |
| |
| /*! \brief assert the RESET line of the modem */ |
| int st_modem_reset_active(struct st_slot *slot) |
| { |
| return _modem_reset(slot, 1, 0); |
| } |
| |
| /*! \brief de-assert the RESET line of the modem */ |
| int st_modem_reset_inactive(struct st_slot *slot) |
| { |
| return _modem_reset(slot, 0, 0); |
| } |
| |
| static int _modem_sim_select(struct st_slot *slot, uint8_t remote_sim) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| struct st_modem_sim_select *ss; |
| |
| ss = (struct st_modem_sim_select *) msgb_put(msg, sizeof(*ss)); |
| ss->remote_sim = remote_sim; |
| |
| return st_slot_tx_msg(slot, msg, SIMTRACE_MSGC_MODEM, SIMTRACE_MSGT_DT_MODEM_SIM_SELECT); |
| } |
| |
| /*! \brief select local (physical) SIM for given slot */ |
| int st_modem_sim_select_local(struct st_slot *slot) |
| { |
| return _modem_sim_select(slot, 0); |
| } |
| |
| /*! \brief select remote (emulated/forwarded) SIM for given slot */ |
| int st_modem_sim_select_remote(struct st_slot *slot) |
| { |
| return _modem_sim_select(slot, 1); |
| } |
| |
| /*! \brief Request slot to send us status information about the modem */ |
| int st_modem_get_status(struct st_slot *slot) |
| { |
| struct msgb *msg = st_msgb_alloc(); |
| |
| return st_slot_tx_msg(slot, msg, SIMTRACE_MSGC_MODEM, SIMTRACE_MSGT_BD_MODEM_STATUS); |
| } |