From 75b1d9cd016cfefc857df81721a814abf8592c9a Mon Sep 17 00:00:00 2001
From: merlokk <807634+merlokk@users.noreply.github.com>
Date: Thu, 4 Jul 2019 17:38:34 +0300
Subject: [PATCH] offset calc refactoring

---
 targets/stm32l432/src/nfc.c | 18 ++++++++++++------
 targets/stm32l432/src/nfc.h |  2 ++
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/targets/stm32l432/src/nfc.c b/targets/stm32l432/src/nfc.c
index 54c3859..7e65def 100644
--- a/targets/stm32l432/src/nfc.c
+++ b/targets/stm32l432/src/nfc.c
@@ -14,6 +14,16 @@
 
 #define IS_IRQ_ACTIVE()         (1  == (LL_GPIO_ReadInputPort(SOLO_AMS_IRQ_PORT) & SOLO_AMS_IRQ_PIN))
 
+uint8_t p14443_block_offset(uint8_t pcb) {
+    uint8_t offset = 1;
+    // NAD following
+    if (pcb & 0x04) offset++;
+    // CID following
+    if (pcb & 0x08) offset++;
+    
+    return offset;
+}
+
 // Capability container
 const CAPABILITY_CONTAINER NFC_CC = {
     .cclen_hi = 0x00, .cclen_lo = 0x0f,
@@ -443,14 +453,10 @@ void nfc_process_iblock(uint8_t * buf, int len)
     printf1(TAG_NFC,"Iblock: ");
 	dump_hex1(TAG_NFC, buf, len);
 
-    uint8_t apdu_offset = 1;
-    // NAD following
-    if (buf[0] & 0x04) apdu_offset++;
-    // CID following
-    if (buf[0] & 0x08) apdu_offset++;
+    uint8_t block_offset = p14443_block_offset(buf[0]);
 
     APDU_STRUCT apdu;
-    if (apdu_decode(buf + apdu_offset, len - apdu_offset, &apdu)) {
+    if (apdu_decode(buf + block_offset, len - block_offset, &apdu)) {
         printf1(TAG_NFC,"apdu decode error\n");
         nfc_write_response(buf[0], SW_COND_USE_NOT_SATISFIED);
         return;
diff --git a/targets/stm32l432/src/nfc.h b/targets/stm32l432/src/nfc.h
index 9871537..f284a8e 100644
--- a/targets/stm32l432/src/nfc.h
+++ b/targets/stm32l432/src/nfc.h
@@ -40,6 +40,8 @@ typedef struct
 #define NFC_CMD_SBLOCK                0xc0
 #define IS_SBLOCK(x)                  ( (((x) & 0xc0) == NFC_CMD_SBLOCK) && (((x) & 0x02) == 0x02) )
 
+extern uint8_t p14443_block_offset(uint8_t pcb);
+
 #define NFC_SBLOCK_DESELECT           0x30
 #define NFC_SBLOCK_WTX                0x30