change writeback method and add errors

ctaphid.c

@@ -1,3 +1,4 @@
+#include <arpa/inet.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -29,12 +30,14 @@ 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 uint16_t ctap_buffer_bcnt;
 static int ctap_buffer_offset;
 static int ctap_packet_seq;
 
 static uint32_t _next_cid = 0;
 
+static void ctaphid_write(void * _data, int len);
+
 void ctaphid_init()
 {
     state = IDLE;
@@ -43,6 +46,7 @@ void ctaphid_init()
     ctap_buffer_bcnt = 0;
     ctap_buffer_offset = 0;
     ctap_packet_seq = 0;
+    ctaphid_write(NULL, -1);
 }
 
 static uint32_t set_next_cid(uint32_t cid)
@@ -146,6 +150,59 @@ static int buffer_cmd()
     return ctap_buffer_cmd;
 }
 
+static int buffer_len()
+{
+    return ctap_buffer_bcnt;
+}
+
+// Buffer data and send in HID_MESSAGE_SIZE chunks
+static void ctaphid_write(void * _data, int len)
+{
+    static uint8_t buf[HID_MESSAGE_SIZE];
+    static int offset = 0;
+
+    uint8_t * data = (uint8_t *) _data;
+
+    if (len == 0)
+    {
+        if (offset > 0)
+        {
+            memset(buf + offset, 0, HID_MESSAGE_SIZE - offset);
+            ctaphid_write_block(buf);
+        }
+        return;
+    }
+    else if (len == -1)
+    {
+        memset(buf, 0, HID_MESSAGE_SIZE);
+        offset = 0;
+    }
+
+    int i;
+    for (i = 0; i < len; i++)
+    {
+        buf[offset++] = data[i];
+        if (offset == HID_MESSAGE_SIZE)
+        {
+            ctaphid_write_block(buf);
+            offset = 0;
+        }
+    }
+}
+
+static void ctaphid_send_error(uint32_t cid, uint8_t error)
+{
+    uint8_t buf[HID_MESSAGE_SIZE];
+    memset(buf,0,sizeof(buf));
+    CTAPHID_ERROR_RESPONSE * resp = (CTAPHID_ERROR_RESPONSE *) buf;
+    resp->cid = cid;
+    resp->cmd = CTAPHID_ERROR;
+    resp->bcnth = 0;
+    resp->bcntl = 1;
+    resp->error = error;
+    ctaphid_write_block(buf);
+}
+
 void ctaphid_handle_packet(uint8_t * pkt_raw, CTAPHID_STATUS * stat)
 {
     CTAPHID_PACKET * pkt = (CTAPHID_PACKET *)(pkt_raw);
@@ -157,6 +214,8 @@ void ctaphid_handle_packet(uint8_t * pkt_raw, CTAPHID_STATUS * stat)
 
     int ret;
     static CTAPHID_INIT_RESPONSE init_resp;
+    static CTAPHID_PING_RESPONSE ping_resp;
+    static CTAPHID_WINK_RESPONSE wink_resp;
 
     memset(stat, 0, sizeof(CTAPHID_STATUS));
 
@@ -174,6 +233,8 @@ start_over:
             else
             {
                 printf("Error, unknown request\n");
+                ctaphid_send_error(pkt->cid, ERR_INVALID_PAR);
+                return;
             }
             break;
         case HANDLING_REQUEST:
@@ -183,30 +244,49 @@ start_over:
                 {
                     printf("received abort request from %08x\n", pkt->cid);
 
+                    set_next_cid(active_cid);   // reuse last CID in current channel
                     ctaphid_init();
                     buffer_packet(pkt);
                 }
-
+                else if (!is_cont_pkt(pkt) && buffer_status() == BUFFERING)
-                if (!is_cont_pkt(pkt) && buffer_status() == BUFFERING)
                 {
                     printf("Error, expecting cont packet\n");
+                    ctaphid_send_error(pkt->cid, ERR_INVALID_PAR);
+                    return;
                 }
+                else if(!is_cont_pkt(pkt))
+                {
+                    if (ctaphid_packet_len(pkt) > CTAPHID_BUFFER_SIZE)
+                    {
+                        printf("Error, internal buffer not big enough\n");
+                        ctaphid_send_error(pkt->cid, ERR_INVALID_LEN);
+                        ctaphid_init();
+                        return;
+                    }
+                }
+
                 active_cid_timestamp = millis();
                 ret = buffer_packet(pkt);
                 if (ret == SEQUENCE_ERROR)
                 {
                     printf("Sequence error\n");
+                    ctaphid_send_error(pkt->cid, ERR_INVALID_SEQ);
+                    ctaphid_init();
+                    return;
                 }
             }
             else if (is_timed_out())
             {
                 ctaphid_init();
                 printf("dropping last channel -- timeout");
+                ctaphid_send_error(pkt->cid, ERR_MSG_TIMEOUT);
                 goto start_over;
             }
             else
             {
+                ctaphid_send_error(pkt->cid, ERR_CHANNEL_BUSY);
                 printf("Too busy with current transaction\n");
+                return;
             }
             break;
         default:
