mirrors/pico-keys-sdk
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Harmonizing coding style.

Browse source 
Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
This commit is contained in:
Pol Henarejos 2023-02-14 23:11:51 +01:00
parent 31e66007d3
commit 12bdcbd1f9
No known key found for this signature in database
GPG key ID: C0095B7870A4CCD3
30 changed files with 1475 additions and 1036 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

114
src/apdu.c
View file

@ -24,112 +24,122 @@ uint8_t *rdata_gr = NULL;
uint16_t rdata_bk = 0x0;
extern uint32_t timeout;
int process_apdu() {
int process_apdu()
{
led_set_blink(BLINK_PROCESSING);
if (INS(apdu) == 0xA4 && P1(apdu) == 0x04 && (P2(apdu) == 0x00 || P2(apdu) == 0x4)) { //select by AID
if (current_app && current_app->unload)
if (current_app && current_app->unload) {
current_app->unload();
}
for (int a = 0; a < num_apps; a++) {
if ((current_app = apps[a].select_aid(&apps[a], apdu.data, apdu.nc))) {
return set_res_sw(0x90,0x00);
return set_res_sw(0x90, 0x00);
}
}
return set_res_sw(0x6a, 0x82);
}
if (current_app && current_app->process_apdu)
if (current_app && current_app->process_apdu) {
return current_app->process_apdu();
}
return set_res_sw(0x6D, 0x00);
}
size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size) {
apdu.header = (uint8_t *)buffer;
size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size)
{
apdu.header = (uint8_t *) buffer;
apdu.nc = apdu.ne = 0;
if (buffer_size == 4) {
apdu.nc = apdu.ne = 0;
if (apdu.ne == 0)
if (apdu.ne == 0) {
apdu.ne = 256;
}
else if (buffer_size == 5) {
}
} else if (buffer_size == 5) {
apdu.nc = 0;
apdu.ne = apdu.header[4];
if (apdu.ne == 0)
if (apdu.ne == 0) {
apdu.ne = 256;
}
else if (apdu.header[4] == 0x0 && buffer_size >= 7) {
}
} else if (apdu.header[4] == 0x0 && buffer_size >= 7) {
if (buffer_size == 7) {
apdu.ne = (apdu.header[5] << 8) | apdu.header[6];
if (apdu.ne == 0)
if (apdu.ne == 0) {
apdu.ne = 65536;
}
else {
}
} else {
apdu.ne = 0;
apdu.nc = (apdu.header[5] << 8) | apdu.header[6];
apdu.data = apdu.header+7;
if (apdu.nc+7+2 == buffer_size) {
apdu.ne = (apdu.header[buffer_size-2] << 8) | apdu.header[buffer_size-1];
if (apdu.ne == 0)
if (apdu.ne == 0) {
apdu.ne = 65536;
}
}
}
}
else {
} else {
apdu.nc = apdu.header[4];
apdu.data = apdu.header+5;
apdu.ne = 0;
if (apdu.nc+5+1 == buffer_size) {
apdu.ne = apdu.header[buffer_size-1];
if (apdu.ne == 0)
if (apdu.ne == 0) {
apdu.ne = 256;
}
}
}
//printf("apdu.nc %ld, apdu.ne %ld\r\n",apdu.nc,apdu.ne);
if (apdu.header[1] == 0xc0) {
//printf("apdu.ne %u, apdu.rlen %d, bk %x\r\n",apdu.ne,apdu.rlen,rdata_bk);
timeout_stop();
*(uint16_t *)rdata_gr = rdata_bk;
*(uint16_t *) rdata_gr = rdata_bk;
if (apdu.rlen <= apdu.ne) {
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
driver_exec_finished_cont_hid(apdu.rlen+2, rdata_gr-usb_get_tx(itf));
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
driver_exec_finished_cont_ccid(apdu.rlen+2, rdata_gr-usb_get_tx(itf));
}
#endif
#ifdef ENABLE_EMULATION
if (itf == ITF_EMUL)
if (itf == ITF_EMUL) {
driver_exec_finished_cont_emul(apdu.rlen+2, rdata_gr-usb_get_tx(itf));
}
#endif
//Prepare next RAPDU
apdu.sw = 0;
apdu.rlen = 0;
usb_prepare_response(itf);
}
else {
} else {
rdata_gr += apdu.ne;
rdata_bk = *rdata_gr;
rdata_gr[0] = 0x61;
if (apdu.rlen - apdu.ne >= 256)
if (apdu.rlen - apdu.ne >= 256) {
rdata_gr[1] = 0;
else
} else {
rdata_gr[1] = apdu.rlen - apdu.ne;
}
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
driver_exec_finished_cont_hid(apdu.ne+2, rdata_gr-apdu.ne-usb_get_tx(itf));
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
driver_exec_finished_cont_ccid(apdu.ne+2, rdata_gr-apdu.ne-usb_get_tx(itf));
}
#endif
#ifdef ENABLE_EMULATION
if (itf == ITF_EMUL)
if (itf == ITF_EMUL) {
driver_exec_finished_cont_emul(apdu.ne+2, rdata_gr-apdu.ne-usb_get_tx(itf));
}
#endif
apdu.rlen -= apdu.ne;
}
return 0;
}
else {
} else {
apdu.sw = 0;
apdu.rlen = 0;
apdu.rdata = usb_prepare_response(itf);
@ -139,31 +149,33 @@ size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size) {
return 0;
}
uint16_t set_res_sw(uint8_t sw1, uint8_t sw2) {
uint16_t set_res_sw(uint8_t sw1, uint8_t sw2)
{
apdu.sw = (sw1 << 8) | sw2;
if (sw1 != 0x90)
if (sw1 != 0x90) {
res_APDU_size = 0;
}
return make_uint16_t(sw1, sw2);
}
#ifndef ENABLE_EMULATION
void apdu_thread() {
void apdu_thread()
{
card_init_core1();
while (1) {
uint32_t m = 0;
queue_remove_blocking(&usb_to_card_q, &m);
if (m == EV_VERIFY_CMD_AVAILABLE || m == EV_MODIFY_CMD_AVAILABLE){
set_res_sw (0x6f, 0x00);
goto done;
}
else if (m == EV_EXIT) {
if (m == EV_VERIFY_CMD_AVAILABLE || m == EV_MODIFY_CMD_AVAILABLE) {
set_res_sw(0x6f, 0x00);
goto done;
} else if (m == EV_EXIT) {
break;
}
process_apdu();
done:;
done: ;
apdu_finish();
finished_data_size = apdu_next();
@ -178,30 +190,32 @@ void apdu_thread() {
}
#endif
void apdu_finish() {
void apdu_finish()
{
apdu.rdata[apdu.rlen] = apdu.sw >> 8;
apdu.rdata[apdu.rlen+1] = apdu.sw & 0xff;
timeout_stop();
#ifndef ENABLE_EMULATION
if ((apdu.rlen + 2 + 10) % 64 == 0)
{ // FIX for strange behaviour with PSCS and multiple of 64
if ((apdu.rlen + 2 + 10) % 64 == 0) { // FIX for strange behaviour with PSCS and multiple of 64
apdu.ne = apdu.rlen - 2;
}
#endif
}
size_t apdu_next() {
size_t apdu_next()
{
if (apdu.sw != 0) {
if (apdu.rlen <= apdu.ne)
if (apdu.rlen <= apdu.ne) {
return apdu.rlen + 2;
else {
} else {
rdata_gr = apdu.rdata+apdu.ne;
rdata_bk = *(uint16_t *)rdata_gr;
rdata_bk = *(uint16_t *) rdata_gr;
rdata_gr[0] = 0x61;
if (apdu.rlen - apdu.ne >= 256)
if (apdu.rlen - apdu.ne >= 256) {
rdata_gr[1] = 0;
else
} else {
rdata_gr[1] = apdu.rlen - apdu.ne;
}
apdu.rlen -= apdu.ne;
}
return apdu.ne + 2;

39
src/apdu.h
View file

@ -28,36 +28,35 @@
typedef struct app {
const uint8_t *aid;
int (*process_apdu)();
struct app* (*select_aid)(struct app *, const uint8_t *, uint8_t);
struct app * (*select_aid)(struct app *, const uint8_t *, uint8_t);
int (*unload)();
} app_t;
extern int register_app(app_t * (*)(app_t *, const uint8_t *, uint8_t));
extern int register_app(app_t *(*)(app_t *, const uint8_t *, uint8_t));
typedef struct cmd
{
typedef struct cmd {
uint8_t ins;
int (*cmd_handler)();
} cmd_t;
#if defined(DEBUG_APDU) && DEBUG_APDU == 1
#define DEBUG_PAYLOAD(_p,_s) { \
printf("Payload %s (%d bytes):\r\n", #_p,(int)(_s));\
for (int _i = 0; _i < _s; _i += 16) {\
printf("%"PRIxPTR"h : ",(uintptr_t)(_i+_p));\
for (int _j = 0; _j < 16; _j++) {\
if (_j < _s-_i) printf("%02X ",(_p)[_i+_j]);\
else printf(" ");\
if (_j == 7) printf(" ");\
#define DEBUG_PAYLOAD(_p, _s) { \
printf("Payload %s (%d bytes):\r\n", #_p, (int) (_s)); \
for (int _i = 0; _i < _s; _i += 16) { \
printf("%" PRIxPTR "h : ", (uintptr_t) (_i+_p)); \
for (int _j = 0; _j < 16; _j++) { \
if (_j < _s-_i) printf("%02X ", (_p)[_i+_j]); \
else printf(" "); \
if (_j == 7) printf(" "); \
} printf(": "); \
printf("\r\n");\
printf("\r\n"); \
} printf("\r\n"); \
}
}
#define DEBUG_DATA(_p, _s) \
{ \
printf("Data %s (%d bytes):\r\n", #_p, (int)(_s)); \
char *tmp = (char *)calloc(1, 2 * _s + 1); \
printf("Data %s (%d bytes):\r\n", #_p, (int) (_s)); \
char *tmp = (char *) calloc(1, 2 * _s + 1); \
for (int _i = 0; _i < _s; _i++) \
{ \
sprintf(&tmp[2 * _i], "%02X", (_p)[_i]); \
@ -67,8 +66,8 @@ typedef struct cmd
}
#else
#define DEBUG_PAYLOAD(_p,_s)
#define DEBUG_DATA(_p,_s)
#define DEBUG_PAYLOAD(_p, _s)
#define DEBUG_DATA(_p, _s)
#endif
extern uint8_t num_apps;
@ -83,7 +82,7 @@ struct apdu {
uint16_t sw;
uint8_t *rdata;
uint16_t rlen;
} __attribute__ ((__packed__));
} __attribute__((__packed__));
#define CLA(a) a.header[0]
#define INS(a) a.header[1]
@ -95,7 +94,7 @@ struct apdu {
extern struct apdu apdu;
extern uint16_t set_res_sw (uint8_t sw1, uint8_t sw2);
extern uint16_t set_res_sw(uint8_t sw1, uint8_t sw2);
extern int process_apdu();
extern size_t apdu_process(uint8_t, const uint8_t *buffer, size_t buffer_size);
extern void apdu_finish();

62
src/asn1.c
View file

@ -17,27 +17,29 @@
#include "asn1.h"
size_t asn1_len_tag(uint16_t tag, size_t len) {
size_t asn1_len_tag(uint16_t tag, size_t len)
{
size_t ret = 1+format_tlv_len(len, NULL)+len;
if (tag > 0x00ff)
if (tag > 0x00ff) {
return ret+1;
}
return ret;
}
int format_tlv_len(size_t len, uint8_t *out) {
int format_tlv_len(size_t len, uint8_t *out)
{
if (len < 128) {
if (out)
if (out) {
*out = len;
}
return 1;
}
else if (len < 256) {
} else if (len < 256) {
if (out) {
*out++ = 0x81;
*out++ = len;
}
return 2;
}
else {
} else {
if (out) {
*out++ = 0x82;
*out++ = (len >> 8) & 0xff;
@ -48,13 +50,22 @@ int format_tlv_len(size_t len, uint8_t *out) {
return 0;
}
int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint16_t *tag, size_t *tag_len, uint8_t **data) {
if (!p)
int walk_tlv(const uint8_t *cdata,
size_t cdata_len,
uint8_t **p,
uint16_t *tag,
size_t *tag_len,
uint8_t **data)
{
if (!p) {
return 0;
if (!*p)
*p = (uint8_t *)cdata;
if (*p-cdata >= cdata_len)
}
if (!*p) {
*p = (uint8_t *) cdata;
}
if (*p-cdata >= cdata_len) {
return 0;
}
uint16_t tg = 0x0;
size_t tgl = 0;
tg = *(*p)++;
@ -66,31 +77,40 @@ int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint16_t *tag,
if (tgl == 0x82) {
tgl = *(*p)++ << 8;
tgl |= *(*p)++;
}
else if (tgl == 0x81) {
} else if (tgl == 0x81) {
tgl = *(*p)++;
}
if (tag)
if (tag) {
*tag = tg;
if (tag_len)
}
if (tag_len) {
*tag_len = tgl;
if (data)
}
if (data) {
*data = *p;
}
*p = *p+tgl;
return 1;
}
bool asn1_find_tag(const uint8_t *data, size_t data_len, uint16_t itag, size_t *tag_len, uint8_t **tag_data) {
bool asn1_find_tag(const uint8_t *data,
size_t data_len,
uint16_t itag,
size_t *tag_len,
uint8_t **tag_data)
{
uint16_t tag = 0x0;
uint8_t *p = NULL;
uint8_t *tdata = NULL;
size_t tlen = 0;
while (walk_tlv(data, data_len, &p, &tag, &tlen, &tdata)) {
if (itag == tag) {
if (tag_data != NULL)
if (tag_data != NULL) {
*tag_data = tdata;
if (tag_len != NULL)
}
if (tag_len != NULL) {
*tag_len = tlen;
}
return true;
}
}

13
src/asn1.h
View file

@ -26,9 +26,18 @@
#include <stdbool.h>
#endif
extern int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint16_t *tag, size_t *tag_len, uint8_t **data);
extern int walk_tlv(const uint8_t *cdata,
size_t cdata_len,
uint8_t **p,
uint16_t *tag,
size_t *tag_len,
uint8_t **data);
extern int format_tlv_len(size_t len, uint8_t *out);
extern bool asn1_find_tag(const uint8_t *data, size_t data_len, uint16_t itag, size_t *tag_len, uint8_t **tag_data);
extern bool asn1_find_tag(const uint8_t *data,
size_t data_len,
uint16_t itag,
size_t *tag_len,
uint8_t **tag_data);
extern size_t asn1_len_tag(uint16_t tag, size_t len);
#endif

130
src/crypto_utils.c
View file

@ -24,15 +24,18 @@
#include "crypto_utils.h"
#include "hsm.h"
void double_hash_pin(const uint8_t *pin, size_t len, uint8_t output[32]) {
void double_hash_pin(const uint8_t *pin, size_t len, uint8_t output[32])
{
uint8_t o1[32];
hash_multi(pin, len, o1);
for (int i = 0; i < sizeof(o1); i++)
for (int i = 0; i < sizeof(o1); i++) {
o1[i] ^= pin[i%len];
}
hash_multi(o1, sizeof(o1), output);
}
void hash_multi(const uint8_t *input, size_t len, uint8_t output[32]) {
void hash_multi(const uint8_t *input, size_t len, uint8_t output[32])
{
mbedtls_sha256_context ctx;
mbedtls_sha256_init(&ctx);
int iters = 256;
@ -41,74 +44,96 @@ void hash_multi(const uint8_t *input, size_t len, uint8_t output[32]) {
pico_get_unique_board_id(&unique_id);
#endif
mbedtls_sha256_starts (&ctx, 0);
mbedtls_sha256_starts(&ctx, 0);
#ifndef ENABLE_EMULATION
mbedtls_sha256_update (&ctx, unique_id.id, sizeof(unique_id.id));
mbedtls_sha256_update(&ctx, unique_id.id, sizeof(unique_id.id));
#endif
while (iters > len)
{
mbedtls_sha256_update (&ctx, input, len);
while (iters > len) {
mbedtls_sha256_update(&ctx, input, len);
iters -= len;
}
if (iters > 0) // remaining iterations
mbedtls_sha256_update (&ctx, input, iters);
mbedtls_sha256_finish (&ctx, output);
mbedtls_sha256_free (&ctx);
if (iters > 0) { // remaining iterations
mbedtls_sha256_update(&ctx, input, iters);
}
mbedtls_sha256_finish(&ctx, output);
mbedtls_sha256_free(&ctx);
}
void hash256(const uint8_t *input, size_t len, uint8_t output[32]) {
void hash256(const uint8_t *input, size_t len, uint8_t output[32])
{
mbedtls_sha256_context ctx;
mbedtls_sha256_init(&ctx);
mbedtls_sha256_starts (&ctx, 0);
mbedtls_sha256_update (&ctx, input, len);
mbedtls_sha256_starts(&ctx, 0);
mbedtls_sha256_update(&ctx, input, len);
mbedtls_sha256_finish (&ctx, output);
mbedtls_sha256_free (&ctx);
mbedtls_sha256_finish(&ctx, output);
mbedtls_sha256_free(&ctx);
}
void generic_hash(mbedtls_md_type_t md, const uint8_t *input, size_t len, uint8_t *output) {
void generic_hash(mbedtls_md_type_t md, const uint8_t *input, size_t len, uint8_t *output)
{
mbedtls_md(mbedtls_md_info_from_type(md), input, len, output);
}
int aes_encrypt(const uint8_t *key, const uint8_t *iv, int key_size, int mode, uint8_t *data, int len) {
int aes_encrypt(const uint8_t *key,
const uint8_t *iv,
int key_size,
int mode,
uint8_t *data,
int len)
{
mbedtls_aes_context aes;
mbedtls_aes_init(&aes);
uint8_t tmp_iv[IV_SIZE];
size_t iv_offset = 0;
memset(tmp_iv, 0, IV_SIZE);
if (iv)
if (iv) {
memcpy(tmp_iv, iv, IV_SIZE);
}
int r = mbedtls_aes_setkey_enc(&aes, key, key_size);
if (r != 0)
if (r != 0) {
return CCID_EXEC_ERROR;
if (mode == HSM_AES_MODE_CBC)
}
if (mode == HSM_AES_MODE_CBC) {
return mbedtls_aes_crypt_cbc(&aes, MBEDTLS_AES_ENCRYPT, len, tmp_iv, data, data);
}
return mbedtls_aes_crypt_cfb128(&aes, MBEDTLS_AES_ENCRYPT, len, &iv_offset, tmp_iv, data, data);
}
int aes_decrypt(const uint8_t *key, const uint8_t *iv, int key_size, int mode, uint8_t *data, int len) {
int aes_decrypt(const uint8_t *key,
const uint8_t *iv,
int key_size,
int mode,
uint8_t *data,
int len)
{
mbedtls_aes_context aes;
mbedtls_aes_init(&aes);
uint8_t tmp_iv[IV_SIZE];
size_t iv_offset = 0;
memset(tmp_iv, 0, IV_SIZE);
if (iv)
if (iv) {
memcpy(tmp_iv, iv, IV_SIZE);
}
int r = mbedtls_aes_setkey_dec(&aes, key, key_size);
if (r != 0)
if (r != 0) {
return CCID_EXEC_ERROR;
if (mode == HSM_AES_MODE_CBC)
}
if (mode == HSM_AES_MODE_CBC) {
return mbedtls_aes_crypt_cbc(&aes, MBEDTLS_AES_DECRYPT, len, tmp_iv, data, data);
}
r = mbedtls_aes_setkey_enc(&aes, key, key_size); //CFB requires set_enc instead set_dec
return mbedtls_aes_crypt_cfb128(&aes, MBEDTLS_AES_DECRYPT, len, &iv_offset, tmp_iv, data, data);
}
int aes_encrypt_cfb_256(const uint8_t *key, const uint8_t *iv, uint8_t *data, int len) {
int aes_encrypt_cfb_256(const uint8_t *key, const uint8_t *iv, uint8_t *data, int len)
{
return aes_encrypt(key, iv, 256, HSM_AES_MODE_CFB, data, len);
}
int aes_decrypt_cfb_256(const uint8_t *key, const uint8_t *iv, uint8_t *data, int len) {
int aes_decrypt_cfb_256(const uint8_t *key, const uint8_t *iv, uint8_t *data, int len)
{
return aes_decrypt(key, iv, 256, HSM_AES_MODE_CFB, data, len);
}
@ -122,26 +147,49 @@ struct ec_curve_mbed_id {
mbedtls_ecp_group_id id;
};
struct ec_curve_mbed_id ec_curves_mbed[] = {
{ { (unsigned char *) "\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF", 24}, MBEDTLS_ECP_DP_SECP192R1 },
{ { (unsigned char *) "\xFF\xFF\xFF\xFF\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF", 32}, MBEDTLS_ECP_DP_SECP256R1 },
{ { (unsigned char *) "\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xFF\xFF\x00\x00\x00\x00\x00\x00\x00\x00\xFF\xFF\xFF\xFF", 48}, MBEDTLS_ECP_DP_SECP384R1 },
{ { (unsigned char *) "\x01\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF", 66}, MBEDTLS_ECP_DP_SECP521R1 },
{ { (unsigned char *) "\xA9\xFB\x57\xDB\xA1\xEE\xA9\xBC\x3E\x66\x0A\x90\x9D\x83\x8D\x72\x6E\x3B\xF6\x23\xD5\x26\x20\x28\x20\x13\x48\x1D\x1F\x6E\x53\x77", 32}, MBEDTLS_ECP_DP_BP256R1 },
{ { (unsigned char *) "\x8C\xB9\x1E\x82\xA3\x38\x6D\x28\x0F\x5D\x6F\x7E\x50\xE6\x41\xDF\x15\x2F\x71\x09\xED\x54\x56\xB4\x12\xB1\xDA\x19\x7F\xB7\x11\x23\xAC\xD3\xA7\x29\x90\x1D\x1A\x71\x87\x47\x00\x13\x31\x07\xEC\x53", 48}, MBEDTLS_ECP_DP_BP384R1 },
{ { (unsigned char *) "\xAA\xDD\x9D\xB8\xDB\xE9\xC4\x8B\x3F\xD4\xE6\xAE\x33\xC9\xFC\x07\xCB\x30\x8D\xB3\xB3\xC9\xD2\x0E\xD6\x63\x9C\xCA\x70\x33\x08\x71\x7D\x4D\x9B\x00\x9B\xC6\x68\x42\xAE\xCD\xA1\x2A\xE6\xA3\x80\xE6\x28\x81\xFF\x2F\x2D\x82\xC6\x85\x28\xAA\x60\x56\x58\x3A\x48\xF3", 64}, MBEDTLS_ECP_DP_BP512R1 },
{ { (unsigned char *) "\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xEE\x37", 24}, MBEDTLS_ECP_DP_SECP192K1 },
{ { (unsigned char *) "\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xFC\x2F", 32}, MBEDTLS_ECP_DP_SECP256K1 },
{ { (unsigned char *) "\x7f\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xed", 32}, MBEDTLS_ECP_DP_CURVE25519 },
{ { (unsigned char *) "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff", 56}, MBEDTLS_ECP_DP_CURVE448 },
{ { (unsigned char *)
"\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF",
24 }, MBEDTLS_ECP_DP_SECP192R1 },
{ { (unsigned char *)
"\xFF\xFF\xFF\xFF\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF",
32 }, MBEDTLS_ECP_DP_SECP256R1 },
{ { (unsigned char *)
"\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xFF\xFF\x00\x00\x00\x00\x00\x00\x00\x00\xFF\xFF\xFF\xFF",
48 }, MBEDTLS_ECP_DP_SECP384R1 },
{ { (unsigned char *)
"\x01\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF",
66 }, MBEDTLS_ECP_DP_SECP521R1 },
{ { (unsigned char *)
"\xA9\xFB\x57\xDB\xA1\xEE\xA9\xBC\x3E\x66\x0A\x90\x9D\x83\x8D\x72\x6E\x3B\xF6\x23\xD5\x26\x20\x28\x20\x13\x48\x1D\x1F\x6E\x53\x77",
32 }, MBEDTLS_ECP_DP_BP256R1 },
{ { (unsigned char *)
"\x8C\xB9\x1E\x82\xA3\x38\x6D\x28\x0F\x5D\x6F\x7E\x50\xE6\x41\xDF\x15\x2F\x71\x09\xED\x54\x56\xB4\x12\xB1\xDA\x19\x7F\xB7\x11\x23\xAC\xD3\xA7\x29\x90\x1D\x1A\x71\x87\x47\x00\x13\x31\x07\xEC\x53",
48 }, MBEDTLS_ECP_DP_BP384R1 },
{ { (unsigned char *)
"\xAA\xDD\x9D\xB8\xDB\xE9\xC4\x8B\x3F\xD4\xE6\xAE\x33\xC9\xFC\x07\xCB\x30\x8D\xB3\xB3\xC9\xD2\x0E\xD6\x63\x9C\xCA\x70\x33\x08\x71\x7D\x4D\x9B\x00\x9B\xC6\x68\x42\xAE\xCD\xA1\x2A\xE6\xA3\x80\xE6\x28\x81\xFF\x2F\x2D\x82\xC6\x85\x28\xAA\x60\x56\x58\x3A\x48\xF3",
64 }, MBEDTLS_ECP_DP_BP512R1 },
{ { (unsigned char *)
"\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xEE\x37",
24 }, MBEDTLS_ECP_DP_SECP192K1 },
{ { (unsigned char *)
"\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE\xFF\xFF\xFC\x2F",
32 }, MBEDTLS_ECP_DP_SECP256K1 },
{ { (unsigned char *)
"\x7f\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xed",
32 }, MBEDTLS_ECP_DP_CURVE25519 },
{ { (unsigned char *)
"\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff",
56 }, MBEDTLS_ECP_DP_CURVE448 },
{ { NULL, 0 }, MBEDTLS_ECP_DP_NONE }
};
mbedtls_ecp_group_id ec_get_curve_from_prime(const uint8_t *prime, size_t prime_len) {
mbedtls_ecp_group_id ec_get_curve_from_prime(const uint8_t *prime, size_t prime_len)
{
for (struct ec_curve_mbed_id *ec = ec_curves_mbed; ec->id != MBEDTLS_ECP_DP_NONE; ec++) {
if (prime_len == ec->curve.len && memcmp(prime, ec->curve.value, prime_len) == 0) {
return ec->id;
}
}
return MBEDTLS_ECP_DP_NONE;
}
}

14
src/crypto_utils.h
View file

@ -41,8 +41,18 @@ extern void double_hash_pin(const uint8_t *pin, size_t len, uint8_t output[32]);
extern void hash_multi(const uint8_t *input, size_t len, uint8_t output[32]);
extern void hash256(const uint8_t *input, size_t len, uint8_t output[32]);
extern void generic_hash(mbedtls_md_type_t md, const uint8_t *input, size_t len, uint8_t *output);
extern int aes_encrypt(const uint8_t *key, const uint8_t *iv, int key_size, int mode, uint8_t *data, int len);
extern int aes_decrypt(const uint8_t *key, const uint8_t *iv, int key_size, int mode, uint8_t *data, int len);
extern int aes_encrypt(const uint8_t *key,
const uint8_t *iv,
int key_size,
int mode,
uint8_t *data,
int len);
extern int aes_decrypt(const uint8_t *key,
const uint8_t *iv,
int key_size,
int mode,
uint8_t *data,
int len);
extern int aes_encrypt_cfb_256(const uint8_t *key, const uint8_t *iv, uint8_t *data, int len);
extern int aes_decrypt_cfb_256(const uint8_t *key, const uint8_t *iv, uint8_t *data, int len);
extern mbedtls_ecp_group_id ec_get_curve_from_prime(const uint8_t *prime, size_t prime_len);

124
src/eac.c
View file

@ -32,16 +32,25 @@ static uint8_t sm_iv[16];
size_t sm_session_pin_len = 0;
uint8_t sm_session_pin[16];
bool is_secured_apdu() {
return (CLA(apdu) & 0xC);
bool is_secured_apdu()
{
return CLA(apdu) & 0xC;
}
void sm_derive_key(const uint8_t *input, size_t input_len, uint8_t counter, const uint8_t *nonce, size_t nonce_len, uint8_t *out) {
uint8_t *b = (uint8_t *)calloc(1, input_len+nonce_len+4);
if (input)
void sm_derive_key(const uint8_t *input,
size_t input_len,
uint8_t counter,
const uint8_t *nonce,
size_t nonce_len,
uint8_t *out)
{
uint8_t *b = (uint8_t *) calloc(1, input_len+nonce_len+4);
if (input) {
memcpy(b, input, input_len);
if (nonce)
}
if (nonce) {
memcpy(b+input_len, nonce, nonce_len);
}
b[input_len+nonce_len+3] = counter;
uint8_t digest[20];
generic_hash(MBEDTLS_MD_SHA1, b, input_len+nonce_len+4, digest);
@ -49,7 +58,8 @@ void sm_derive_key(const uint8_t *input, size_t input_len, uint8_t counter, cons
free(b);
}
void sm_derive_all_keys(const uint8_t *derived, size_t derived_len) {
void sm_derive_all_keys(const uint8_t *derived, size_t derived_len)
{
memcpy(nonce, random_bytes_get(8), 8);
sm_derive_key(derived, derived_len, 1, nonce, sizeof(nonce), sm_kenc);
sm_derive_key(derived, derived_len, 2, nonce, sizeof(nonce), sm_kmac);
@ -60,36 +70,50 @@ void sm_derive_all_keys(const uint8_t *derived, size_t derived_len) {
sm_session_pin_len = 0;
}
void sm_set_protocol(MSE_protocol proto) {
void sm_set_protocol(MSE_protocol proto)
{
sm_protocol = proto;
if (proto == MSE_AES)
if (proto == MSE_AES) {
sm_blocksize = 16;
else if (proto == MSE_3DES)
} else if (proto == MSE_3DES) {
sm_blocksize = 8;
}
}
MSE_protocol sm_get_protocol() {
MSE_protocol sm_get_protocol()
{
return sm_protocol;
}
uint8_t *sm_get_nonce() {
uint8_t *sm_get_nonce()
{
return nonce;
}
int sm_sign(uint8_t *in, size_t in_len, uint8_t *out) {
return mbedtls_cipher_cmac(mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_AES_128_ECB), sm_kmac, 128, in, in_len, out);
int sm_sign(uint8_t *in, size_t in_len, uint8_t *out)
{
return mbedtls_cipher_cmac(mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_AES_128_ECB),
sm_kmac,
128,
in,
in_len,
out);
}
int sm_unwrap() {
int sm_unwrap()
{
uint8_t sm_indicator = (CLA(apdu) >> 2) & 0x3;
if (sm_indicator == 0)
if (sm_indicator == 0) {
return CCID_OK;
}
int r = sm_verify();
if (r != CCID_OK)
if (r != CCID_OK) {
return r;
}
int le = sm_get_le();
if (le >= 0)
if (le >= 0) {
apdu.ne = le;
}
uint8_t *body = NULL;
size_t body_size = 0;
bool is87 = false;
@ -117,14 +141,16 @@ int sm_unwrap() {
aes_decrypt(sm_kenc, sm_iv, 128, HSM_AES_MODE_CBC, body, body_size);
memmove(apdu.data, body, body_size);
apdu.nc = sm_remove_padding(apdu.data, body_size);
DEBUG_PAYLOAD(apdu.data, (int)apdu.nc);
DEBUG_PAYLOAD(apdu.data, (int) apdu.nc);
return CCID_OK;
}
int sm_wrap() {
int sm_wrap()
{
uint8_t sm_indicator = (CLA(apdu) >> 2) & 0x3;
if (sm_indicator == 0)
if (sm_indicator == 0) {
return CCID_OK;
}
uint8_t input[1024];
size_t input_len = 0;
memset(input, 0, sizeof(input));
@ -133,8 +159,9 @@ int sm_wrap() {
mbedtls_mpi_add_int(&ssc, &sm_mSSC, 1);
mbedtls_mpi_copy(&sm_mSSC, &ssc);
int r = mbedtls_mpi_write_binary(&ssc, input, sm_blocksize);
if (r != 0)
if (r != 0) {
return CCID_EXEC_ERROR;
}
input_len += sm_blocksize;
mbedtls_mpi_free(&ssc);
if (res_APDU_size > 0) {
@ -151,14 +178,12 @@ int sm_wrap() {
memmove(res_APDU+2, res_APDU, res_APDU_size);
res_APDU[1] = res_APDU_size;
res_APDU_size += 2;
}
else if (res_APDU_size < 256) {
} else if (res_APDU_size < 256) {
memmove(res_APDU+3, res_APDU, res_APDU_size);
res_APDU[1] = 0x81;
res_APDU[2] = res_APDU_size;
res_APDU_size += 3;
}
else {
} else {
memmove(res_APDU+4, res_APDU, res_APDU_size);
res_APDU[1] = 0x82;
res_APDU[2] = res_APDU_size >> 8;
@ -179,27 +204,31 @@ int sm_wrap() {
res_APDU[res_APDU_size++] = 0x8E;
res_APDU[res_APDU_size++] = 8;
res_APDU_size += 8;
if (apdu.ne > 0)
if (apdu.ne > 0) {
apdu.ne = res_APDU_size;
}
return CCID_OK;
}
int sm_get_le() {
int sm_get_le()
{
uint16_t tag = 0x0;
uint8_t *tag_data = NULL, *p = NULL;
size_t tag_len = 0;
while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) {
if (tag == 0x97) {
uint32_t le = 0;
for (int t = 1; t <= tag_len; t++)
for (int t = 1; t <= tag_len; t++) {
le |= (*tag_data++) << (tag_len-t);
}
return le;
}
}
return -1;
}
void sm_update_iv() {
void sm_update_iv()
{
uint8_t tmp_iv[16], sc_counter[16];
memset(tmp_iv, 0, sizeof(tmp_iv)); //IV is always 0 for encryption of IV based on counter
mbedtls_mpi_write_binary(&sm_mSSC, sc_counter, sizeof(sc_counter));
@ -207,16 +236,19 @@ void sm_update_iv() {
memcpy(sm_iv, sc_counter, sizeof(sc_counter));
}
int sm_verify() {
int sm_verify()
{
uint8_t input[1024];
memset(input, 0, sizeof(input));
int input_len = 0, r = 0;
bool add_header = (CLA(apdu) & 0xC) == 0xC;
int data_len = (int)(apdu.nc/sm_blocksize)*sm_blocksize;
if (data_len % sm_blocksize)
int data_len = (int) (apdu.nc/sm_blocksize)*sm_blocksize;
if (data_len % sm_blocksize) {
data_len += sm_blocksize;
if (data_len+(add_header ? sm_blocksize : 0) > 1024)
}
if (data_len+(add_header ? sm_blocksize : 0) > 1024) {
return CCID_WRONG_LENGTH;
}
mbedtls_mpi ssc;
mbedtls_mpi_init(&ssc);
mbedtls_mpi_add_int(&ssc, &sm_mSSC, 1);
@ -224,8 +256,9 @@ int sm_verify() {
r = mbedtls_mpi_write_binary(&ssc, input, sm_blocksize);
input_len += sm_blocksize;
mbedtls_mpi_free(&ssc);
if (r != 0)
if (r != 0) {
return CCID_EXEC_ERROR;
}
if (add_header) {
input[input_len++] = CLA(apdu);
input[input_len++] = INS(apdu);
@ -254,25 +287,32 @@ int sm_verify() {
mac_len = tag_len;
}
}
if (!mac)
if (!mac) {
return CCID_WRONG_DATA;
}
if (some_added) {
input[input_len++] = 0x80;
input_len += (sm_blocksize - (input_len%sm_blocksize));
}
uint8_t signature[16];
r = sm_sign(input, input_len, signature);
if (r != 0)
if (r != 0) {
return CCID_EXEC_ERROR;
if (memcmp(signature, mac, mac_len) == 0)
}
if (memcmp(signature, mac, mac_len) == 0) {
return CCID_OK;
}
return CCID_VERIFICATION_FAILED;
}
int sm_remove_padding(const uint8_t *data, size_t data_len) {
int sm_remove_padding(const uint8_t *data, size_t data_len)
{
int i = data_len-1;
for (; i >= 0 && data[i] == 0; i--);
if (i < 0 || data[i] != 0x80)
for (; i >= 0 && data[i] == 0; i--) {
;
}
if (i < 0 || data[i] != 0x80) {
return -1;
}
return i;
}
}

2
src/eac.h
View file

@ -28,7 +28,7 @@ typedef enum MSE_protocol {
MSE_AES = 0,
MSE_3DES,
MSE_NONE
}MSE_protocol;
} MSE_protocol;
extern void sm_derive_all_keys(const uint8_t *input, size_t input_len);
extern void sm_set_protocol(MSE_protocol proto);

256
src/fs/file.c
View file

@ -27,10 +27,13 @@ extern const uintptr_t start_data_pool;
extern const uintptr_t end_rom_pool;
extern const uintptr_t start_rom_pool;
extern int flash_write_data_to_file(file_t *file, const uint8_t *data, uint16_t len);
extern int flash_write_data_to_file_offset(file_t *file, const uint8_t *data, uint16_t len, uint16_t offset);
extern int flash_program_halfword (uintptr_t addr, uint16_t data);
extern int flash_program_word (uintptr_t addr, uint32_t data);
extern int flash_program_uintptr (uintptr_t addr, uintptr_t data);
extern int flash_write_data_to_file_offset(file_t *file,
const uint8_t *data,
uint16_t len,
uint16_t offset);
extern int flash_program_halfword(uintptr_t addr, uint16_t data);
extern int flash_program_word(uintptr_t addr, uint32_t data);
extern int flash_program_uintptr(uintptr_t addr, uintptr_t data);
extern int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len);
extern uintptr_t flash_read_uintptr(uintptr_t addr);
extern uint16_t flash_read_uint16(uintptr_t addr);
@ -39,7 +42,8 @@ extern uint8_t *flash_read(uintptr_t addr);
extern void low_flash_available();
//puts FCI in the RAPDU
void process_fci(const file_t *pe, int fmd) {
void process_fci(const file_t *pe, int fmd)
{
res_APDU_size = 0;
if (fmd) {
res_APDU[res_APDU_size++] = 0x6f;
@ -56,14 +60,12 @@ void process_fci(const file_t *pe, int fmd) {
uint16_t len = ((int (*)(const file_t *, int))(pe->data))(pe, 0);
res_APDU[res_APDU_size++] = (len >> 8) & 0xff;
res_APDU[res_APDU_size++] = len & 0xff;
}
else {
} else {
uint16_t v = file_get_size(pe);
res_APDU[res_APDU_size++] = v >> 8;
res_APDU[res_APDU_size++] = v & 0xff;
}
}
else {
} else {
memset(res_APDU+res_APDU_size, 0, 2);
res_APDU_size += 2;
}
@ -71,12 +73,13 @@ void process_fci(const file_t *pe, int fmd) {
res_APDU[res_APDU_size++] = 0x82;
res_APDU[res_APDU_size++] = 1;
res_APDU[res_APDU_size] = 0;
if (pe->type == FILE_TYPE_INTERNAL_EF)
if (pe->type == FILE_TYPE_INTERNAL_EF) {
res_APDU[res_APDU_size++] |= 0x08;
else if (pe->type == FILE_TYPE_WORKING_EF)
} else if (pe->type == FILE_TYPE_WORKING_EF) {
res_APDU[res_APDU_size++] |= pe->ef_structure & 0x7;
else if (pe->type == FILE_TYPE_DF)
} else if (pe->type == FILE_TYPE_DF) {
res_APDU[res_APDU_size++] |= 0x38;
}
res_APDU[res_APDU_size++] = 0x83;
res_APDU[res_APDU_size++] = 2;
@ -84,9 +87,9 @@ void process_fci(const file_t *pe, int fmd) {
res_APDU_size += 2;
if (pe->name) {
res_APDU[res_APDU_size++] = 0x84;
res_APDU[res_APDU_size++] = MIN(pe->name[0],16);
memcpy(res_APDU+res_APDU_size, pe->name+2, MIN(pe->name[0],16));
res_APDU_size += MIN(pe->name[0],16);
res_APDU[res_APDU_size++] = MIN(pe->name[0], 16);
memcpy(res_APDU+res_APDU_size, pe->name+2, MIN(pe->name[0], 16));
res_APDU_size += MIN(pe->name[0], 16);
}
memcpy(res_APDU+res_APDU_size, "\x8A\x01\x05", 3); //life-cycle (5 -> activated)
res_APDU_size += 3;
@ -96,12 +99,13 @@ void process_fci(const file_t *pe, int fmd) {
res_APDU[res_APDU_size++] = 0xA5;
res_APDU[res_APDU_size++] = 0x81;
res_APDU[res_APDU_size++] = meta_size;
memcpy(res_APDU+res_APDU_size,meta_data,meta_size);
memcpy(res_APDU+res_APDU_size, meta_data, meta_size);
res_APDU_size += meta_size;
}
res_APDU[1] = res_APDU_size-2;
if (fmd)
if (fmd) {
res_APDU[3] = res_APDU_size-4;
}
}
#define MAX_DYNAMIC_FILES 128
@ -110,19 +114,24 @@ file_t dynamic_file[MAX_DYNAMIC_FILES];
bool card_terminated = false;
bool is_parent(const file_t *child, const file_t *parent) {
if (child == parent)
bool is_parent(const file_t *child, const file_t *parent)
{
if (child == parent) {
return true;
if (child == MF)
}
if (child == MF) {
return false;
}
return is_parent(&file_entries[child->parent], parent);
}
file_t *get_parent(file_t *f) {
file_t *get_parent(file_t *f)
{
return &file_entries[f->parent];
}
file_t *search_by_name(uint8_t *name, uint16_t namelen) {
file_t *search_by_name(uint8_t *name, uint16_t namelen)
{
for (file_t *p = file_entries; p != file_last; p++) {
if (p->name && *p->name == apdu.nc && memcmp(p->name+1, name, namelen) == 0) {
return p;
@ -131,29 +140,37 @@ file_t *search_by_name(uint8_t *name, uint16_t namelen) {
return NULL;
}
file_t *search_by_fid(const uint16_t fid, const file_t *parent, const uint8_t sp) {
file_t *search_by_fid(const uint16_t fid, const file_t *parent, const uint8_t sp)
{
for (file_t *p = file_entries; p != file_last; p++) {
if (p->fid != 0x0000 && p->fid == fid) {
if (!parent || (parent && is_parent(p, parent))) {
if (!sp || sp == SPECIFY_ANY || (((sp & SPECIFY_EF) && (p->type & FILE_TYPE_INTERNAL_EF)) || ((sp & SPECIFY_DF) && p->type == FILE_TYPE_DF)))
if (!sp || sp == SPECIFY_ANY ||
(((sp & SPECIFY_EF) && (p->type & FILE_TYPE_INTERNAL_EF)) ||
((sp & SPECIFY_DF) && p->type == FILE_TYPE_DF))) {
return p;
}
}
}
}
return NULL;
}
uint8_t make_path_buf(const file_t *pe, uint8_t *buf, uint8_t buflen, const file_t *top) {
if (!buflen)
uint8_t make_path_buf(const file_t *pe, uint8_t *buf, uint8_t buflen, const file_t *top)
{
if (!buflen) {
return 0;
if (pe == top) //MF or relative DF
}
if (pe == top) { //MF or relative DF
return 0;
}
put_uint16_t(pe->fid, buf);
return make_path_buf(&file_entries[pe->parent], buf+2, buflen-2, top)+2;
}
uint8_t make_path(const file_t *pe, const file_t *top, uint8_t *path) {
uint8_t make_path(const file_t *pe, const file_t *top, uint8_t *path)
{
uint8_t buf[MAX_DEPTH*2], *p = path;
put_uint16_t(pe->fid, buf);
uint8_t depth = make_path_buf(&file_entries[pe->parent], buf+2, sizeof(buf)-2, top)+2;
@ -164,15 +181,17 @@ uint8_t make_path(const file_t *pe, const file_t *top, uint8_t *path) {
return depth;
}
file_t *search_by_path(const uint8_t *pe_path, uint8_t pathlen, const file_t *parent) {
file_t *search_by_path(const uint8_t *pe_path, uint8_t pathlen, const file_t *parent)
{
uint8_t path[MAX_DEPTH*2];
if (pathlen > sizeof(path)) {
return NULL;
}
for (file_t *p = file_entries; p != file_last; p++) {
uint8_t depth = make_path(p, parent, path);
if (pathlen == depth && memcmp(path, pe_path, depth) == 0)
if (pathlen == depth && memcmp(path, pe_path, depth) == 0) {
return p;
}
}
return NULL;
}
@ -182,65 +201,73 @@ file_t *currentDF = NULL;
const file_t *selected_applet = NULL;
bool isUserAuthenticated = false;
bool authenticate_action(const file_t *ef, uint8_t op) {
bool authenticate_action(const file_t *ef, uint8_t op)
{
uint8_t acl = ef->acl[op];
if (acl == 0x0)
if (acl == 0x0) {
return true;
else if (acl == 0xff)
} else if (acl == 0xff) {
return false;
else if (acl == 0x90 || (acl & 0x9F) == 0x10) {
// PIN required.
} else if (acl == 0x90 || (acl & 0x9F) == 0x10) {
// PIN required.
if (isUserAuthenticated) {
return true;
}
else {
} else {
return false;
}
}
return false;
}
void initialize_flash(bool hard) {
void initialize_flash(bool hard)
{
if (hard) {
const uint8_t empty[8] = { 0 };
flash_program_block(end_data_pool, empty, sizeof(empty));
low_flash_available();
}
for (file_t *f = file_entries; f != file_last; f++) {
if ((f->type & FILE_DATA_FLASH) == FILE_DATA_FLASH)
if ((f->type & FILE_DATA_FLASH) == FILE_DATA_FLASH) {
f->data = NULL;
}
}
dynamic_files = 0;
}
void scan_region(bool persistent) {
void scan_region(bool persistent)
{
uintptr_t endp = end_data_pool, startp = start_data_pool;
if (persistent) {
endp = end_rom_pool;
startp = start_rom_pool;
}
for (uintptr_t base = flash_read_uintptr(endp); base >= startp; base = flash_read_uintptr(base)) {
if (base == 0x0) //all is empty
for (uintptr_t base = flash_read_uintptr(endp); base >= startp;
base = flash_read_uintptr(base)) {
if (base == 0x0) { //all is empty
break;
}
uint16_t fid = flash_read_uint16(base+sizeof(uintptr_t)+sizeof(uintptr_t));
printf("[%x] scan fid %x, len %d\r\n",(unsigned int)base,fid,flash_read_uint16(base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t)));
file_t *file = (file_t *)search_by_fid(fid, NULL, SPECIFY_EF);
printf("[%x] scan fid %x, len %d\r\n", (unsigned int) base, fid,
flash_read_uint16(base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t)));
file_t *file = (file_t *) search_by_fid(fid, NULL, SPECIFY_EF);
if (!file) {
file = file_new(fid);
}
if (file)
file->data = (uint8_t *)(base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t));
if (file) {
file->data = (uint8_t *) (base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t));
}
if (flash_read_uintptr(base) == 0x0) {
break;
}
}
}
void wait_flash_finish();
void scan_flash() {
void scan_flash()
{
initialize_flash(false); //soft initialization
if (*(uintptr_t *)flash_read(end_rom_pool) == 0xffffffff && *(uintptr_t *)flash_read(end_rom_pool+sizeof(uintptr_t)) == 0xffffffff)
{
if (*(uintptr_t *) flash_read(end_rom_pool) == 0xffffffff &&
*(uintptr_t *) flash_read(end_rom_pool+sizeof(uintptr_t)) == 0xffffffff) {
printf("First initialization (or corrupted!)\r\n");
uint8_t empty[sizeof(uintptr_t)*2+sizeof(uint32_t)];
memset(empty, 0, sizeof(empty));
@ -254,43 +281,55 @@ void scan_flash() {
scan_region(false);
}
uint8_t *file_read(const uint8_t *addr) {
return flash_read((uintptr_t)addr);
uint8_t *file_read(const uint8_t *addr)
{
return flash_read((uintptr_t) addr);
}
uint16_t file_read_uint16(const uint8_t *addr) {
return flash_read_uint16((uintptr_t)addr);
uint16_t file_read_uint16(const uint8_t *addr)
{
return flash_read_uint16((uintptr_t) addr);
}
uint8_t file_read_uint8(const uint8_t *addr) {
return flash_read_uint8((uintptr_t)addr);
uint8_t file_read_uint8(const uint8_t *addr)
{
return flash_read_uint8((uintptr_t) addr);
}
uint8_t *file_get_data(const file_t *tf) {
if (!tf || !tf->data)
uint8_t *file_get_data(const file_t *tf)
{
if (!tf || !tf->data) {
return NULL;
}
return file_read(tf->data+sizeof(uint16_t));
}
uint16_t file_get_size(const file_t *tf) {
if (!tf || !tf->data)
uint16_t file_get_size(const file_t *tf)
{
if (!tf || !tf->data) {
return 0;
}
return file_read_uint16(tf->data);
}
file_t *search_dynamic_file(uint16_t fid) {
file_t *search_dynamic_file(uint16_t fid)
{
for (int i = 0; i < dynamic_files; i++) {
if (dynamic_file[i].fid == fid)
if (dynamic_file[i].fid == fid) {
return &dynamic_file[i];
}
}
return NULL;
}
int delete_dynamic_file(file_t *f) {
if (f == NULL)
int delete_dynamic_file(file_t *f)
{
if (f == NULL) {
return CCID_ERR_FILE_NOT_FOUND;
}
for (int i = 0; i < dynamic_files; i++) {
if (dynamic_file[i].fid == f->fid) {
for (int j = i+1; j < dynamic_files; j++)
for (int j = i+1; j < dynamic_files; j++) {
memcpy(&dynamic_file[j-1], &dynamic_file[j], sizeof(file_t));
}
dynamic_files--;
return CCID_OK;
}
@ -298,12 +337,15 @@ int delete_dynamic_file(file_t *f) {
return CCID_ERR_FILE_NOT_FOUND;
}
file_t *file_new(uint16_t fid) {
file_t *file_new(uint16_t fid)
{
file_t *f;
if ((f = search_dynamic_file(fid)) || (f = search_by_fid(fid, NULL, SPECIFY_EF)))
if ((f = search_dynamic_file(fid)) || (f = search_by_fid(fid, NULL, SPECIFY_EF))) {
return f;
if (dynamic_files == MAX_DYNAMIC_FILES)
}
if (dynamic_files == MAX_DYNAMIC_FILES) {
return NULL;
}
f = &dynamic_file[dynamic_files];
dynamic_files++;
file_t file = {
@ -313,50 +355,57 @@ file_t *file_new(uint16_t fid) {
.type = FILE_TYPE_WORKING_EF,
.ef_structure = FILE_EF_TRANSPARENT,
.data = NULL,
.acl = {0}
.acl = { 0 }
};
memcpy(f, &file, sizeof(file_t));
//memset((uint8_t *)f->acl, 0x90, sizeof(f->acl));
return f;
}
int meta_find(uint16_t fid, uint8_t **out) {
int meta_find(uint16_t fid, uint8_t **out)
{
file_t *ef = search_by_fid(EF_META, NULL, SPECIFY_EF);
if (!ef)
if (!ef) {
return CCID_ERR_FILE_NOT_FOUND;
}
uint16_t tag = 0x0;
uint8_t *tag_data = NULL, *p = NULL, *data = file_get_data(ef);
size_t tag_len = 0, data_len = file_get_size(ef);
while (walk_tlv(data, data_len, &p, &tag, &tag_len, &tag_data)) {
if (tag_len < 2)
if (tag_len < 2) {
continue;
}
uint16_t cfid = (tag_data[0] << 8 | tag_data[1]);
if (cfid == fid) {
if (out)
if (out) {
*out = tag_data+2;
}
return tag_len-2;
}
}
return 0;
}
int meta_delete(uint16_t fid) {
int meta_delete(uint16_t fid)
{
file_t *ef = search_by_fid(EF_META, NULL, SPECIFY_EF);
if (!ef)
if (!ef) {
return CCID_ERR_FILE_NOT_FOUND;
}
uint16_t tag = 0x0;
uint8_t *tag_data = NULL, *p = NULL, *data = file_get_data(ef);
size_t tag_len = 0, data_len = file_get_size(ef);
uint8_t *fdata = NULL;
while (walk_tlv(data, data_len, &p, &tag, &tag_len, &tag_data)) {
uint8_t *tpos = p-tag_len-format_tlv_len(tag_len, NULL)-1;
if (tag_len < 2)
if (tag_len < 2) {
continue;
}
uint16_t cfid = (tag_data[0] << 8 | tag_data[1]);
if (cfid == fid) {
size_t new_len = data_len-1-tag_len-format_tlv_len(tag_len, NULL);
if (new_len == 0)
if (new_len == 0) {
flash_clear_file(ef);
else {
fdata = (uint8_t *)calloc(1, new_len);
} else {
fdata = (uint8_t *) calloc(1, new_len);
if (tpos > data) {
memcpy(fdata, data, tpos-data);
}
@ -365,8 +414,9 @@ int meta_delete(uint16_t fid) {
}
int r = flash_write_data_to_file(ef, fdata, new_len);
free(fdata);
if (r != CCID_OK)
if (r != CCID_OK) {
return CCID_EXEC_ERROR;
}
}
low_flash_available();
break;
@ -374,41 +424,44 @@ int meta_delete(uint16_t fid) {
}
return CCID_OK;
}
int meta_add(uint16_t fid, const uint8_t *data, uint16_t len) {
int meta_add(uint16_t fid, const uint8_t *data, uint16_t len)
{
int r;
file_t *ef = search_by_fid(EF_META, NULL, SPECIFY_EF);
if (!ef)
if (!ef) {
return CCID_ERR_FILE_NOT_FOUND;
}
uint16_t ef_size = file_get_size(ef);
uint8_t *fdata = (uint8_t *)calloc(1, ef_size);
uint8_t *fdata = (uint8_t *) calloc(1, ef_size);
memcpy(fdata, file_get_data(ef), ef_size);
uint16_t tag = 0x0;
uint8_t *tag_data = NULL, *p = NULL;
size_t tag_len = 0;
while (walk_tlv(fdata, ef_size, &p, &tag, &tag_len, &tag_data)) {
if (tag_len < 2)
if (tag_len < 2) {
continue;
}
uint16_t cfid = (tag_data[0] << 8 | tag_data[1]);
if (cfid == fid) {
if (tag_len-2 == len) { //an update
memcpy(p-tag_len+2, data, len);
r = flash_write_data_to_file(ef, fdata, ef_size);
free(fdata);
if (r != CCID_OK)
if (r != CCID_OK) {
return CCID_EXEC_ERROR;
}
return CCID_OK;
}
else { //needs reallocation
} else { //needs reallocation
uint8_t *tpos = p-asn1_len_tag(tag, tag_len);
memmove(tpos, p, fdata+ef_size-p);
tpos += fdata+ef_size-p;
volatile uintptr_t meta_offset = tpos-fdata;
ef_size += len - (tag_len-2);
if (len > tag_len-2) {
uint8_t *fdata_new = (uint8_t *)realloc(fdata, ef_size);
if (fdata_new != NULL)
uint8_t *fdata_new = (uint8_t *) realloc(fdata, ef_size);
if (fdata_new != NULL) {
fdata = fdata_new;
else {
} else {
free(fdata);
return CCID_ERR_MEMORY_FATAL;
}
@ -421,13 +474,14 @@ int meta_add(uint16_t fid, const uint8_t *data, uint16_t len) {
memcpy(f, data, len);
r = flash_write_data_to_file(ef, fdata, ef_size);
free(fdata);
if (r != CCID_OK)
if (r != CCID_OK) {
return CCID_EXEC_ERROR;
}
return CCID_OK;
}
}
}
fdata = (uint8_t *)realloc(fdata, ef_size+asn1_len_tag(fid & 0x1f, len+2));
fdata = (uint8_t *) realloc(fdata, ef_size+asn1_len_tag(fid & 0x1f, len+2));
uint8_t *f = fdata+ef_size;
*f++ = fid & 0x1f;
f += format_tlv_len(len+2, f);
@ -436,23 +490,29 @@ int meta_add(uint16_t fid, const uint8_t *data, uint16_t len) {
memcpy(f, data, len);
r = flash_write_data_to_file(ef, fdata, ef_size+asn1_len_tag(fid & 0x1f, len+2));
free(fdata);
if (r != CCID_OK)
if (r != CCID_OK) {
return CCID_EXEC_ERROR;
}
return CCID_OK;
}
bool file_has_data(file_t *f) {
return (f != NULL && f->data != NULL && file_get_size(f) > 0);
bool file_has_data(file_t *f)
{
return f != NULL && f->data != NULL && file_get_size(f) > 0;
}
int delete_file(file_t *ef) {
if (ef == NULL)
int delete_file(file_t *ef)
{
if (ef == NULL) {
return CCID_OK;
}
meta_delete(ef->fid);
if (flash_clear_file(ef) != CCID_OK)
if (flash_clear_file(ef) != CCID_OK) {
return CCID_EXEC_ERROR;
if (delete_dynamic_file(ef) != CCID_OK)
}
if (delete_dynamic_file(ef) != CCID_OK) {
return CCID_EXEC_ERROR;
}
low_flash_available();
return CCID_OK;
}

6
src/fs/file.h
View file

@ -29,7 +29,7 @@
#define FILE_TYPE_UNKNOWN 0x00
#define FILE_TYPE_DF 0x04
#define FILE_TYPE_INTERNAL_EF 0x03
#define FILE_TYPE_INTERNAL_EF 0x03
#define FILE_TYPE_WORKING_EF 0x01
#define FILE_TYPE_BSO 0x10
#define FILE_PERSISTENT 0x20
@ -68,8 +68,7 @@
#define MAX_DEPTH 4
typedef struct file
{
typedef struct file {
const uint16_t fid;
const uint8_t parent; //entry number in the whole table!!
const uint8_t *name;
@ -126,4 +125,3 @@ extern int meta_add(uint16_t fid, const uint8_t *data, uint16_t len);
extern int delete_file(file_t *ef);
#endif

86
src/fs/flash.c
View file

@ -45,12 +45,14 @@
//To avoid possible future allocations, data region starts at the end of flash and goes upwards to the center region
const uintptr_t start_data_pool = (XIP_BASE + FLASH_TARGET_OFFSET);
const uintptr_t end_data_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE-FLASH_PERMANENT_REGION-FLASH_DATA_HEADER_SIZE-4; //This is a fixed value. DO NOT CHANGE
const uintptr_t end_data_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE-
FLASH_PERMANENT_REGION-FLASH_DATA_HEADER_SIZE-4; //This is a fixed value. DO NOT CHANGE
const uintptr_t end_rom_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE-4; //This is a fixed value. DO NOT CHANGE
const uintptr_t start_rom_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE-FLASH_PERMANENT_REGION; //This is a fixed value. DO NOT CHANGE
const uintptr_t start_rom_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE-
FLASH_PERMANENT_REGION; //This is a fixed value. DO NOT CHANGE
extern int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len);
extern int flash_program_halfword (uintptr_t addr, uint16_t data);
extern int flash_program_halfword(uintptr_t addr, uint16_t data);
extern int flash_program_uintptr(uintptr_t, uintptr_t);
extern uintptr_t flash_read_uintptr(uintptr_t addr);
extern uint16_t flash_read_uint16(uintptr_t addr);
@ -58,9 +60,11 @@ extern uint8_t *flash_read(uintptr_t addr);
extern void low_flash_available();
uintptr_t allocate_free_addr(uint16_t size, bool persistent) {
if (size > FLASH_SECTOR_SIZE)
uintptr_t allocate_free_addr(uint16_t size, bool persistent)
{
if (size > FLASH_SECTOR_SIZE) {
return 0x0; //ERROR
}
size_t real_size = size+sizeof(uint16_t)+sizeof(uintptr_t)+sizeof(uint16_t)+sizeof(uintptr_t); //len+len size+next address+fid+prev_addr size
uintptr_t next_base = 0x0, endp = end_data_pool, startp = start_data_pool;
if (persistent) {
@ -75,14 +79,12 @@ uintptr_t allocate_free_addr(uint16_t size, bool persistent) {
//printf("fid %x\r\n",flash_read_uint16(next_base+sizeof(uintptr_t)));
if (next_base == 0x0) { //we are at the end
//now we check if we fit in the current sector
if (addr_alg <= potential_addr) //it fits in the current sector
{
if (addr_alg <= potential_addr) { //it fits in the current sector
flash_program_uintptr(potential_addr, 0x0);
flash_program_uintptr(potential_addr+sizeof(uintptr_t), base);
flash_program_uintptr(base, potential_addr);
return potential_addr;
}
else if (addr_alg-FLASH_SECTOR_SIZE >= startp) { //check whether it fits in the next sector, so we take addr_aligned as the base
} else if (addr_alg-FLASH_SECTOR_SIZE >= startp) { //check whether it fits in the next sector, so we take addr_aligned as the base
potential_addr = addr_alg-real_size;
flash_program_uintptr(potential_addr, 0x0);
flash_program_uintptr(potential_addr+sizeof(uintptr_t), base);
@ -92,7 +94,13 @@ uintptr_t allocate_free_addr(uint16_t size, bool persistent) {
return 0x0;
}
//we check if |base-(next_addr+size_next_addr)| > |base-potential_addr| only if fid != 1xxx (not size blocked)
else if (addr_alg <= potential_addr && base-(next_base+flash_read_uint16(next_base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t))+2*sizeof(uint16_t)+2*sizeof(uintptr_t)) > base-potential_addr && (flash_read_uint16(next_base+2*sizeof(uintptr_t)) & 0x1000) != 0x1000) {
else if (addr_alg <= potential_addr &&
base-
(next_base+
flash_read_uint16(next_base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t))+
2*
sizeof(uint16_t)+2*sizeof(uintptr_t)) > base-potential_addr &&
(flash_read_uint16(next_base+2*sizeof(uintptr_t)) & 0x1000) != 0x1000) {
flash_program_uintptr(potential_addr, next_base);
flash_program_uintptr(next_base+sizeof(uintptr_t), potential_addr);
flash_program_uintptr(potential_addr+sizeof(uintptr_t), base);
@ -103,17 +111,21 @@ uintptr_t allocate_free_addr(uint16_t size, bool persistent) {
return 0x0; //probably never reached
}
int flash_clear_file(file_t *file) {
if (file == NULL)
int flash_clear_file(file_t *file)
{
if (file == NULL) {
return CCID_OK;
uintptr_t base_addr = (uintptr_t)(file->data-sizeof(uintptr_t)-sizeof(uint16_t)-sizeof(uintptr_t));
}
uintptr_t base_addr =
(uintptr_t) (file->data-sizeof(uintptr_t)-sizeof(uint16_t)-sizeof(uintptr_t));
uintptr_t prev_addr = flash_read_uintptr(base_addr+sizeof(uintptr_t));
uintptr_t next_addr = flash_read_uintptr(base_addr);
//printf("nc %lx->%lx %lx->%lx\r\n",prev_addr,flash_read_uintptr(prev_addr),base_addr,next_addr);
flash_program_uintptr(prev_addr, next_addr);
flash_program_halfword((uintptr_t)file->data, 0);
if (next_addr > 0)
flash_program_halfword((uintptr_t) file->data, 0);
if (next_addr > 0) {
flash_program_uintptr(next_addr+sizeof(uintptr_t), prev_addr);
}
flash_program_uintptr(base_addr, 0);
flash_program_uintptr(base_addr+sizeof(uintptr_t), 0);
file->data = NULL;
@ -121,25 +133,29 @@ int flash_clear_file(file_t *file) {
return CCID_OK;
}
int flash_write_data_to_file_offset(file_t *file, const uint8_t *data, uint16_t len, uint16_t offset) {
if (!file)
int flash_write_data_to_file_offset(file_t *file, const uint8_t *data, uint16_t len,
uint16_t offset)
{
if (!file) {
return CCID_ERR_NULL_PARAM;
uint16_t size_file_flash = file->data ? flash_read_uint16((uintptr_t)file->data) : 0;
}
uint16_t size_file_flash = file->data ? flash_read_uint16((uintptr_t) file->data) : 0;
uint8_t *old_data = NULL;
if (offset+len > FLASH_SECTOR_SIZE || offset > size_file_flash)
if (offset+len > FLASH_SECTOR_SIZE || offset > size_file_flash) {
return CCID_ERR_NO_MEMORY;
}
if (file->data) { //already in flash
if (offset+len <= size_file_flash) { //it fits, no need to move it
flash_program_halfword((uintptr_t)file->data, offset+len);
if (data)
flash_program_block((uintptr_t)file->data+sizeof(uint16_t)+offset, data, len);
flash_program_halfword((uintptr_t) file->data, offset+len);
if (data) {
flash_program_block((uintptr_t) file->data+sizeof(uint16_t)+offset, data, len);
}
return CCID_OK;
}
else { //we clear the old file
} else { //we clear the old file
flash_clear_file(file);
if (offset > 0) {
old_data = (uint8_t *)calloc(1, offset+len);
memcpy(old_data, flash_read((uintptr_t)(file->data+sizeof(uint16_t))), offset);
old_data = (uint8_t *) calloc(1, offset+len);
memcpy(old_data, flash_read((uintptr_t) (file->data+sizeof(uint16_t))), offset);
memcpy(old_data+offset, data, len);
len = offset+len;
data = old_data;
@ -149,17 +165,21 @@ int flash_write_data_to_file_offset(file_t *file, const uint8_t *data, uint16_t
uintptr_t new_addr = allocate_free_addr(len, (file->type & FILE_PERSISTENT) == FILE_PERSISTENT);
//printf("na %x\r\n",new_addr);
if (new_addr == 0x0)
if (new_addr == 0x0) {
return CCID_ERR_NO_MEMORY;
file->data = (uint8_t *)new_addr+sizeof(uintptr_t)+sizeof(uint16_t)+sizeof(uintptr_t); //next addr+fid+prev addr
}
file->data = (uint8_t *) new_addr+sizeof(uintptr_t)+sizeof(uint16_t)+sizeof(uintptr_t); //next addr+fid+prev addr
flash_program_halfword(new_addr+sizeof(uintptr_t)+sizeof(uintptr_t), file->fid);
flash_program_halfword((uintptr_t)file->data, len);
if (data)
flash_program_block((uintptr_t)file->data+sizeof(uint16_t), data, len);
if (old_data)
flash_program_halfword((uintptr_t) file->data, len);
if (data) {
flash_program_block((uintptr_t) file->data+sizeof(uint16_t), data, len);
}
if (old_data) {
free(old_data);
}
return CCID_OK;
}
int flash_write_data_to_file(file_t *file, const uint8_t *data, uint16_t len) {
int flash_write_data_to_file(file_t *file, const uint8_t *data, uint16_t len)
{
return flash_write_data_to_file_offset(file, data, len, 0);
}

174
src/fs/low_flash.c
View file

@ -71,73 +71,88 @@ bool flash_available = false;
//this function has to be called from the core 0
void do_flash() {
void do_flash()
{
#ifndef ENABLE_EMULATION
if (mutex_try_enter(&mtx_flash, NULL) == true) {
#endif
if (locked_out == true && flash_available == true && ready_pages > 0) {
//printf(" DO_FLASH AVAILABLE\r\n");
for (int r = 0; r < TOTAL_FLASH_PAGES; r++) {
if (flash_pages[r].ready == true) {
if (locked_out == true && flash_available == true && ready_pages > 0) {
//printf(" DO_FLASH AVAILABLE\r\n");
for (int r = 0; r < TOTAL_FLASH_PAGES; r++) {
if (flash_pages[r].ready == true) {
#ifndef ENABLE_EMULATION
//printf("WRITTING %X\r\n",flash_pages[r].address-XIP_BASE);
while (multicore_lockout_start_timeout_us(1000) == false);
//printf("WRITTING %X\r\n",flash_pages[r].address-XIP_BASE);
uint32_t ints = save_and_disable_interrupts();
flash_range_erase(flash_pages[r].address-XIP_BASE, FLASH_SECTOR_SIZE);
flash_range_program(flash_pages[r].address-XIP_BASE, flash_pages[r].page, FLASH_SECTOR_SIZE);
restore_interrupts (ints);
while (multicore_lockout_end_timeout_us(1000) == false);
//printf("WRITEN %X !\r\n",flash_pages[r].address);
#else
memcpy(map + flash_pages[r].address, flash_pages[r].page, FLASH_SECTOR_SIZE);
#endif
flash_pages[r].ready = false;
ready_pages--;
//printf("WRITTING %X\r\n",flash_pages[r].address-XIP_BASE);
while (multicore_lockout_start_timeout_us(1000) == false) {
;
}
else if (flash_pages[r].erase == true) {
//printf("WRITTING %X\r\n",flash_pages[r].address-XIP_BASE);
uint32_t ints = save_and_disable_interrupts();
flash_range_erase(flash_pages[r].address-XIP_BASE, FLASH_SECTOR_SIZE);
flash_range_program(flash_pages[r].address-XIP_BASE,
flash_pages[r].page,
FLASH_SECTOR_SIZE);
restore_interrupts(ints);
while (multicore_lockout_end_timeout_us(1000) == false) {
;
}
//printf("WRITEN %X !\r\n",flash_pages[r].address);
#else
memcpy(map + flash_pages[r].address, flash_pages[r].page, FLASH_SECTOR_SIZE);
#endif
flash_pages[r].ready = false;
ready_pages--;
} else if (flash_pages[r].erase == true) {
#ifndef ENABLE_EMULATION
while (multicore_lockout_start_timeout_us(1000) == false);
//printf("WRITTING\r\n");
flash_range_erase(flash_pages[r].address-XIP_BASE, flash_pages[r].page_size ? ((int)(flash_pages[r].page_size/FLASH_SECTOR_SIZE))*FLASH_SECTOR_SIZE : FLASH_SECTOR_SIZE);
while (multicore_lockout_end_timeout_us(1000) == false);
#else
memset(map + flash_pages[r].address, 0, FLASH_SECTOR_SIZE);
#endif
flash_pages[r].erase = false;
ready_pages--;
while (multicore_lockout_start_timeout_us(1000) == false) {
;
}
}
#ifdef ENABLE_EMULATION
msync(map, PICO_FLASH_SIZE_BYTES, MS_SYNC);
//printf("WRITTING\r\n");
flash_range_erase(flash_pages[r].address-XIP_BASE,
flash_pages[r].page_size ? ((int) (flash_pages[r].page_size/
FLASH_SECTOR_SIZE))*
FLASH_SECTOR_SIZE : FLASH_SECTOR_SIZE);
while (multicore_lockout_end_timeout_us(1000) == false) {
;
}
#else
memset(map + flash_pages[r].address, 0, FLASH_SECTOR_SIZE);
#endif
if (ready_pages != 0) {
printf("ERROR: DO FLASH DOES NOT HAVE ZERO PAGES\n");
flash_pages[r].erase = false;
ready_pages--;
}
}
flash_available = false;
#ifndef ENABLE_EMULATION
mutex_exit(&mtx_flash);
#ifdef ENABLE_EMULATION
msync(map, PICO_FLASH_SIZE_BYTES, MS_SYNC);
#endif
if (ready_pages != 0) {
printf("ERROR: DO FLASH DOES NOT HAVE ZERO PAGES\n");
}
}
sem_release(&sem_wait);
flash_available = false;
#ifndef ENABLE_EMULATION
mutex_exit(&mtx_flash);
}
sem_release(&sem_wait);
#endif
}
//this function has to be called from the core 0
void low_flash_init() {
void low_flash_init()
{
memset(flash_pages, 0, sizeof(page_flash_t)*TOTAL_FLASH_PAGES);
#ifndef ENABLE_EMULATION
mutex_init(&mtx_flash);
sem_init(&sem_wait, 0, 1);
#else
fd_map = open("memory.flash", O_RDWR | O_CREAT, (mode_t)0600);
fd_map = open("memory.flash", O_RDWR | O_CREAT, (mode_t) 0600);
lseek(fd_map, PICO_FLASH_SIZE_BYTES-1, SEEK_SET);
write(fd_map, "", 1);
map = mmap(0, PICO_FLASH_SIZE_BYTES, PROT_READ | PROT_WRITE, MAP_SHARED, fd_map, 0);
#endif
}
void low_flash_init_core1() {
void low_flash_init_core1()
{
#ifndef ENABLE_EMULATION
mutex_enter_blocking(&mtx_flash);
multicore_lockout_victim_init();
@ -148,7 +163,8 @@ void low_flash_init_core1() {
#endif
}
void wait_flash_finish() {
void wait_flash_finish()
{
#ifndef ENABLE_EMULATION
sem_acquire_blocking(&sem_wait); //blocks until released
//wake up
@ -156,7 +172,8 @@ void wait_flash_finish() {
#endif
}
void low_flash_available() {
void low_flash_available()
{
#ifndef ENABLE_EMULATION
mutex_enter_blocking(&mtx_flash);
#endif
@ -166,20 +183,22 @@ void low_flash_available() {
#endif
}
page_flash_t *find_free_page(uintptr_t addr) {
page_flash_t *find_free_page(uintptr_t addr)
{
uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE;
page_flash_t *p = NULL;
for (int r = 0; r < TOTAL_FLASH_PAGES; r++)
{
if ((!flash_pages[r].ready && !flash_pages[r].erase) || flash_pages[r].address == addr_alg) //first available
{
for (int r = 0; r < TOTAL_FLASH_PAGES; r++) {
if ((!flash_pages[r].ready && !flash_pages[r].erase) ||
flash_pages[r].address == addr_alg) { //first available
p = &flash_pages[r];
if (!flash_pages[r].ready && !flash_pages[r].erase)
{
if (!flash_pages[r].ready && !flash_pages[r].erase) {
#ifndef ENABLE_EMULATION
memcpy(p->page, (uint8_t *)addr_alg, FLASH_SECTOR_SIZE);
memcpy(p->page, (uint8_t *) addr_alg, FLASH_SECTOR_SIZE);
#else
memcpy(p->page, (addr >= start_data_pool && addr <= end_rom_pool) ? (uint8_t *)(map+addr_alg) : (uint8_t *)addr_alg, FLASH_SECTOR_SIZE);
memcpy(p->page,
(addr >= start_data_pool &&
addr <= end_rom_pool) ? (uint8_t *) (map+addr_alg) : (uint8_t *) addr_alg,
FLASH_SECTOR_SIZE);
#endif
ready_pages++;
p->address = addr_alg;
@ -191,11 +210,13 @@ page_flash_t *find_free_page(uintptr_t addr) {
return NULL;
}
int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len) {
int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len)
{
page_flash_t *p = NULL;
if (!data || len == 0)
if (!data || len == 0) {
return CCID_ERR_NULL_PARAM;
}
#ifndef ENABLE_EMULATION
mutex_enter_blocking(&mtx_flash);
@ -207,8 +228,7 @@ int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len) {
printf("ERROR: ALL FLASH PAGES CACHED\r\n");
return CCID_ERR_NO_MEMORY;
}
if (!(p = find_free_page(addr)))
{
if (!(p = find_free_page(addr))) {
#ifndef ENABLE_EMULATION
mutex_exit(&mtx_flash);
#endif
@ -223,19 +243,23 @@ int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len) {
return CCID_OK;
}
int flash_program_halfword (uintptr_t addr, uint16_t data) {
return flash_program_block(addr, (const uint8_t *)&data, sizeof(uint16_t));
int flash_program_halfword(uintptr_t addr, uint16_t data)
{
return flash_program_block(addr, (const uint8_t *) &data, sizeof(uint16_t));
}
int flash_program_word (uintptr_t addr, uint32_t data) {
return flash_program_block(addr, (const uint8_t *)&data, sizeof(uint32_t));
int flash_program_word(uintptr_t addr, uint32_t data)
{
return flash_program_block(addr, (const uint8_t *) &data, sizeof(uint32_t));
}
int flash_program_uintptr (uintptr_t addr, uintptr_t data) {
return flash_program_block(addr, (const uint8_t *)&data, sizeof(uintptr_t));
int flash_program_uintptr(uintptr_t addr, uintptr_t data)
{
return flash_program_block(addr, (const uint8_t *) &data, sizeof(uintptr_t));
}
uint8_t *flash_read(uintptr_t addr) {
uint8_t *flash_read(uintptr_t addr)
{
uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE;
#ifndef ENABLE_EMULATION
mutex_enter_blocking(&mtx_flash);
@ -251,25 +275,28 @@ uint8_t *flash_read(uintptr_t addr) {
}
}
}
uint8_t *v = (uint8_t *)addr;
uint8_t *v = (uint8_t *) addr;
#ifndef ENABLE_EMULATION
mutex_exit(&mtx_flash);
#else
if (addr >= start_data_pool && addr <= end_rom_pool)
v += (uintptr_t)map;
if (addr >= start_data_pool && addr <= end_rom_pool) {
v += (uintptr_t) map;
}
#endif
return v;
}
uintptr_t flash_read_uintptr(uintptr_t addr) {
uintptr_t flash_read_uintptr(uintptr_t addr)
{
uint8_t *p = flash_read(addr);
uintptr_t v = 0x0;
for (int i = 0; i < sizeof(uintptr_t); i++) {
v |= (uintptr_t)p[i]<<(8*i);
v |= (uintptr_t) p[i]<<(8*i);
}
return v;
}
uint16_t flash_read_uint16(uintptr_t addr) {
uint16_t flash_read_uint16(uintptr_t addr)
{
uint8_t *p = flash_read(addr);
uint16_t v = 0x0;
for (int i = 0; i < sizeof(uint16_t); i++) {
@ -277,11 +304,13 @@ uint16_t flash_read_uint16(uintptr_t addr) {
}
return v;
}
uint8_t flash_read_uint8(uintptr_t addr) {
uint8_t flash_read_uint8(uintptr_t addr)
{
return *flash_read(addr);
}
int flash_erase_page (uintptr_t addr, size_t page_size) {
int flash_erase_page(uintptr_t addr, size_t page_size)
{
page_flash_t *p = NULL;
#ifndef ENABLE_EMULATION
@ -316,8 +345,9 @@ bool flash_check_blank(const uint8_t *p_start, size_t size)
const uint8_t *p;
for (p = p_start; p < p_start + size; p++) {
if (*p != 0xff)
if (*p != 0xff) {
return false;
}
}
return true;
}

103
src/hsm.h
View file

@ -24,14 +24,14 @@
#else
#include <stdint.h>
extern uint32_t board_millis();
#define MIN(a,b) \
({ __typeof__ (a) _a = (a); \
#define MIN(a, b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a < _b ? _a : _b; })
#define MAX(a,b) \
({ __typeof__ (a) _a = (a); \
_a < _b ? _a : _b; })
#define MAX(a, b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
_a > _b ? _a : _b; })
#endif
#include <string.h>
@ -39,13 +39,16 @@ extern bool wait_button();
extern void low_flash_init_core1();
static inline const uint16_t make_uint16_t(uint8_t b1, uint8_t b2) {
static inline const uint16_t make_uint16_t(uint8_t b1, uint8_t b2)
{
return (b1 << 8) | b2;
}
static inline const uint16_t get_uint16_t(const uint8_t *b, uint16_t offset) {
static inline const uint16_t get_uint16_t(const uint8_t *b, uint16_t offset)
{
return make_uint16_t(b[offset], b[offset+1]);
}
static inline void put_uint16_t(uint16_t n, uint8_t *b) {
static inline void put_uint16_t(uint16_t n, uint8_t *b)
{
*b++ = (n >> 8) & 0xff;
*b = n & 0xff;
}
@ -71,61 +74,61 @@ extern void led_set_blink(uint32_t mode);
extern bool is_req_button_pending();
extern uint32_t button_timeout;
#define SW_BYTES_REMAINING_00() set_res_sw (0x61, 0x00)
#define SW_WARNING_STATE_UNCHANGED() set_res_sw (0x62, 0x00)
#define SW_WARNING_CORRUPTED() set_res_sw (0x62, 0x81)
#define SW_WARNING_EOF() set_res_sw (0x62, 0x82)
#define SW_WARNING_EF_DEACTIVATED() set_res_sw (0x62, 0x83)
#define SW_WARNING_WRONG_FCI() set_res_sw (0x62, 0x84)
#define SW_WARNING_EF_TERMINATED() set_res_sw (0x62, 0x85)
#define SW_BYTES_REMAINING_00() set_res_sw(0x61, 0x00)
#define SW_WARNING_STATE_UNCHANGED() set_res_sw(0x62, 0x00)
#define SW_WARNING_CORRUPTED() set_res_sw(0x62, 0x81)
#define SW_WARNING_EOF() set_res_sw(0x62, 0x82)
#define SW_WARNING_EF_DEACTIVATED() set_res_sw(0x62, 0x83)
#define SW_WARNING_WRONG_FCI() set_res_sw(0x62, 0x84)
#define SW_WARNING_EF_TERMINATED() set_res_sw(0x62, 0x85)
#define SW_WARNING_NOINFO() set_res_sw (0x63, 0x00)
#define SW_WARNING_FILLUP() set_res_sw (0x63, 0x81)
#define SW_WARNING_NOINFO() set_res_sw(0x63, 0x00)
#define SW_WARNING_FILLUP() set_res_sw(0x63, 0x81)
#define SW_EXEC_ERROR() set_res_sw (0x64, 0x00)
#define SW_EXEC_ERROR() set_res_sw(0x64, 0x00)
#define SW_MEMORY_FAILURE() set_res_sw (0x65, 0x81)
#define SW_MEMORY_FAILURE() set_res_sw(0x65, 0x81)
#define SW_SECURE_MESSAGE_EXEC_ERROR() set_res_sw (0x66, 0x00)
#define SW_SECURE_MESSAGE_EXEC_ERROR() set_res_sw(0x66, 0x00)
#define SW_WRONG_LENGTH() set_res_sw (0x67, 0x00)
#define SW_WRONG_DATA() set_res_sw (0x67, 0x00)
#define SW_WRONG_LENGTH() set_res_sw(0x67, 0x00)
#define SW_WRONG_DATA() set_res_sw(0x67, 0x00)
#define SW_LOGICAL_CHANNEL_NOT_SUPPORTED() set_res_sw (0x68, 0x81)
#define SW_SECURE_MESSAGING_NOT_SUPPORTED() set_res_sw (0x68, 0x82)
#define SW_LOGICAL_CHANNEL_NOT_SUPPORTED() set_res_sw(0x68, 0x81)
#define SW_SECURE_MESSAGING_NOT_SUPPORTED() set_res_sw(0x68, 0x82)
#define SW_COMMAND_INCOMPATIBLE() set_res_sw (0x69, 0x81)
#define SW_SECURITY_STATUS_NOT_SATISFIED() set_res_sw (0x69, 0x82)
#define SW_PIN_BLOCKED() set_res_sw (0x69, 0x83)
#define SW_DATA_INVALID() set_res_sw (0x69, 0x84)
#define SW_CONDITIONS_NOT_SATISFIED() set_res_sw (0x69, 0x85)
#define SW_COMMAND_NOT_ALLOWED() set_res_sw (0x69, 0x86)
#define SW_SECURE_MESSAGING_MISSING_DO() set_res_sw (0x69, 0x87)
#define SW_SECURE_MESSAGING_INCORRECT_DO() set_res_sw (0x69, 0x88)
#define SW_APPLET_SELECT_FAILED() set_res_sw (0x69, 0x99)
#define SW_COMMAND_INCOMPATIBLE() set_res_sw(0x69, 0x81)
#define SW_SECURITY_STATUS_NOT_SATISFIED() set_res_sw(0x69, 0x82)
#define SW_PIN_BLOCKED() set_res_sw(0x69, 0x83)
#define SW_DATA_INVALID() set_res_sw(0x69, 0x84)
#define SW_CONDITIONS_NOT_SATISFIED() set_res_sw(0x69, 0x85)
#define SW_COMMAND_NOT_ALLOWED() set_res_sw(0x69, 0x86)
#define SW_SECURE_MESSAGING_MISSING_DO() set_res_sw(0x69, 0x87)
#define SW_SECURE_MESSAGING_INCORRECT_DO() set_res_sw(0x69, 0x88)
#define SW_APPLET_SELECT_FAILED() set_res_sw(0x69, 0x99)
#define SW_INCORRECT_PARAMS() set_res_sw (0x6A, 0x80)
#define SW_FUNC_NOT_SUPPORTED() set_res_sw (0x6A, 0x81)
#define SW_FILE_NOT_FOUND() set_res_sw (0x6A, 0x82)
#define SW_RECORD_NOT_FOUND() set_res_sw (0x6A, 0x83)
#define SW_FILE_FULL() set_res_sw (0x6A, 0x84)
#define SW_WRONG_NE() set_res_sw (0x6A, 0x85)
#define SW_INCORRECT_P1P2() set_res_sw (0x6A, 0x86)
#define SW_WRONG_NC() set_res_sw (0x6A, 0x87)
#define SW_REFERENCE_NOT_FOUND() set_res_sw (0x6A, 0x88)
#define SW_FILE_EXISTS() set_res_sw (0x6A, 0x89)
#define SW_INCORRECT_PARAMS() set_res_sw(0x6A, 0x80)
#define SW_FUNC_NOT_SUPPORTED() set_res_sw(0x6A, 0x81)
#define SW_FILE_NOT_FOUND() set_res_sw(0x6A, 0x82)
#define SW_RECORD_NOT_FOUND() set_res_sw(0x6A, 0x83)
#define SW_FILE_FULL() set_res_sw(0x6A, 0x84)
#define SW_WRONG_NE() set_res_sw(0x6A, 0x85)
#define SW_INCORRECT_P1P2() set_res_sw(0x6A, 0x86)
#define SW_WRONG_NC() set_res_sw(0x6A, 0x87)
#define SW_REFERENCE_NOT_FOUND() set_res_sw(0x6A, 0x88)
#define SW_FILE_EXISTS() set_res_sw(0x6A, 0x89)
#define SW_WRONG_P1P2() set_res_sw (0x6B, 0x00)
#define SW_WRONG_P1P2() set_res_sw(0x6B, 0x00)
#define SW_CORRECT_LENGTH_00() set_res_sw (0x6C, 0x00)
#define SW_CORRECT_LENGTH_00() set_res_sw(0x6C, 0x00)
#define SW_INS_NOT_SUPPORTED() set_res_sw (0x6D, 0x00)
#define SW_INS_NOT_SUPPORTED() set_res_sw(0x6D, 0x00)
#define SW_CLA_NOT_SUPPORTED() set_res_sw (0x6E, 0x00)
#define SW_CLA_NOT_SUPPORTED() set_res_sw(0x6E, 0x00)
#define SW_UNKNOWN() set_res_sw (0x6F, 0x00)
#define SW_UNKNOWN() set_res_sw(0x6F, 0x00)
#define SW_OK() set_res_sw (0x90, 0x00)
#define SW_OK() set_res_sw(0x90, 0x00)
#define CCID_OK 0
#define CCID_ERR_NO_MEMORY -1000

1
src/hsm_version.h
View file

@ -24,4 +24,3 @@
#define HSM_SDK_VERSION_MINOR (HSM_SDK_VERSION & 0xff)
#endif

59
src/main.c
View file

@ -62,7 +62,8 @@ app_t *current_app = NULL;
const uint8_t *ccid_atr = NULL;
int register_app(app_t * (*select_aid)(app_t *, const uint8_t *, uint8_t)) {
int register_app(app_t *(*select_aid)(app_t *, const uint8_t *, uint8_t))
{
if (num_apps < sizeof(apps)/sizeof(app_t)) {
apps[num_apps].select_aid = select_aid;
num_apps++;
@ -73,16 +74,19 @@ int register_app(app_t * (*select_aid)(app_t *, const uint8_t *, uint8_t)) {
static uint32_t blink_interval_ms = BLINK_NOT_MOUNTED;
void led_set_blink(uint32_t mode) {
void led_set_blink(uint32_t mode)
{
blink_interval_ms = mode;
}
uint32_t timeout = 0;
void timeout_stop() {
void timeout_stop()
{
timeout = 0;
}
void timeout_start() {
void timeout_start()
{
timeout = board_millis();
}
@ -90,7 +94,8 @@ void execute_tasks();
static bool req_button_pending = false;
bool is_req_button_pending() {
bool is_req_button_pending()
{
return req_button_pending;
}
@ -98,14 +103,16 @@ uint32_t button_timeout = 15000;
bool cancel_button = false;
#ifdef ENABLE_EMULATION
uint32_t board_millis() {
uint32_t board_millis()
{
struct timeval start;
gettimeofday(&start, NULL);
return (start.tv_sec * 1000 + start.tv_usec/1000);
return start.tv_sec * 1000 + start.tv_usec/1000;
}
#else
bool wait_button() {
bool wait_button()
{
uint32_t start_button = board_millis();
bool timeout = false;
cancel_button = false;
@ -137,7 +144,8 @@ bool wait_button() {
struct apdu apdu;
void led_blinking_task() {
void led_blinking_task()
{
#ifdef PICO_DEFAULT_LED_PIN
static uint32_t start_ms = 0;
static uint8_t led_state = false;
@ -150,8 +158,9 @@ void led_blinking_task() {
// Blink every interval ms
if (board_millis() - start_ms < interval)
if (board_millis() - start_ms < interval) {
return; // not enough time
}
start_ms += interval;
gpio_put(led_color, led_state);
@ -159,7 +168,8 @@ void led_blinking_task() {
#endif
}
void led_off_all() {
void led_off_all()
{
#ifdef PIMORONI_TINY2040
gpio_put(TINY2040_LED_R_PIN, 1);
gpio_put(TINY2040_LED_G_PIN, 1);
@ -171,17 +181,18 @@ void led_off_all() {
#endif
}
void init_rtc() {
void init_rtc()
{
#ifndef ENABLE_EMULATION
rtc_init();
datetime_t dt = {
.year = 2020,
.month = 1,
.day = 1,
.dotw = 3, // 0 is Sunday, so 5 is Friday
.hour = 00,
.min = 00,
.sec = 00
.year = 2020,
.month = 1,
.day = 1,
.dotw = 3, // 0 is Sunday, so 5 is Friday
.hour = 00,
.min = 00,
.sec = 00
};
rtc_set_datetime(&dt);
#endif
@ -190,7 +201,8 @@ void init_rtc() {
extern void neug_task();
extern void usb_task();
void execute_tasks() {
void execute_tasks()
{
usb_task();
#ifndef ENABLE_EMULATION
tud_task(); // tinyusb device task
@ -198,7 +210,8 @@ void execute_tasks() {
led_blinking_task();
}
int main(void) {
int main(void)
{
#ifndef ENABLE_EMULATION
usb_init();
@ -225,7 +238,7 @@ int main(void) {
//prepare_ccid();
#else
emul_init("127.0.0.1",35963);
emul_init("127.0.0.1", 35963);
#endif
random_init();
@ -243,4 +256,4 @@ int main(void) {
}
return 0;
}
}

114
src/rng/hwrng.c
View file

@ -36,7 +36,8 @@ mbedtls_ctr_drbg_context ctr_drbg;
extern uint32_t board_millis();
#endif
void adc_start() {
void adc_start()
{
#ifndef ENABLE_EMULATION
adc_init();
adc_gpio_init(27);
@ -44,10 +45,12 @@ void adc_start() {
#endif
}
void adc_stop() {
void adc_stop()
{
}
#ifdef ENABLE_EMULATION
uint32_t adc_read() {
uint32_t adc_read()
{
return 0;
}
#endif
@ -55,19 +58,25 @@ uint32_t adc_read() {
static uint64_t random_word = 0xcbf29ce484222325;
static uint8_t ep_round = 0;
static void ep_init() {
static void ep_init()
{
random_word = 0xcbf29ce484222325;
ep_round = 0;
#ifdef ENABLE_EMULATION
mbedtls_entropy_context entropy;
mbedtls_entropy_init( &entropy );
mbedtls_ctr_drbg_init( &ctr_drbg );
mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy, (const unsigned char *) "RANDOM_GEN", 10 );
mbedtls_entropy_init(&entropy);
mbedtls_ctr_drbg_init(&ctr_drbg);
mbedtls_ctr_drbg_seed(&ctr_drbg,
mbedtls_entropy_func,
&entropy,
(const unsigned char *) "RANDOM_GEN",
10);
#endif
}
/* Here, we assume a little endian architecture. */
static int ep_process () {
static int ep_process()
{
if (ep_round == 0) {
ep_init();
}
@ -75,16 +84,15 @@ static int ep_process () {
#ifndef ENABLE_EMULATION
for (int n = 0; n < 64; n++) {
uint8_t bit1, bit2;
do
{
do {
bit1 = rosc_hw->randombit&0xff;
//sleep_ms(1);
bit2 = rosc_hw->randombit&0xff;
} while(bit1 == bit2);
} while (bit1 == bit2);
word = (word << 1) | bit1;
}
#else
mbedtls_ctr_drbg_random( &ctr_drbg, (uint8_t *)&word, sizeof( word ) );
mbedtls_ctr_drbg_random(&ctr_drbg, (uint8_t *) &word, sizeof(word));
#endif
random_word ^= word^board_millis()^adc_read();
random_word *= 0x00000100000001B3;
@ -95,19 +103,21 @@ static int ep_process () {
return 0;
}
static const uint32_t *ep_output() {
return (uint32_t *)&random_word;
static const uint32_t *ep_output()
{
return (uint32_t *) &random_word;
}
struct rng_rb {
uint32_t *buf;
uint8_t head, tail;
uint8_t size;
unsigned int full :1;
unsigned int empty :1;
unsigned int full : 1;
unsigned int empty : 1;
};
static void rb_init(struct rng_rb *rb, uint32_t *p, uint8_t size) {
static void rb_init(struct rng_rb *rb, uint32_t *p, uint8_t size)
{
#ifdef ENABLE_EMULATION
#endif
rb->buf = p;
@ -117,22 +127,28 @@ static void rb_init(struct rng_rb *rb, uint32_t *p, uint8_t size) {
rb->empty = 1;
}
static void rb_add(struct rng_rb *rb, uint32_t v) {
static void rb_add(struct rng_rb *rb, uint32_t v)
{
rb->buf[rb->tail++] = v;
if (rb->tail == rb->size)
if (rb->tail == rb->size) {
rb->tail = 0;
if (rb->tail == rb->head)
}
if (rb->tail == rb->head) {
rb->full = 1;
}
rb->empty = 0;
}
static uint32_t rb_del(struct rng_rb *rb) {
static uint32_t rb_del(struct rng_rb *rb)
{
uint32_t v = rb->buf[rb->head++];
if (rb->head == rb->size)
if (rb->head == rb->size) {
rb->head = 0;
if (rb->head == rb->tail)
}
if (rb->head == rb->tail) {
rb->empty = 1;
}
rb->full = 0;
return v;
@ -140,26 +156,29 @@ static uint32_t rb_del(struct rng_rb *rb) {
static struct rng_rb the_ring_buffer;
void *neug_task() {
void *neug_task()
{
struct rng_rb *rb = &the_ring_buffer;
int n;
if ((n = ep_process())) {
int i;
const uint32_t *vp;
vp = ep_output();
int i;
const uint32_t *vp;
vp = ep_output();
for (i = 0; i < n; i++) {
rb_add (rb, *vp++);
if (rb->full)
break;
}
}
for (i = 0; i < n; i++) {
rb_add(rb, *vp++);
if (rb->full) {
break;
}
}
}
return NULL;
}
void neug_init(uint32_t *buf, uint8_t size) {
void neug_init(uint32_t *buf, uint8_t size)
{
struct rng_rb *rb = &the_ring_buffer;
rb_init(rb, buf, size);
@ -169,40 +188,45 @@ void neug_init(uint32_t *buf, uint8_t size) {
ep_init();
}
void neug_flush(void) {
void neug_flush(void)
{
struct rng_rb *rb = &the_ring_buffer;
while (!rb->empty)
rb_del (rb);
while (!rb->empty) {
rb_del(rb);
}
}
uint32_t neug_get() {
uint32_t neug_get()
{
struct rng_rb *rb = &the_ring_buffer;
uint32_t v;
while (rb->empty)
while (rb->empty) {
neug_task();
}
v = rb_del(rb);
return v;
}
void neug_wait_full() {
void neug_wait_full()
{
struct rng_rb *rb = &the_ring_buffer;
#ifndef ENABLE_EMULATION
uint core = get_core_num();
#endif
while (!rb->full) {
#ifndef ENABLE_EMULATION
if (core == 1)
if (core == 1) {
sleep_ms(1);
else
} else
#endif
neug_task();
neug_task();
}
}
void neug_fini(void) {
void neug_fini(void)
{
neug_get();
}

2
src/rng/hwrng.h
View file

@ -31,4 +31,4 @@ void neug_flush(void);
void neug_wait_full();
void neug_fini(void);
#endif
#endif

60
src/rng/random.c
View file

@ -22,43 +22,49 @@
#include "hwrng.h"
#define RANDOM_BYTES_LENGTH 32
static uint32_t random_word[RANDOM_BYTES_LENGTH/sizeof (uint32_t)];
static uint32_t random_word[RANDOM_BYTES_LENGTH/sizeof(uint32_t)];
void random_init(void) {
void random_init(void)
{
int i;
neug_init(random_word, RANDOM_BYTES_LENGTH/sizeof (uint32_t));
neug_init(random_word, RANDOM_BYTES_LENGTH/sizeof(uint32_t));
for (i = 0; i < NEUG_PRE_LOOP; i++)
for (i = 0; i < NEUG_PRE_LOOP; i++) {
neug_get();
}
}
void random_fini(void) {
neug_fini ();
void random_fini(void)
{
neug_fini();
}
/*
* Return pointer to random 32-byte
*/
void random_bytes_free (const uint8_t *p);
void random_bytes_free(const uint8_t *p);
#define MAX_RANDOM_BUFFER 1024
const uint8_t * random_bytes_get(size_t len) {
if (len > MAX_RANDOM_BUFFER)
const uint8_t *random_bytes_get(size_t len)
{
if (len > MAX_RANDOM_BUFFER) {
return NULL;
}
static uint32_t return_word[MAX_RANDOM_BUFFER/sizeof(uint32_t)];
for (int ix = 0; ix < len; ix += RANDOM_BYTES_LENGTH) {
neug_wait_full();
memcpy(return_word+ix/sizeof(uint32_t), random_word, RANDOM_BYTES_LENGTH);
random_bytes_free((const uint8_t *)random_word);
random_bytes_free((const uint8_t *) random_word);
}
return (const uint8_t *)return_word;
return (const uint8_t *) return_word;
}
/*
* Free pointer to random 32-byte
*/
void random_bytes_free(const uint8_t *p) {
(void)p;
void random_bytes_free(const uint8_t *p)
{
(void) p;
memset(random_word, 0, RANDOM_BYTES_LENGTH);
neug_flush();
}
@ -66,21 +72,23 @@ void random_bytes_free(const uint8_t *p) {
/*
* Return 4-byte salt
*/
void random_get_salt(uint8_t *p) {
void random_get_salt(uint8_t *p)
{
uint32_t rnd;
rnd = neug_get();
memcpy(p, &rnd, sizeof (uint32_t));
memcpy(p, &rnd, sizeof(uint32_t));
rnd = neug_get();
memcpy(p + sizeof (uint32_t), &rnd, sizeof (uint32_t));
memcpy(p + sizeof(uint32_t), &rnd, sizeof(uint32_t));
}
/*
* Random byte iterator
*/
int random_gen(void *arg, unsigned char *out, size_t out_len) {
uint8_t *index_p = (uint8_t *)arg;
int random_gen(void *arg, unsigned char *out, size_t out_len)
{
uint8_t *index_p = (uint8_t *) arg;
uint8_t index = index_p ? *index_p : 0;
size_t n;
@ -88,22 +96,24 @@ int random_gen(void *arg, unsigned char *out, size_t out_len) {
neug_wait_full();
n = RANDOM_BYTES_LENGTH - index;
if (n > out_len)
n = out_len;
if (n > out_len) {
n = out_len;
}
memcpy(out, ((unsigned char *)random_word) + index, n);
memcpy(out, ((unsigned char *) random_word) + index, n);
out += n;
out_len -= n;
index += n;
if (index >= RANDOM_BYTES_LENGTH) {
index = 0;
neug_flush();
}
index = 0;
neug_flush();
}
}
if (index_p)
if (index_p) {
*index_p = index;
}
return 0;
}

12
src/rng/random.h
View file

@ -25,17 +25,17 @@
#include "pico/stdlib.h"
#endif
void random_init (void);
void random_fini (void);
void random_init(void);
void random_fini(void);
/* 32-byte random bytes */
const uint8_t *random_bytes_get (size_t);
void random_bytes_free (const uint8_t *p);
const uint8_t *random_bytes_get(size_t);
void random_bytes_free(const uint8_t *p);
/* 8-byte salt */
void random_get_salt (uint8_t *p);
void random_get_salt(uint8_t *p);
/* iterator returning a byta at a time */
extern int random_gen(void *arg, unsigned char *output, size_t output_len);
#endif
#endif

201
src/usb/ccid/ccid.c
View file

@ -50,33 +50,33 @@
#define USB_BUF_SIZE (MAX_CMD_APDU_DATA_SIZE+20+9)
#endif
#define CCID_SET_PARAMS 0x61 /* non-ICCD command */
#define CCID_POWER_ON 0x62
#define CCID_POWER_OFF 0x63
#define CCID_SLOT_STATUS 0x65 /* non-ICCD command */
#define CCID_SECURE 0x69 /* non-ICCD command */
#define CCID_GET_PARAMS 0x6C /* non-ICCD command */
#define CCID_RESET_PARAMS 0x6D /* non-ICCD command */
#define CCID_XFR_BLOCK 0x6F
#define CCID_DATA_BLOCK_RET 0x80
#define CCID_SLOT_STATUS_RET 0x81 /* non-ICCD result */
#define CCID_PARAMS_RET 0x82 /* non-ICCD result */
#define CCID_SET_PARAMS 0x61 /* non-ICCD command */
#define CCID_POWER_ON 0x62
#define CCID_POWER_OFF 0x63
#define CCID_SLOT_STATUS 0x65 /* non-ICCD command */
#define CCID_SECURE 0x69 /* non-ICCD command */
#define CCID_GET_PARAMS 0x6C /* non-ICCD command */
#define CCID_RESET_PARAMS 0x6D /* non-ICCD command */
#define CCID_XFR_BLOCK 0x6F
#define CCID_DATA_BLOCK_RET 0x80
#define CCID_SLOT_STATUS_RET 0x81 /* non-ICCD result */
#define CCID_PARAMS_RET 0x82 /* non-ICCD result */
#define CCID_MSG_SEQ_OFFSET 6
#define CCID_MSG_STATUS_OFFSET 7
#define CCID_MSG_ERROR_OFFSET 8
#define CCID_MSG_CHAIN_OFFSET 9
#define CCID_MSG_DATA_OFFSET 10 /* == CCID_MSG_HEADER_SIZE */
#define CCID_MAX_MSG_DATA_SIZE USB_BUF_SIZE
#define CCID_MSG_SEQ_OFFSET 6
#define CCID_MSG_STATUS_OFFSET 7
#define CCID_MSG_ERROR_OFFSET 8
#define CCID_MSG_CHAIN_OFFSET 9
#define CCID_MSG_DATA_OFFSET 10 /* == CCID_MSG_HEADER_SIZE */
#define CCID_MAX_MSG_DATA_SIZE USB_BUF_SIZE
#define CCID_STATUS_RUN 0x00
#define CCID_STATUS_PRESENT 0x01
#define CCID_STATUS_NOTPRESENT 0x02
#define CCID_CMD_STATUS_OK 0x00
#define CCID_CMD_STATUS_ERROR 0x40
#define CCID_CMD_STATUS_TIMEEXT 0x80
#define CCID_STATUS_RUN 0x00
#define CCID_STATUS_PRESENT 0x01
#define CCID_STATUS_NOTPRESENT 0x02
#define CCID_CMD_STATUS_OK 0x00
#define CCID_CMD_STATUS_ERROR 0x40
#define CCID_CMD_STATUS_TIMEEXT 0x80
#define CCID_ERROR_XFR_OVERRUN 0xFC
#define CCID_ERROR_XFR_OVERRUN 0xFC
/*
* Since command-byte is at offset 0,
@ -97,67 +97,78 @@ struct ccid_header {
uint8_t abRFU0;
uint16_t abRFU1;
uint8_t apdu; //Actually it is an array
} __attribute__ ((__packed__));
} __attribute__((__packed__));
uint8_t ccid_status = 1;
static uint8_t itf_num;
void ccid_write_offset(uint16_t size, uint16_t offset) {
if (*usb_get_tx(ITF_CCID)+offset != 0x81)
DEBUG_PAYLOAD(usb_get_tx(ITF_CCID)+offset,size+10);
void ccid_write_offset(uint16_t size, uint16_t offset)
{
if (*usb_get_tx(ITF_CCID)+offset != 0x81) {
DEBUG_PAYLOAD(usb_get_tx(ITF_CCID)+offset, size+10);
}
usb_write_offset(ITF_CCID, size+10, offset);
}
void ccid_write(uint16_t size) {
void ccid_write(uint16_t size)
{
ccid_write_offset(size, 0);
}
struct ccid_header *ccid_response;
struct ccid_header *ccid_header;
int driver_init_ccid() {
ccid_header = (struct ccid_header *)usb_get_rx(ITF_CCID);
int driver_init_ccid()
{
ccid_header = (struct ccid_header *) usb_get_rx(ITF_CCID);
// apdu.header = &ccid_header->apdu;
ccid_response = (struct ccid_header *)usb_get_tx(ITF_CCID);
ccid_response = (struct ccid_header *) usb_get_tx(ITF_CCID);
usb_set_timeout_counter(ITF_CCID, 1500);
return CCID_OK;
}
void tud_vendor_rx_cb(uint8_t itf) {
void tud_vendor_rx_cb(uint8_t itf)
{
(void) itf;
uint32_t len = tud_vendor_available();
usb_rx(ITF_CCID, NULL, len);
}
void tud_vendor_tx_cb(uint8_t itf, uint32_t sent_bytes) {
printf("written %ld\n",sent_bytes);
void tud_vendor_tx_cb(uint8_t itf, uint32_t sent_bytes)
{
printf("written %ld\n", sent_bytes);
usb_write_flush(ITF_CCID);
}
int driver_write_ccid(const uint8_t *buffer, size_t buffer_size) {
int driver_write_ccid(const uint8_t *buffer, size_t buffer_size)
{
return tud_vendor_write(buffer, buffer_size);
}
size_t driver_read_ccid(uint8_t *buffer, size_t buffer_size) {
size_t driver_read_ccid(uint8_t *buffer, size_t buffer_size)
{
return tud_vendor_read(buffer, buffer_size);
}
int driver_process_usb_nopacket_ccid() {
int driver_process_usb_nopacket_ccid()
{
return 0;
}
int driver_process_usb_packet_ccid(uint16_t rx_read) {
int driver_process_usb_packet_ccid(uint16_t rx_read)
{
if (rx_read >= 10) {
driver_init_ccid();
//printf("%d %d %x\r\n",tccid->dwLength,rx_read-10,tccid->bMessageType);
if (ccid_header->dwLength <= rx_read-10) {
size_t apdu_sent = 0;
if (ccid_header->bMessageType != 0x65)
DEBUG_PAYLOAD(usb_get_rx(ITF_CCID),usb_read_available(ITF_CCID));
if (ccid_header->bMessageType != 0x65) {
DEBUG_PAYLOAD(usb_get_rx(ITF_CCID), usb_read_available(ITF_CCID));
}
if (ccid_header->bMessageType == 0x65) {
ccid_response->bMessageType = CCID_SLOT_STATUS_RET;
ccid_response->dwLength = 0;
@ -166,8 +177,7 @@ int driver_process_usb_packet_ccid(uint16_t rx_read) {
ccid_response->abRFU0 = ccid_status;
ccid_response->abRFU1 = 0;
ccid_write(0);
}
else if (ccid_header->bMessageType == 0x62) {
} else if (ccid_header->bMessageType == 0x62) {
size_t size_atr = (ccid_atr ? ccid_atr[0] : 0);
ccid_response->bMessageType = 0x80;
ccid_response->dwLength = size_atr;
@ -180,10 +190,10 @@ int driver_process_usb_packet_ccid(uint16_t rx_read) {
card_start(apdu_thread);
ccid_status = 0;
ccid_write(size_atr);
}
else if (ccid_header->bMessageType == 0x63) {
if (ccid_status == 0)
} else if (ccid_header->bMessageType == 0x63) {
if (ccid_status == 0) {
card_exit(0);
}
ccid_status = 1;
ccid_response->bMessageType = CCID_SLOT_STATUS_RET;
ccid_response->dwLength = 0;
@ -192,8 +202,7 @@ int driver_process_usb_packet_ccid(uint16_t rx_read) {
ccid_response->abRFU0 = ccid_status;
ccid_response->abRFU1 = 0;
ccid_write(0);
}
else if (ccid_header->bMessageType == 0x6F) {
} else if (ccid_header->bMessageType == 0x6F) {
apdu_sent = apdu_process(ITF_CCID, &ccid_header->apdu, ccid_header->dwLength);
}
usb_clear_rx(ITF_CCID);
@ -201,23 +210,25 @@ int driver_process_usb_packet_ccid(uint16_t rx_read) {
}
}
/*
if (usb_read_available() && c->epo->ready) {
if (usb_read_available() && c->epo->ready) {
if ()
uint32_t count = usb_read(endp1_rx_buf, sizeof(endp1_rx_buf));
//if (endp1_rx_buf[0] != 0x65)
DEBUG_PAYLOAD(endp1_rx_buf, count);
//DEBUG_PAYLOAD(endp1_rx_buf, count);
ccid_rx_ready(count);
}
*/
}
*/
return 0;
}
bool driver_mounted_ccid() {
bool driver_mounted_ccid()
{
return tud_vendor_mounted();
}
void driver_exec_timeout_ccid() {
void driver_exec_timeout_ccid()
{
ccid_response->bMessageType = CCID_DATA_BLOCK_RET;
ccid_response->dwLength = 0;
ccid_response->bSlot = 0;
@ -227,7 +238,8 @@ void driver_exec_timeout_ccid() {
ccid_write(0);
}
void driver_exec_finished_ccid(size_t size_next) {
void driver_exec_finished_ccid(size_t size_next)
{
ccid_response->bMessageType = CCID_DATA_BLOCK_RET;
ccid_response->dwLength = size_next;
ccid_response->bSlot = 0;
@ -237,9 +249,10 @@ void driver_exec_finished_ccid(size_t size_next) {
ccid_write(size_next);
}
void driver_exec_finished_cont_ccid(size_t size_next, size_t offset) {
void driver_exec_finished_cont_ccid(size_t size_next, size_t offset)
{
ccid_response = (struct ccid_header *)(usb_get_tx(ITF_CCID)+offset-10);
ccid_response = (struct ccid_header *) (usb_get_tx(ITF_CCID)+offset-10);
ccid_response->bMessageType = CCID_DATA_BLOCK_RET;
ccid_response->dwLength = size_next;
ccid_response->bSlot = 0;
@ -249,40 +262,47 @@ void driver_exec_finished_cont_ccid(size_t size_next, size_t offset) {
ccid_write_offset(size_next, offset-10);
}
uint8_t *driver_prepare_response_ccid() {
ccid_response = (struct ccid_header *)usb_get_tx(ITF_CCID);
uint8_t *driver_prepare_response_ccid()
{
ccid_response = (struct ccid_header *) usb_get_tx(ITF_CCID);
apdu.rdata = &ccid_response->apdu;
return &ccid_response->apdu;
}
#define USB_CONFIG_ATT_ONE TU_BIT(7)
#define MAX_USB_POWER 1
#define MAX_USB_POWER 1
static void ccid_init_cb(void) {
static void ccid_init_cb(void)
{
TU_LOG1("-------- CCID INIT\r\n");
vendord_init();
//ccid_notify_slot_change(c);
}
static void ccid_reset_cb(uint8_t rhport) {
static void ccid_reset_cb(uint8_t rhport)
{
TU_LOG1("-------- CCID RESET\r\n");
itf_num = 0;
vendord_reset(rhport);
}
static uint16_t ccid_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
uint8_t *itf_vendor = (uint8_t *)malloc(sizeof(uint8_t)*max_len);
static uint16_t ccid_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
{
uint8_t *itf_vendor = (uint8_t *) malloc(sizeof(uint8_t)*max_len);
//TU_LOG1("-------- CCID OPEN\r\n");
TU_VERIFY(itf_desc->bInterfaceClass == TUSB_CLASS_SMART_CARD && itf_desc->bInterfaceSubClass == 0 && itf_desc->bInterfaceProtocol == 0, 0);
TU_VERIFY(
itf_desc->bInterfaceClass == TUSB_CLASS_SMART_CARD && itf_desc->bInterfaceSubClass == 0 && itf_desc->bInterfaceProtocol == 0,
0);
//vendord_open expects a CLASS_VENDOR interface class
memcpy(itf_vendor, itf_desc, sizeof(uint8_t)*max_len);
((tusb_desc_interface_t *)itf_vendor)->bInterfaceClass = TUSB_CLASS_VENDOR_SPECIFIC;
vendord_open(rhport, (tusb_desc_interface_t *)itf_vendor, max_len);
((tusb_desc_interface_t *) itf_vendor)->bInterfaceClass = TUSB_CLASS_VENDOR_SPECIFIC;
vendord_open(rhport, (tusb_desc_interface_t *) itf_vendor, max_len);
free(itf_vendor);
uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(struct ccid_class_descriptor) + 2*sizeof(tusb_desc_endpoint_t);
uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(struct ccid_class_descriptor) +
2*sizeof(tusb_desc_endpoint_t);
TU_VERIFY(max_len >= drv_len, 0);
itf_num = itf_desc->bInterfaceNumber;
@ -290,44 +310,56 @@ static uint16_t ccid_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc,
}
// Support for parameterized reset via vendor interface control request
static bool ccid_control_xfer_cb(uint8_t __unused rhport, uint8_t stage, tusb_control_request_t const * request) {
static bool ccid_control_xfer_cb(uint8_t __unused rhport,
uint8_t stage,
tusb_control_request_t const *request)
{
// nothing to do with DATA & ACK stage
TU_LOG2("-------- CCID CTRL XFER\r\n");
if (stage != CONTROL_STAGE_SETUP) return true;
if (stage != CONTROL_STAGE_SETUP) {
return true;
}
if (request->wIndex == itf_num)
{
TU_LOG2("-------- bmRequestType %x, bRequest %x, wValue %x, wLength %x\r\n",request->bmRequestType,request->bRequest, request->wValue, request->wLength);
if (request->wIndex == itf_num) {
TU_LOG2("-------- bmRequestType %x, bRequest %x, wValue %x, wLength %x\r\n",
request->bmRequestType,
request->bRequest,
request->wValue,
request->wLength);
/*
#if PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL
#if PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_BOOTSEL
if (request->bRequest == RESET_REQUEST_BOOTSEL) {
#ifdef PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED
#ifdef PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED
uint gpio_mask = 1u << PICO_STDIO_USB_RESET_BOOTSEL_ACTIVITY_LED;
#else
#else
uint gpio_mask = 0u;
#endif
#if !PICO_STDIO_USB_RESET_BOOTSEL_FIXED_ACTIVITY_LED
#endif
#if !PICO_STDIO_USB_RESET_BOOTSEL_FIXED_ACTIVITY_LED
if (request->wValue & 0x100) {
gpio_mask = 1u << (request->wValue >> 9u);
}
#endif
#endif
reset_usb_boot(gpio_mask, (request->wValue & 0x7f) | PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK);
// does not return, otherwise we'd return true
}
#endif
#if PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT
#endif
#if PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_RESET_TO_FLASH_BOOT
if (request->bRequest == RESET_REQUEST_FLASH) {
watchdog_reboot(0, 0, PICO_STDIO_USB_RESET_RESET_TO_FLASH_DELAY_MS);
return true;
}
#endif
*/
#endif
*/
return true;
}
return false;
}
static bool ccid_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
static bool ccid_xfer_cb(uint8_t rhport,
uint8_t ep_addr,
xfer_result_t result,
uint32_t xferred_bytes)
{
//printf("------ CALLED XFER_CB\r\n");
return vendord_xfer_cb(rhport, ep_addr, result, xferred_bytes);
//return true;
@ -346,7 +378,8 @@ static const usbd_class_driver_t ccid_driver = {
};
// Implement callback to add our custom driver
usbd_class_driver_t const *usbd_app_driver_get_cb(uint8_t *driver_count) {
usbd_class_driver_t const *usbd_app_driver_get_cb(uint8_t *driver_count)
{
*driver_count = 1;
return &ccid_driver;
}

16
src/usb/ccid/ccid.h
View file

@ -26,16 +26,16 @@ extern const uint8_t historical_bytes[];
#define USB_LL_BUF_SIZE 64
enum ccid_state {
CCID_STATE_NOCARD, /* No card available */
CCID_STATE_START, /* Initial */
CCID_STATE_WAIT, /* Waiting APDU */
CCID_STATE_NOCARD, /* No card available */
CCID_STATE_START, /* Initial */
CCID_STATE_WAIT, /* Waiting APDU */
CCID_STATE_EXECUTE, /* Executing command */
CCID_STATE_ACK_REQUIRED_0, /* Ack required (executing)*/
CCID_STATE_ACK_REQUIRED_1, /* Waiting user's ACK (execution finished) */
CCID_STATE_EXECUTE, /* Executing command */
CCID_STATE_ACK_REQUIRED_0, /* Ack required (executing)*/
CCID_STATE_ACK_REQUIRED_1, /* Waiting user's ACK (execution finished) */
CCID_STATE_EXITED, /* CCID Thread Terminated */
CCID_STATE_EXEC_REQUESTED, /* Exec requested */
CCID_STATE_EXITED, /* CCID Thread Terminated */
CCID_STATE_EXEC_REQUESTED, /* Exec requested */
};
extern const uint8_t *ccid_atr;

175
src/usb/ccid/usb_common.h.notused
View file

@ -133,29 +133,29 @@ struct usb_endpoint_descriptor_long {
struct ccid_class_descriptor {
uint8_t bLength;
uint8_t bDescriptorType;
uint16_t bcdCCID;
uint8_t bMaxSlotIndex;
uint8_t bVoltageSupport;
uint32_t dwProtocols;
uint32_t dwDefaultClock;
uint32_t dwMaximumClock;
uint8_t bNumClockSupport;
uint32_t dwDataRate;
uint32_t dwMaxDataRate;
uint8_t bNumDataRatesSupported;
uint32_t dwMaxIFSD;
uint32_t dwSynchProtocols;
uint32_t dwMechanical;
uint32_t dwFeatures;
uint32_t dwMaxCCIDMessageLength;
uint8_t bClassGetResponse;
uint8_t bclassEnvelope;
uint16_t wLcdLayout;
uint8_t bPINSupport;
uint8_t bMaxCCIDBusySlots;
} __attribute__ ((__packed__));
uint8_t bLength;
uint8_t bDescriptorType;
uint16_t bcdCCID;
uint8_t bMaxSlotIndex;
uint8_t bVoltageSupport;
uint32_t dwProtocols;
uint32_t dwDefaultClock;
uint32_t dwMaximumClock;
uint8_t bNumClockSupport;
uint32_t dwDataRate;
uint32_t dwMaxDataRate;
uint8_t bNumDataRatesSupported;
uint32_t dwMaxIFSD;
uint32_t dwSynchProtocols;
uint32_t dwMechanical;
uint32_t dwFeatures;
uint32_t dwMaxCCIDMessageLength;
uint8_t bClassGetResponse;
uint8_t bclassEnvelope;
uint16_t wLcdLayout;
uint8_t bPINSupport;
uint8_t bMaxCCIDBusySlots;
} __attribute__((__packed__));
static const struct ccid_class_descriptor ccid_desc = {
.bLength = sizeof(struct ccid_class_descriptor),
@ -164,8 +164,8 @@ static const struct ccid_class_descriptor ccid_desc = {
.bMaxSlotIndex = 0,
.bVoltageSupport = 0x01, // 5.0V
.dwProtocols = (
0x01| // T=0
0x02), // T=1
0x01| // T=0
0x02), // T=1
.dwDefaultClock = (0xDFC),
.dwMaximumClock = (0xDFC),
.bNumClockSupport = 0,
@ -219,90 +219,90 @@ struct usb_device_configuration {
// EP0 IN and OUT
static const struct usb_endpoint_descriptor ep0_out = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP0_OUT_ADDR, // EP number 0, OUT from host (rx to device)
.bmAttributes = USB_TRANSFER_TYPE_CONTROL,
.wMaxPacketSize = 64,
.bInterval = 0
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP0_OUT_ADDR, // EP number 0, OUT from host (rx to device)
.bmAttributes = USB_TRANSFER_TYPE_CONTROL,
.wMaxPacketSize = 64,
.bInterval = 0
};
static const struct usb_endpoint_descriptor ep0_in = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP0_IN_ADDR, // EP number 0, OUT from host (rx to device)
.bmAttributes = USB_TRANSFER_TYPE_CONTROL,
.wMaxPacketSize = 64,
.bInterval = 0
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP0_IN_ADDR, // EP number 0, OUT from host (rx to device)
.bmAttributes = USB_TRANSFER_TYPE_CONTROL,
.wMaxPacketSize = 64,
.bInterval = 0
};
// Descriptors
static const struct usb_device_descriptor device_descriptor = {
.bLength = sizeof(struct usb_device_descriptor),
.bDescriptorType = USB_DT_DEVICE,
.bcdUSB = 0x0200, // USB 1.1 device
.bDeviceClass = 0, // Specified in interface descriptor
.bDeviceSubClass = 0, // No subclass
.bDeviceProtocol = 0, // No protocol
.bMaxPacketSize0 = 64, // Max packet size for ep0
.idVendor = 0x20a0, // Your vendor id
.idProduct = 0x4230, // Your product ID
.bcdDevice = 0x0101, // No device revision number
.iManufacturer = 1, // Manufacturer string index
.iProduct = 2, // Product string index
.iSerialNumber = 3, // No serial number
.bNumConfigurations = 1 // One configuration
.bLength = sizeof(struct usb_device_descriptor),
.bDescriptorType = USB_DT_DEVICE,
.bcdUSB = 0x0200, // USB 1.1 device
.bDeviceClass = 0, // Specified in interface descriptor
.bDeviceSubClass = 0, // No subclass
.bDeviceProtocol = 0, // No protocol
.bMaxPacketSize0 = 64, // Max packet size for ep0
.idVendor = 0x20a0, // Your vendor id
.idProduct = 0x4230, // Your product ID
.bcdDevice = 0x0101, // No device revision number
.iManufacturer = 1, // Manufacturer string index
.iProduct = 2, // Product string index
.iSerialNumber = 3, // No serial number
.bNumConfigurations = 1 // One configuration
};
static const struct usb_interface_descriptor interface_descriptor = {
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bAlternateSetting = 0,
.bNumEndpoints = 2, // Interface has 2 endpoints
.bInterfaceClass = 0x0b, // Vendor specific endpoint
.bInterfaceSubClass = 0,
.bInterfaceProtocol = 0,
.iInterface = 5
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bAlternateSetting = 0,
.bNumEndpoints = 2, // Interface has 2 endpoints
.bInterfaceClass = 0x0b, // Vendor specific endpoint
.bInterfaceSubClass = 0,
.bInterfaceProtocol = 0,
.iInterface = 5
};
static const struct usb_endpoint_descriptor ep1_out = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP1_OUT_ADDR, // EP number 1, OUT from host (rx to device)
.bmAttributes = USB_TRANSFER_TYPE_BULK,
.wMaxPacketSize = 64,
.bInterval = 0
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP1_OUT_ADDR, // EP number 1, OUT from host (rx to device)
.bmAttributes = USB_TRANSFER_TYPE_BULK,
.wMaxPacketSize = 64,
.bInterval = 0
};
static const struct usb_endpoint_descriptor ep2_in = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP2_IN_ADDR, // EP number 2, IN from host (tx from device)
.bmAttributes = USB_TRANSFER_TYPE_BULK,
.wMaxPacketSize = 64,
.bInterval = 0
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = EP2_IN_ADDR, // EP number 2, IN from host (tx from device)
.bmAttributes = USB_TRANSFER_TYPE_BULK,
.wMaxPacketSize = 64,
.bInterval = 0
};
static const struct usb_configuration_descriptor config_descriptor = {
.bLength = sizeof(struct usb_configuration_descriptor),
.bDescriptorType = USB_DT_CONFIG,
.wTotalLength = (sizeof(config_descriptor) +
sizeof(interface_descriptor) +
sizeof(ccid_desc) +
sizeof(ep1_out) +
sizeof(ep2_in)),
.bNumInterfaces = 1,
.bConfigurationValue = 1, // Configuration 1
.iConfiguration = 4, // No string
.bmAttributes = 0xa0, // attributes: self powered, no remote wakeup
.bMaxPower = 0x32 // 100ma
.bLength = sizeof(struct usb_configuration_descriptor),
.bDescriptorType = USB_DT_CONFIG,
.wTotalLength = (sizeof(config_descriptor) +
sizeof(interface_descriptor) +
sizeof(ccid_desc) +
sizeof(ep1_out) +
sizeof(ep2_in)),
.bNumInterfaces = 1,
.bConfigurationValue = 1, // Configuration 1
.iConfiguration = 4, // No string
.bmAttributes = 0xa0, // attributes: self powered, no remote wakeup
.bMaxPower = 0x32 // 100ma
};
static const unsigned char lang_descriptor[] = {
4, // bLength
0x03, // bDescriptorType == String Descriptor
0x09, 0x04 // language id = us english
4, // bLength
0x03, // bDescriptorType == String Descriptor
0x09, 0x04 // language id = us english
};
@ -322,4 +322,3 @@ extern bool usb_write_available();
extern uint32_t usb_write_flush();
#endif

55
src/usb/emulation/emulation.c
View file

@ -38,8 +38,7 @@ int msleep(long msec)
struct timespec ts;
int res;
if (msec < 0)
{
if (msec < 0) {
errno = EINVAL;
return -1;
}
@ -54,7 +53,8 @@ int msleep(long msec)
return res;
}
int emul_init(char *host, uint16_t port) {
int emul_init(char *host, uint16_t port)
{
struct sockaddr_in serv_addr;
fprintf(stderr, "\n Starting emulation envionrment\n");
if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
@ -72,38 +72,43 @@ int emul_init(char *host, uint16_t port) {
return -1;
}
if (connect(sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
if (connect(sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
fprintf(stderr, "\nConnection Failed \n");
return -1;
}
int x = fcntl(sock ,F_GETFL, 0);
int x = fcntl(sock, F_GETFL, 0);
fcntl(sock, F_SETFL, x | O_NONBLOCK);
return 0;
}
uint8_t *driver_prepare_response_emul() {
uint8_t *driver_prepare_response_emul()
{
apdu.rdata = usb_get_tx(ITF_EMUL);
return apdu.rdata;
}
int driver_write_emul(const uint8_t *buffer, size_t buffer_size) {
int driver_write_emul(const uint8_t *buffer, size_t buffer_size)
{
uint16_t size = htons(buffer_size);
//DEBUG_PAYLOAD(buffer,buffer_size);
int ret = 0;
do {
ret = send(sock, &size, sizeof(size), 0);
if (ret < 0)
if (ret < 0) {
msleep(10);
} while(ret <= 0);
}
} while (ret <= 0);
do {
ret = send(sock, buffer, (uint16_t)buffer_size, 0);
if (ret < 0)
ret = send(sock, buffer, (uint16_t) buffer_size, 0);
if (ret < 0) {
msleep(10);
} while(ret <= 0);
}
} while (ret <= 0);
return buffer_size;
}
uint32_t emul_write_offset(uint16_t size, uint16_t offset) {
uint32_t emul_write_offset(uint16_t size, uint16_t offset)
{
if (size > 0) {
//DEBUG_PAYLOAD(usb_get_tx(ITF_EMUL)+offset, size);
return usb_write_offset(ITF_EMUL, size, offset);
@ -111,29 +116,33 @@ uint32_t emul_write_offset(uint16_t size, uint16_t offset) {
return 0;
}
uint32_t emul_write(uint16_t size) {
uint32_t emul_write(uint16_t size)
{
return emul_write_offset(size, 0);
}
void driver_exec_finished_cont_emul(size_t size_next, size_t offset) {
void driver_exec_finished_cont_emul(size_t size_next, size_t offset)
{
emul_write_offset(size_next, offset);
}
int driver_process_usb_packet_emul(uint16_t len) {
int driver_process_usb_packet_emul(uint16_t len)
{
if (len > 0) {
uint8_t *data = usb_get_rx(ITF_EMUL), *rdata = usb_get_tx(ITF_EMUL);
if (len == 1) {
uint8_t c = data[0];
if (c == 4) {
if (ccid_atr)
if (ccid_atr) {
memcpy(rdata, ccid_atr+1, ccid_atr[0]);
}
emul_write(ccid_atr ? ccid_atr[0] : 0);
}
}
else {
} else {
DEBUG_PAYLOAD(data, len);
if (apdu_process(ITF_EMUL, data, len) > 0)
if (apdu_process(ITF_EMUL, data, len) > 0) {
process_apdu();
}
apdu_finish();
size_t ret = apdu_next();
DEBUG_PAYLOAD(rdata, ret);
@ -144,7 +153,8 @@ int driver_process_usb_packet_emul(uint16_t len) {
return 0;
}
uint16_t emul_read() {
uint16_t emul_read()
{
uint16_t len = 0;
fd_set input;
FD_ZERO(&input);
@ -165,8 +175,9 @@ uint16_t emul_read() {
if (len > 0) {
while (true) {
valread = recv(sock, usb_get_rx(ITF_EMUL), len, 0);
if (valread > 0)
if (valread > 0) {
return valread;
}
msleep(10);
}
}

54
src/usb/hid/ctap_hid.h
View file

@ -45,21 +45,21 @@ extern "C" {
#define TYPE_CONT 0x00 // Continuation frame identifier
typedef struct {
uint32_t cid; // Channel identifier
union {
uint8_t type; // Frame type - b7 defines type
struct {
uint8_t cmd; // Command - b7 set
uint8_t bcnth; // Message byte count - high part
uint8_t bcntl; // Message byte count - low part
uint8_t data[HID_RPT_SIZE - 7]; // Data payload
} init;
struct {
uint8_t seq; // Sequence number - b7 cleared
uint8_t data[HID_RPT_SIZE - 5]; // Data payload
} cont;
};
} __attribute__ ((__packed__)) CTAPHID_FRAME;
uint32_t cid; // Channel identifier
union {
uint8_t type; // Frame type - b7 defines type
struct {
uint8_t cmd; // Command - b7 set
uint8_t bcnth; // Message byte count - high part
uint8_t bcntl; // Message byte count - low part
uint8_t data[HID_RPT_SIZE - 7]; // Data payload
} init;
struct {
uint8_t seq; // Sequence number - b7 cleared
uint8_t data[HID_RPT_SIZE - 5]; // Data payload
} cont;
};
} __attribute__((__packed__)) CTAPHID_FRAME;
extern CTAPHID_FRAME *ctap_req, *ctap_resp;
@ -108,27 +108,27 @@ extern CTAPHID_FRAME *ctap_req, *ctap_resp;
#define CAPFLAG_CBOR 0x04 // Device supports CBOR command
typedef struct {
uint8_t nonce[INIT_NONCE_SIZE]; // Client application nonce
} __attribute__ ((__packed__)) CTAPHID_INIT_REQ;
uint8_t nonce[INIT_NONCE_SIZE]; // Client application nonce
} __attribute__((__packed__)) CTAPHID_INIT_REQ;
typedef struct {
uint8_t nonce[INIT_NONCE_SIZE]; // Client application nonce
uint32_t cid; // Channel identifier
uint8_t versionInterface; // Interface version
uint8_t versionMajor; // Major version number
uint8_t versionMinor; // Minor version number
uint8_t versionBuild; // Build version number
uint8_t capFlags; // Capabilities flags
} __attribute__ ((__packed__)) CTAPHID_INIT_RESP;
uint8_t nonce[INIT_NONCE_SIZE]; // Client application nonce
uint32_t cid; // Channel identifier
uint8_t versionInterface; // Interface version
uint8_t versionMajor; // Major version number
uint8_t versionMinor; // Minor version number
uint8_t versionBuild; // Build version number
uint8_t capFlags; // Capabilities flags
} __attribute__((__packed__)) CTAPHID_INIT_RESP;
// CTAPHID_SYNC command defines
typedef struct {
uint8_t nonce; // Client application nonce
uint8_t nonce; // Client application nonce
} CTAPHID_SYNC_REQ;
typedef struct {
uint8_t nonce; // Client application nonce
uint8_t nonce; // Client application nonce
} CTAPHID_SYNC_RESP;
// Low-level error codes. Return as negatives.

276
src/usb/hid/hid.c
View file

@ -31,7 +31,7 @@ typedef struct msg_packet {
uint16_t len;
uint16_t current_len;
uint8_t data[CTAP_MAX_PACKET_SIZE];
} __attribute__ ((__packed__)) msg_packet_t;
} __attribute__((__packed__)) msg_packet_t;
msg_packet_t msg_packet = { 0 };
@ -40,18 +40,20 @@ void tud_mount_cb()
mounted = true;
}
bool driver_mounted_hid() {
bool driver_mounted_hid()
{
return mounted;
}
CTAPHID_FRAME *ctap_req = NULL, *ctap_resp = NULL;
int driver_init_hid() {
int driver_init_hid()
{
tud_init(BOARD_TUD_RHPORT);
ctap_req = (CTAPHID_FRAME *)usb_get_rx(ITF_HID);
ctap_req = (CTAPHID_FRAME *) usb_get_rx(ITF_HID);
apdu.header = ctap_req->init.data;
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
apdu.rdata = ctap_resp->init.data;
usb_set_timeout_counter(ITF_HID, 200);
@ -66,29 +68,36 @@ int driver_init_hid() {
// Invoked when received GET_REPORT control request
// Application must fill buffer report's content and return its length.
// Return zero will cause the stack to STALL request
uint16_t tud_hid_get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen)
uint16_t tud_hid_get_report_cb(uint8_t itf,
uint8_t report_id,
hid_report_type_t report_type,
uint8_t *buffer,
uint16_t reqlen)
{
// TODO not Implemented
(void) itf;
(void) report_id;
(void) report_type;
(void) buffer;
(void) reqlen;
printf("get_report %d %d %d\n",itf,report_id,report_type);
DEBUG_PAYLOAD(buffer, reqlen);
buffer[1] = HSM_SDK_VERSION_MAJOR;
buffer[2] = HSM_SDK_VERSION_MINOR;
// TODO not Implemented
(void) itf;
(void) report_id;
(void) report_type;
(void) buffer;
(void) reqlen;
printf("get_report %d %d %d\n", itf, report_id, report_type);
DEBUG_PAYLOAD(buffer, reqlen);
buffer[1] = HSM_SDK_VERSION_MAJOR;
buffer[2] = HSM_SDK_VERSION_MINOR;
return reqlen;
return reqlen;
}
uint32_t hid_write_offset(uint16_t size, uint16_t offset) {
if (*usb_get_tx(ITF_HID) != 0x81)
uint32_t hid_write_offset(uint16_t size, uint16_t offset)
{
if (*usb_get_tx(ITF_HID) != 0x81) {
DEBUG_PAYLOAD(usb_get_tx(ITF_HID)+offset, size);
}
return usb_write_offset(ITF_HID, size, offset);
}
uint32_t hid_write(uint16_t size) {
uint32_t hid_write(uint16_t size)
{
return hid_write_offset(size, 0);
}
@ -101,36 +110,41 @@ static const uint8_t conv_table[128][2] = { HID_ASCII_TO_KEYCODE };
static uint8_t keyboard_w = 0;
static bool sent_key = false;
void add_keyboard_buffer(const uint8_t *data, size_t data_len) {
void add_keyboard_buffer(const uint8_t *data, size_t data_len)
{
keyboard_buffer_len = MIN(sizeof(keyboard_buffer), data_len);
memcpy(keyboard_buffer, data, keyboard_buffer_len);
}
static void send_hid_report(uint8_t report_id) {
if (!tud_hid_ready())
static void send_hid_report(uint8_t report_id)
{
if (!tud_hid_ready()) {
return;
}
switch(report_id) {
switch (report_id) {
case REPORT_ID_KEYBOARD: {
if (keyboard_w < keyboard_buffer_len) {
if (sent_key == false) {
uint8_t keycode[6] = { 0 };
uint8_t modifier = 0;
uint8_t chr = keyboard_buffer[keyboard_w];
if (conv_table[chr][0])
if (conv_table[chr][0]) {
modifier = KEYBOARD_MODIFIER_LEFTSHIFT;
}
keycode[0] = conv_table[chr][1];
if (tud_hid_n_keyboard_report(ITF_KEYBOARD, REPORT_ID_KEYBOARD, modifier, keycode) == true)
if (tud_hid_n_keyboard_report(ITF_KEYBOARD, REPORT_ID_KEYBOARD, modifier,
keycode) == true) {
sent_key = true;
}
else {
if (tud_hid_n_keyboard_report(ITF_KEYBOARD, REPORT_ID_KEYBOARD, 0, NULL) == true) {
}
} else {
if (tud_hid_n_keyboard_report(ITF_KEYBOARD, REPORT_ID_KEYBOARD, 0,
NULL) == true) {
keyboard_w++;
sent_key = false;
}
}
}
else if (keyboard_w == keyboard_buffer_len && keyboard_buffer_len > 0) {
} else if (keyboard_w == keyboard_buffer_len && keyboard_buffer_len > 0) {
keyboard_w = keyboard_buffer_len = 0;
}
}
@ -140,66 +154,78 @@ static void send_hid_report(uint8_t report_id) {
}
}
void hid_task(void) {
void hid_task(void)
{
// Poll every 10ms
const uint32_t interval_ms = 10;
static uint32_t start_ms = 0;
if (board_millis() - start_ms < interval_ms)
if (board_millis() - start_ms < interval_ms) {
return;
}
start_ms += interval_ms;
// Remote wakeup
if ( tud_suspended() && keyboard_buffer_len > 0) {
if (tud_suspended() && keyboard_buffer_len > 0) {
tud_remote_wakeup();
}
else {
} else {
send_hid_report(REPORT_ID_KEYBOARD);
}
}
void tud_hid_report_complete_cb(uint8_t instance, uint8_t const* report, /*uint16_t*/ uint8_t len) {
void tud_hid_report_complete_cb(uint8_t instance, uint8_t const *report, /*uint16_t*/ uint8_t len)
{
if (send_buffer_size > 0 && instance == ITF_HID) {
uint8_t seq = report[4] & TYPE_MASK ? 0 : report[4] + 1;
if (last_write_result == true) {
ctap_resp->cid = ctap_req->cid;
ctap_resp->cont.seq = seq;
}
if (hid_write_offset(64, (uint8_t *)ctap_resp - (usb_get_tx(ITF_HID))) > 0) {
if (hid_write_offset(64, (uint8_t *) ctap_resp - (usb_get_tx(ITF_HID))) > 0) {
send_buffer_size -= MIN(64 - 5, send_buffer_size);
ctap_resp = (CTAPHID_FRAME *)((uint8_t *)ctap_resp + 64 - 5);
ctap_resp = (CTAPHID_FRAME *) ((uint8_t *) ctap_resp + 64 - 5);
}
}
}
int driver_write_hid(const uint8_t *buffer, size_t buffer_size) {
int driver_write_hid(const uint8_t *buffer, size_t buffer_size)
{
last_write_result = tud_hid_n_report(ITF_HID, 0, buffer, buffer_size);
printf("result %d\n", last_write_result);
if (last_write_result == false)
if (last_write_result == false) {
return 0;
}
return MIN(64, buffer_size);
}
size_t driver_read_hid(uint8_t *buffer, size_t buffer_size) {
size_t driver_read_hid(uint8_t *buffer, size_t buffer_size)
{
return 0;
}
// Invoked when received SET_REPORT control request or
// received data on OUT endpoint ( Report ID = 0, Type = 0 )
void tud_hid_set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize) {
void tud_hid_set_report_cb(uint8_t itf,
uint8_t report_id,
hid_report_type_t report_type,
uint8_t const *buffer,
uint16_t bufsize)
{
// This example doesn't use multiple report and report ID
(void) itf;
(void) report_id;
(void) report_type;
printf("set_report %d %d %d\n",itf,report_id,report_type);
if (itf == ITF_KEYBOARD)
DEBUG_PAYLOAD(buffer,bufsize);
printf("set_report %d %d %d\n", itf, report_id, report_type);
if (itf == ITF_KEYBOARD) {
DEBUG_PAYLOAD(buffer, bufsize);
}
usb_rx(itf, buffer, bufsize);
}
uint32_t last_cmd_time = 0, last_packet_time = 0;
int ctap_error(uint8_t error) {
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
int ctap_error(uint8_t error)
{
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
memset(ctap_resp, 0, sizeof(CTAPHID_FRAME));
ctap_resp->cid = ctap_req->cid;
ctap_resp->init.cmd = CTAPHID_ERROR;
@ -220,7 +246,8 @@ uint8_t thread_type = 0; //1 is APDU, 2 is CBOR
extern void cbor_thread();
extern bool cancel_button;
int driver_process_usb_nopacket_hid() {
int driver_process_usb_nopacket_hid()
{
if (last_packet_time > 0 && last_packet_time+500 < board_millis()) {
ctap_error(CTAP1_ERR_MSG_TIMEOUT);
last_packet_time = 0;
@ -231,32 +258,38 @@ int driver_process_usb_nopacket_hid() {
extern const uint8_t fido_aid[];
int driver_process_usb_packet_hid(uint16_t read) {
int driver_process_usb_packet_hid(uint16_t read)
{
int apdu_sent = 0;
if (read >= 5) {
driver_init_hid();
last_packet_time = board_millis();
DEBUG_PAYLOAD(usb_get_rx(ITF_HID),64);
DEBUG_PAYLOAD(usb_get_rx(ITF_HID), 64);
memset(ctap_resp, 0, sizeof(CTAPHID_FRAME));
if (ctap_req->cid == 0x0 || (ctap_req->cid == CID_BROADCAST && ctap_req->init.cmd != CTAPHID_INIT))
if (ctap_req->cid == 0x0 ||
(ctap_req->cid == CID_BROADCAST && ctap_req->init.cmd != CTAPHID_INIT)) {
return ctap_error(CTAP1_ERR_INVALID_CHANNEL);
if (board_millis() < lock && ctap_req->cid != last_req.cid && last_cmd_time+100 > board_millis())
}
if (board_millis() < lock && ctap_req->cid != last_req.cid &&
last_cmd_time+100 > board_millis()) {
return ctap_error(CTAP1_ERR_CHANNEL_BUSY);
if (FRAME_TYPE(ctap_req) == TYPE_INIT)
{
if (MSG_LEN(ctap_req) > CTAP_MAX_PACKET_SIZE)
}
if (FRAME_TYPE(ctap_req) == TYPE_INIT) {
if (MSG_LEN(ctap_req) > CTAP_MAX_PACKET_SIZE) {
return ctap_error(CTAP1_ERR_INVALID_LEN);
if (msg_packet.len > 0 && last_cmd_time+100 > board_millis() && ctap_req->init.cmd != CTAPHID_INIT) {
if (last_req.cid != ctap_req->cid) //We are in a transaction
}
if (msg_packet.len > 0 && last_cmd_time+100 > board_millis() &&
ctap_req->init.cmd != CTAPHID_INIT) {
if (last_req.cid != ctap_req->cid) { //We are in a transaction
return ctap_error(CTAP1_ERR_CHANNEL_BUSY);
else
} else {
return ctap_error(CTAP1_ERR_INVALID_SEQ);
}
}
printf("command %x\n", FRAME_CMD(ctap_req));
printf("len %d\n", MSG_LEN(ctap_req));
msg_packet.len = msg_packet.current_len = 0;
if (MSG_LEN(ctap_req) > 64 - 7)
{
if (MSG_LEN(ctap_req) > 64 - 7) {
msg_packet.len = MSG_LEN(ctap_req);
memcpy(msg_packet.data + msg_packet.current_len, ctap_req->init.data, 64-7);
msg_packet.current_len += 64 - 7;
@ -265,29 +298,31 @@ int driver_process_usb_packet_hid(uint16_t read) {
last_cmd = ctap_req->init.cmd;
last_seq = 0;
last_cmd_time = board_millis();
}
else {
if (msg_packet.len == 0) //Received a cont with a prior init pkt
} else {
if (msg_packet.len == 0) { //Received a cont with a prior init pkt
return 0;
if (last_seq != ctap_req->cont.seq)
}
if (last_seq != ctap_req->cont.seq) {
return ctap_error(CTAP1_ERR_INVALID_SEQ);
}
if (last_req.cid == ctap_req->cid) {
memcpy(msg_packet.data + msg_packet.current_len, ctap_req->cont.data, MIN(64 - 5, msg_packet.len - msg_packet.current_len));
memcpy(msg_packet.data + msg_packet.current_len, ctap_req->cont.data,
MIN(64 - 5, msg_packet.len - msg_packet.current_len));
msg_packet.current_len += MIN(64 - 5, msg_packet.len - msg_packet.current_len);
memcpy(&last_req, ctap_req, sizeof(CTAPHID_FRAME));
last_seq++;
}
else if (last_cmd_time+100 > board_millis())
} else if (last_cmd_time+100 > board_millis()) {
return ctap_error(CTAP1_ERR_CHANNEL_BUSY);
}
}
if (ctap_req->init.cmd == CTAPHID_INIT) {
init_fido();
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
memset(ctap_resp, 0, 64);
CTAPHID_INIT_REQ *req = (CTAPHID_INIT_REQ *)ctap_req->init.data;
CTAPHID_INIT_RESP *resp = (CTAPHID_INIT_RESP *)ctap_resp->init.data;
CTAPHID_INIT_REQ *req = (CTAPHID_INIT_REQ *) ctap_req->init.data;
CTAPHID_INIT_RESP *resp = (CTAPHID_INIT_RESP *) ctap_resp->init.data;
memcpy(resp->nonce, req->nonce, sizeof(resp->nonce));
resp->cid = 0x01000000;
resp->versionInterface = CTAPHID_IF_VERSION;
@ -302,25 +337,24 @@ int driver_process_usb_packet_hid(uint16_t read) {
hid_write(64);
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
}
else if (ctap_req->init.cmd == CTAPHID_WINK) {
} else if (ctap_req->init.cmd == CTAPHID_WINK) {
if (MSG_LEN(ctap_req) != 0) {
return ctap_error(CTAP1_ERR_INVALID_LEN);
}
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
memcpy(ctap_resp, ctap_req, sizeof(CTAPHID_FRAME));
sleep_ms(1000); //For blinking the device during 1 seg
hid_write(64);
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
}
else if ((last_cmd == CTAPHID_PING || last_cmd == CTAPHID_SYNC) && (msg_packet.len == 0 || (msg_packet.len == msg_packet.current_len && msg_packet.len > 0))) {
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
} else if ((last_cmd == CTAPHID_PING || last_cmd == CTAPHID_SYNC) &&
(msg_packet.len == 0 ||
(msg_packet.len == msg_packet.current_len && msg_packet.len > 0))) {
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
if (msg_packet.current_len == msg_packet.len && msg_packet.len > 0) {
memcpy(ctap_resp->init.data, msg_packet.data, msg_packet.len);
driver_exec_finished_hid(msg_packet.len);
}
else {
} else {
memcpy(ctap_resp->init.data, ctap_req->init.data, MSG_LEN(ctap_req));
ctap_resp->cid = ctap_req->cid;
ctap_resp->init.cmd = last_cmd;
@ -330,24 +364,27 @@ int driver_process_usb_packet_hid(uint16_t read) {
}
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
}
else if (ctap_req->init.cmd == CTAPHID_LOCK) {
if (MSG_LEN(ctap_req) != 1)
} else if (ctap_req->init.cmd == CTAPHID_LOCK) {
if (MSG_LEN(ctap_req) != 1) {
return ctap_error(CTAP1_ERR_INVALID_LEN);
if (ctap_req->init.data[0] > 10)
}
if (ctap_req->init.data[0] > 10) {
return ctap_error(CTAP1_ERR_INVALID_PARAMETER);
}
lock = board_millis() + ctap_req->init.data[0] * 1000;
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
memset(ctap_resp, 0, 64);
ctap_resp->cid = ctap_req->cid;
ctap_resp->init.cmd = ctap_req->init.cmd;
hid_write(64);
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
}
else if (last_cmd == CTAPHID_MSG && (msg_packet.len == 0 || (msg_packet.len == msg_packet.current_len && msg_packet.len > 0))) {
} else if (last_cmd == CTAPHID_MSG &&
(msg_packet.len == 0 ||
(msg_packet.len == msg_packet.current_len && msg_packet.len > 0))) {
if (current_app == NULL || memcmp(current_app->aid, fido_aid+1, MIN(current_app->aid[0], fido_aid[0])) != 0) {
if (current_app == NULL ||
memcmp(current_app->aid, fido_aid+1, MIN(current_app->aid[0], fido_aid[0])) != 0) {
for (int a = 0; a < num_apps; a++) {
if ((current_app = apps[a].select_aid(&apps[a], fido_aid+1, fido_aid[0]))) {
break;
@ -355,40 +392,44 @@ int driver_process_usb_packet_hid(uint16_t read) {
}
}
//if (thread_type != 1)
card_start(apdu_thread);
card_start(apdu_thread);
thread_type = 1;
if (msg_packet.current_len == msg_packet.len && msg_packet.len > 0)
if (msg_packet.current_len == msg_packet.len && msg_packet.len > 0) {
apdu_sent = apdu_process(ITF_HID, msg_packet.data, msg_packet.len);
else
} else {
apdu_sent = apdu_process(ITF_HID, ctap_req->init.data, MSG_LEN(ctap_req));
DEBUG_PAYLOAD(apdu.data, (int)apdu.nc);
}
DEBUG_PAYLOAD(apdu.data, (int) apdu.nc);
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
}
else if ((last_cmd == CTAPHID_CBOR || (last_cmd >= CTAPHID_VENDOR_FIRST && last_cmd <= CTAPHID_VENDOR_LAST)) && (msg_packet.len == 0 || (msg_packet.len == msg_packet.current_len && msg_packet.len > 0))) {
} else if ((last_cmd == CTAPHID_CBOR ||
(last_cmd >= CTAPHID_VENDOR_FIRST && last_cmd <= CTAPHID_VENDOR_LAST)) &&
(msg_packet.len == 0 ||
(msg_packet.len == msg_packet.current_len && msg_packet.len > 0))) {
//if (thread_type != 2)
card_start(cbor_thread);
card_start(cbor_thread);
thread_type = 2;
if (msg_packet.current_len == msg_packet.len && msg_packet.len > 0)
if (msg_packet.current_len == msg_packet.len && msg_packet.len > 0) {
apdu_sent = cbor_process(last_cmd, msg_packet.data, msg_packet.len);
else
} else {
apdu_sent = cbor_process(last_cmd, ctap_req->init.data, MSG_LEN(ctap_req));
}
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
if (apdu_sent < 0)
if (apdu_sent < 0) {
return ctap_error(-apdu_sent);
}
else if (ctap_req->init.cmd == CTAPHID_CANCEL) {
}
} else if (ctap_req->init.cmd == CTAPHID_CANCEL) {
ctap_error(0x2D);
msg_packet.len = msg_packet.current_len = 0;
last_packet_time = 0;
cancel_button = true;
}
else {
if (msg_packet.len == 0)
} else {
if (msg_packet.len == 0) {
return ctap_error(CTAP1_ERR_INVALID_CMD);
}
}
// echo back anything we received from host
//tud_hid_report(0, buffer, bufsize);
@ -398,8 +439,9 @@ int driver_process_usb_packet_hid(uint16_t read) {
return apdu_sent;
}
void send_keepalive() {
CTAPHID_FRAME *resp = (CTAPHID_FRAME *)(usb_get_tx(ITF_HID) + 4096);
void send_keepalive()
{
CTAPHID_FRAME *resp = (CTAPHID_FRAME *) (usb_get_tx(ITF_HID) + 4096);
//memset(ctap_resp, 0, sizeof(CTAPHID_FRAME));
resp->cid = ctap_req->cid;
resp->init.cmd = CTAPHID_KEEPALIVE;
@ -409,39 +451,45 @@ void send_keepalive() {
hid_write_offset(64, 4096);
}
void driver_exec_timeout_hid() {
if (thread_type == 2)
void driver_exec_timeout_hid()
{
if (thread_type == 2) {
send_keepalive();
}
}
uint8_t *driver_prepare_response_hid() {
ctap_resp = (CTAPHID_FRAME *)usb_get_tx(ITF_HID);
uint8_t *driver_prepare_response_hid()
{
ctap_resp = (CTAPHID_FRAME *) usb_get_tx(ITF_HID);
apdu.rdata = ctap_resp->init.data;
send_buffer_size = 0;
memset(usb_get_tx(ITF_HID), 0, 4096);
return ctap_resp->init.data;
}
void driver_exec_finished_hid(size_t size_next) {
void driver_exec_finished_hid(size_t size_next)
{
if (size_next > 0) {
if (thread_type == 2 && apdu.sw != 0)
if (thread_type == 2 && apdu.sw != 0) {
ctap_error(apdu.sw & 0xff);
else
} else {
driver_exec_finished_cont_hid(size_next, 7);
}
}
apdu.sw = 0;
}
void driver_exec_finished_cont_hid(size_t size_next, size_t offset) {
void driver_exec_finished_cont_hid(size_t size_next, size_t offset)
{
offset -= 7;
ctap_resp = (CTAPHID_FRAME *)(usb_get_tx(ITF_HID) + offset);
ctap_resp = (CTAPHID_FRAME *) (usb_get_tx(ITF_HID) + offset);
ctap_resp->cid = ctap_req->cid;
ctap_resp->init.cmd = last_cmd;
ctap_resp->init.bcnth = size_next >> 8;
ctap_resp->init.bcntl = size_next & 0xff;
send_buffer_size = size_next;
if (hid_write_offset(64, offset) > 0) {
ctap_resp = (CTAPHID_FRAME *)((uint8_t *)ctap_resp + 64 - 5);
ctap_resp = (CTAPHID_FRAME *) ((uint8_t *) ctap_resp + 64 - 5);
send_buffer_size -= MIN(64-7, send_buffer_size);
}

10
src/usb/tusb_config.h
View file

@ -29,7 +29,7 @@
#include "usb.h"
#ifdef __cplusplus
extern "C" {
extern "C" {
#endif
//--------------------------------------------------------------------+
@ -55,7 +55,8 @@
#error CFG_TUSB_MCU must be defined
#endif
#if CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX || \
#if CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || \
CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX || \
CFG_TUSB_MCU == OPT_MCU_NUC505 || CFG_TUSB_MCU == OPT_MCU_CXD56
#define CFG_TUSB_RHPORT0_MODE (OPT_MODE_DEVICE | OPT_MODE_HIGH_SPEED)
#else
@ -88,7 +89,7 @@
#endif
#ifndef CFG_TUSB_MEM_ALIGN
#define CFG_TUSB_MEM_ALIGN __attribute__ ((aligned(4)))
#define CFG_TUSB_MEM_ALIGN __attribute__((aligned(4)))
#endif
//--------------------------------------------------------------------
@ -121,8 +122,7 @@
#define CFG_TUD_HID_EP_BUFSIZE 64
#ifdef __cplusplus
}
}
#endif
#endif /* _TUSB_CONFIG_H_ */

148
src/usb/usb.c
View file

@ -1,4 +1,3 @@
/*
* This file is part of the Pico HSM SDK distribution (https://github.com/polhenarejos/pico-hsm-sdk).
* Copyright (c) 2022 Pol Henarejos.
@ -34,76 +33,89 @@
#include <stdlib.h>
// Device specific functions
static uint8_t rx_buffer[ITF_TOTAL][4096] = {0}, tx_buffer[ITF_TOTAL][4096+64] = {0};
static uint16_t w_offset[ITF_TOTAL] = {0}, r_offset[ITF_TOTAL] = {0};
static uint16_t w_len[ITF_TOTAL] = {0}, tx_r_offset[ITF_TOTAL] = {0};
static uint32_t timeout_counter[ITF_TOTAL] = {0};
static uint8_t rx_buffer[ITF_TOTAL][4096] = { 0 }, tx_buffer[ITF_TOTAL][4096+64] = { 0 };
static uint16_t w_offset[ITF_TOTAL] = { 0 }, r_offset[ITF_TOTAL] = { 0 };
static uint16_t w_len[ITF_TOTAL] = { 0 }, tx_r_offset[ITF_TOTAL] = { 0 };
static uint32_t timeout_counter[ITF_TOTAL] = { 0 };
uint8_t card_locked_itf = ITF_TOTAL; // no locked
void usb_set_timeout_counter(uint8_t itf, uint32_t v) {
void usb_set_timeout_counter(uint8_t itf, uint32_t v)
{
timeout_counter[itf] = v;
}
uint32_t usb_write_offset(uint8_t itf, uint16_t len, uint16_t offset) {
uint32_t usb_write_offset(uint8_t itf, uint16_t len, uint16_t offset)
{
#ifndef ENABLE_EMULATION
uint8_t pkt_max = 64;
#endif
int w = 0;
if (len > sizeof(tx_buffer[itf]))
if (len > sizeof(tx_buffer[itf])) {
len = sizeof(tx_buffer[itf]);
}
w_len[itf] = len;
tx_r_offset[itf] = offset;
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
w = driver_write_hid(tx_buffer[itf]+offset, MIN(len, pkt_max));
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
w = driver_write_ccid(tx_buffer[itf]+offset, MIN(len, pkt_max));
}
#endif
#ifdef ENABLE_EMULATION
if (itf == ITF_EMUL)
if (itf == ITF_EMUL) {
w = driver_write_emul(tx_buffer[itf]+offset, len);
}
#endif
w_len[itf] -= w;
tx_r_offset[itf] += w;
return w;
}
size_t usb_rx(uint8_t itf, const uint8_t *buffer, size_t len) {
size_t usb_rx(uint8_t itf, const uint8_t *buffer, size_t len)
{
uint16_t size = MIN(sizeof(rx_buffer[itf]) - w_offset[itf], len);
if (size > 0) {
if (buffer == NULL) {
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
size = driver_read_hid(rx_buffer[itf] + w_offset[itf], size);
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
size = driver_read_ccid(rx_buffer[itf] + w_offset[itf], size);
}
#endif
}
else
} else {
memcpy(rx_buffer[itf] + w_offset[itf], buffer, size);
}
w_offset[itf] += size;
}
return size;
}
uint32_t usb_write_flush(uint8_t itf) {
uint32_t usb_write_flush(uint8_t itf)
{
int w = 0;
if (w_len[itf] > 0) {
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
w = driver_write_hid(tx_buffer[itf]+tx_r_offset[itf], MIN(w_len[itf], 64));
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
w = driver_write_ccid(tx_buffer[itf]+tx_r_offset[itf], MIN(w_len[itf], 64));
}
#endif
#ifdef ENABLE_EMULATION
if (itf == ITF_EMUL)
if (itf == ITF_EMUL) {
w = driver_write_emul(tx_buffer[itf]+tx_r_offset[itf], w_len[itf]);
}
#endif
tx_r_offset[itf] += w;
w_len[itf] -= w;
@ -111,27 +123,33 @@ uint32_t usb_write_flush(uint8_t itf) {
return w;
}
uint32_t usb_write(uint8_t itf, uint16_t len) {
uint32_t usb_write(uint8_t itf, uint16_t len)
{
return usb_write_offset(itf, len, 0);
}
uint16_t usb_read_available(uint8_t itf) {
uint16_t usb_read_available(uint8_t itf)
{
return w_offset[itf] - r_offset[itf];
}
uint16_t usb_write_available(uint8_t itf) {
uint16_t usb_write_available(uint8_t itf)
{
return w_len[itf] > 0;
}
uint8_t *usb_get_rx(uint8_t itf) {
uint8_t *usb_get_rx(uint8_t itf)
{
return rx_buffer[itf];
}
uint8_t *usb_get_tx(uint8_t itf) {
uint8_t *usb_get_tx(uint8_t itf)
{
return tx_buffer[itf];
}
void usb_clear_rx(uint8_t itf) {
void usb_clear_rx(uint8_t itf)
{
w_offset[itf] = r_offset[itf] = 0;
}
@ -149,7 +167,8 @@ queue_t usb_to_card_q;
queue_t card_to_usb_q;
#endif
void usb_init() {
void usb_init()
{
#ifndef ENABLE_EMULATION
queue_init(&card_to_usb_q, sizeof(uint32_t), 64);
queue_init(&usb_to_card_q, sizeof(uint32_t), 64);
@ -159,7 +178,8 @@ void usb_init() {
extern int driver_process_usb_nopacket();
extern uint32_t timeout;
static int usb_event_handle(uint8_t itf) {
static int usb_event_handle(uint8_t itf)
{
#ifndef ENABLE_EMULATION
uint16_t rx_read = usb_read_available(itf);
#else
@ -167,16 +187,19 @@ static int usb_event_handle(uint8_t itf) {
#endif
int proc_packet = 0;
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
proc_packet = driver_process_usb_packet_hid(rx_read);
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
proc_packet = driver_process_usb_packet_ccid(rx_read);
}
#endif
#ifdef ENABLE_EMULATION
if (itf == ITF_EMUL)
if (itf == ITF_EMUL) {
proc_packet = driver_process_usb_packet_emul(rx_read);
}
#endif
if (proc_packet > 0) {
card_locked_itf = itf;
@ -185,22 +208,24 @@ static int usb_event_handle(uint8_t itf) {
queue_add_blocking(&usb_to_card_q, &flag);
#endif
timeout_start();
}
else {
} else {
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
driver_process_usb_nopacket_hid();
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
driver_process_usb_nopacket_ccid();
}
#endif
}
return 0;
}
extern void low_flash_init();
void card_init_core1() {
void card_init_core1()
{
#ifndef ENABLE_EMULATION
low_flash_init_core1();
#endif
@ -208,16 +233,19 @@ void card_init_core1() {
size_t finished_data_size = 0;
void card_start(void (*func)(void)) {
void card_start(void (*func)(void))
{
#ifndef ENABLE_EMULATION
uint32_t m = 0;
while (queue_is_empty(&usb_to_card_q) == false) {
if (queue_try_remove(&usb_to_card_q, &m) == false)
if (queue_try_remove(&usb_to_card_q, &m) == false) {
break;
}
}
while (queue_is_empty(&card_to_usb_q) == false) {
if (queue_try_remove(&card_to_usb_q, &m) == false)
if (queue_try_remove(&card_to_usb_q, &m) == false) {
break;
}
}
multicore_reset_core1();
multicore_launch_core1(func);
@ -225,7 +253,8 @@ void card_start(void (*func)(void)) {
#endif
}
void card_exit() {
void card_exit()
{
#ifndef ENABLE_EMULATION
uint32_t flag = EV_EXIT;
queue_try_add(&usb_to_card_q, &flag);
@ -234,7 +263,8 @@ void card_exit() {
card_locked_itf = ITF_TOTAL;
}
extern void hid_task();
void usb_task() {
void usb_task()
{
#ifndef ENABLE_EMULATION
bool mounted = false;
#else
@ -242,12 +272,14 @@ void usb_task() {
#endif
for (uint8_t itf = 0; itf < ITF_TOTAL; itf++) {
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
mounted = driver_mounted_hid();
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
mounted = driver_mounted_ccid();
}
#endif
if (mounted == true) {
@ -265,31 +297,33 @@ void usb_task() {
if (m == EV_EXEC_FINISHED) {
timeout_stop();
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
driver_exec_finished_hid(finished_data_size);
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
driver_exec_finished_ccid(finished_data_size);
}
#endif
led_set_blink(BLINK_MOUNTED);
card_locked_itf = ITF_TOTAL;
}
else if (m == EV_PRESS_BUTTON) {
} else if (m == EV_PRESS_BUTTON) {
uint32_t flag = wait_button() ? EV_BUTTON_TIMEOUT : EV_BUTTON_PRESSED;
queue_try_add(&usb_to_card_q, &flag);
}
}
else {
} else {
if (timeout > 0) {
if (timeout + timeout_counter[itf] < board_millis()) {
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
driver_exec_timeout_hid();
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
driver_exec_timeout_ccid();
}
#endif
timeout = board_millis();
}
@ -305,18 +339,22 @@ void usb_task() {
}
uint8_t *usb_prepare_response(uint8_t itf) {
uint8_t *usb_prepare_response(uint8_t itf)
{
#ifdef USB_ITF_HID
if (itf == ITF_HID)
if (itf == ITF_HID) {
return driver_prepare_response_hid();
}
#endif
#ifdef USB_ITF_CCID
if (itf == ITF_CCID)
if (itf == ITF_CCID) {
return driver_prepare_response_ccid();
}
#endif
#ifdef ENABLE_EMULATION
if (itf == ITF_EMUL)
if (itf == ITF_EMUL) {
return driver_prepare_response_emul();
}
#endif
return NULL;
}

3
src/usb/usb.h
View file

@ -56,8 +56,7 @@ enum {
ITF_TOTAL
};
enum
{
enum {
REPORT_ID_KEYBOARD = 1,
REPORT_ID_COUNT
};

156
src/usb/usb_descriptors.c
View file

@ -32,7 +32,7 @@
#define USB_CONFIG_ATT_ONE TU_BIT(7)
#define MAX_USB_POWER 1
#define MAX_USB_POWER 1
//--------------------------------------------------------------------+
@ -60,7 +60,7 @@ tusb_desc_device_t const desc_device =
.bNumConfigurations = 1
};
uint8_t const * tud_descriptor_device_cb(void)
uint8_t const *tud_descriptor_device_cb(void)
{
return (uint8_t const *) &desc_device;
}
@ -73,20 +73,21 @@ uint8_t const * tud_descriptor_device_cb(void)
tusb_desc_configuration_t const desc_config =
{
.bLength = sizeof(tusb_desc_configuration_t),
.bDescriptorType = TUSB_DESC_CONFIGURATION,
.bDescriptorType = TUSB_DESC_CONFIGURATION,
.wTotalLength = (sizeof(tusb_desc_configuration_t)
#ifdef USB_ITF_CCID
+ sizeof(tusb_desc_interface_t) + sizeof(struct ccid_class_descriptor) + 2*sizeof(tusb_desc_endpoint_t)
+ sizeof(tusb_desc_interface_t) + sizeof(struct ccid_class_descriptor) +
2*sizeof(tusb_desc_endpoint_t)
#endif
#ifdef USB_ITF_HID
+ TUD_HID_INOUT_DESC_LEN + TUD_HID_DESC_LEN
+ TUD_HID_INOUT_DESC_LEN + TUD_HID_DESC_LEN
#endif
),
.bNumInterfaces = ITF_TOTAL,
.bConfigurationValue = 1,
.iConfiguration = 4,
.bmAttributes = USB_CONFIG_ATT_ONE | TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP,
.bMaxPower = TUSB_DESC_CONFIG_POWER_MA(MAX_USB_POWER+1),
),
.bNumInterfaces = ITF_TOTAL,
.bConfigurationValue = 1,
.iConfiguration = 4,
.bmAttributes = USB_CONFIG_ATT_ONE | TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP,
.bMaxPower = TUSB_DESC_CONFIG_POWER_MA(MAX_USB_POWER+1),
};
#ifdef USB_ITF_CCID
@ -97,8 +98,8 @@ static const struct ccid_class_descriptor desc_ccid = {
.bMaxSlotIndex = 0,
.bVoltageSupport = 0x01, // 5.0V
.dwProtocols = (
0x01| // T=0
0x02), // T=1
0x01| // T=0
0x02), // T=1
.dwDefaultClock = (0xDFC),
.dwMaximumClock = (0xDFC),
.bNumClockSupport = 0,
@ -133,64 +134,65 @@ tusb_desc_interface_t const desc_interface =
tusb_desc_endpoint_t const desc_ep1 =
{
.bLength = sizeof(tusb_desc_endpoint_t),
.bDescriptorType = TUSB_DESC_ENDPOINT,
.bEndpointAddress = TUSB_DIR_IN_MASK | 1,
.bmAttributes.xfer = TUSB_XFER_BULK,
.wMaxPacketSize.size = (64),
.bInterval = 0
.bDescriptorType = TUSB_DESC_ENDPOINT,
.bEndpointAddress = TUSB_DIR_IN_MASK | 1,
.bmAttributes.xfer = TUSB_XFER_BULK,
.wMaxPacketSize.size = (64),
.bInterval = 0
};
tusb_desc_endpoint_t const desc_ep2 =
{
.bLength = sizeof(tusb_desc_endpoint_t),
.bDescriptorType = TUSB_DESC_ENDPOINT,
.bEndpointAddress = 2,
.bmAttributes.xfer = TUSB_XFER_BULK,
.wMaxPacketSize.size = (64),
.bInterval = 0
.bDescriptorType = TUSB_DESC_ENDPOINT,
.bEndpointAddress = 2,
.bmAttributes.xfer = TUSB_XFER_BULK,
.wMaxPacketSize.size = (64),
.bInterval = 0
};
#endif
static uint8_t desc_config_extended[sizeof(tusb_desc_configuration_t)
#ifdef USB_ITF_CCID
+ sizeof(tusb_desc_interface_t) + sizeof(struct ccid_class_descriptor) + 2*sizeof(tusb_desc_endpoint_t)
+ sizeof(tusb_desc_interface_t) +
sizeof(struct ccid_class_descriptor) + 2*
sizeof(tusb_desc_endpoint_t)
#endif
#ifdef USB_ITF_HID
+ TUD_HID_INOUT_DESC_LEN + TUD_HID_DESC_LEN
+ TUD_HID_INOUT_DESC_LEN + TUD_HID_DESC_LEN
#endif
];
#ifdef USB_ITF_HID
#define HID_USAGE_PAGE_FIDO 0xF1D0
enum
{
HID_USAGE_FIDO_U2FHID = 0x01, // U2FHID usage for top-level collection
HID_USAGE_FIDO_DATA_IN = 0x20, // Raw IN data report
HID_USAGE_FIDO_DATA_OUT = 0x21 // Raw OUT data report
enum {
HID_USAGE_FIDO_U2FHID = 0x01,// U2FHID usage for top-level collection
HID_USAGE_FIDO_DATA_IN = 0x20,// Raw IN data report
HID_USAGE_FIDO_DATA_OUT = 0x21 // Raw OUT data report
};
#define TUD_HID_REPORT_DESC_FIDO_U2F(report_size, ...) \
HID_USAGE_PAGE_N ( HID_USAGE_PAGE_FIDO, 2 ) ,\
HID_USAGE ( HID_USAGE_FIDO_U2FHID ) ,\
HID_COLLECTION ( HID_COLLECTION_APPLICATION ) ,\
HID_USAGE_PAGE_N(HID_USAGE_PAGE_FIDO, 2), \
HID_USAGE(HID_USAGE_FIDO_U2FHID), \
HID_COLLECTION(HID_COLLECTION_APPLICATION), \
/* Report ID if any */ \
__VA_ARGS__ \
/* Usage Data In */ \
HID_USAGE ( HID_USAGE_FIDO_DATA_IN ) ,\
HID_LOGICAL_MIN ( 0 ) ,\
HID_LOGICAL_MAX_N ( 0xff, 2 ) ,\
HID_REPORT_SIZE ( 8 ) ,\
HID_REPORT_COUNT ( report_size ) ,\
HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
HID_USAGE(HID_USAGE_FIDO_DATA_IN), \
HID_LOGICAL_MIN(0), \
HID_LOGICAL_MAX_N(0xff, 2), \
HID_REPORT_SIZE(8), \
HID_REPORT_COUNT(report_size), \
HID_INPUT(HID_DATA | HID_VARIABLE | HID_ABSOLUTE), \
/* Usage Data Out */ \
HID_USAGE ( HID_USAGE_FIDO_DATA_OUT ) ,\
HID_LOGICAL_MIN ( 0 ) ,\
HID_LOGICAL_MAX_N ( 0xff, 2 ) ,\
HID_REPORT_SIZE ( 8 ) ,\
HID_REPORT_COUNT ( report_size ) ,\
HID_OUTPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
HID_COLLECTION_END \
HID_USAGE(HID_USAGE_FIDO_DATA_OUT), \
HID_LOGICAL_MIN(0), \
HID_LOGICAL_MAX_N(0xff, 2), \
HID_REPORT_SIZE(8), \
HID_REPORT_COUNT(report_size), \
HID_OUTPUT(HID_DATA | HID_VARIABLE | HID_ABSOLUTE), \
HID_COLLECTION_END \
uint8_t const desc_hid_report[] =
{
@ -203,46 +205,57 @@ uint8_t const desc_hid_report_kb[] =
#define EPNUM_HID 0x03
static uint8_t desc_hid[] = {
TUD_HID_INOUT_DESCRIPTOR(ITF_HID, ITF_HID+5, HID_ITF_PROTOCOL_NONE, sizeof(desc_hid_report), EPNUM_HID, 0x80 | EPNUM_HID, CFG_TUD_HID_EP_BUFSIZE, 10)
TUD_HID_INOUT_DESCRIPTOR(ITF_HID,
ITF_HID+5,
HID_ITF_PROTOCOL_NONE,
sizeof(desc_hid_report),
EPNUM_HID,
0x80 | EPNUM_HID,
CFG_TUD_HID_EP_BUFSIZE,
10)
};
static uint8_t desc_hid_kb[] = {
TUD_HID_DESCRIPTOR(ITF_KEYBOARD, ITF_KEYBOARD+5, HID_ITF_PROTOCOL_NONE, sizeof(desc_hid_report_kb), 0x80 | (EPNUM_HID+1), 16, 5)
TUD_HID_DESCRIPTOR(ITF_KEYBOARD, ITF_KEYBOARD+5, HID_ITF_PROTOCOL_NONE,
sizeof(desc_hid_report_kb), 0x80 | (EPNUM_HID+1), 16, 5)
};
uint8_t const * tud_hid_descriptor_report_cb(uint8_t itf)
uint8_t const *tud_hid_descriptor_report_cb(uint8_t itf)
{
printf("report_cb %d\n", itf);
if (itf == ITF_HID)
if (itf == ITF_HID) {
return desc_hid_report;
else if (itf == ITF_KEYBOARD)
} else if (itf == ITF_KEYBOARD) {
return desc_hid_report_kb;
}
return NULL;
}
#endif
uint8_t const * tud_descriptor_configuration_cb(uint8_t index)
uint8_t const *tud_descriptor_configuration_cb(uint8_t index)
{
(void) index; // for multiple configurations
static uint8_t initd = 0;
if (initd == 0)
{
if (initd == 0) {
uint8_t *p = desc_config_extended;
memcpy(p, &desc_config, sizeof(tusb_desc_configuration_t)); p += sizeof(tusb_desc_configuration_t);
memcpy(p, &desc_config, sizeof(tusb_desc_configuration_t));
p += sizeof(tusb_desc_configuration_t);
#ifdef USB_ITF_HID
memcpy(p, &desc_hid, sizeof(desc_hid)); p += sizeof(desc_hid);
memcpy(p, &desc_hid_kb, sizeof(desc_hid_kb)); p += sizeof(desc_hid_kb);
#endif
#ifdef USB_ITF_CCID
memcpy(p, &desc_interface, sizeof(tusb_desc_interface_t)); p += sizeof(tusb_desc_interface_t);
memcpy(p, &desc_ccid, sizeof(struct ccid_class_descriptor)); p += sizeof(struct ccid_class_descriptor);
memcpy(p, &desc_interface, sizeof(tusb_desc_interface_t));
p += sizeof(tusb_desc_interface_t);
memcpy(p, &desc_ccid, sizeof(struct ccid_class_descriptor));
p += sizeof(struct ccid_class_descriptor);
memcpy(p, &desc_ep1, sizeof(tusb_desc_endpoint_t)); p += sizeof(tusb_desc_endpoint_t);
memcpy(p, &desc_ep2, sizeof(tusb_desc_endpoint_t)); p += sizeof(tusb_desc_endpoint_t);
#endif
initd = 1;
}
return (const uint8_t *)desc_config_extended;
return (const uint8_t *) desc_config_extended;
}
#define BOS_TOTAL_LEN (TUD_BOS_DESC_LEN)
@ -255,7 +268,7 @@ uint8_t const desc_bos[] =
TUD_BOS_DESCRIPTOR(BOS_TOTAL_LEN, 2)
};
uint8_t const * tud_descriptor_bos_cb(void)
uint8_t const *tud_descriptor_bos_cb(void)
{
return desc_bos;
}
@ -265,7 +278,7 @@ uint8_t const * tud_descriptor_bos_cb(void)
//--------------------------------------------------------------------+
// array of pointer to string descriptors
char const* string_desc_arr [] =
char const *string_desc_arr [] =
{
(const char[]) { 0x09, 0x04 }, // 0: is supported language is English (0x0409)
"Pol Henarejos", // 1: Manufacturer
@ -273,17 +286,17 @@ char const* string_desc_arr [] =
"11223344", // 3: Serials, should use chip ID
"Pico Key Config" // 4: Vendor Interface
#ifdef USB_ITF_HID
,"Pico Key HID Interface"
,"Pico Key HID Keyboard Interface"
, "Pico Key HID Interface"
, "Pico Key HID Keyboard Interface"
#endif
#ifdef USB_ITF_CCID
,"Pico Key CCID Interface"
, "Pico Key CCID Interface"
#endif
};
static uint16_t _desc_str[32];
uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid)
uint16_t const *tud_descriptor_string_cb(uint8_t index, uint16_t langid)
{
(void) langid;
@ -292,15 +305,15 @@ uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid)
if (index == 0) {
memcpy(&_desc_str[1], string_desc_arr[0], 2);
chr_count = 1;
}
else {
} else {
// Note: the 0xEE index string is a Microsoft OS 1.0 Descriptors.
// https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/microsoft-defined-usb-descriptors
if ( !(index < sizeof(string_desc_arr)/sizeof(string_desc_arr[0])) )
if (!(index < sizeof(string_desc_arr)/sizeof(string_desc_arr[0]))) {
return NULL;
}
const char* str = string_desc_arr[index];
const char *str = string_desc_arr[index];
char unique_id_str[2 * PICO_UNIQUE_BOARD_ID_SIZE_BYTES + 1];
if (index == 3) {
pico_unique_board_id_t unique_id;
@ -310,16 +323,17 @@ uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid)
}
chr_count = strlen(str);
if ( chr_count > 31 )
if (chr_count > 31) {
chr_count = 31;
}
// Convert ASCII string into UTF-16
for(uint8_t i=0; i<chr_count; i++) {
for (uint8_t i = 0; i < chr_count; i++) {
_desc_str[1+i] = str[i];
}
}
_desc_str[0] = (TUSB_DESC_STRING << 8 ) | (2*chr_count + 2);
_desc_str[0] = (TUSB_DESC_STRING << 8) | (2*chr_count + 2);
return _desc_str;
}

46
src/usb/usb_descriptors.h
View file

@ -19,28 +19,28 @@
#define USB_DESCRIPTORS_H_
struct ccid_class_descriptor {
uint8_t bLength;
uint8_t bDescriptorType;
uint16_t bcdCCID;
uint8_t bMaxSlotIndex;
uint8_t bVoltageSupport;
uint32_t dwProtocols;
uint32_t dwDefaultClock;
uint32_t dwMaximumClock;
uint8_t bNumClockSupport;
uint32_t dwDataRate;
uint32_t dwMaxDataRate;
uint8_t bNumDataRatesSupported;
uint32_t dwMaxIFSD;
uint32_t dwSynchProtocols;
uint32_t dwMechanical;
uint32_t dwFeatures;
uint32_t dwMaxCCIDMessageLength;
uint8_t bClassGetResponse;
uint8_t bclassEnvelope;
uint16_t wLcdLayout;
uint8_t bPINSupport;
uint8_t bMaxCCIDBusySlots;
} __attribute__ ((__packed__));
uint8_t bLength;
uint8_t bDescriptorType;
uint16_t bcdCCID;
uint8_t bMaxSlotIndex;
uint8_t bVoltageSupport;
uint32_t dwProtocols;
uint32_t dwDefaultClock;
uint32_t dwMaximumClock;
uint8_t bNumClockSupport;
uint32_t dwDataRate;
uint32_t dwMaxDataRate;
uint8_t bNumDataRatesSupported;
uint32_t dwMaxIFSD;
uint32_t dwSynchProtocols;
uint32_t dwMechanical;
uint32_t dwFeatures;
uint32_t dwMaxCCIDMessageLength;
uint8_t bClassGetResponse;
uint8_t bclassEnvelope;
uint16_t wLcdLayout;
uint8_t bPINSupport;
uint8_t bMaxCCIDBusySlots;
} __attribute__((__packed__));
#endif /* USB_DESCRIPTORS_H_ */