347 lines
9.2 KiB
C
347 lines
9.2 KiB
C
#include <string.h>
|
|
|
|
#include "stm32l4xx.h"
|
|
|
|
#include "nfc.h"
|
|
#include "ams.h"
|
|
#include "log.h"
|
|
#include "util.h"
|
|
#include "device.h"
|
|
|
|
// Capability container
|
|
|
|
|
|
const CAPABILITY_CONTAINER NFC_CC = {
|
|
.cclen_hi = 0x00, .cclen_lo = 0x0f,
|
|
.version = 0x01,
|
|
.MLe_hi = 0x00, .MLe_lo = 0xff,
|
|
.MLc_hi = 0x00, .MLc_lo = 0xff,
|
|
.tlv = { 0x04,0x06,
|
|
0x11,0x11,
|
|
0x00,0xff,
|
|
0x00,0xff }
|
|
};
|
|
|
|
uint8_t NDEF_SAMPLE[] = "\x00\x13\xD1\x01\x0ET\x02enHello World";
|
|
|
|
static struct
|
|
{
|
|
uint8_t max_frame_size;
|
|
uint8_t cid;
|
|
uint8_t block_num;
|
|
uint8_t selected_applet;
|
|
} NFC_STATE;
|
|
|
|
void nfc_state_init()
|
|
{
|
|
memset(&NFC_STATE,0,sizeof(NFC_STATE));
|
|
NFC_STATE.max_frame_size = 32;
|
|
NFC_STATE.block_num = 1;
|
|
}
|
|
|
|
void nfc_init()
|
|
{
|
|
|
|
|
|
nfc_state_init();
|
|
ams_init();
|
|
}
|
|
|
|
void nfc_write_frame(uint8_t * data, uint8_t len)
|
|
{
|
|
if (len > 32)
|
|
{
|
|
len = 32;
|
|
}
|
|
ams_write_command(AMS_CMD_CLEAR_BUFFER);
|
|
ams_write_buffer(data,len);
|
|
ams_write_command(AMS_CMD_TRANSMIT_BUFFER);
|
|
|
|
}
|
|
|
|
int answer_rats(uint8_t parameter)
|
|
{
|
|
|
|
uint8_t fsdi = (parameter & 0xf0) >> 4;
|
|
uint8_t cid = (parameter & 0x0f);
|
|
|
|
NFC_STATE.cid = cid;
|
|
|
|
if (fsdi == 0)
|
|
NFC_STATE.max_frame_size = 16;
|
|
else if (fsdi == 1)
|
|
NFC_STATE.max_frame_size = 24;
|
|
else
|
|
NFC_STATE.max_frame_size = 32;
|
|
|
|
uint8_t res[3 + 11];
|
|
res[0] = sizeof(res);
|
|
res[1] = 2 | (1<<5); // 2 FSCI == 32 byte frame size, TB is enabled
|
|
|
|
// frame wait time = (256 * 16 / 13.56MHz) * 2^FWI
|
|
// FWI=0, FMT=0.3ms (min)
|
|
// FWI=4, FMT=4.8ms (default)
|
|
// FWI=10, FMT=309ms
|
|
// FWI=12, FMT=1237ms
|
|
// FWI=14, FMT=4949ms (max)
|
|
res[2] = (12<<4) | (0); // TB (FWI << 4) | (SGTI)
|
|
|
|
// historical bytes
|
|
memcpy(&res[3], (uint8_t *)"SoloKey tap", 11);
|
|
|
|
nfc_write_frame(res, sizeof(res));
|
|
return 0;
|
|
}
|
|
|
|
void rblock_acknowledge()
|
|
{
|
|
uint8_t buf[32];
|
|
NFC_STATE.block_num = !NFC_STATE.block_num;
|
|
buf[0] = NFC_CMD_RBLOCK | NFC_STATE.block_num;
|
|
nfc_write_frame(buf,1);
|
|
}
|
|
|
|
// Selects application. Returns 1 if success, 0 otherwise
|
|
int select_applet(uint8_t * aid, int len)
|
|
{
|
|
if (memcmp(aid,AID_NDEF_TYPE_4,sizeof(AID_NDEF_TYPE_4)) == 0)
|
|
{
|
|
NFC_STATE.selected_applet = APP_NDEF_TYPE_4;
|
|
return 1;
|
|
} else if (memcmp(aid,AID_NDEF_MIFARE_TYPE_4,sizeof(AID_NDEF_MIFARE_TYPE_4)) == 0)
|
|
{
|
|
NFC_STATE.selected_applet = APP_MIFARE_TYPE_4;
|
|
return 1;
|
|
} else if (memcmp(aid,AID_CAPABILITY_CONTAINER,sizeof(AID_CAPABILITY_CONTAINER)) == 0)
|
|
{
|
|
NFC_STATE.selected_applet = APP_CAPABILITY_CONTAINER;
|
|
return 1;
|
|
} else if (memcmp(aid,AID_NDEF_TAG,sizeof(AID_NDEF_TAG)) == 0)
|
|
{
|
|
NFC_STATE.selected_applet = APP_NDEF_TAG;
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void nfc_process_iblock(uint8_t * buf, int len)
|
|
{
|
|
APDU_HEADER * apdu = (APDU_HEADER *)(buf+1);
|
|
uint8_t * payload = buf + 1 + 5;
|
|
uint8_t plen = apdu->lc;
|
|
int selected;
|
|
uint8_t res[32];
|
|
|
|
printf1(TAG_NFC,">> "); dump_hex1(TAG_NFC,buf,len);
|
|
|
|
|
|
// TODO this needs to be organized better
|
|
switch(apdu->ins)
|
|
{
|
|
case APDU_INS_SELECT:
|
|
if (plen > len - 6)
|
|
{
|
|
printf1(TAG_ERR, "Truncating APDU length %d\r\n", apdu->lc);
|
|
plen = len-6;
|
|
}
|
|
// if (apdu->p1 == 0 && apdu->p2 == 0x0c)
|
|
// {
|
|
// printf1(TAG_NFC,"Select NDEF\r\n");
|
|
//
|
|
// NFC_STATE.selected_applet = APP_NDEF_TAG;
|
|
// // Select NDEF file!
|
|
// res[0] = NFC_CMD_IBLOCK | (buf[0] & 1);
|
|
// res[1] = APDU_STATUS_SUCCESS>>8;
|
|
// res[2] = APDU_STATUS_SUCCESS & 0xff;
|
|
// nfc_write_frame(res, 3);
|
|
// printf1(TAG_NFC,"<< "); dump_hex1(TAG_NFC,res, 3);
|
|
// }
|
|
// else
|
|
{
|
|
selected = select_applet(payload, plen);
|
|
if (selected)
|
|
{
|
|
|
|
// block = buf[0] & 1;
|
|
// block = NFC_STATE.block_num;
|
|
// block = !block;
|
|
// NFC_STATE.block_num = block;
|
|
res[0] = NFC_CMD_IBLOCK | (buf[0] & 1);
|
|
res[1] = APDU_STATUS_SUCCESS>>8;
|
|
res[2] = APDU_STATUS_SUCCESS & 0xff;
|
|
nfc_write_frame(res, 3);
|
|
printf1(TAG_NFC,"<< "); dump_hex1(TAG_NFC,res, 3);
|
|
}
|
|
else
|
|
{
|
|
printf1(TAG_NFC, "NOT selected\r\n");
|
|
}
|
|
}
|
|
break;
|
|
case APDU_INS_READ_BINARY:
|
|
|
|
|
|
switch(NFC_STATE.selected_applet)
|
|
{
|
|
case APP_CAPABILITY_CONTAINER:
|
|
printf1(TAG_NFC,"APP_CAPABILITY_CONTAINER\r\n");
|
|
if (plen > 15)
|
|
{
|
|
printf1(TAG_ERR, "Truncating requested CC length %d\r\n", apdu->lc);
|
|
plen = 15;
|
|
}
|
|
memmove(res+1, &NFC_CC, plen);
|
|
break;
|
|
case APP_NDEF_TAG:
|
|
printf1(TAG_NFC,"APP_NDEF_TAG\r\n");
|
|
if (plen > (sizeof(NDEF_SAMPLE) - 1))
|
|
{
|
|
printf1(TAG_ERR, "Truncating requested CC length %d\r\n", apdu->lc);
|
|
plen = sizeof(NDEF_SAMPLE) - 1;
|
|
}
|
|
memmove(res+1, NDEF_SAMPLE, plen);
|
|
break;
|
|
default:
|
|
printf1(TAG_ERR, "No binary applet selected!\r\n");
|
|
return;
|
|
break;
|
|
}
|
|
res[0] = NFC_CMD_IBLOCK | (buf[0] & 1);
|
|
|
|
res[1+plen] = APDU_STATUS_SUCCESS>>8;
|
|
res[2+plen] = APDU_STATUS_SUCCESS & 0xff;
|
|
nfc_write_frame(res, 3+plen);
|
|
printf1(TAG_NFC,"APDU_INS_READ_BINARY\r\n");
|
|
printf1(TAG_NFC,"<< "); dump_hex1(TAG_NFC,res, 3+plen);
|
|
break;
|
|
default:
|
|
printf1(TAG_NFC, "Unknown INS %02x\r\n", apdu->ins);
|
|
break;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void nfc_process_block(uint8_t * buf, int len)
|
|
{
|
|
if (IS_PPSS_CMD(buf[0]))
|
|
{
|
|
printf1(TAG_NFC, "NFC_CMD_PPSS\r\n");
|
|
}
|
|
else if (IS_IBLOCK(buf[0]))
|
|
{
|
|
nfc_process_iblock(buf,len);
|
|
printf1(TAG_NFC, "NFC_CMD_IBLOCK\r\n");
|
|
}
|
|
else if (IS_RBLOCK(buf[0]))
|
|
{
|
|
rblock_acknowledge();
|
|
printf1(TAG_NFC, "NFC_CMD_RBLOCK\r\n");
|
|
}
|
|
else if (IS_SBLOCK(buf[0]))
|
|
{
|
|
|
|
if ((buf[0] & NFC_SBLOCK_DESELECT) == 0)
|
|
{
|
|
nfc_write_frame(buf, 1);
|
|
printf1(TAG_NFC, "NFC_CMD_SBLOCK, DESELECTED\r\n");
|
|
nfc_state_init();
|
|
}
|
|
else
|
|
{
|
|
printf1(TAG_NFC, "NFC_CMD_SBLOCK, Unknown\r\n");
|
|
}
|
|
dump_hex1(TAG_NFC, buf, len);
|
|
}
|
|
else
|
|
{
|
|
printf1(TAG_NFC, "unknown NFC request\r\n len[%d]:", len);
|
|
dump_hex1(TAG_NFC, buf, len);
|
|
}
|
|
}
|
|
|
|
void nfc_loop()
|
|
{
|
|
static uint32_t t1 = 0;
|
|
uint8_t buf[32];
|
|
AMS_DEVICE ams;
|
|
int len = 0;
|
|
// uint8_t def[] = "\x00\x00\x05\x40\x00\x00\x00\x00\x80\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x14\x02\x01\x00";
|
|
|
|
|
|
// if (millis() - t1 > interval)
|
|
if (1)
|
|
{
|
|
t1 = millis();
|
|
read_reg_block(&ams);
|
|
// if (memcmp(def,ams.buf,sizeof(AMS_DEVICE)) != 0)
|
|
// {
|
|
// printf1(TAG_NFC,"regs: "); dump_hex1(TAG_NFC,ams.buf,sizeof(AMS_DEVICE));
|
|
// }
|
|
if (ams.regs.rfid_status)
|
|
{
|
|
// uint8_t state = AMS_STATE_MASK & ams.regs.rfid_status;
|
|
// if (state != AMS_STATE_SENSE)
|
|
// printf1(TAG_NFC," %s %d\r\n", ams_get_state_string(ams.regs.rfid_status), millis());
|
|
}
|
|
if (ams.regs.int0 & AMS_INT_INIT)
|
|
{
|
|
// Initialize chip!
|
|
nfc_state_init();
|
|
}
|
|
if (ams.regs.int1)
|
|
{
|
|
// ams_print_int1(ams.regs.int1);
|
|
}
|
|
if (ams.regs.buffer_status2 && (ams.regs.int0 & AMS_INT_RXE))
|
|
{
|
|
if (ams.regs.buffer_status2 & AMS_BUF_INVALID)
|
|
{
|
|
printf1(TAG_NFC,"Buffer being updated!\r\n");
|
|
}
|
|
else
|
|
{
|
|
len = ams.regs.buffer_status2 & AMS_BUF_LEN_MASK;
|
|
ams_read_buffer(buf, len);
|
|
}
|
|
}
|
|
|
|
if (len)
|
|
{
|
|
|
|
// ISO 14443-3
|
|
switch(buf[0])
|
|
{
|
|
case NFC_CMD_REQA:
|
|
printf1(TAG_NFC, "NFC_CMD_REQA\r\n");
|
|
break;
|
|
case NFC_CMD_WUPA:
|
|
printf1(TAG_NFC, "NFC_CMD_WUPA\r\n");
|
|
break;
|
|
case NFC_CMD_HLTA:
|
|
printf1(TAG_NFC, "HLTA/Halt\r\n");
|
|
break;
|
|
case NFC_CMD_RATS:
|
|
t1 = millis();
|
|
answer_rats(buf[1]);
|
|
NFC_STATE.block_num = 1;
|
|
printf1(TAG_NFC,"RATS answered %d (took %d)\r\n",millis(), millis() - t1);
|
|
break;
|
|
default:
|
|
|
|
// ISO 14443-4
|
|
nfc_process_block(buf,len);
|
|
|
|
|
|
break;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
}
|