Skip to content

Commit

Permalink
invalid cla and r-block works
Browse files Browse the repository at this point in the history
  • Loading branch information
@merlokk
merlokk committed Jul 4, 2019
1 parent 4845d2c commit f2ebaf6
Show file tree
Hide file tree
Showing 2 changed files with 56 additions and 35 deletions.
1 change: 1 addition & 0 deletions fido2/apdu.h
View file
Expand Up @@ -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_
90 changes: 55 additions & 35 deletions targets/stm32l432/src/nfc.c
View file
Expand Up @@ -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;
}
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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;
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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];
Expand All @@ -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;

Expand All @@ -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);
Expand All @@ -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);

Expand All @@ -706,15 +726,14 @@ 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();
}
}
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]))
Expand All @@ -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);
}
Expand Down

0 comments on commit f2ebaf6

Please sign in to comment.