1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
|
/* 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/simtrace_prot.h>
#include <osmocom/simtrace2/simtrace2_api.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");
}
/*! \brief Transmit a given command to the SIMtrace2 device */
int osmo_st2_transp_tx_msg(struct osmo_st2_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 osmo_st2_slot_tx_msg(struct osmo_st2_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 osmo_st2_transp_tx_msg(slot->transp, msg);
}
/***********************************************************************
* Card Emulation protocol
***********************************************************************/
/*! \brief Request the SIMtrace2 to generate a card-insert signal */
int osmo_st2_cardem_request_card_insert(struct osmo_st2_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 osmo_st2_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 osmo_st2_cardem_request_pb_and_rx(struct osmo_st2_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 osmo_st2_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 osmo_st2_cardem_request_pb_and_tx(struct osmo_st2_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 osmo_st2_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 osmo_st2_cardem_request_sw_tx(struct osmo_st2_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 osmo_st2_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA);
}
int osmo_st2_cardem_request_set_atr(struct osmo_st2_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 osmo_st2_slot_tx_msg(ci->slot, msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_SET_ATR);
}
/***********************************************************************
* Modem Control protocol
***********************************************************************/
static int _modem_reset(struct osmo_st2_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 osmo_st2_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 osmo_st2_modem_reset_pulse(struct osmo_st2_slot *slot, uint16_t duration_ms)
{
return _modem_reset(slot, 2, duration_ms);
}
/*! \brief assert the RESET line of the modem */
int osmo_st2_modem_reset_active(struct osmo_st2_slot *slot)
{
return _modem_reset(slot, 1, 0);
}
/*! \brief de-assert the RESET line of the modem */
int osmo_st2_modem_reset_inactive(struct osmo_st2_slot *slot)
{
return _modem_reset(slot, 0, 0);
}
static int _modem_sim_select(struct osmo_st2_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 osmo_st2_slot_tx_msg(slot, msg, SIMTRACE_MSGC_MODEM, SIMTRACE_MSGT_DT_MODEM_SIM_SELECT);
}
/*! \brief select local (physical) SIM for given slot */
int osmo_st2_modem_sim_select_local(struct osmo_st2_slot *slot)
{
return _modem_sim_select(slot, 0);
}
/*! \brief select remote (emulated/forwarded) SIM for given slot */
int osmo_st2_modem_sim_select_remote(struct osmo_st2_slot *slot)
{
return _modem_sim_select(slot, 1);
}
/*! \brief Request slot to send us status information about the modem */
int osmo_st2_modem_get_status(struct osmo_st2_slot *slot)
{
struct msgb *msg = st_msgb_alloc();
return osmo_st2_slot_tx_msg(slot, msg, SIMTRACE_MSGC_MODEM, SIMTRACE_MSGT_BD_MODEM_STATUS);
}
|
