diff --git a/board/meson.build b/board/meson.build index ed763ff..922d34b 100644 --- a/board/meson.build +++ b/board/meson.build @@ -9,6 +9,11 @@ board_lib = static_library( sources : [ 'io3.c', 'io3.h', + 'sg-cmd.c', + 'sg-cmd.h', + 'sg-nfc.c', + 'sg-nfc.h', + 'sg-nfc-cmd.h', 'sg-frame.c', 'sg-frame.h', ], diff --git a/board/sg-nfc-cmd.h b/board/sg-nfc-cmd.h new file mode 100644 index 0000000..70a4f51 --- /dev/null +++ b/board/sg-nfc-cmd.h @@ -0,0 +1,86 @@ +#pragma once + +#include + +enum { + SG_NFC_CMD_GET_FW_VERSION = 0x30, + SG_NFC_CMD_GET_HW_VERSION = 0x32, + SG_NFC_CMD_40_POLL = 0x40, + SG_NFC_CMD_41_POLL = 0x41, + SG_NFC_CMD_MIFARE_POLL = 0x42, + SG_NFC_CMD_MIFARE_SELECT_TAG = 0x43, + SG_NFC_CMD_MIFARE_50 = 0x50, + SG_NFC_CMD_MIFARE_READ_BLOCK = 0x52, + SG_NFC_CMD_MIFARE_SET_KEY = 0x54, + SG_NFC_CMD_MIFARE_55 = 0x55, + SG_NFC_CMD_RESET = 0x62, +}; + +struct sg_nfc_resp_get_fw_version { + struct sg_resp_header resp; + char version[23]; +}; + +struct sg_nfc_resp_get_hw_version { + struct sg_resp_header resp; + char version[23]; +}; + +struct sg_nfc_req_mifare_set_key { + struct sg_req_header req; + uint8_t key_a[6]; +}; + +struct sg_nfc_req_mifare_50 { + struct sg_req_header req; + uint8_t payload[6]; +}; + +struct sg_nfc_req_poll_40 { + struct sg_req_header req; + uint8_t payload; +}; + +struct sg_nfc_resp_mifare_poll { + struct sg_resp_header resp; + union { + uint8_t none; + uint8_t some[7]; + } payload; +}; + +struct sg_nfc_req_mifare_select_tag { + struct sg_resp_header resp; + uint8_t uid[4]; +}; + +struct sg_nfc_req_mifare_read_block { + struct sg_req_header req; + struct { + uint8_t uid[4]; + uint8_t block_no; + } payload; +}; + +struct sg_nfc_resp_mifare_read_block { + struct sg_resp_header resp; + uint8_t block[16]; +}; + +union sg_nfc_req_any { + uint8_t bytes[256]; + struct sg_req_header simple; + struct sg_nfc_req_mifare_set_key mifare_set_key; + struct sg_nfc_req_mifare_read_block mifare_read_block; + struct sg_nfc_req_mifare_50 mifare_50; + struct sg_nfc_req_poll_40 poll_40; +}; + +union sg_nfc_resp_any { + uint8_t bytes[256]; + struct sg_resp_header simple; + struct sg_nfc_resp_get_fw_version get_fw_version; + struct sg_nfc_resp_get_hw_version get_hw_version; + struct sg_nfc_resp_mifare_poll mifare_poll; + struct sg_nfc_resp_mifare_read_block mifare_read_block; +}; diff --git a/board/sg-nfc.c b/board/sg-nfc.c new file mode 100644 index 0000000..0afd1b0 --- /dev/null +++ b/board/sg-nfc.c @@ -0,0 +1,326 @@ +#include + +#include +#include +#include +#include +#include + +#include "board/sg-cmd.h" +#include "board/sg-nfc.h" +#include "board/sg-nfc-cmd.h" + +#include "util/dprintf.h" + +static HRESULT sg_nfc_dispatch( + void *ctx, + const void *v_req, + void *v_resp); + +static HRESULT sg_nfc_cmd_reset( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_resp_header *resp); + +static HRESULT sg_nfc_cmd_get_fw_version( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_nfc_resp_get_fw_version *resp); + +static HRESULT sg_nfc_cmd_get_hw_version( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_nfc_resp_get_hw_version *resp); + +static HRESULT sg_nfc_cmd_mifare_poll( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_nfc_resp_mifare_poll *resp); + +static HRESULT sg_nfc_cmd_mifare_read_block( + const struct sg_nfc *nfc, + const struct sg_nfc_req_mifare_read_block *req, + struct sg_nfc_resp_mifare_read_block *resp); + +static HRESULT sg_nfc_mifare_read_block_1( + const struct sg_nfc *nfc, + uint8_t *block); + +static HRESULT sg_nfc_mifare_read_block_2( + const struct sg_nfc *nfc, + uint8_t *block); + +static HRESULT sg_nfc_cmd_dummy( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_resp_header *resp); + +static const uint8_t sg_nfc_block_1[] = { + 'S', 'B', 'D', 'T', 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x4E, 0xC6, 0x22, +}; + +void sg_nfc_init( + struct sg_nfc *nfc, + uint8_t addr, + const struct sg_nfc_ops *ops, + void *ops_ctx) +{ + assert(nfc != NULL); + assert(ops != NULL); + + nfc->ops = ops; + nfc->ops_ctx = ops_ctx; + nfc->addr = addr; +} + +#ifdef NDEBUG +#define sg_nfc_dprintfv(nfc, fmt, ap) +#define sg_nfc_dprintf(nfc, fmt, ...) +#else +static void sg_nfc_dprintfv( + const struct sg_nfc *nfc, + const char *fmt, + va_list ap) +{ + dprintf("NFC %02x: ", nfc->addr); + dprintfv(fmt, ap); +} + +static void sg_nfc_dprintf( + const struct sg_nfc *nfc, + const char *fmt, + ...) +{ + va_list ap; + + va_start(ap, fmt); + sg_nfc_dprintfv(nfc, fmt, ap); + va_end(ap); +} +#endif + +void sg_nfc_transact( + struct sg_nfc *nfc, + struct iobuf *resp_frame, + const void *req_bytes, + size_t req_nbytes) +{ + assert(nfc != NULL); + assert(resp_frame != NULL); + assert(req_bytes != NULL); + + sg_req_transact(resp_frame, req_bytes, req_nbytes, sg_nfc_dispatch, nfc); +} + +static HRESULT sg_nfc_dispatch( + void *ctx, + const void *v_req, + void *v_resp) +{ + const struct sg_nfc *nfc; + const union sg_nfc_req_any *req; + union sg_nfc_resp_any *resp; + + nfc = ctx; + req = v_req; + resp = v_resp; + + if (req->simple.hdr.addr != nfc->addr) { + /* Not addressed to us, don't send a response */ + return S_FALSE; + } + + switch (req->simple.hdr.cmd) { + case SG_NFC_CMD_RESET: + return sg_nfc_cmd_reset(nfc, &req->simple, &resp->simple); + + case SG_NFC_CMD_GET_FW_VERSION: + return sg_nfc_cmd_get_fw_version( + nfc, + &req->simple, + &resp->get_fw_version); + + case SG_NFC_CMD_GET_HW_VERSION: + return sg_nfc_cmd_get_hw_version( + nfc, + &req->simple, + &resp->get_hw_version); + + case SG_NFC_CMD_MIFARE_POLL: + return sg_nfc_cmd_mifare_poll( + nfc, + &req->simple, + &resp->mifare_poll); + + case SG_NFC_CMD_MIFARE_READ_BLOCK: + return sg_nfc_cmd_mifare_read_block( + nfc, + &req->mifare_read_block, + &resp->mifare_read_block); + + case SG_NFC_CMD_40_POLL: + case SG_NFC_CMD_41_POLL: + case SG_NFC_CMD_MIFARE_SET_KEY: + case SG_NFC_CMD_MIFARE_SELECT_TAG: + case SG_NFC_CMD_MIFARE_50: + case SG_NFC_CMD_MIFARE_55: + return sg_nfc_cmd_dummy(nfc, &req->simple, &resp->simple); + + default: + sg_nfc_dprintf(nfc, "Unimpl command %02x\n", req->simple.hdr.cmd); + + return E_NOTIMPL; + } +} + +static HRESULT sg_nfc_cmd_reset( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_resp_header *resp) +{ + sg_nfc_dprintf(nfc, "Reset\n"); + sg_resp_init(resp, req, 0); + resp->status = 3; + + return S_OK; +} + +static HRESULT sg_nfc_cmd_get_fw_version( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_nfc_resp_get_fw_version *resp) +{ + /* Dest version is not NUL terminated, this is intentional */ + sg_resp_init(&resp->resp, req, sizeof(resp->version)); + memcpy(resp->version, "TN32MSEC003S F/W Ver1.2E", sizeof(resp->version)); + + return S_OK; +} + +static HRESULT sg_nfc_cmd_get_hw_version( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_nfc_resp_get_hw_version *resp) +{ + /* Dest version is not NUL terminated, this is intentional */ + sg_resp_init(&resp->resp, req, sizeof(resp->version)); + memcpy(resp->version, "TN32MSEC003S H/W Ver3.0J", sizeof(resp->version)); + + return S_OK; +} + +static HRESULT sg_nfc_cmd_mifare_poll( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_nfc_resp_mifare_poll *resp) +{ + HRESULT hr; + + hr = nfc->ops->mifare_poll(nfc->ops_ctx); + + if (hr == S_OK) { + sg_nfc_dprintf(nfc, "Mifare card is present\n"); + + sg_resp_init(&resp->resp, req, sizeof(resp->payload.some)); + resp->payload.some[0] = 0x01; /* Chunk size? */ + resp->payload.some[1] = 0x10; /* Unknown */ + resp->payload.some[2] = 0x04; /* Chunk size? */ + resp->payload.some[3] = 0x52; /* UID byte 0 */ + resp->payload.some[4] = 0xCC; /* UID byte 1 */ + resp->payload.some[5] = 0x55; /* UID byte 2 */ + resp->payload.some[6] = 0x25; /* UID byte 3 */ + + return S_OK; + } else if (hr == S_FALSE) { + sg_resp_init(&resp->resp, req, sizeof(resp->payload.none)); + resp->payload.none = 0x00; + + return S_OK; + } else if (FAILED(hr)) { + sg_nfc_dprintf(nfc, "nfc->ops->mifare_poll error: %x\n", (int) hr); + + return hr; + } else { + sg_nfc_dprintf(nfc, "nfc->ops->mifare_poll bad return: %x\n", (int) hr); + + return E_UNEXPECTED; + } +} + +static HRESULT sg_nfc_cmd_mifare_read_block( + const struct sg_nfc *nfc, + const struct sg_nfc_req_mifare_read_block *req, + struct sg_nfc_resp_mifare_read_block *resp) +{ + if (req->req.payload_len != sizeof(req->payload)) { + sg_nfc_dprintf(nfc, "%s: Payload size is incorrect\n", __func__); + + return E_FAIL; + } + + sg_nfc_dprintf(nfc, "Read block %i\n", req->payload.block_no); + + sg_resp_init(&resp->resp, &req->req, sizeof(resp->block)); + + switch (req->payload.block_no) { + case 1: + return sg_nfc_mifare_read_block_1(nfc, resp->block); + + case 2: + return sg_nfc_mifare_read_block_2(nfc, resp->block); + + case 0: + case 3: + sg_nfc_dprintf( + nfc, + "Block %i access not implemented\n", + req->payload.block_no); + + return E_NOTIMPL; + + default: + sg_nfc_dprintf( + nfc, + "Read from invalid mifare block nr %i\n", + req->payload.block_no); + + return E_INVALIDARG; + } +} + +static HRESULT sg_nfc_mifare_read_block_1( + const struct sg_nfc *nfc, + uint8_t *block) +{ + memcpy(block, sg_nfc_block_1, sizeof(sg_nfc_block_1)); + + return S_OK; +} + +static HRESULT sg_nfc_mifare_read_block_2( + const struct sg_nfc *nfc, + uint8_t *block) +{ + HRESULT hr; + + hr = nfc->ops->mifare_read_luid(nfc->ops_ctx, &block[6], 10); + + if (FAILED(hr)) { + return hr; + } + + memset(block, 0, 6); + + return S_OK; +} + +static HRESULT sg_nfc_cmd_dummy( + const struct sg_nfc *nfc, + const struct sg_req_header *req, + struct sg_resp_header *resp) +{ + sg_resp_init(resp, req, 0); + + return S_OK; +} diff --git a/board/sg-nfc.h b/board/sg-nfc.h new file mode 100644 index 0000000..507c6a8 --- /dev/null +++ b/board/sg-nfc.h @@ -0,0 +1,31 @@ +#pragma once + +#include + +#include +#include + +#include "hook/iobuf.h" + +struct sg_nfc_ops { + HRESULT (*mifare_poll)(void *ctx); + HRESULT (*mifare_read_luid)(void *ctx, uint8_t *luid, size_t nbytes); +}; + +struct sg_nfc { + const struct sg_nfc_ops *ops; + void *ops_ctx; + uint8_t addr; +}; + +void sg_nfc_init( + struct sg_nfc *nfc, + uint8_t addr, + const struct sg_nfc_ops *ops, + void *ops_ctx); + +void sg_nfc_transact( + struct sg_nfc *nfc, + struct iobuf *resp_frame, + const void *req_bytes, + size_t req_nbytes);