From e64292d47870efb3c5a3de4edd7b3249999d7f99 Mon Sep 17 00:00:00 2001 From: Willy-JL <49810075+Willy-JL@users.noreply.github.com> Date: Tue, 11 Mar 2025 08:58:57 +0000 Subject: [PATCH] NFC: Initial ISO14443-4 PCB listener handling --- lib/nfc/helpers/iso14443_4_layer.c | 111 +++++++++++++++++- lib/nfc/helpers/iso14443_4_layer.h | 29 ++++- lib/nfc/protocols/iso14443_4a/iso14443_4a_i.h | 3 +- .../iso14443_4a/iso14443_4a_listener.c | 51 +++++--- .../iso14443_4a/iso14443_4a_listener_i.c | 11 +- .../iso14443_4a/iso14443_4a_listener_i.h | 3 +- .../iso14443_4a/iso14443_4a_poller_i.c | 4 +- .../iso14443_4b/iso14443_4b_poller_i.c | 4 +- 8 files changed, 185 insertions(+), 31 deletions(-) diff --git a/lib/nfc/helpers/iso14443_4_layer.c b/lib/nfc/helpers/iso14443_4_layer.c index 4c5dcd6a4..3089ec59c 100644 --- a/lib/nfc/helpers/iso14443_4_layer.c +++ b/lib/nfc/helpers/iso14443_4_layer.c @@ -6,6 +6,8 @@ #define ISO14443_4_BLOCK_PCB_MASK (0x03) #define ISO14443_4_BLOCK_PCB_I (0U) +#define ISO14443_4_BLOCK_PCB_I_MASK (1U << 1) +#define ISO14443_4_BLOCK_PCB_I_ZERO_MASK (7U << 5) #define ISO14443_4_BLOCK_PCB_I_NAD_OFFSET (2) #define ISO14443_4_BLOCK_PCB_I_CID_OFFSET (3) #define ISO14443_4_BLOCK_PCB_I_CHAIN_OFFSET (4) @@ -25,8 +27,14 @@ #define ISO14443_4_BLOCK_PCB_S_CID_MASK (1U << ISO14443_4_BLOCK_PCB_R_CID_OFFSET) #define ISO14443_4_BLOCK_PCB_S_WTX_DESELECT_MASK (3U << ISO14443_4_BLOCK_PCB_S_WTX_DESELECT_OFFSET) +#define ISO14443_4_BLOCK_CID_MASK (0x0F) + #define ISO14443_4_BLOCK_PCB_BITS_ACTIVE(pcb, mask) (((pcb) & (mask)) == (mask)) +#define ISO14443_4_BLOCK_PCB_IS_I_BLOCK(pcb) \ + (ISO14443_4_BLOCK_PCB_BITS_ACTIVE(pcb, ISO14443_4_BLOCK_PCB_I_MASK) && \ + (((pcb) & ISO14443_4_BLOCK_PCB_I_ZERO_MASK) == 0)) + #define ISO14443_4_BLOCK_PCB_IS_R_BLOCK(pcb) \ ISO14443_4_BLOCK_PCB_BITS_ACTIVE(pcb, ISO14443_4_BLOCK_PCB_R_MASK) @@ -39,9 +47,16 @@ #define ISO14443_4_BLOCK_PCB_R_NACK_ACTIVE(pcb) \ ISO14443_4_BLOCK_PCB_BITS_ACTIVE(pcb, ISO14443_4_BLOCK_PCB_R_NACK_MASK) +#define ISO14443_4_LAYER_NAD_NOT_SUPPORTED ((uint8_t) - 1) +#define ISO14443_4_LAYER_NAD_NOT_SET ((uint8_t) - 2) + struct Iso14443_4Layer { uint8_t pcb; uint8_t pcb_prev; + + // Listener specific + uint8_t cid; + uint8_t nad; }; static inline void iso14443_4_layer_update_pcb(Iso14443_4Layer* instance) { @@ -65,6 +80,9 @@ void iso14443_4_layer_reset(Iso14443_4Layer* instance) { furi_assert(instance); instance->pcb_prev = 0; instance->pcb = ISO14443_4_BLOCK_PCB_I | ISO14443_4_BLOCK_PCB; + + instance->cid = ISO14443_4_LAYER_CID_NOT_SUPPORTED; + instance->nad = ISO14443_4_LAYER_NAD_NOT_SUPPORTED; } void iso14443_4_layer_set_i_block(Iso14443_4Layer* instance, bool chaining, bool CID_present) { @@ -88,7 +106,7 @@ void iso14443_4_layer_set_s_block(Iso14443_4Layer* instance, bool deselect, bool (CID_present << ISO14443_4_BLOCK_PCB_S_CID_OFFSET) | ISO14443_4_BLOCK_PCB; } -void iso14443_4_layer_encode_block( +void iso14443_4_layer_encode_command( Iso14443_4Layer* instance, const BitBuffer* input_data, BitBuffer* block_data) { @@ -105,7 +123,7 @@ static inline uint8_t iso14443_4_layer_get_response_pcb(const BitBuffer* block_d return data[0]; } -bool iso14443_4_layer_decode_block( +bool iso14443_4_layer_decode_response( Iso14443_4Layer* instance, BitBuffer* output_data, const BitBuffer* block_data) { @@ -138,3 +156,92 @@ bool iso14443_4_layer_decode_block( return ret; } + +void iso14443_4_layer_set_cid(Iso14443_4Layer* instance, uint8_t cid) { + instance->cid = cid; +} + +void iso14443_4_layer_set_nad_supported(Iso14443_4Layer* instance, bool nad) { + instance->nad = nad ? 0 : ISO14443_4_LAYER_NAD_NOT_SUPPORTED; +} + +Iso14443_4LayerStatus iso14443_4_layer_decode_command( + Iso14443_4Layer* instance, + const BitBuffer* input_data, + BitBuffer* block_data) { + uint8_t prologue_len = 0; + instance->pcb_prev = bit_buffer_get_byte(input_data, prologue_len++); + + if(ISO14443_4_BLOCK_PCB_IS_I_BLOCK(instance->pcb_prev)) { + if(instance->pcb_prev & ISO14443_4_BLOCK_PCB_I_CID_MASK) { + const uint8_t cid = bit_buffer_get_byte(input_data, prologue_len++) & + ISO14443_4_BLOCK_CID_MASK; + if(instance->cid == ISO14443_4_LAYER_CID_NOT_SUPPORTED || cid != instance->cid) { + return Iso14443_4LayerStatusIgnore; + } + } else if(instance->cid != ISO14443_4_LAYER_CID_NOT_SUPPORTED && instance->cid != 0) { + return Iso14443_4LayerStatusIgnore; + } + if(instance->pcb_prev & ISO14443_4_BLOCK_PCB_I_NAD_MASK) { + if(instance->nad == ISO14443_4_LAYER_NAD_NOT_SUPPORTED) { + return Iso14443_4LayerStatusIgnore; + } + instance->nad = bit_buffer_get_byte(input_data, prologue_len++); + // FIXME: unset NAD when chaining after first block + } + // FIXME: chaining + bit_buffer_copy_right(block_data, input_data, prologue_len); + return Iso14443_4LayerStatusOk; + + } else if(ISO14443_4_BLOCK_PCB_IS_S_BLOCK(instance->pcb_prev)) { + if(instance->pcb_prev & ISO14443_4_BLOCK_PCB_S_CID_MASK) { + const uint8_t cid = bit_buffer_get_byte(input_data, prologue_len++) & + ISO14443_4_BLOCK_CID_MASK; + if(instance->cid == ISO14443_4_LAYER_CID_NOT_SUPPORTED || cid != instance->cid) { + return Iso14443_4LayerStatusIgnore; + } + } else if(instance->cid != ISO14443_4_LAYER_CID_NOT_SUPPORTED && instance->cid != 0) { + return Iso14443_4LayerStatusIgnore; + } + if((instance->pcb_prev & ISO14443_4_BLOCK_PCB_S_WTX_DESELECT_MASK) == 0) { + // DESELECT + bit_buffer_copy(block_data, input_data); + return Iso14443_4LayerStatusSendAndHalt; + } else { + // WTX ACK or wrong value + return Iso14443_4LayerStatusIgnore; + } + + // FIXME: R blocks + } + return Iso14443_4LayerStatusIgnore; +} + +bool iso14443_4_layer_encode_response( + Iso14443_4Layer* instance, + const BitBuffer* input_data, + BitBuffer* block_data) { + if(ISO14443_4_BLOCK_PCB_IS_I_BLOCK(instance->pcb_prev)) { + instance->pcb = instance->pcb_prev; + bit_buffer_append_byte(block_data, instance->pcb); + if(instance->pcb_prev & ISO14443_4_BLOCK_PCB_I_CID_MASK) { + bit_buffer_append_byte(block_data, instance->cid); + } + if(instance->pcb_prev & ISO14443_4_BLOCK_PCB_I_NAD_MASK && + instance->nad != ISO14443_4_LAYER_NAD_NOT_SET) { + bit_buffer_append_byte(block_data, instance->nad); + instance->nad = ISO14443_4_LAYER_NAD_NOT_SET; + // FIXME: unset NAD when chaining after first block + } else { + instance->pcb &= ~ISO14443_4_BLOCK_PCB_I_NAD_MASK; + } + // FIXME: chaining + instance->pcb &= ~ISO14443_4_BLOCK_PCB_I_CHAIN_MASK; + bit_buffer_set_byte(block_data, 0, instance->pcb); + bit_buffer_append(block_data, input_data); + return true; + + // FIXME: R blocks + } + return false; +} diff --git a/lib/nfc/helpers/iso14443_4_layer.h b/lib/nfc/helpers/iso14443_4_layer.h index 67a7f37fe..344d3ccf5 100644 --- a/lib/nfc/helpers/iso14443_4_layer.h +++ b/lib/nfc/helpers/iso14443_4_layer.h @@ -18,16 +18,41 @@ void iso14443_4_layer_set_i_block(Iso14443_4Layer* instance, bool chaining, bool void iso14443_4_layer_set_r_block(Iso14443_4Layer* instance, bool acknowledged, bool CID_present); void iso14443_4_layer_set_s_block(Iso14443_4Layer* instance, bool deselect, bool CID_present); -void iso14443_4_layer_encode_block( +// Poller mode + +void iso14443_4_layer_encode_command( Iso14443_4Layer* instance, const BitBuffer* input_data, BitBuffer* block_data); -bool iso14443_4_layer_decode_block( +bool iso14443_4_layer_decode_response( Iso14443_4Layer* instance, BitBuffer* output_data, const BitBuffer* block_data); +// Listener mode + +typedef enum { + Iso14443_4LayerStatusOk, + Iso14443_4LayerStatusIgnore, + Iso14443_4LayerStatusSendAndHalt, +} Iso14443_4LayerStatus; + +Iso14443_4LayerStatus iso14443_4_layer_decode_command( + Iso14443_4Layer* instance, + const BitBuffer* input_data, + BitBuffer* block_data); + +bool iso14443_4_layer_encode_response( + Iso14443_4Layer* instance, + const BitBuffer* input_data, + BitBuffer* block_data); + +#define ISO14443_4_LAYER_CID_NOT_SUPPORTED ((uint8_t) - 1) +void iso14443_4_layer_set_cid(Iso14443_4Layer* instance, uint8_t cid); + +void iso14443_4_layer_set_nad_supported(Iso14443_4Layer* instance, bool nad); + #ifdef __cplusplus } #endif diff --git a/lib/nfc/protocols/iso14443_4a/iso14443_4a_i.h b/lib/nfc/protocols/iso14443_4a/iso14443_4a_i.h index 9b0230975..f28b26ddd 100644 --- a/lib/nfc/protocols/iso14443_4a/iso14443_4a_i.h +++ b/lib/nfc/protocols/iso14443_4a/iso14443_4a_i.h @@ -2,7 +2,8 @@ #include "iso14443_4a.h" -#define ISO14443_4A_CMD_READ_ATS (0xE0) +#define ISO14443_4A_CMD_READ_ATS (0xE0) +#define ISO14443_4A_READ_ATS_CID_MASK (0x0F) // ATS bit definitions #define ISO14443_4A_ATS_T0_TA1 (1U << 4) diff --git a/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener.c b/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener.c index b2e1a4e24..1d216a460 100644 --- a/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener.c +++ b/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener.c @@ -14,6 +14,7 @@ static Iso14443_4aListener* Iso14443_4aListener* instance = malloc(sizeof(Iso14443_4aListener)); instance->iso14443_3a_listener = iso14443_3a_listener; instance->data = data; + instance->iso14443_4_layer = iso14443_4_layer_alloc(); instance->tx_buffer = bit_buffer_alloc(ISO14443_4A_LISTENER_BUF_SIZE); @@ -30,10 +31,16 @@ static void iso14443_4a_listener_free(Iso14443_4aListener* instance) { furi_assert(instance->data); furi_assert(instance->tx_buffer); + iso14443_4_layer_free(instance->iso14443_4_layer); bit_buffer_free(instance->tx_buffer); free(instance); } +static void iso14443_4a_listener_reset(Iso14443_4aListener* instance) { + instance->state = Iso14443_4aListenerStateIdle; + iso14443_4_layer_reset(instance->iso14443_4_layer); +} + static void iso14443_4a_listener_set_callback( Iso14443_4aListener* instance, NfcGenericCallback callback, @@ -68,28 +75,44 @@ static NfcCommand iso14443_4a_listener_run(NfcGenericEvent event, void* context) if(iso14443_4a_listener_send_ats(instance, &instance->data->ats_data) == Iso14443_4aErrorNone) { instance->state = Iso14443_4aListenerStateActive; + if(iso14443_4a_supports_frame_option( + instance->data, Iso14443_4aFrameOptionCid)) { + const uint8_t cid = bit_buffer_get_byte(rx_buffer, 1) & + ISO14443_4A_READ_ATS_CID_MASK; + iso14443_4_layer_set_cid(instance->iso14443_4_layer, cid); + } + iso14443_4_layer_set_nad_supported( + instance->iso14443_4_layer, + iso14443_4a_supports_frame_option( + instance->data, Iso14443_4aFrameOptionNad)); } } - } else if(bit_buffer_get_size_bytes(rx_buffer) > 1) { - // TODO: This is a rudimentary PCB implementation! - // Just ignores its meaning and saves it for sending blocks to reader, - // there is no handling of S, R, I blocks and their different flags. - // We have iso14443_4_layer helper but it is entirely designed for poller, - // will need large rework for listener, for now this works. - instance->pcb_prev = bit_buffer_get_byte(rx_buffer, 0); - bit_buffer_copy_right(rx_buffer, rx_buffer, 1); + } else { + Iso14443_4LayerStatus status = + iso14443_4_layer_decode_command(instance->iso14443_4_layer, rx_buffer, rx_buffer); + if(status == Iso14443_4LayerStatusSendAndHalt) { + if(iso14443_3a_listener_send_standard_frame( + instance->iso14443_3a_listener, rx_buffer) == Iso14443_3aErrorNone) { + iso14443_4a_listener_reset(instance); + if(instance->callback) { + instance->iso14443_4a_event.type = Iso14443_4aListenerEventTypeHalted; + instance->callback(instance->generic_event, instance->context); + } + command = NfcCommandSleep; + } + } else if(status == Iso14443_4LayerStatusOk) { + instance->iso14443_4a_event.type = Iso14443_4aListenerEventTypeReceivedData; + instance->iso14443_4a_event.data->buffer = rx_buffer; - instance->iso14443_4a_event.type = Iso14443_4aListenerEventTypeReceivedData; - instance->iso14443_4a_event.data->buffer = rx_buffer; - - if(instance->callback) { - command = instance->callback(instance->generic_event, instance->context); + if(instance->callback) { + command = instance->callback(instance->generic_event, instance->context); + } } } } else if( iso14443_3a_event->type == Iso14443_3aListenerEventTypeHalted || iso14443_3a_event->type == Iso14443_3aListenerEventTypeFieldOff) { - instance->state = Iso14443_4aListenerStateIdle; + iso14443_4a_listener_reset(instance); instance->iso14443_4a_event.type = iso14443_3a_event->type == Iso14443_3aListenerEventTypeHalted ? diff --git a/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.c b/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.c index 20ffa3542..6ea5ae5e4 100644 --- a/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.c +++ b/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.c @@ -35,13 +35,10 @@ Iso14443_4aError iso14443_4a_listener_send_block(Iso14443_4aListener* instance, const BitBuffer* tx_buffer) { bit_buffer_reset(instance->tx_buffer); - // TODO: This is a rudimentary PCB implementation! - // Just sends back same PCB that was last received from reader, - // there is no handling of S, R, I blocks and their different flags. - // We have iso14443_4_layer helper but it is entirely designed for poller, - // will need large rework for listener, for now this works. - bit_buffer_append_byte(instance->tx_buffer, instance->pcb_prev); - bit_buffer_append(instance->tx_buffer, tx_buffer); + if(!iso14443_4_layer_encode_response( + instance->iso14443_4_layer, tx_buffer, instance->tx_buffer)) { + return Iso14443_4aErrorProtocol; + } const Iso14443_3aError error = iso14443_3a_listener_send_standard_frame( instance->iso14443_3a_listener, instance->tx_buffer); diff --git a/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.h b/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.h index 9588d6849..46233f21d 100644 --- a/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.h +++ b/lib/nfc/protocols/iso14443_4a/iso14443_4a_listener_i.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "iso14443_4a_listener.h" #include "iso14443_4a_i.h" @@ -17,7 +18,7 @@ typedef enum { struct Iso14443_4aListener { Iso14443_3aListener* iso14443_3a_listener; Iso14443_4aData* data; - uint8_t pcb_prev; + Iso14443_4Layer* iso14443_4_layer; Iso14443_4aListenerState state; BitBuffer* tx_buffer; diff --git a/lib/nfc/protocols/iso14443_4a/iso14443_4a_poller_i.c b/lib/nfc/protocols/iso14443_4a/iso14443_4a_poller_i.c index 427973f4a..ce8a14f08 100644 --- a/lib/nfc/protocols/iso14443_4a/iso14443_4a_poller_i.c +++ b/lib/nfc/protocols/iso14443_4a/iso14443_4a_poller_i.c @@ -64,7 +64,7 @@ Iso14443_4aError iso14443_4a_poller_send_block( furi_check(rx_buffer); bit_buffer_reset(instance->tx_buffer); - iso14443_4_layer_encode_block(instance->iso14443_4_layer, tx_buffer, instance->tx_buffer); + iso14443_4_layer_encode_command(instance->iso14443_4_layer, tx_buffer, instance->tx_buffer); Iso14443_4aError error = Iso14443_4aErrorNone; @@ -105,7 +105,7 @@ Iso14443_4aError iso14443_4a_poller_send_block( } while(bit_buffer_starts_with_byte(instance->rx_buffer, ISO14443_4A_SWTX)); } - if(!iso14443_4_layer_decode_block( + if(!iso14443_4_layer_decode_response( instance->iso14443_4_layer, rx_buffer, instance->rx_buffer)) { error = Iso14443_4aErrorProtocol; break; diff --git a/lib/nfc/protocols/iso14443_4b/iso14443_4b_poller_i.c b/lib/nfc/protocols/iso14443_4b/iso14443_4b_poller_i.c index da82e1417..2db2da9c1 100644 --- a/lib/nfc/protocols/iso14443_4b/iso14443_4b_poller_i.c +++ b/lib/nfc/protocols/iso14443_4b/iso14443_4b_poller_i.c @@ -24,7 +24,7 @@ Iso14443_4bError iso14443_4b_poller_send_block( furi_check(rx_buffer); bit_buffer_reset(instance->tx_buffer); - iso14443_4_layer_encode_block(instance->iso14443_4_layer, tx_buffer, instance->tx_buffer); + iso14443_4_layer_encode_command(instance->iso14443_4_layer, tx_buffer, instance->tx_buffer); Iso14443_4bError error = Iso14443_4bErrorNone; @@ -36,7 +36,7 @@ Iso14443_4bError iso14443_4b_poller_send_block( error = iso14443_4b_process_error(iso14443_3b_error); break; - } else if(!iso14443_4_layer_decode_block( + } else if(!iso14443_4_layer_decode_response( instance->iso14443_4_layer, rx_buffer, instance->rx_buffer)) { error = Iso14443_4bErrorProtocol; break;