USB: add flags for sniff data and centralise transfer

Also fix issue in usb_msg_alloc_hdr and add cosmetic spaces around
operations.

Change-Id: I768a0ad639aa5e648a630af72d01f7b68082b6b6

3 files changed, 134 insertions, 136 deletions

diff --git a/firmware/libcommon/include/simtrace_prot.h b/firmware/libcommon/include/simtrace_prot.h
index 1e9d72f..f1f736b 100644
--- a/firmware/libcommon/include/simtrace_prot.h
+++ b/firmware/libcommon/include/simtrace_prot.h
@@ -294,6 +294,9 @@ struct st_modem_status {
 #define SNIFF_CHANGE_FLAG_RESET_HOLD (1<<2)
 #define SNIFF_CHANGE_FLAG_RESET_RELEASE (1<<3)
 #define SNIFF_CHANGE_FLAG_TIMEOUT_WT (1<<4)
+/* SIMTRACE_MSGT_SNIFF_ATR, SIMTRACE_MSGT_SNIFF_PPS, SIMTRACE_MSGT_SNIFF_TPDU flags */
+#define SNIFF_DATA_FLAG_ERROR_INCOMPLETE (1<<5)
+#define SNIFF_DATA_FLAG_ERROR_MALFORMED (1<<6)
 
 /* SIMTRACE_MSGT_SNIFF_CHANGE */
 struct sniff_change {
@@ -309,8 +312,8 @@ struct sniff_fidi {
 
 /* SIMTRACE_MSGT_SNIFF_ATR, SIMTRACE_MSGT_SNIFF_PPS, SIMTRACE_MSGT_SNIFF_TPDU */
 struct sniff_data {
-	/* if the data is complete (an error might have occurred during transmission */
-	bool complete;
+	/* data flags */
+	uint32_t flags;
 	/* data length */
 	uint16_t length;
 	/* data */
diff --git a/firmware/libcommon/source/sniffer.c b/firmware/libcommon/source/sniffer.c
index 81a6712..4ec56d9 100644
--- a/firmware/libcommon/source/sniffer.c
+++ b/firmware/libcommon/source/sniffer.c
@@ -234,8 +234,8 @@ static struct msgb *usb_msg_alloc_hdr(uint8_t ep, uint8_t msg_class, uint8_t msg
 	usb_msg->l1h = msgb_put(usb_msg, sizeof(*usb_msg_header));
 	usb_msg_header = (struct simtrace_msg_hdr *) usb_msg->l1h;
 	memset(usb_msg_header, 0, sizeof(*usb_msg_header));
-	usb_msg_header->msg_class = SIMTRACE_MSGC_SNIFF;
-	usb_msg_header->msg_type = SIMTRACE_MSGT_SNIFF_CHANGE;
+	usb_msg_header->msg_class = msg_class;
+	usb_msg_header->msg_type = msg_type;
 	usb_msg->l2h = usb_msg->l1h + sizeof(*usb_msg_header);
 
 	return usb_msg;
@@ -296,11 +296,74 @@ static void change_state(enum iso7816_3_sniff_state iso_state_new)
 	//TRACE_INFO("Changed to ISO 7816-3 state %u\n\r", iso_state); /* don't print since this is function is also called by ISRs */
 }
 
+static void usb_send_data(enum simtrace_msg_type_sniff type, const uint8_t* data, uint16_t length, uint32_t flags)
+{
+	/* Sanity check */
+	if (type != SIMTRACE_MSGT_SNIFF_ATR && type != SIMTRACE_MSGT_SNIFF_PPS && type != SIMTRACE_MSGT_SNIFF_TPDU) {
+		return;
+	}
+
+	/* Show activity on LED */
+	led_blink(LED_GREEN, BLINK_2O_F);
+
+	/* Send data over USB */
+	struct msgb *usb_msg = usb_msg_alloc_hdr(SIMTRACE_USB_EP_CARD_DATAIN, SIMTRACE_MSGC_SNIFF, type);
+	if (!usb_msg) {
+		return;
+	}
+	struct sniff_data *usb_sniff_data = (struct sniff_data *) msgb_put(usb_msg, sizeof(*usb_sniff_data));
+	usb_sniff_data->flags = flags;
+	usb_sniff_data->length = length;
+	uint8_t *sniff_data = msgb_put(usb_msg, usb_sniff_data->length);
+	memcpy(sniff_data, data, length);
+	usb_msg_upd_len_and_submit(usb_msg);
+
+	/* Print message */
+	switch (type) {
+	case SIMTRACE_MSGT_SNIFF_ATR:
+		printf("ATR");
+		break;
+	case SIMTRACE_MSGT_SNIFF_PPS:
+		printf("PPS");
+		break;
+	case SIMTRACE_MSGT_SNIFF_TPDU:
+		printf("TPDU");
+		break;
+	default:
+		printf("???");
+		break;
+	}
+	if (flags) {
+		printf(" (");
+		if (flags & SNIFF_DATA_FLAG_ERROR_INCOMPLETE) {
+			printf("incomplete");
+			flags &= ~SNIFF_DATA_FLAG_ERROR_INCOMPLETE;
+			if (flags) {
+				printf(", ");
+			}
+		}
+		if (flags & SNIFF_DATA_FLAG_ERROR_MALFORMED) {
+			printf("malformed");
+			flags &= ~SNIFF_DATA_FLAG_ERROR_MALFORMED;
+			if (flags) {
+				printf(", ");
+			}
+		}
+		printf(")");
+	}
+	printf(": ");
+	uint16_t i;
+	for (i = 0; i < length; i++) {
+		printf("%02x ", data[i]);
+	}
+	printf("\n\r");
+}
+
 /*! Send current ATR over USB
- *  @param[in] complete if the ATR is complete
+ *  @param[in] flags SNIFF_DATA_FLAG_ data flags 
  *  @note Also print the ATR to debug console
  */
-static void usb_send_atr(bool complete)
+static void usb_send_atr(uint32_t flags)
 {
 	/* Check state */
 	if (ISO7816_S_IN_ATR != iso_state) {
@@ -312,28 +375,8 @@ static void usb_send_atr(bool complete)
 		return;
 	}
 
-	/* Show activity on LED */
-	led_blink(LED_GREEN, BLINK_2O_F);
-
-	/* Print ATR */
-	printf("ATR%s: ", complete ? "" : " (incomplete)");
-	uint8_t i;
-	for (i = 0; i < atr_i; i++) {
-		printf("%02x ", atr[i]);
-	}
-	printf("\n\r");
-
 	/* Send ATR over USB */
-	struct msgb *usb_msg = usb_msg_alloc_hdr(SIMTRACE_USB_EP_CARD_DATAIN, SIMTRACE_MSGC_SNIFF, SIMTRACE_MSGT_SNIFF_ATR);
-	if (!usb_msg) {
-		return;
-	}
-	struct sniff_data *usb_sniff_data_atr = (struct sniff_data *) msgb_put(usb_msg, sizeof(*usb_sniff_data_atr));
-	usb_sniff_data_atr->complete = complete;
-	usb_sniff_data_atr->length = atr_i;
-	uint8_t *data = msgb_put(usb_msg, usb_sniff_data_atr->length);
-	memcpy(data, atr, atr_i);
-	usb_msg_upd_len_and_submit(usb_msg);
+	usb_send_data(SIMTRACE_MSGT_SNIFF_ATR, atr, atr_i, flags);
 }
 
 /*! Process ATR byte
@@ -428,7 +471,7 @@ static void process_byte_atr(uint8_t byte)
 		}
 	case ATR_S_WAIT_TCK:  /* see ISO/IEC 7816-3:2006 section 8.2.5 */
 		/* we could verify the checksum, but we are just here to sniff */
-		usb_send_atr(true); /* send ATR to host software using USB */
+		usb_send_atr(0); /* send ATR to host software using USB */
 		change_state(ISO7816_S_WAIT_TPDU); /* go to next state */
 		break;
 	default:
@@ -437,10 +480,10 @@ static void process_byte_atr(uint8_t byte)
 }
 
 /*! Send current PPS over USB
- *  @param[in] complete if the PPS is complete
+ *  @param[in] flags SNIFF_DATA_FLAG_ data flags
  *  @note Also print the PPS over the debug console
  */
-static void usb_send_pps(bool complete)
+static void usb_send_pps(uint32_t flags)
 {
 	uint8_t *pps_cur; /* current PPS (request or response) */
 
@@ -475,29 +518,9 @@ static void usb_send_pps(bool complete)
 	if (pps_state > PPS_S_WAIT_PCK) {
 		pps[pps_i++] = pps_cur[5];
 	}
-	
-	/* Show activity on LED */
-	led_blink(LED_GREEN, BLINK_2O_F);
-
-	/* Print PPS */
-	printf("PPS%s: ", complete ? "" : " (incomplete)");
-	uint8_t i;
-	for (i = 0;  i < pps_i; i++) {
-		printf("%02x ", pps[i]);
-	}
-	printf("\n\r");
 
 	/* Send message over USB */
-	struct msgb *usb_msg = usb_msg_alloc_hdr(SIMTRACE_USB_EP_CARD_DATAIN, SIMTRACE_MSGC_SNIFF, SIMTRACE_MSGT_SNIFF_PPS);
-	if (!usb_msg) {
-		return;
-	}
-	struct sniff_data *usb_sniff_data_pps = (struct sniff_data *) msgb_put(usb_msg, sizeof(*usb_sniff_data_pps));
-	usb_sniff_data_pps->complete = complete;
-	usb_sniff_data_pps->length = pps_i;
-	uint8_t *data = msgb_put(usb_msg, usb_sniff_data_pps->length);
-	memcpy(data, pps, pps_i);
-	usb_msg_upd_len_and_submit(usb_msg);
+	usb_send_data(SIMTRACE_MSGT_SNIFF_PPS, pps, pps_i, flags);
 }
 
 /*! Send Fi/Di change over USB
@@ -579,7 +602,7 @@ static void process_byte_pps(uint8_t byte)
 		}
 		check ^= pps_cur[5];
 		pps_state = PPS_S_WAIT_END;
-		usb_send_pps(true); /* send PPS to host software using USB */
+		usb_send_pps(0); /* send PPS to host software using USB */
 		if (ISO7816_S_IN_PPS_REQ == iso_state) {
 			if (0 == check) { /* checksum is valid */
 				change_state(ISO7816_S_WAIT_PPS_RSP); /* go to next state */
@@ -616,10 +639,10 @@ static void process_byte_pps(uint8_t byte)
 }
 
 /*! Send current TPDU over USB
- *  @param[in] complete if the TPDU is complete
+ *  @param[in] flags SNIFF_DATA_FLAG_ data flags
  *  @note Also print the TPDU over the debug console
  */
-static void usb_send_tpdu(bool complete)
+static void usb_send_tpdu(uint32_t flags)
 {
 	/* Check state */
 	if (ISO7816_S_IN_TPDU != iso_state) {
@@ -627,28 +650,8 @@ static void usb_send_tpdu(bool complete)
 		return;
 	}
 
-	/* Show activity on LED */
-	led_blink(LED_GREEN, BLINK_2O_F);
-
-	/* Print TPDU */
-	printf("TPDU%s: ", complete ? "" : " (incomplete)");
-	uint16_t i;
-	for (i = 0; i < tpdu_packet_i && i < ARRAY_SIZE(tpdu_packet); i++) {
-		printf("%02x ", tpdu_packet[i]);
-	}
-	printf("\n\r");
-
 	/* Send ATR over USB */
-	struct msgb *usb_msg = usb_msg_alloc_hdr(SIMTRACE_USB_EP_CARD_DATAIN, SIMTRACE_MSGC_SNIFF, SIMTRACE_MSGT_SNIFF_TPDU);
-	if (!usb_msg) {
-		return;
-	}
-	struct sniff_data *usb_sniff_data_tpdu = (struct sniff_data *) msgb_put(usb_msg, sizeof(*usb_sniff_data_tpdu));
-	usb_sniff_data_tpdu->complete = complete;
-	usb_sniff_data_tpdu->length = tpdu_packet_i;
-	uint8_t *data = msgb_put(usb_msg, usb_sniff_data_tpdu->length);
-	memcpy(data, tpdu_packet, tpdu_packet_i);
-	usb_msg_upd_len_and_submit(usb_msg);
+	usb_send_data(SIMTRACE_MSGT_SNIFF_TPDU, tpdu_packet, tpdu_packet_i, flags);
 }
 
 static void process_byte_tpdu(uint8_t byte)
@@ -722,7 +725,7 @@ static void process_byte_tpdu(uint8_t byte)
 		break;
 	case TPDU_S_SW2:
 		tpdu_packet[tpdu_packet_i++] = byte;
-		usb_send_tpdu(true); /* send TPDU to host software using USB */
+		usb_send_tpdu(0); /* send TPDU to host software using USB */
 		change_state(ISO7816_S_WAIT_TPDU); /* this is the end of the TPDU */
 		break;
 	case TPDU_S_DATA_SINGLE:
@@ -993,16 +996,16 @@ void Sniffer_run(void)
 			/* Use timeout to detect interrupted data transmission */
 			switch (iso_state) {
 			case ISO7816_S_IN_ATR:
-				usb_send_atr(false); /* send incomplete ATR to host software using USB */
+				usb_send_atr(SNIFF_DATA_FLAG_ERROR_INCOMPLETE); /* send incomplete ATR to host software using USB */
 				change_state(ISO7816_S_WAIT_ATR);
 				break;
 			case ISO7816_S_IN_TPDU:
-				usb_send_tpdu(false); /* send incomplete PPS to host software using USB */
+				usb_send_tpdu(SNIFF_DATA_FLAG_ERROR_INCOMPLETE); /* send incomplete PPS to host software using USB */
 				change_state(ISO7816_S_WAIT_TPDU);
 				break;
 			case ISO7816_S_IN_PPS_REQ:
 			case ISO7816_S_IN_PPS_RSP:
-				usb_send_pps(false); /* send incomplete TPDU to host software using USB */
+				usb_send_pps(SNIFF_DATA_FLAG_ERROR_INCOMPLETE); /* send incomplete TPDU to host software using USB */
 				change_state(ISO7816_S_WAIT_TPDU);
 				break;
 			default:
diff --git a/host/simtrace2-sniff.c b/host/simtrace2-sniff.c
index b61b7d0..eb547b0 100644
--- a/host/simtrace2-sniff.c
+++ b/host/simtrace2-sniff.c
@@ -97,7 +97,7 @@ static int gsmtap_send_sim(const uint8_t *apdu, unsigned int len)
 static int process_change(const uint8_t *buf, int len)
 {
 	/* check if there is enough data for the structure */
-	if (len<sizeof(struct sniff_change)) {
+	if (len < sizeof(struct sniff_change)) {
 		return -1;
 	}
 	struct sniff_change *change = (struct sniff_change *)buf;
@@ -141,73 +141,69 @@ static int process_fidi(const uint8_t *buf, int len)
 	return 0;
 }
 
-static int process_atr(const uint8_t *buf, int len)
+static int process_data(enum simtrace_msg_type_sniff type, const uint8_t *buf, int len)
 {
 	/* check if there is enough data for the structure */
-	if (len<sizeof(struct sniff_data)) {
+	if (len < sizeof(struct sniff_data)) {
 		return -1;
 	}
-	struct sniff_data *atr = (struct sniff_data *)buf;
+	struct sniff_data *data = (struct sniff_data *)buf;
 
 	/* check if the data is available */
-	if (len<sizeof(struct sniff_data)+atr->length) {
+	if (len < sizeof(struct sniff_data) + data->length) {
 		return -2;
 	}
 
-	printf("ATR%s: ", atr->complete ? "" : " (incomplete)");
-	uint16_t i;
-	for (i = 0; i < atr->length; i++) {
-		printf("%02x ", atr->data[i]);
+	/* check type */
+	if (type != SIMTRACE_MSGT_SNIFF_ATR && type != SIMTRACE_MSGT_SNIFF_PPS && type != SIMTRACE_MSGT_SNIFF_TPDU) {
+		return -3;
 	}
-	printf("\n");
-	return 0;
-}
 
-static int process_pps(const uint8_t *buf, int len)
-{
-	/* check if there is enough data for the structure */
-	if (len<sizeof(struct sniff_data)) {
-		return -1;
+	/* Print message */
+	switch (type) {
+	case SIMTRACE_MSGT_SNIFF_ATR:
+		printf("ATR");
+		break;
+	case SIMTRACE_MSGT_SNIFF_PPS:
+		printf("PPS");
+		break;
+	case SIMTRACE_MSGT_SNIFF_TPDU:
+		printf("TPDU");
+		break;
+	default:
+		printf("???");
+		break;
 	}
-	struct sniff_data *pps = (struct sniff_data *)buf;
-
-	/* check if the data is available */
-	if (len<sizeof(struct sniff_data)+pps->length) {
-		return -2;
+	if (data->flags) {
+		printf(" (");
+		if (data->flags & SNIFF_DATA_FLAG_ERROR_INCOMPLETE) {
+			printf("incomplete");
+			data->flags &= ~SNIFF_DATA_FLAG_ERROR_INCOMPLETE;
+			if (data->flags) {
+				printf(", ");
+			}
+		}
+		if (data->flags & SNIFF_DATA_FLAG_ERROR_MALFORMED) {
+			printf("malformed");
+			data->flags &= ~SNIFF_DATA_FLAG_ERROR_MALFORMED;
+			if (data->flags) {
+				printf(", ");
+			}
+		}
+		printf(")");
 	}
-
-	printf("PPS%s: ", pps->complete ? "" : " (incomplete) ");
+	printf(": ");
 	uint16_t i;
-	for (i = 0; i < pps->length; i++) {
-		printf("%02x ", pps->data[i]);
+	for (i = 0; i < data->length; i++) {
+		printf("%02x ", data->data[i]);
 	}
 	printf("\n");
-	return 0;
-}
 
-static int process_tpdu(const uint8_t *buf, int len)
-{
-	/* check if there is enough data for the structure */
-	if (len<sizeof(struct sniff_data)) {
-		return -1;
+	if (SIMTRACE_MSGT_SNIFF_TPDU == type) {
+		/* send TPDU (now considered as APDU) to GSMTAP */
+		gsmtap_send_sim(data->data, data->length);
 	}
-	struct sniff_data *tpdu = (struct sniff_data *)buf;
 
-	/* check if the data is available */
-	if (len<sizeof(struct sniff_data)+tpdu->length) {
-		return -2;
-	}
-
-	/* print TPDU */
-	printf("TPDU%s: ", tpdu->complete ? "" : " (incomplete)");
-	uint16_t i;
-	for (i = 0; i < tpdu->length; i++) {
-		printf("%02x ", tpdu->data[i]);
-	}
-	printf("\n");
-
-	/* send TPDU (now considered as APDU) to GSMTAP */
-	gsmtap_send_sim(tpdu->data, tpdu->length);
 	return 0;
 }
 
@@ -215,19 +211,19 @@ static int process_tpdu(const uint8_t *buf, int len)
 static int process_usb_msg(const uint8_t *buf, int len)
 {
 	/* check if enough data for the header is present */
-	if (len<sizeof(struct simtrace_msg_hdr)) {
+	if (len < sizeof(struct simtrace_msg_hdr)) {
 		return 0;
 	}
 
 	/* check if message is complete */
 	struct simtrace_msg_hdr *msg_hdr = (struct simtrace_msg_hdr *)buf;
-	if (len<msg_hdr->msg_len) {
+	if (len < msg_hdr->msg_len) {
 		return 0;
 	}
 	//printf("msg: %s\n", osmo_hexdump(buf, msg_hdr->msg_len));
 
 	/* check for message class */
-	if (SIMTRACE_MSGC_SNIFF!=msg_hdr->msg_class) { /* we only care about sniffing messages */
+	if (SIMTRACE_MSGC_SNIFF != msg_hdr->msg_class) { /* we only care about sniffing messages */
 		return msg_hdr->msg_len; /* discard non-sniffing messaged */
 	}
 
@@ -242,13 +238,9 @@ static int process_usb_msg(const uint8_t *buf, int len)
 		process_fidi(buf, len);
 		break;
 	case SIMTRACE_MSGT_SNIFF_ATR:
-		process_atr(buf, len);
-		break;
 	case SIMTRACE_MSGT_SNIFF_PPS:
-		process_pps(buf, len);
-		break;
 	case SIMTRACE_MSGT_SNIFF_TPDU:
-		process_tpdu(buf, len);
+		process_data(msg_hdr->msg_type, buf, len);
 		break;
 	default:
 		printf("unknown SIMtrace msg type 0x%02x\n", msg_hdr->msg_type);