@@ -220,17 +300,28 @@ start_over:
         case BUFFERING:
             printf("BUFFERING\n");
             stat->status = NO_RESPONSE;
-            stat->length = 0;
-            stat->data= NULL;
             active_cid_timestamp = millis();
             break;
+
+        case EMPTY:
+            printf("empty buffer!\n");
         case BUFFERED:
             switch(buffer_cmd())
             {
                 case CTAPHID_INIT:
                     printf("CTAPHID_INIT\n");
 
+                    if (buffer_len() != 8)
+                    {
+                        printf("Error,invalid length field for init packet\n");
+                        ctaphid_send_error(pkt->cid, ERR_INVALID_LEN);
+                        ctaphid_init();
+                        return;
+                    }
+
                     active_cid = get_new_cid();
+
+                    printf("cid: %08x\n",active_cid);
                     active_cid_timestamp = millis();
 
                     init_resp.broadcast = CTAPHID_BROADCAST_CID;
@@ -245,24 +336,52 @@ start_over:
                     init_resp.build_version = 0;//?
                     init_resp.capabilities = CAPABILITY_WINK | CAPABILITY_CBOR;
 
+                    ctaphid_write(&init_resp,sizeof(CTAPHID_INIT_RESPONSE));
+                    ctaphid_write(NULL,0);
+
                     stat->status = CTAPHID_RESPONSE;
-                    stat->length = sizeof(CTAPHID_INIT_RESPONSE);
-                    stat->data = (uint8_t *)&init_resp;
 
                     break;
                 case CTAPHID_PING:
+
+                    ping_resp.cid = active_cid;
+                    ping_resp.cmd = CTAPHID_PING;
+                    ping_resp.bcnth = (buffer_len() & 0xff00) >> 8;
+                    ping_resp.bcntl = (buffer_len() & 0xff) >> 0;
+
+                    ctaphid_write(&ping_resp,sizeof(CTAPHID_PING_RESPONSE));
+                    ctaphid_write(ctap_buffer, buffer_len());
+                    ctaphid_write(NULL,0);
+
                     printf("CTAPHID_PING\n");
                     break;
+
                 case CTAPHID_WINK:
+
+                    if (buffer_len() != 0)
+                    {
+                        printf("Error,invalid length field for wink packet\n");
+                        ctaphid_send_error(pkt->cid, ERR_INVALID_LEN);
+                        ctaphid_init();
+                        return;
+                    }
+
+                    wink_resp.cid = active_cid;
+                    wink_resp.cmd = CTAPHID_WINK;
+                    wink_resp.bcnt = 0;
+
+                    ctaphid_write(&wink_resp,sizeof(CTAPHID_WINK_RESPONSE));
+                    ctaphid_write(NULL,0);
+
                     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);
@@ -293,11 +412,12 @@ void ctaphid_dump_status(CTAPHID_STATUS * stat)
             printf("\ninvalid status %d; abort\n", stat->status);
             exit(1);
     }
-    printf(" (%d)\n  ", stat->length);
+    printf("\n");
-    if (stat->length > 0)
+    /*printf(" (%d)\n  ", stat->length);*/
-    {
+    /*if (stat->length > 0)*/
-        dump_hex(stat->data, stat->length);
+    /*{*/
-    }
+        /*dump_hex(stat->data, stat->length);*/
+    /*}*/
 }
 
 

ctaphid.h

@@ -3,16 +3,25 @@
 
 #include "usbhid.h"
 
