<p>Harald Welte <strong>merged</strong> this change.</p><p><a href="https://gerrit.osmocom.org/9866">View Change</a></p><div style="white-space:pre-wrap">Approvals:
Jenkins Builder: Verified
Harald Welte: Looks good to me, approved
</div><pre style="font-family: monospace,monospace; white-space: pre-wrap;">sniff: add TPDU parsing (TPDUs become APDUs on the upper layer)<br><br>Change-Id: I09d050d95bd2ab140fe6b4926a37278eb08cc347<br>---<br>M firmware/libcommon/source/sniffer.c<br>1 file changed, 143 insertions(+), 10 deletions(-)<br><br></pre><pre style="font-family: monospace,monospace; white-space: pre-wrap;"><span>diff --git a/firmware/libcommon/source/sniffer.c b/firmware/libcommon/source/sniffer.c</span><br><span>index 4b3545f..bada46e 100644</span><br><span>--- a/firmware/libcommon/source/sniffer.c</span><br><span>+++ b/firmware/libcommon/source/sniffer.c</span><br><span>@@ -54,8 +54,8 @@</span><br><span> ISO7816_S_RESET, /*!< in Reset */</span><br><span> ISO7816_S_WAIT_ATR, /*!< waiting for ATR to start */</span><br><span> ISO7816_S_IN_ATR, /*!< while we are receiving the ATR */</span><br><span style="color: hsl(0, 100%, 40%);">- ISO7816_S_WAIT_APDU, /*!< waiting for start of new APDU */</span><br><span style="color: hsl(0, 100%, 40%);">- ISO7816_S_IN_APDU, /*!< inside a single APDU */</span><br><span style="color: hsl(120, 100%, 40%);">+ ISO7816_S_WAIT_TPDU, /*!< waiting for start of new TPDU */</span><br><span style="color: hsl(120, 100%, 40%);">+ ISO7816_S_IN_TPDU, /*!< inside a single TPDU */</span><br><span> ISO7816_S_IN_PPS_REQ, /*!< while we are inside the PPS request */</span><br><span> ISO7816_S_WAIT_PPS_RSP, /*!< waiting for start of the PPS response */</span><br><span> ISO7816_S_IN_PPS_RSP, /*!< while we are inside the PPS request */</span><br><span>@@ -87,6 +87,23 @@</span><br><span> PPS_S_WAIT_PCK, /*!< check byte */</span><br><span> };</span><br><span> </span><br><span style="color: hsl(120, 100%, 40%);">+/*! Transport Protocol Data Unit (TPDU) sub-states of ISO7816_S_IN_TPDU</span><br><span style="color: hsl(120, 100%, 40%);">+ * @note defined in ISO/IEC 7816-3:2006(E) section 10 and 12</span><br><span style="color: hsl(120, 100%, 40%);">+ * @remark APDUs are formed by one or more command+response TPDUs</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+enum tpdu_sniff_state {</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_CLA, /*!< class byte */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_INS, /*!< instruction byte */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_P1, /*!< first parameter byte for the instruction */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_P2, /*!< second parameter byte for the instruction */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_P3, /*!< third parameter byte encoding the data length */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_PROCEDURE, /*!< procedure byte (could also be SW1) */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_DATA_REMAINING, /*!< remaining data bytes */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_DATA_SINGLE, /*!< single data byte */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_SW1, /*!< first status word */</span><br><span style="color: hsl(120, 100%, 40%);">+ TPDU_S_SW2, /*!< second status word */</span><br><span style="color: hsl(120, 100%, 40%);">+};</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span> /*------------------------------------------------------------------------------</span><br><span> * Internal variables</span><br><span> *------------------------------------------------------------------------------*/</span><br><span>@@ -141,6 +158,15 @@</span><br><span> uint8_t pps_req[MAX_PPS_SIZE];</span><br><span> /*! PPS response data */</span><br><span> uint8_t pps_rsp[MAX_PPS_SIZE];</span><br><span style="color: hsl(120, 100%, 40%);">+/*! TPDU state */</span><br><span style="color: hsl(120, 100%, 40%);">+enum tpdu_sniff_state tpdu_state;</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Final TPDU packet</span><br><span style="color: hsl(120, 100%, 40%);">+ * @note this is the complete command+response TPDU, including header, data, and status words</span><br><span style="color: hsl(120, 100%, 40%);">+ * @remark this does not include the procedure bytes</span><br><span style="color: hsl(120, 100%, 40%);">+ */</span><br><span style="color: hsl(120, 100%, 40%);">+uint8_t tpdu_packet[5+256+2];</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Current index in TPDU packet */</span><br><span style="color: hsl(120, 100%, 40%);">+uint8_t tpdu_packet_i = 0;</span><br><span> </span><br><span> /*------------------------------------------------------------------------------</span><br><span> * Internal functions</span><br><span>@@ -183,6 +209,10 @@</span><br><span> case ISO7816_S_IN_PPS_RSP:</span><br><span> pps_state = PPS_S_WAIT_PPSS;</span><br><span> break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case ISO7816_S_WAIT_TPDU:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_CLA;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet_i = 0;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span> default:</span><br><span> break;</span><br><span> }</span><br><span>@@ -291,7 +321,7 @@</span><br><span> case ATR_S_WAIT_TCK: /* see ISO/IEC 7816-3:2006 section 8.2.5 */</span><br><span> /* we could verify the checksum, but we are just here to sniff */</span><br><span> print_atr(); /* print ATR for info */</span><br><span style="color: hsl(0, 100%, 40%);">- change_state(ISO7816_S_WAIT_APDU); /* go to next state */</span><br><span style="color: hsl(120, 100%, 40%);">+ change_state(ISO7816_S_WAIT_TPDU); /* go to next state */</span><br><span> break;</span><br><span> default:</span><br><span> TRACE_INFO("Unknown ATR state %u\n\r", atr_state);</span><br><span>@@ -352,7 +382,7 @@</span><br><span> pps_state = PPS_S_WAIT_PPS0; /* go to next state */</span><br><span> } else {</span><br><span> TRACE_INFO("Invalid PPSS received\n\r");</span><br><span style="color: hsl(0, 100%, 40%);">- change_state(ISO7816_S_WAIT_APDU); /* go back to APDU state */</span><br><span style="color: hsl(120, 100%, 40%);">+ change_state(ISO7816_S_WAIT_TPDU); /* go back to TPDU state */</span><br><span> }</span><br><span> break;</span><br><span> case PPS_S_WAIT_PPS0: /*!< format byte */</span><br><span>@@ -398,7 +428,7 @@</span><br><span> if (0==check) { /* checksum is valid */</span><br><span> change_state(ISO7816_S_WAIT_PPS_RSP); /* go to next state */</span><br><span> } else { /* checksum is invalid */</span><br><span style="color: hsl(0, 100%, 40%);">- change_state(ISO7816_S_WAIT_APDU); /* go to next state */</span><br><span style="color: hsl(120, 100%, 40%);">+ change_state(ISO7816_S_WAIT_TPDU); /* go to next state */</span><br><span> }</span><br><span> } else if (ISO7816_S_IN_PPS_RSP==iso_state) {</span><br><span> if (0==check) { /* checksum is valid */</span><br><span>@@ -415,7 +445,7 @@</span><br><span> } else { /* checksum is invalid */</span><br><span> TRACE_INFO("PPS negotiation failed\n\r");</span><br><span> }</span><br><span style="color: hsl(0, 100%, 40%);">- change_state(ISO7816_S_WAIT_APDU); /* co to next state */</span><br><span style="color: hsl(120, 100%, 40%);">+ change_state(ISO7816_S_WAIT_TPDU); /* go to next state */</span><br><span> }</span><br><span> break;</span><br><span> default:</span><br><span>@@ -423,8 +453,108 @@</span><br><span> }</span><br><span> }</span><br><span> </span><br><span style="color: hsl(0, 100%, 40%);">-static void process_byte_apdu(uint8_t byte)</span><br><span style="color: hsl(120, 100%, 40%);">+/*! Print current TPDU */</span><br><span style="color: hsl(120, 100%, 40%);">+static void print_tpdu(void)</span><br><span> {</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ISO7816_S_IN_TPDU!=iso_state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ TRACE_WARNING("Can't print TPDU in ISO 7816-3 state %u\n\r", iso_state);</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ led_blink(LED_GREEN, BLINK_2O_F);</span><br><span style="color: hsl(120, 100%, 40%);">+ printf("TPDU: ");</span><br><span style="color: hsl(120, 100%, 40%);">+ uint16_t i;</span><br><span style="color: hsl(120, 100%, 40%);">+ for (i = 0; i < tpdu_packet_i && i < ARRAY_SIZE(tpdu_packet); i++) {</span><br><span style="color: hsl(120, 100%, 40%);">+ printf("%02x ", tpdu_packet[i]);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ printf("\n\r");</span><br><span style="color: hsl(120, 100%, 40%);">+}</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+static void process_byte_tpdu(uint8_t byte)</span><br><span style="color: hsl(120, 100%, 40%);">+{</span><br><span style="color: hsl(120, 100%, 40%);">+ /* sanity check */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ISO7816_S_IN_TPDU!=iso_state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ TRACE_ERROR("Processing TPDU data in wrong ISO 7816-3 state %u\n\r", iso_state);</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ if (tpdu_packet_i>=ARRAY_SIZE(tpdu_packet)) {</span><br><span style="color: hsl(120, 100%, 40%);">+ TRACE_ERROR("TPDU data overflow\n\r");</span><br><span style="color: hsl(120, 100%, 40%);">+ return;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+</span><br><span style="color: hsl(120, 100%, 40%);">+ /* handle TPDU byte depending on current state */</span><br><span style="color: hsl(120, 100%, 40%);">+ switch (tpdu_state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_CLA:</span><br><span style="color: hsl(120, 100%, 40%);">+ if (0xff==byte) {</span><br><span style="color: hsl(120, 100%, 40%);">+ TRACE_WARNING("0xff is not a valid class byte\n\r");</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet_i = 0;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_INS;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_INS:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet_i = 1;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_P1;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_P1:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet_i = 2;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_P2;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_P2:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet_i = 3;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_P3;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_P3:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet_i = 4;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_PROCEDURE;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_PROCEDURE:</span><br><span style="color: hsl(120, 100%, 40%);">+ if (0x60==byte) { /* wait for next procedure byte */</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ } else if (tpdu_packet[1]==byte) { /* get all remaining data bytes */</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_DATA_REMAINING;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ } else if ((~tpdu_packet[1])==byte) { /* get single data byte */</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_DATA_SINGLE;</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_SW1:</span><br><span style="color: hsl(120, 100%, 40%);">+ if ((0x60==(byte&0xf0)) || (0x90==(byte&0xf0))) { /* this procedure byte is SW1 */</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_SW2;</span><br><span style="color: hsl(120, 100%, 40%);">+ } else {</span><br><span style="color: hsl(120, 100%, 40%);">+ TRACE_WARNING("invalid SW1 0x%02x\n\r", byte);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_SW2:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ print_tpdu(); /* print TPDU for info */</span><br><span style="color: hsl(120, 100%, 40%);">+ change_state(ISO7816_S_WAIT_TPDU); /* this is the end of the TPDU */</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_DATA_SINGLE:</span><br><span style="color: hsl(120, 100%, 40%);">+ case TPDU_S_DATA_REMAINING:</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_packet[tpdu_packet_i++] = byte;</span><br><span style="color: hsl(120, 100%, 40%);">+ if (0==tpdu_packet[4]) {</span><br><span style="color: hsl(120, 100%, 40%);">+ if (5+256<=tpdu_packet_i) {</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_SW1;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ } else {</span><br><span style="color: hsl(120, 100%, 40%);">+ if (5+tpdu_packet[4]<=tpdu_packet_i) {</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_SW1;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ if (TPDU_S_DATA_SINGLE==tpdu_state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ tpdu_state = TPDU_S_PROCEDURE;</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ break;</span><br><span style="color: hsl(120, 100%, 40%);">+ default:</span><br><span style="color: hsl(120, 100%, 40%);">+ TRACE_ERROR("unhandled TPDU state %u\n\r", tpdu_state);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span> }</span><br><span> </span><br><span> static void check_sniffed_data(void)</span><br><span>@@ -441,7 +571,7 @@</span><br><span> case ISO7816_S_IN_ATR: /* More ATR data incoming */</span><br><span> process_byte_atr(byte);</span><br><span> break;</span><br><span style="color: hsl(0, 100%, 40%);">- case ISO7816_S_WAIT_APDU: /* After the ATR we expect APDU or PPS data */</span><br><span style="color: hsl(120, 100%, 40%);">+ case ISO7816_S_WAIT_TPDU: /* After the ATR we expect TPDU or PPS data */</span><br><span> case ISO7816_S_WAIT_PPS_RSP:</span><br><span> if (byte == 0xff) {</span><br><span> if (ISO7816_S_WAIT_PPS_RSP==iso_state) {</span><br><span>@@ -452,8 +582,11 @@</span><br><span> process_byte_pps(byte);</span><br><span> break;</span><br><span> }</span><br><span style="color: hsl(0, 100%, 40%);">- case ISO7816_S_IN_APDU: /* More APDU data incoming */</span><br><span style="color: hsl(0, 100%, 40%);">- process_byte_apdu(byte);</span><br><span style="color: hsl(120, 100%, 40%);">+ case ISO7816_S_IN_TPDU: /* More TPDU data incoming */</span><br><span style="color: hsl(120, 100%, 40%);">+ if (ISO7816_S_WAIT_TPDU==iso_state) {</span><br><span style="color: hsl(120, 100%, 40%);">+ change_state(ISO7816_S_IN_TPDU);</span><br><span style="color: hsl(120, 100%, 40%);">+ }</span><br><span style="color: hsl(120, 100%, 40%);">+ process_byte_tpdu(byte);</span><br><span> break;</span><br><span> case ISO7816_S_IN_PPS_REQ:</span><br><span> case ISO7816_S_IN_PPS_RSP:</span><br><span></span><br></pre><p>To view, visit <a href="https://gerrit.osmocom.org/9866">change 9866</a>. To unsubscribe, or for help writing mail filters, visit <a href="https://gerrit.osmocom.org/settings">settings</a>.</p><div itemscope itemtype="http://schema.org/EmailMessage"><div itemscope itemprop="action" itemtype="http://schema.org/ViewAction"><link itemprop="url" href="https://gerrit.osmocom.org/9866"/><meta itemprop="name" content="View Change"/></div></div>
<div style="display:none"> Gerrit-Project: simtrace2 </div>
<div style="display:none"> Gerrit-Branch: master </div>
<div style="display:none"> Gerrit-MessageType: merged </div>
<div style="display:none"> Gerrit-Change-Id: I09d050d95bd2ab140fe6b4926a37278eb08cc347 </div>
<div style="display:none"> Gerrit-Change-Number: 9866 </div>
<div style="display:none"> Gerrit-PatchSet: 6 </div>
<div style="display:none"> Gerrit-Owner: Kévin Redon <kredon@sysmocom.de> </div>
<div style="display:none"> Gerrit-Reviewer: Harald Welte <laforge@gnumonks.org> </div>
<div style="display:none"> Gerrit-Reviewer: Jenkins Builder </div>