shimun/solo
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

invalid cla and r-block works

Browse Source
This commit is contained in:
merlokk 2019-07-04 19:14:26 +03:00
parent 4845d2c172
commit f2ebaf6abe
2 changed files with 56 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
fido2/apdu.h
View File

@ -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_

86
targets/stm32l432/src/nfc.c
View File

@ -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;
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);
int vlen = MIN(32 - block_offset, len - sendlen);
res[0] = iBlock;
memcpy(&res[1], &data[sendlen], vlen);
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);
}