-#define TYPE_MASK               0x80    // Frame type mask
+#define TYPE_INIT               0x80
-#define TYPE_INIT               0x80    // Initial frame identifier
+#define TYPE_CONT               0x00
-#define TYPE_CONT               0x00    // Continuation frame identifier
+
+#define CTAPHID_PING         (TYPE_INIT | 0x01)
+#define CTAPHID_MSG          (TYPE_INIT | 0x03)
+#define CTAPHID_LOCK         (TYPE_INIT | 0x04)
+#define CTAPHID_INIT         (TYPE_INIT | 0x06)
+#define CTAPHID_WINK         (TYPE_INIT | 0x08)
+#define CTAPHID_CBOR         (TYPE_INIT | 0x10)
+#define CTAPHID_CANCEL       (TYPE_INIT | 0x11)
+#define CTAPHID_ERROR        (TYPE_INIT | 0x3f)
+
+    #define ERR_INVALID_CMD         0x01
+    #define ERR_INVALID_PAR         0x02
+    #define ERR_INVALID_LEN         0x03
+    #define ERR_INVALID_SEQ         0x04
+    #define ERR_MSG_TIMEOUT         0x05
+    #define ERR_CHANNEL_BUSY        0x06
 
-#define CTAPHID_PING         (TYPE_INIT | 0x01) // Echo data through local processor only
-#define CTAPHID_MSG          (TYPE_INIT | 0x03) // Send U2F message frame
-#define CTAPHID_LOCK         (TYPE_INIT | 0x04) // Send lock channel command
-#define CTAPHID_INIT         (TYPE_INIT | 0x06) // Channel initialization
-#define CTAPHID_WINK         (TYPE_INIT | 0x08) // Send device identification wink
-#define CTAPHID_ERROR        (TYPE_INIT | 0x3f) // Error response
 
 #define CTAPHID_INIT_PAYLOAD_SIZE  (HID_MESSAGE_SIZE-7)
 #define CTAPHID_CONT_PAYLOAD_SIZE  (HID_MESSAGE_SIZE-5)
@@ -59,6 +68,34 @@ typedef struct
     uint8_t capabilities;
 } __attribute__((packed)) CTAPHID_INIT_RESPONSE;
 
+typedef struct
+{
+    uint32_t cid;
+    uint8_t cmd;
+    uint8_t bcnth;
+    uint8_t bcntl;
+} __attribute__((packed)) CTAPHID_PING_RESPONSE;
+
+
+typedef struct
+{
+    uint32_t cid;
+    uint8_t cmd;
+    uint16_t bcnt;
+} __attribute__((packed)) CTAPHID_WINK_RESPONSE;
+
+
+typedef struct
+{
+    uint32_t cid;
+    uint8_t cmd;
+    uint8_t bcnth;
+    uint8_t bcntl;
+    uint8_t error;
+} __attribute__((packed)) CTAPHID_ERROR_RESPONSE;
+
+
+
 
 // API specific, not protocol specific //
 typedef enum
@@ -72,8 +109,6 @@ typedef enum
 typedef struct
 {
     int status;
-    uint8_t * data;
-    int length;
 } CTAPHID_STATUS;
 ////////
 
@@ -84,6 +119,10 @@ void ctaphid_handle_packet(uint8_t * pkt_raw, CTAPHID_STATUS * stat);
 
 void ctaphid_dump_status(CTAPHID_STATUS * stat);
 
+// Must be implemented by application
+// data is HID_MESSAGE_SIZE long in bytes
+extern void ctaphid_write_block(uint8_t * data);
+
 #define ctaphid_packet_len(pkt)     ((uint16_t)((pkt)->pkt.init.bcnth << 8) | ((pkt)->pkt.init.bcntl))
 
 #endif

main.c

@@ -18,6 +18,14 @@ void check_ret(CborError ret)
 }
 
 
+void ctaphid_write_block(uint8_t * data)
+{
+    printf("usbhid send\n");
+    dump_hex(data, 64);
+    usbhid_send(data);
+}
+
+
 int main(int argc, char * argv[])
 {
     /*CborError ret;*/
@@ -51,14 +59,14 @@ int main(int argc, char * argv[])
         memset(hidmsg, 0, sizeof(hidmsg));
         ctaphid_dump_status(&res);
 
-        int i;
+        /*int i;*/
-        for(i = 0; i < res.length; i += 64)
+        /*for(i = 0; i < res.length; i += 64)*/
-        {
+        /*{*/
-            memmove(hidmsg, res.data + i, MIN(res.length - i, 64));
+            /*memmove(hidmsg, res.data + i, MIN(res.length - i, 64));*/
-            usbhid_send(hidmsg);
+            /*usbhid_send(hidmsg);*/
-            printf("<< "); dump_hex(hidmsg,sizeof(hidmsg));
+            /*printf("<< "); dump_hex(hidmsg,sizeof(hidmsg));*/
-        }
+        /*}*/
-        printf("\n");
+        /*printf("\n");*/
     }