diff --git a/ctaphid.c b/ctaphid.c index 0325cf1..7ed0318 100644 --- a/ctaphid.c +++ b/ctaphid.c @@ -1,8 +1,10 @@ #include #include +#include #include "ctaphid.h" #include "time.h" +#include "util.h" typedef enum { @@ -10,16 +12,33 @@ typedef enum HANDLING_REQUEST, } CTAP_STATE; +typedef enum +{ + EMPTY = 0, + BUFFERING, + BUFFERED, +} CTAP_BUFFER_STATE; + +#define SUCESS 0 +#define SEQUENCE_ERROR 1 + static int state; static int active_cid; static uint64_t active_cid_timestamp; static uint8_t ctap_buffer[CTAPHID_BUFFER_SIZE]; +static int ctap_buffer_cmd; +static int ctap_buffer_bcnt; +static int ctap_buffer_offset; +static int ctap_packet_seq; void ctaphid_init() { state = IDLE; active_cid = 0; active_cid_timestamp = millis(); + ctap_buffer_bcnt = 0; + ctap_buffer_offset = 0; + ctap_packet_seq = 0; } uint32_t get_new_cid() @@ -38,6 +57,11 @@ static int is_init_pkt(CTAPHID_PACKET * pkt) return (pkt->pkt.init.cmd == CTAPHID_INIT); } +static int is_cont_pkt(CTAPHID_PACKET * pkt) +{ + return (pkt->pkt.init.cmd == CTAPHID_INIT); +} + static int is_active_cid(CTAPHID_PACKET * pkt) { return (pkt->cid == active_cid); @@ -48,11 +72,65 @@ static int is_timed_out() return (millis() - active_cid_timestamp > 500); } - - -void ctaphid_handle_packet(uint8_t * pkt_raw) +static int buffer_packet(CTAPHID_PACKET * pkt) +{ + if (pkt->pkt.init.cmd & TYPE_INIT) + { + ctap_buffer_bcnt = ctaphid_packet_len(pkt); + int pkt_len = (ctap_buffer_bcnt < CTAPHID_INIT_PAYLOAD_SIZE) ? ctap_buffer_bcnt : CTAPHID_INIT_PAYLOAD_SIZE; + ctap_buffer_cmd = pkt->pkt.init.cmd; + ctap_buffer_offset = pkt_len; + ctap_packet_seq = -1; + memmove(ctap_buffer, pkt->pkt.init.payload, pkt_len); + } + else + { + int leftover = ctap_buffer_bcnt - ctap_buffer_offset; + int diff = leftover - CTAPHID_CONT_PAYLOAD_SIZE; + ctap_packet_seq++; + if (ctap_packet_seq != pkt->pkt.cont.seq) + { + return SEQUENCE_ERROR; + } + + if (diff <= 0) + { + // only move the leftover amount + memmove(ctap_buffer + ctap_buffer_offset, pkt->pkt.cont.payload, leftover); + ctap_buffer_offset += leftover; + } + else + { + memmove(ctap_buffer + ctap_buffer_offset, pkt->pkt.cont.payload, CTAPHID_CONT_PAYLOAD_SIZE); + ctap_buffer_offset += CTAPHID_CONT_PAYLOAD_SIZE; + } + } + return SUCESS; +} + +static int buffer_status() +{ + if (ctap_buffer_bcnt == 0) + { + return EMPTY; + } + else if (ctap_buffer_offset == ctap_buffer_bcnt) + { + return BUFFERED; + } + else + { + return BUFFERING; + } +} + +static int buffer_cmd() +{ + return ctap_buffer_cmd; +} + +void ctaphid_handle_packet(uint8_t * pkt_raw, CTAPHID_STATUS * stat) { -/*HID_MESSAGE_SIZE*/ CTAPHID_PACKET * pkt = (CTAPHID_PACKET *)(pkt_raw); printf("Recv packet\n"); @@ -60,6 +138,11 @@ void ctaphid_handle_packet(uint8_t * pkt_raw) printf(" cmd: %02x\n", pkt->pkt.init.cmd); printf(" length: %d\n", ctaphid_packet_len(pkt)); + int ret; + static CTAPHID_INIT_RESPONSE init_resp; + + memset(stat, 0, sizeof(CTAPHID_STATUS)); + start_over: switch (state) @@ -69,8 +152,7 @@ start_over: { printf("starting a new request\n"); state = HANDLING_REQUEST; - active_cid = get_new_cid(); - active_cid_timestamp = millis(); + buffer_packet(pkt); } else { @@ -86,11 +168,22 @@ start_over: ctaphid_init(); } + + if (!is_cont_pkt(pkt) && buffer_status() == BUFFERING) + { + printf("Error, expecting cont packet\n"); + } active_cid_timestamp = millis(); + ret = buffer_packet(pkt); + if (ret == SEQUENCE_ERROR) + { + printf("Sequence error\n"); + } } else if (is_timed_out()) { ctaphid_init(); + printf("dropping last channel -- timeout"); goto start_over; } else @@ -99,11 +192,94 @@ start_over: } break; default: - printf("invalid state\n"); + printf("invalid state; abort\n"); exit(1); } + + switch(buffer_status()) + { + case BUFFERING: + printf("BUFFERING\n"); + stat->status = NO_RESPONSE; + stat->length = 0; + stat->data= NULL; + active_cid_timestamp = millis(); + break; + case BUFFERED: + switch(buffer_cmd()) + { + case CTAPHID_INIT: + printf("CTAPHID_INIT\n"); + + active_cid = get_new_cid(); + active_cid_timestamp = millis(); + + init_resp.broadcast = CTAPHID_BROADCAST_CID; + init_resp.cmd = CTAPHID_INIT; + init_resp.bcnth = (17 & 0xff00) >> 8; + init_resp.bcntl = (17 & 0xff) >> 0; + memmove(init_resp.nonce, pkt->pkt.init.payload, 8); + init_resp.cid = active_cid; + init_resp.protocol_version = 0;//? + init_resp.version_major = 0;//? + init_resp.version_minor = 0;//? + init_resp.build_version = 0;//? + init_resp.capabilities = CAPABILITY_WINK | CAPABILITY_CBOR; + + stat->status = CTAPHID_RESPONSE; + stat->length = sizeof(CTAPHID_INIT_RESPONSE); + stat->data = (uint8_t *)&init_resp; + + break; + case CTAPHID_PING: + printf("CTAPHID_PING\n"); + break; + case CTAPHID_WINK: + printf("CTAPHID_WINK\n"); + break; + default: + printf("error, unimplemented HID cmd: %02x\r\n", buffer_cmd()); + break; + } + break; + case EMPTY: + printf("empty buffer!\n"); + default: + printf("invalid buffer state; abort\n"); + exit(1); + break; + } + + } +void ctaphid_dump_status(CTAPHID_STATUS * stat) +{ + switch(stat->status) + { + case NO_RESPONSE: + printf("NO_RESPONSE"); + break; + case CTAPHID_RESPONSE: + printf("CTAPHID_RESPONSE"); + break; + case U2F_RESPONSE: + printf("U2F_RESPONSE"); + break; + case CBOR_RESPONSE: + printf("CBOR_RESPONSE"); + break; + default: + printf("\ninvalid status %d; abort\n", stat->status); + exit(1); + } + printf(" (%d)\n ", stat->length); + if (stat->length > 0) + { + dump_hex(stat->data, stat->length); + } +} + diff --git a/ctaphid.h b/ctaphid.h index 8241172..bcae951 100644 --- a/ctaphid.h +++ b/ctaphid.h @@ -21,6 +21,11 @@ #define CTAPHID_BUFFER_SIZE 4096 +#define CAPABILITY_WINK 0x01 +#define CAPABILITY_LOCK 0x02 +#define CAPABILITY_CBOR 0x04 +#define CAPABILITY_NMSG 0x08 + typedef struct { uint32_t cid; @@ -38,9 +43,46 @@ typedef struct } pkt; } CTAPHID_PACKET; + +typedef struct +{ + uint32_t broadcast; + uint8_t cmd; + uint8_t bcnth; + uint8_t bcntl; + uint8_t nonce[8]; + uint32_t cid; + uint8_t protocol_version; + uint8_t version_major; + uint8_t version_minor; + uint8_t build_version; + uint8_t capabilities; +} __attribute__((packed)) CTAPHID_INIT_RESPONSE; + + +// API specific, not protocol specific // +typedef enum +{ + NO_RESPONSE = 0, + CTAPHID_RESPONSE, + U2F_RESPONSE, + CBOR_RESPONSE, +} CTAPHID_CODE; + +typedef struct +{ + int status; + uint8_t * data; + int length; +} CTAPHID_STATUS; +//////// + + void ctaphid_init(); -void ctaphid_handle_packet(uint8_t * pkt_raw); +void ctaphid_handle_packet(uint8_t * pkt_raw, CTAPHID_STATUS * stat); + +void ctaphid_dump_status(CTAPHID_STATUS * stat); #define ctaphid_packet_len(pkt) ((uint16_t)((pkt)->pkt.init.bcnth << 8) | ((pkt)->pkt.init.bcntl)) diff --git a/main.c b/main.c index 5791755..aefd542 100644 --- a/main.c +++ b/main.c @@ -17,6 +17,7 @@ void check_ret(CborError ret) } } + int main(int argc, char * argv[]) { /*CborError ret;*/ @@ -34,7 +35,9 @@ int main(int argc, char * argv[]) printf("init ctaphid\n"); ctaphid_init(); + int count = 0; uint8_t hidmsg[64]; + CTAPHID_STATUS res; memset(hidmsg,0,sizeof(hidmsg)); printf("recv'ing hid msg \n"); @@ -42,11 +45,20 @@ int main(int argc, char * argv[]) while(1) { usbhid_recv(hidmsg); - printf(">> "); dump_hex(hidmsg,sizeof(hidmsg)); + printf("%d>> ",count++); dump_hex(hidmsg,sizeof(hidmsg)); - ctaphid_handle_packet(hidmsg); - printf("<< "); dump_hex(hidmsg,sizeof(hidmsg)); - usbhid_send(hidmsg); + ctaphid_handle_packet(hidmsg, &res); + memset(hidmsg, 0, sizeof(hidmsg)); + ctaphid_dump_status(&res); + + int i; + for(i = 0; i < res.length; i += 64) + { + memmove(hidmsg, res.data + i, MIN(res.length - i, 64)); + usbhid_send(hidmsg); + printf("<< "); dump_hex(hidmsg,sizeof(hidmsg)); + } + printf("\n"); } diff --git a/util.h b/util.h index 4fdc134..d347e5a 100644 --- a/util.h +++ b/util.h @@ -4,4 +4,7 @@ void dump_hex(uint8_t * buf, int size); +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) + #endif