bpreader: add poll card handler

board/bpreader.c

@@ -10,6 +10,7 @@
 
 #include "util/dprintf.h"
 #include "util/dump.h"
+#include "util/hexstr.h"
 
 #include "board/bpreader.h"
 
@@ -18,7 +19,14 @@ const uint8_t BPREADER_CMD_GO_NEXT[6] = { 0x00, 0x00, 0xFF, 0x00, 0xFF, 0x00 };
 static HRESULT bp_handle_irp(struct irp *irp);
 static HRESULT bp_handle_irp_locked(struct irp *irp);
 static HRESULT crack_bpreader_request();
-static HRESULT build_bpreader_response();
+static size_t build_bpreader_response(
+    const uint8_t *data,
+    const size_t len_data,
+    struct bpreader_cmd_header *resp_header,
+    uint8_t *response,
+    const size_t resp_size);
+
+static HRESULT bpreader_poll_card();
 
 static struct bpreader_config *config;
 static struct uart bp_uart;
@@ -57,12 +65,29 @@ void bpreader_congif_load(struct bpreader_config *cfg, const wchar_t *filename)
     cfg->enable = GetPrivateProfileIntW(L"reader", L"enable", 1, filename);
     cfg->port = GetPrivateProfileIntW(L"reader", L"port", 0, filename);
     GetPrivateProfileStringW(
-            L"reader",
+        L"reader",
-            L"access_code",
+        L"access_code",
-            L"",
+        L"00000000000000000000",
-            cfg->access_code,
+        cfg->access_code,
-            _countof(cfg->access_code),
+        _countof(cfg->access_code),
-            filename);
+        filename);
+    GetPrivateProfileStringW(
+        L"reader",
+        L"chip_id",
+        L"00000000000000000000000000000000",
+        cfg->chip_id,
+        _countof(cfg->chip_id),
+        filename);
+    
+    HRESULT hr = wstr_to_bytes(cfg->access_code, 20, cfg->access_code_bytes, sizeof(cfg->access_code_bytes));
+    if (FAILED(hr)) {
+        dprintf("Reader: wstr_to_bytes 1 failed! 0x%lX", hr);
+    }
+    
+    hr = wstr_to_bytes(cfg->chip_id, 32, cfg->chip_id_bytes, sizeof(cfg->chip_id_bytes));
+    if (FAILED(hr)) {
+        dprintf("Reader: wstr_to_bytes 2 failed! 0x%lX", hr);
+    }
 }
 
 static HRESULT bp_handle_irp(struct irp *irp)
