From 12bdcbd1f91f5b5763ad66b212be897ae9eb87a8 Mon Sep 17 00:00:00 2001 From: Pol Henarejos Date: Tue, 14 Feb 2023 23:11:51 +0100 Subject: [PATCH] Harmonizing coding style. Signed-off-by: Pol Henarejos --- src/apdu.c | 114 ++++++------ src/apdu.h | 39 ++--- src/asn1.c | 62 ++++--- src/asn1.h | 13 +- src/crypto_utils.c | 130 +++++++++----- src/crypto_utils.h | 14 +- src/eac.c | 124 +++++++++----- src/eac.h | 2 +- src/fs/file.c | 256 ++++++++++++++++----------- src/fs/file.h | 6 +- src/fs/flash.c | 86 ++++++---- src/fs/low_flash.c | 174 +++++++++++-------- src/hsm.h | 103 +++++------ src/hsm_version.h | 1 - src/main.c | 59 ++++--- src/rng/hwrng.c | 114 +++++++----- src/rng/hwrng.h | 2 +- src/rng/random.c | 60 ++++--- src/rng/random.h | 12 +- src/usb/ccid/ccid.c | 201 +++++++++++++--------- src/usb/ccid/ccid.h | 16 +- src/usb/ccid/usb_common.h.notused | 175 ++++++++++--------- src/usb/emulation/emulation.c | 55 +++--- src/usb/hid/ctap_hid.h | 54 +++--- src/usb/hid/hid.c | 276 ++++++++++++++++++------------ src/usb/tusb_config.h | 10 +- src/usb/usb.c | 148 ++++++++++------ src/usb/usb.h | 3 +- src/usb/usb_descriptors.c | 156 +++++++++-------- src/usb/usb_descriptors.h | 46 ++--- 30 files changed, 1475 insertions(+), 1036 deletions(-) diff --git a/src/apdu.c b/src/apdu.c index e719659..3839052 100644 --- a/src/apdu.c +++ b/src/apdu.c @@ -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; diff --git a/src/apdu.h b/src/apdu.h index 94f9c83..ef4d884 100644 --- a/src/apdu.h +++ b/src/apdu.h @@ -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(); diff --git a/src/asn1.c b/src/asn1.c index a9b65f2..b86e824 100644 --- a/src/asn1.c +++ b/src/asn1.c @@ -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; } } diff --git a/src/asn1.h b/src/asn1.h index 9945fb2..6751582 100644 --- a/src/asn1.h +++ b/src/asn1.h @@ -26,9 +26,18 @@ #include #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 diff --git a/src/crypto_utils.c b/src/crypto_utils.c index 4c95429..096c935 100644 --- a/src/crypto_utils.c +++ b/src/crypto_utils.c @@ -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; -} \ No newline at end of file +} diff --git a/src/crypto_utils.h b/src/crypto_utils.h index a7311bd..409666b 100644 --- a/src/crypto_utils.h +++ b/src/crypto_utils.h @@ -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); diff --git a/src/eac.c b/src/eac.c index c0565e1..e13f04c 100644 --- a/src/eac.c +++ b/src/eac.c @@ -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; -} \ No newline at end of file +} diff --git a/src/eac.h b/src/eac.h index c55f559..e4b9332 100644 --- a/src/eac.h +++ b/src/eac.h @@ -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); diff --git a/src/fs/file.c b/src/fs/file.c index 1bfdd36..9f6c8f3 100644 --- a/src/fs/file.c +++ b/src/fs/file.c @@ -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; } diff --git a/src/fs/file.h b/src/fs/file.h index 73335c5..1bed734 100644 --- a/src/fs/file.h +++ b/src/fs/file.h @@ -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 - diff --git a/src/fs/flash.c b/src/fs/flash.c index 5576c66..c38d03b 100644 --- a/src/fs/flash.c +++ b/src/fs/flash.c @@ -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); } diff --git a/src/fs/low_flash.c b/src/fs/low_flash.c index 73b2abf..57b690b 100644 --- a/src/fs/low_flash.c +++ b/src/fs/low_flash.c @@ -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; } diff --git a/src/hsm.h b/src/hsm.h index a6e6406..28e03bf 100644 --- a/src/hsm.h +++ b/src/hsm.h @@ -24,14 +24,14 @@ #else #include 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 @@ -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 diff --git a/src/hsm_version.h b/src/hsm_version.h index b619ac4..7a18bbb 100644 --- a/src/hsm_version.h +++ b/src/hsm_version.h @@ -24,4 +24,3 @@ #define HSM_SDK_VERSION_MINOR (HSM_SDK_VERSION & 0xff) #endif - diff --git a/src/main.c b/src/main.c index d5c99d3..53a53e4 100644 --- a/src/main.c +++ b/src/main.c @@ -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; -} \ No newline at end of file +} diff --git a/src/rng/hwrng.c b/src/rng/hwrng.c index 627f982..ded3c41 100644 --- a/src/rng/hwrng.c +++ b/src/rng/hwrng.c @@ -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(); } - diff --git a/src/rng/hwrng.h b/src/rng/hwrng.h index a9c2d8f..f217d55 100644 --- a/src/rng/hwrng.h +++ b/src/rng/hwrng.h @@ -31,4 +31,4 @@ void neug_flush(void); void neug_wait_full(); void neug_fini(void); -#endif \ No newline at end of file +#endif diff --git a/src/rng/random.c b/src/rng/random.c index 0ce3b94..c2d636a 100644 --- a/src/rng/random.c +++ b/src/rng/random.c @@ -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; } diff --git a/src/rng/random.h b/src/rng/random.h index 73cb062..f3fc4ce 100644 --- a/src/rng/random.h +++ b/src/rng/random.h @@ -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 \ No newline at end of file +#endif diff --git a/src/usb/ccid/ccid.c b/src/usb/ccid/ccid.c index ce1487c..9cddd3c 100644 --- a/src/usb/ccid/ccid.c +++ b/src/usb/ccid/ccid.c @@ -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; } diff --git a/src/usb/ccid/ccid.h b/src/usb/ccid/ccid.h index 2a5e4d2..c850a75 100644 --- a/src/usb/ccid/ccid.h +++ b/src/usb/ccid/ccid.h @@ -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; diff --git a/src/usb/ccid/usb_common.h.notused b/src/usb/ccid/usb_common.h.notused index 3b5ba4c..5d80d3e 100644 --- a/src/usb/ccid/usb_common.h.notused +++ b/src/usb/ccid/usb_common.h.notused @@ -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 - diff --git a/src/usb/emulation/emulation.c b/src/usb/emulation/emulation.c index c532652..8cbf25a 100644 --- a/src/usb/emulation/emulation.c +++ b/src/usb/emulation/emulation.c @@ -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); } } diff --git a/src/usb/hid/ctap_hid.h b/src/usb/hid/ctap_hid.h index ec0cb33..ac42b40 100644 --- a/src/usb/hid/ctap_hid.h +++ b/src/usb/hid/ctap_hid.h @@ -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. diff --git a/src/usb/hid/hid.c b/src/usb/hid/hid.c index 7ac88e9..81c86e6 100644 --- a/src/usb/hid/hid.c +++ b/src/usb/hid/hid.c @@ -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); } diff --git a/src/usb/tusb_config.h b/src/usb/tusb_config.h index 8ebdf7a..b256d25 100644 --- a/src/usb/tusb_config.h +++ b/src/usb/tusb_config.h @@ -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_ */ - diff --git a/src/usb/usb.c b/src/usb/usb.c index f323106..142467c 100644 --- a/src/usb/usb.c +++ b/src/usb/usb.c @@ -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 // 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; } diff --git a/src/usb/usb.h b/src/usb/usb.h index 7e1cc7e..0a755e6 100644 --- a/src/usb/usb.h +++ b/src/usb/usb.h @@ -56,8 +56,7 @@ enum { ITF_TOTAL }; -enum -{ +enum { REPORT_ID_KEYBOARD = 1, REPORT_ID_COUNT }; diff --git a/src/usb/usb_descriptors.c b/src/usb/usb_descriptors.c index 0c6f9bf..f5bc206 100644 --- a/src/usb/usb_descriptors.c +++ b/src/usb/usb_descriptors.c @@ -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