Harald Welte | 6365374 | 2019-01-03 16:54:16 +0100 | [diff] [blame] | 1 | #include <stdlib.h> |
| 2 | #include <stdio.h> |
| 3 | #include <unistd.h> |
| 4 | #include <assert.h> |
| 5 | |
| 6 | #include <osmocom/core/msgb.h> |
| 7 | #include <osmocom/core/utils.h> |
| 8 | |
| 9 | #include "ccid_proto.h" |
| 10 | |
| 11 | #define NR_SLOTS 8 |
| 12 | |
| 13 | struct ccid_slot { |
| 14 | struct ccid_instance *ci; |
| 15 | uint8_t slot_nr; |
| 16 | bool icc_present; |
| 17 | bool icc_powered; |
| 18 | bool icc_in_reset; |
| 19 | }; |
| 20 | |
| 21 | struct ccid_ops { |
| 22 | int (*send)(struct ccid_instance *ci, struct msgb *msg); |
| 23 | }; |
| 24 | |
| 25 | struct ccid_instance { |
| 26 | struct ccid_slot slot[NR_SLOTS]; |
| 27 | struct ccid_ops ops; |
| 28 | }; |
| 29 | |
| 30 | #define msgb_ccid_out(x) (union ccid_pc_to_rdr *)msgb_data(x) |
| 31 | #define msgb_ccid_in(x) (union ccid_rdr_to_pc *)msgb_data(x) |
| 32 | |
| 33 | static struct ccid_slot *get_ccid_slot(struct ccid_instance *ci, uint8_t slot_nr) |
| 34 | { |
| 35 | if (slot_nr >= sizeof(ci->slot)) |
| 36 | return NULL; |
| 37 | else |
| 38 | return &ci->slot[slot_nr] |
| 39 | } |
| 40 | |
| 41 | static uint8_t get_icc_status(const struct ccid_slot *cs) |
| 42 | { |
| 43 | if (cs->icc_present && cs->icc_powered && !cs->icc_in_reset) |
| 44 | return CCID_ICC_STATUS_PRES_ACT; |
| 45 | else if (!cs->icc_present) |
| 46 | return CCID_ICC_STATUS_NO_ICC; |
| 47 | else |
| 48 | return CCID_ICC_STATUS_PRES_INACT; |
| 49 | } |
| 50 | |
| 51 | #define SET_HDR(x, msg_type, slot, seq) do { \ |
| 52 | (x)->hdr.bMessageType = msg_type; \ |
| 53 | (x)->hdr.dwLength = 0; \ |
| 54 | (x)->hdr.bSlot = slot; \ |
| 55 | (x)->hdr.bSeq = seq; \ |
| 56 | } while (0) |
| 57 | |
| 58 | #define SET_HDR_IN(x, msg_type, slot, seq, status, error) do { \ |
| 59 | SET_HDR(&(x)->hdr, msg_type, slot, seq); \ |
| 60 | (x)->hdr.bStatus = status; \ |
| 61 | (x)->hdr.bError = error; \ |
| 62 | } while (0) |
| 63 | |
| 64 | /*********************************************************************** |
| 65 | * Message generation / sending |
| 66 | ***********************************************************************/ |
| 67 | |
| 68 | static struct msgb *ccid_msgb_alloc(void) |
| 69 | { |
| 70 | struct msgb *msg = msgb_alloc("ccid"); |
| 71 | OSMO_ASSERT(msg); |
| 72 | return msg; |
| 73 | } |
| 74 | |
| 75 | static int ccid_send(struct ccid_instance *ci, struct msgb *msg) |
| 76 | { |
| 77 | return ci->ops.send_in(ci, msg); |
| 78 | } |
| 79 | |
| 80 | static int ccid_slot_send(struct ccid_slot *cs, struct msgb *msg) |
| 81 | { |
| 82 | const struct ccid_header *ch = (const struct ccid_header *) msgb_ccid_in(msg); |
| 83 | |
| 84 | /* patch bSlotNr into message */ |
| 85 | ch->hdr.bSlot = cs->slot_nr; |
| 86 | return ccid_send(cs->ci, msg); |
| 87 | } |
| 88 | |
| 89 | |
| 90 | /* Section 6.2.1 */ |
| 91 | static struct msgb *ccid_gen_data_block(struct ccid_slot *cs, uint8_t seq, uint8_t cmd_sts, |
| 92 | enum ccid_error_code err, const uint8_t *data, |
| 93 | uint32_t data_len) |
| 94 | { |
| 95 | struct msgb *msg = ccid_msgb_alloc(); |
| 96 | struct ccid_rdr_to_pc_data_block *db = msgb_put(msg, sizeof(*db) + data_len); |
| 97 | uint8_t sts = (cmd_sts & CCID_CMD_STATUS_MASK) | get_icc_status(cs); |
| 98 | |
| 99 | SET_HDR_IN(db, RDR_to_PC_DataBlock, cs->slot_nr, seq, sts, err); |
| 100 | db->hdr.dwLength = cpu_to_le32(data_len); |
| 101 | memcpy(db->abData, data, data_len); |
| 102 | return msg; |
| 103 | } |
| 104 | |
| 105 | /* Section 6.2.2 */ |
| 106 | static struct msgb *ccid_gen_slot_status(struct ccid_slot *cs, uint8_t seq, uint8_t cmd_sts, |
| 107 | enum ccid_error_code err) |
| 108 | { |
| 109 | struct msgb *msg = ccid_msgb_alloc(); |
| 110 | struct ccid_rdr_to_pc_slot_status *ss = msgb_put(msg, sizeof(*ss)); |
| 111 | uint8_t sts = (cmd_sts & CCID_CMD_STATUS_MASK) | get_icc_status(cs); |
| 112 | |
| 113 | SET_HDR_IN(ss, RDR_to_PC_SlotStatus, cs->slot_nr, seq, sts, err); |
| 114 | return msg; |
| 115 | } |
| 116 | |
| 117 | /* Section 6.2.3 */ |
| 118 | /* TODO */ |
| 119 | |
| 120 | /* Section 6.2.4 */ |
| 121 | static struct msgb *ccid_gen_escape(struct ccid_slot *cs, uint8_t seq, uint8_t cmd_sts, |
| 122 | enum ccid_error_code err, const uint8_t *data, |
| 123 | uint32_t data_len) |
| 124 | { |
| 125 | struct msgb *msg = ccid_msgb_alloc(); |
| 126 | struct ccid_rdr_to_pc_escape *esc = msgb_put(msg, sizeof(*esc) + data_len); |
| 127 | uint8_t sts = (cmd_sts & CCID_CMD_STATUS_MASK) | get_icc_status(cs); |
| 128 | |
| 129 | SET_HDR_IN(esc, RDR_to_PC_Escape, cs->slot_nr, seq, sts, err); |
| 130 | esc->hdr.dwLength = cpu_to_le32(data_len); |
| 131 | memcpy(esc->abData, data, data_len); |
| 132 | return msg; |
| 133 | } |
| 134 | |
| 135 | /* Section 6.2.5 */ |
| 136 | static struct msgb *ccid_gen_clock_and_rate(struct ccid_slot *cs, uint8_t seq, uint8_t cmd_sts, |
| 137 | enum ccid_error_code err, uint32_t clock_khz, uint32_t rate_bps) |
| 138 | { |
| 139 | struct msgb *msg = ccid_msgb_alloc(); |
| 140 | struct ccid_rdr_to_pc_data_rate_and_clock *drc = msgb_put(msg, sizeof(*drc)); |
| 141 | uint8_t sts = (cmd_sts & CCID_CMD_STATUS_MASK) | get_icc_status(cs); |
| 142 | |
| 143 | SET_HDR_IN(drc, RDR_to_PC_DataRateAndClockFrequency, cs->slot_nr, seq, sts, err); |
| 144 | drc->dwLength = cpu_to_le32(8); /* Message-specific data length (wtf?) */ |
| 145 | drc->dwClockFrequency = cpu_to_le32(clock_khz); /* kHz */ |
| 146 | drc->dwDataRate = cpu_to_le32(rate_bps); /* bps */ |
| 147 | return msg; |
| 148 | } |
| 149 | |
| 150 | |
| 151 | |
| 152 | #if 0 |
| 153 | static struct msgb *gen_err_resp(struct ccid_instance *ci, enum ccid_msg_type msg_type, |
| 154 | enum ccid_error_code err_code) |
| 155 | { |
| 156 | struct c |
| 157 | } |
| 158 | #endif |
| 159 | |
| 160 | /*********************************************************************** |
| 161 | * Message reception / parsing |
| 162 | ***********************************************************************/ |
| 163 | |
| 164 | /* Section 6.1.3 */ |
| 165 | static int ccid_handle_get_slot_status(struct ccid_slot *cs, struct msgb *msg) |
| 166 | { |
| 167 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 168 | struct msgb *resp; |
| 169 | |
| 170 | resp = ccid_gen_slot_status(cs, u->get_slot_status.hdr.bSeq, CCID_CMD_STATUS_OK, 0); |
| 171 | |
| 172 | return ccid_send(cs->ci, resp); |
| 173 | } |
| 174 | |
| 175 | |
| 176 | /* Section 6.1.1 */ |
| 177 | static int ccid_handle_icc_power_on(struct ccid_slot *cs, struct msgb *msg) |
| 178 | { |
| 179 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 180 | struct msgb *resp; |
| 181 | |
| 182 | /* TODO: send actual ATR; handle error cases */ |
| 183 | /* TODO: handle this asynchronously */ |
| 184 | resp = ccid_gen_data_block(cs, u->icc_power_on.hdr.hSeq, CCID_CMD_STATUS_OK, 0, NULL, 0); |
| 185 | |
| 186 | return ccid_send(cs->ci, resp); |
| 187 | } |
| 188 | |
| 189 | /* Section 6.1.2 */ |
| 190 | static int ccid_handle_icc_power_off(struct ccid_slot *cs, struct msgb *msg) |
| 191 | { |
| 192 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 193 | resp = ccid_gen_slot_status(cs, u->get_slot_status.hdr.bSeq, CCID_CMD_STATUS_OK, 0); |
| 194 | return ccid_send(cs->ci, resp); |
| 195 | } |
| 196 | |
| 197 | /* Section 6.1.4 */ |
| 198 | static int ccid_handle_xfr_block(struct ccid_slot *cs, struct msgb *msg) |
| 199 | { |
| 200 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 201 | resp = ccid_gen_data_block(cs, u->icc_power_on.hdr.hSeq, CCID_CMD_STATUS_OK, 0, NULL, 0); |
| 202 | return ccid_send(cs->ci, resp); |
| 203 | } |
| 204 | |
| 205 | /* Section 6.1.5 */ |
| 206 | static int ccid_handle_get_parameters(struct ccid_slot *cs, struct msgb *msg) |
| 207 | { |
| 208 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 209 | } |
| 210 | |
| 211 | /* Section 6.1.6 */ |
| 212 | static int ccid_handle_reset_parameters(struct ccid_slot *cs, struct msgb *msg) |
| 213 | { |
| 214 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 215 | } |
| 216 | |
| 217 | /* Section 6.1.7 */ |
| 218 | static int ccid_handle_set_parameters(struct ccid_slot *cs, struct msgb *msg) |
| 219 | { |
| 220 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 221 | } |
| 222 | |
| 223 | /* Section 6.1.8 */ |
| 224 | static int ccid_handle_escape(struct ccid_slot *cs, struct msgb *msg) |
| 225 | { |
| 226 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 227 | } |
| 228 | |
| 229 | /* Section 6.1.9 */ |
| 230 | static int ccid_handle_icc_clock(struct ccid_slot *cs, struct msgb *msg) |
| 231 | { |
| 232 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 233 | resp = ccid_gen_slot_status(cs, u->get_slot_status.hdr.bSeq, CCID_CMD_STATUS_OK, 0); |
| 234 | return ccid_send(cs->ci, resp); |
| 235 | } |
| 236 | |
| 237 | /* Section 6.1.10 */ |
| 238 | static int ccid_handle_t0apdu(struct ccid_slot *cs, struct msgb *msg) |
| 239 | { |
| 240 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 241 | resp = ccid_gen_slot_status(cs, u->get_slot_status.hdr.bSeq, CCID_CMD_STATUS_OK, 0); |
| 242 | return ccid_send(cs->ci, resp); |
| 243 | } |
| 244 | |
| 245 | /* Section 6.1.11 */ |
| 246 | static int ccid_handle_secure(struct ccid_slot *cs, struct msgb *msg) |
| 247 | { |
| 248 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 249 | } |
| 250 | |
| 251 | /* Section 6.1.12 */ |
| 252 | static int ccid_handle_mechanical(struct ccid_slot *cs, struct msgb *msg) |
| 253 | { |
| 254 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 255 | resp = ccid_gen_slot_status(cs, u->get_slot_status.hdr.bSeq, CCID_CMD_STATUS_OK, 0); |
| 256 | return ccid_send(cs->ci, resp); |
| 257 | } |
| 258 | |
| 259 | /* Section 6.1.13 */ |
| 260 | static int ccid_handle_abort(struct ccid_slot *cs, struct msgb *msg) |
| 261 | { |
| 262 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 263 | resp = ccid_gen_slot_status(cs, u->get_slot_status.hdr.bSeq, CCID_CMD_STATUS_OK, 0); |
| 264 | return ccid_send(cs->ci, resp); |
| 265 | } |
| 266 | |
| 267 | /* Section 6.1.14 */ |
| 268 | static int ccid_handle_set_rate_and_clock(struct ccid_slot *cs, struct msgb *msg) |
| 269 | { |
| 270 | const union ccid_pc_to_rdr *u = msgb_ccid(msg); |
| 271 | } |
| 272 | |
| 273 | /* handle data arriving from the host on the OUT endpoint */ |
| 274 | int ccid_handle_out(struct ccid_instance *ci, struct msgb *msg) |
| 275 | { |
| 276 | const union ccid_pc_to_rdr *u = msgb_ccid_out(msg); |
| 277 | const struct ccid_header *ch = (const struct ccid_header *) u; |
| 278 | unsigned int len = msgb_length(msg); |
| 279 | struct ccid_slot *cs; |
| 280 | |
| 281 | if (len < sizeof(*ch)) { |
| 282 | /* FIXME */ |
| 283 | return -1; |
| 284 | } |
| 285 | |
| 286 | cs = get_ccid_slot(ci, ch->hdr.bSlot); |
| 287 | if (!cs) { |
| 288 | /* FIXME */ |
| 289 | return -1; |
| 290 | } |
| 291 | |
| 292 | switch (ch->bMessageType) { |
| 293 | case PC_to_RDR_GetSlotStatus: |
| 294 | if (len != sizeof(u->get_slot_status)) |
| 295 | goto short_msg; |
| 296 | rc = ccid_handle_get_slot_status(cs, msg); |
| 297 | break; |
| 298 | case PC_to_RDR_IccPowerOn: |
| 299 | if (len != sizeof(u->icc_power_on)) |
| 300 | goto short_msg; |
| 301 | rc = ccid_handle_icc_power_on(cs, msg); |
| 302 | break; |
| 303 | case PC_to_RDR_IccPowerOff: |
| 304 | if (len != sizeof(u->icc_power_off)) |
| 305 | goto short_msg; |
| 306 | rc = ccid_handle_icc_power_off(cs, msg); |
| 307 | break; |
| 308 | case PC_to_RDR_XfrBlock: |
| 309 | if (len < sizeof(u->xfr_block)) |
| 310 | goto short_msg; |
| 311 | rc = ccid_handle_xfr_block(cs, msg); |
| 312 | break; |
| 313 | case PC_to_RDR_GetParameters: |
| 314 | if (len != sizeof(u->get_parameters)) |
| 315 | goto short_msg; |
| 316 | rc = ccid_handle_get_parameters(cs, msg); |
| 317 | break; |
| 318 | case PC_to_RDR_ResetParameters: |
| 319 | if (len != sizeof(u->reset_parameters)) |
| 320 | goto short_msg; |
| 321 | rc = ccid_handle_reset_parameters(cs, msg); |
| 322 | break; |
| 323 | case PC_to_RDR_SetParameters: |
| 324 | if (len != sizeof(u->set_parameters)) |
| 325 | goto short_msg; |
| 326 | rc = ccid_handle_set_parameters(cs, msg); |
| 327 | break; |
| 328 | case PC_to_RDR_Escape: |
| 329 | if (len < sizeof(u->escape)) |
| 330 | goto short_msg; |
| 331 | rc = ccid_handle_escape(cs, msg); |
| 332 | break; |
| 333 | case PC_to_RDR_IccClock: |
| 334 | if (len != sizeof(u->icc_clock)) |
| 335 | goto short_msg; |
| 336 | rc = ccid_handle_icc_clock(cs, msg); |
| 337 | break; |
| 338 | case PC_to_RDR_T0APDU: |
| 339 | if (len != /*FIXME*/ sizeof(u->t0apdu)) |
| 340 | goto short_msg; |
| 341 | rc = ccid_handle_t0_apdu(cs, msg); |
| 342 | break; |
| 343 | case PC_to_RDR_Secure: |
| 344 | if (len < sizeof(u->secure)) |
| 345 | goto short_msg; |
| 346 | rc = ccid_handle_secure(cs, msg); |
| 347 | break; |
| 348 | case PC_to_RDR_Mechanical: |
| 349 | if (len != sizeof(u->mechanical)) |
| 350 | goto short_msg; |
| 351 | rc = ccid_handle_mechanical(cs, msg); |
| 352 | break; |
| 353 | case PC_to_RDR_Abort: |
| 354 | if (len != sizeof(u->abort)) |
| 355 | goto short_msg; |
| 356 | rc = ccid_handle_abort(cs, msg); |
| 357 | break; |
| 358 | case PC_to_RDR_SetDataRateAndClockFrequency: |
| 359 | if (len != sizeof(u->set_rate_and_clock)) |
| 360 | goto short_msg; |
| 361 | rc = ccid_handle_set_rate_and_clock(cs, msg); |
| 362 | break; |
| 363 | default: |
| 364 | FIXME |
| 365 | break; |
| 366 | } |
| 367 | FIXME |
| 368 | short_msg: |
| 369 | FIXME |
| 370 | } |