@@ -116,7 +141,7 @@ static HRESULT bp_handle_irp_locked(struct irp *irp)
     
     else if (irp->op == IRP_OP_READ) {
         if (!read_ct) {            
-            dump_iobuf(&bp_uart.written);
+            //dump_iobuf(&bp_uart.written);
             hr = crack_bpreader_request();
         }
         switch (bp_uart.written.bytes[3]) {
@@ -177,36 +202,6 @@ static HRESULT bp_handle_irp_locked(struct irp *irp)
                 bp_uart.written.pos = 0;
                 break;
 
-            case 0x09:
-                if (!read_ct) {  
-                    dprintf("Reader: Poll card\n");
-                    uint8_t buff[37] = {};
-                    if (GetAsyncKeyState(VK_RETURN) & 0x8000) {
-                        dprintf("Reader: Touch card %ls\n", config->access_code);
-                        uint8_t buff2[] = {
-                            0x00, 0x00, 0xFF, 0x18, 0xE8, 0xD5, 0x4B, 0x01, 0x01, 0x14, 0x01, // Data
-                            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // IDm
-                            0x00, 0xF1, 0x00, 0x00, 0x00, 0x01, 0x43, 0x00, // PMm
-                            0x88, 0xB4, // System Code
-                            0x00, 0x00 };
-                        /*uint8_t buff2[] = {
-                            0x00, 0x00, 0xFF, 0x0C, 0xF4, 0xD5, 0x4B, 0x01, 0x01, 
-                            0x00, 0x04, // ATQA
-                            0x08, //SAK
-                            0x04, // Unknown
-                            0x42, 0xF0, 0x00, 0x00, // S/N
-                            0xB0, 0x00 }; // Footer*/
-                        memcpy_s(buff, sizeof(buff), buff2, sizeof(buff2));
-                    } else {
-                        uint8_t buff2[] = { 0x00, 0x00, 0xFF, 0x03, 0xFD, 0xD5, 0x4B, 0x00, 0xE0, 0x00 };
-                        memcpy_s(buff, sizeof(buff), buff2, sizeof(buff2));
-                    }
-                    
-                    hr = iobuf_write(&bp_uart.readable, buff, sizeof(buff));
-                }
-                bp_uart.written.pos = 0;
-                break;
-            
             case 0x0E:
                 if (!read_ct) {
                     dprintf("Reader: Unknown 0x0E\n");
@@ -316,12 +311,12 @@ static HRESULT crack_bpreader_request() {
             break;
         
         case 0x40:
-            dprintf("Reader: Read Banapass\n"); // 01 03 00 -> Chip ID; 01 30 01 -> Thing after chip ID; 01 60 30 -> send key a
+            dprintf("Reader: Read Banapass\n"); // 01 30 00 -> Chip ID; 01 30 01 -> Thing after chip ID; 01 30 02 -> Access Code 01 60 30 -> send key a
             break;
 
         case 0x4A:
-            dprintf("Reader: Poll Card\n"); // 01 01 00 ff ff 01 00 -> Felica;  01 00 -> Banapass
+            dprintf("Reader: Poll Card\n");
-            break;
+            return bpreader_poll_card();
 
         case 0xA0:
             dprintf("Reader: Read Felica\n");
@@ -334,6 +329,66 @@ static HRESULT crack_bpreader_request() {
     return S_OK;
 }
 
-static HRESULT build_bpreader_response() {
+static size_t build_bpreader_response(
-    return S_OK;
+    const uint8_t *data,
+    const size_t len_data,
+    struct bpreader_cmd_header *resp_header,
+    uint8_t *response,
+    const size_t resp_size) 
+{
+    // TODO: Account for escape characters
+    if (resp_size < 9) {
+        dprintf("Reader: build_bpreader_response resp_size less then 9\n");
+        return 0;
+    }
+
+    uint8_t full_resp_len = len_data + 2 + 7; // calculate full response length
+    uint8_t final_checksum = 0; // initialize checksum
+    
+    resp_header->data_len = len_data + 2;
+    resp_header->header_checksum = 0xFF - ((0xFF + len_data + 2) & 0xFF);
+    
+    memcpy_s(response, resp_size, resp_header, sizeof(*resp_header)); // Copy header
+    memcpy_s(&response[7], resp_size - 7, data, len_data); // copy data
+    
+    for (int i = 0; i < full_resp_len; i++) {
+        final_checksum = (final_checksum + response[i]) & 0xFF; // Calculate checksum
+    }
+
+    response[full_resp_len - 2] = 0xFF - final_checksum; // Assign checksum
+    return full_resp_len;
+}
+
+static HRESULT bpreader_poll_card()
+{
+    struct bpreader_cmd_header header = { 0x00, 0x00, 0xFF, 0x03, 0xFD, 0xD5, 0x4B };
+    struct bpreader_poll_banapass_data data = { 0x00 };    
+    uint8_t buff[32] = { 0x00 };
+    int data_len = 1;
+    size_t resp_len = 0;
+    
+    if (GetAsyncKeyState(VK_RETURN) & 0x8000) {
+        dprintf("Reader: Touch card %ls\n", config->access_code);
+        
+        data.card_present = 0x01;
+        data.unknown1 = 0x01;
+        data.atqa[0] = 0x00;
+        data.atqa[1] = 0x04;
+        data.sak = 0x08;
+        data.unknown5 = 0x04;
+        memcpy_s(data.serial_number, sizeof(data.serial_number), config->chip_id_bytes, 4);
+        data_len = sizeof(data);
+    }
+    
+    resp_len = build_bpreader_response((uint8_t *)&data, data_len, &header, buff, sizeof(buff));
+    
+    if (resp_len > 0) {
+        HRESULT hr = iobuf_write(&bp_uart.readable, buff, resp_len);
+        dump_iobuf(&bp_uart.readable);
+        return hr;
+    
+    } else {
+        dprintf("Reader: No data to return in bpreader_poll_card!\n");
+        return E_FAIL;
+    }
 }

board/bpreader.h

@@ -7,6 +7,9 @@ struct bpreader_config {
     bool enable;
     uint16_t port;
     wchar_t access_code[21];
+    wchar_t chip_id[33];
+    uint8_t access_code_bytes[10];
+    uint8_t chip_id_bytes[16];
 };
 
 HRESULT bpreader_init(struct bpreader_config *cfg, uint16_t port);
@@ -60,3 +63,21 @@ struct bpreader_cmd_footer {
     uint8_t checksum;
     uint8_t padding;
 };
+
+struct bpreader_poll_banapass_data {
+    uint8_t card_present;
+    uint8_t unknown1; // 0x01
+    uint8_t atqa[2];
+    uint8_t sak;
+    uint8_t unknown5; // 0x04
+    uint8_t serial_number[4]; // first 4 bytes of Chip ID
+};
+
+struct bpreader_poll_felica_data {
+    uint8_t card_present;
+    uint8_t unknown1; // 0x01
+    uint8_t unknown2[2]; // 0x14, 0x01
+    uint8_t idm[8];
+    uint8_t pmm[8];
+    uint8_t system_code[2];
+};