diff --git a/fido2/apdu.h b/fido2/apdu.h index ff3940c8..44029b33 100644 --- a/fido2/apdu.h +++ b/fido2/apdu.h @@ -51,6 +51,7 @@ extern int apdu_decode(uint8_t *data, size_t len, APDU_STRUCT *apdu); #define SW_COND_USE_NOT_SATISFIED 0x6985 #define SW_FILE_NOT_FOUND 0x6a82 #define SW_INS_INVALID 0x6d00 // Instruction code not supported or invalid +#define SW_CLA_INVALID 0x6e00 #define SW_INTERNAL_EXCEPTION 0x6f00 #endif //_APDU_H_ diff --git a/targets/stm32l432/src/nfc.c b/targets/stm32l432/src/nfc.c index 0097a64b..4cfa7cae 100644 --- a/targets/stm32l432/src/nfc.c +++ b/targets/stm32l432/src/nfc.c @@ -171,14 +171,18 @@ bool nfc_write_response_ex(uint8_t req0, uint8_t * data, uint8_t len, uint16_t r if (len > 32 - 3) return false; - res[0] = NFC_CMD_IBLOCK | (req0 & 3); + res[0] = NFC_CMD_IBLOCK | (req0 & 0x0f); + res[1] = 0; + res[2] = 0; + + uint8_t block_offset = p14443_block_offset(req0); if (len && data) - memcpy(&res[1], data, len); + memcpy(&res[block_offset], data, len); - res[len + 1] = resp >> 8; - res[len + 2] = resp & 0xff; - nfc_write_frame(res, 3 + len); + res[len + block_offset + 0] = resp >> 8; + res[len + block_offset + 1] = resp & 0xff; + nfc_write_frame(res, block_offset + len + 2); return true; } @@ -192,21 +196,24 @@ void nfc_write_response_chaining(uint8_t req0, uint8_t * data, int len) { uint8_t res[32 + 2]; int sendlen = 0; - uint8_t iBlock = NFC_CMD_IBLOCK | (req0 & 3); + uint8_t iBlock = NFC_CMD_IBLOCK | (req0 & 0x0f); + uint8_t block_offset = p14443_block_offset(req0); if (len <= 31) { uint8_t res[32] = {0}; - res[0] = iBlock; + res[0] = iBlock; if (len && data) - memcpy(&res[1], data, len); - nfc_write_frame(res, len + 1); + memcpy(&res[block_offset], data, len); + nfc_write_frame(res, len + block_offset); } else { do { // transmit I block - int vlen = MIN(31, len - sendlen); - res[0] = iBlock; - memcpy(&res[1], &data[sendlen], vlen); + int vlen = MIN(32 - block_offset, len - sendlen); + res[0] = iBlock; + res[1] = 0; + res[2] = 0; + memcpy(&res[block_offset], &data[sendlen], vlen); // if not a last block if (vlen + sendlen < len) @@ -215,7 +222,7 @@ void nfc_write_response_chaining(uint8_t req0, uint8_t * data, int len) } // send data - nfc_write_frame(res, vlen + 1); + nfc_write_frame(res, vlen + block_offset); sendlen += vlen; // wait for transmit (32 bytes aprox 2,5ms) @@ -236,9 +243,10 @@ void nfc_write_response_chaining(uint8_t req0, uint8_t * data, int len) break; } - if (reclen != 1) + uint8_t rblock_offset = p14443_block_offset(res[0]); + if (reclen != rblock_offset) { - printf1(TAG_NFC, "R block length error. len: %d. %d/%d \r\n", reclen,sendlen,len); + printf1(TAG_NFC, "R block length error. len: %d. %d/%d \r\n", reclen, sendlen, len); dump_hex1(TAG_NFC, recbuf, reclen); break; } @@ -387,12 +395,18 @@ int answer_rats(uint8_t parameter) return 0; } -void rblock_acknowledge() +void rblock_acknowledge(uint8_t req0, bool ack) { - uint8_t buf[32]; + uint8_t buf[32] = {0}; + + uint8_t block_offset = p14443_block_offset(req0); NFC_STATE.block_num = !NFC_STATE.block_num; - buf[0] = NFC_CMD_RBLOCK | NFC_STATE.block_num; - nfc_write_frame(buf,1); + + buf[0] = NFC_CMD_RBLOCK | (req0 & 0x0f); + if (ack) + buf[0] |= NFC_CMD_RBLOCK_ACK; + + nfc_write_frame(buf, block_offset); } // international AID = RID:PIX @@ -464,6 +478,13 @@ void nfc_process_iblock(uint8_t * buf, int len) printf1(TAG_NFC,"apdu ok. %scase=%02x cla=%02x ins=%02x p1=%02x p2=%02x lc=%d le=%d\r\n", apdu.extended_apdu ? "[e]":"", apdu.case_type, apdu.cla, apdu.ins, apdu.p1, apdu.p2, apdu.lc, apdu.le); + // check CLA + if (apdu.cla != 0x00 && apdu.cla != 0x80) { + printf1(TAG_NFC, "Unknown CLA %02x\r\n", apdu.cla); + nfc_write_response(buf[0], SW_CLA_INVALID); + return; + } + // TODO this needs to be organized better switch(apdu.ins) { @@ -639,8 +660,6 @@ void nfc_process_iblock(uint8_t * buf, int len) nfc_write_response(buf[0], SW_INS_INVALID); break; } - - } static uint8_t ibuf[1024]; @@ -654,7 +673,7 @@ void clear_ibuf() void nfc_process_block(uint8_t * buf, unsigned int len) { - + printf1(TAG_NFC, "-----\r\n"); if (!len) return; @@ -664,6 +683,7 @@ void nfc_process_block(uint8_t * buf, unsigned int len) } else if (IS_IBLOCK(buf[0])) { + uint8_t block_offset = p14443_block_offset(buf[0]); if (buf[0] & 0x10) { printf1(TAG_NFC_APDU, "NFC_CMD_IBLOCK chaining blen=%d len=%d\r\n", ibuflen, len); @@ -677,27 +697,27 @@ void nfc_process_block(uint8_t * buf, unsigned int len) printf1(TAG_NFC_APDU,"i> "); dump_hex1(TAG_NFC_APDU, buf, len); - if (len) + if (len > block_offset) { - memcpy(&ibuf[ibuflen], &buf[1], len - 1); - ibuflen += len - 1; + memcpy(&ibuf[ibuflen], &buf[block_offset], len - block_offset); + ibuflen += len - block_offset; } // send R block - uint8_t rb = NFC_CMD_RBLOCK | NFC_CMD_RBLOCK_ACK | (buf[0] & 3); - nfc_write_frame(&rb, 1); + rblock_acknowledge(buf[0], true); } else { if (ibuflen) { - if (len) + if (len > block_offset) { - memcpy(&ibuf[ibuflen], &buf[1], len - 1); - ibuflen += len - 1; + memcpy(&ibuf[ibuflen], &buf[block_offset], len - block_offset); + ibuflen += len - block_offset; } - memmove(&ibuf[1], ibuf, ibuflen); - ibuf[0] = buf[0]; - ibuflen++; + // add last chaining to top of the block + memmove(&ibuf[block_offset], ibuf, ibuflen); + memmove(ibuf, buf, block_offset); + ibuflen += block_offset; printf1(TAG_NFC_APDU, "NFC_CMD_IBLOCK chaining last block. blen=%d len=%d\r\n", ibuflen, len); @@ -706,7 +726,6 @@ void nfc_process_block(uint8_t * buf, unsigned int len) nfc_process_iblock(ibuf, ibuflen); } else { - // printf1(TAG_NFC, "NFC_CMD_IBLOCK\r\n"); nfc_process_iblock(buf, len); } clear_ibuf(); @@ -714,7 +733,7 @@ void nfc_process_block(uint8_t * buf, unsigned int len) } else if (IS_RBLOCK(buf[0])) { - rblock_acknowledge(); + rblock_acknowledge(buf[0], false); printf1(TAG_NFC, "NFC_CMD_RBLOCK\r\n"); } else if (IS_SBLOCK(buf[0])) @@ -733,6 +752,7 @@ void nfc_process_block(uint8_t * buf, unsigned int len) else { printf1(TAG_NFC, "NFC_CMD_SBLOCK, Unknown. len[%d]\r\n", len); + nfc_write_response(buf[0], SW_COND_USE_NOT_SATISFIED); } dump_hex1(TAG_NFC, buf, len); }