Merge pull request #1 from Proxmark/master

sync

armsrc/Makefile

@@ -50,8 +50,7 @@ THUMBSRC = start.c \
 	printf.c \
 	util.c \
 	string.c \
-	usb_cdc.c \
+	usb_cdc.c
-	cmd.c
 
 # Compile these in thumb mode optimized for speed (still smaller than ARM mode)
 THUMBOPTSRC = $(SRC_ISO15693)

armsrc/appmain.c

@@ -13,7 +13,6 @@
 #include <stdarg.h>
 
 #include "usb_cdc.h"
-#include "cmd.h"
 #include "proxmark3.h"
 #include "apps.h"
 #include "fpga.h"
@@ -312,6 +311,7 @@ void set_hw_capabilities(void) {
 
 
 void SendVersion(void) {
+	LED_A_ON();
 	set_hw_capabilities();
 
 	char temp[USB_CMD_DATA_SIZE]; /* Limited data payload in USB packets */
@@ -347,7 +347,8 @@ void SendVersion(void) {
 	// Send Chip ID and used flash memory
 	uint32_t text_and_rodata_section_size = (uint32_t)&__data_src_start__ - (uint32_t)&_flash_start;
 	uint32_t compressed_data_section_size = common_area.arg1;
-	cmd_send(CMD_ACK, *(AT91C_DBGU_CIDR), text_and_rodata_section_size + compressed_data_section_size, hw_capabilities, VersionString, strlen(VersionString));
+	cmd_send(CMD_ACK, *(AT91C_DBGU_CIDR), text_and_rodata_section_size + compressed_data_section_size, hw_capabilities, VersionString, strlen(VersionString) + 1);
+	LED_A_OFF();
 }
 
 // measure the USB Speed by sending SpeedTestBufferSize bytes to client and measuring the elapsed time.
@@ -363,13 +364,11 @@ void printUSBSpeed(void) {
 	uint32_t start_time = end_time = GetTickCount();
 	uint32_t bytes_transferred = 0;
 
-	LED_B_ON();
 	while (end_time < start_time + USB_SPEED_TEST_MIN_TIME) {
 		cmd_send(CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K, 0, USB_CMD_DATA_SIZE, 0, test_data, USB_CMD_DATA_SIZE);
 		end_time = GetTickCount();
 		bytes_transferred += USB_CMD_DATA_SIZE;
 	}
-	LED_B_OFF();
 
 	Dbprintf("  Time elapsed:      %dms", end_time - start_time);
 	Dbprintf("  Bytes transferred: %d", bytes_transferred);
@@ -382,6 +381,7 @@ void printUSBSpeed(void) {
   * Prints runtime information about the PM3.
 **/
 void SendStatus(void) {
+	LED_A_ON();
 	BigBuf_print_status();
 	Fpga_print_status();
 #ifdef WITH_SMARTCARD
@@ -395,6 +395,7 @@ void SendStatus(void) {
 	Dbprintf("  ToSendBit..........%d", ToSendBit);
 
 	cmd_send(CMD_ACK, 1, 0, 0, 0, 0);
+	LED_A_OFF();
 }
 
 #if defined(WITH_ISO14443a_StandAlone) || defined(WITH_LF_StandAlone)
@@ -936,9 +937,7 @@ void ListenReaderField(int limit) {
 }
 
 
-void UsbPacketReceived(uint8_t *packet, int len) {
+void UsbPacketReceived(UsbCommand *c) {
-
-	UsbCommand *c = (UsbCommand *)packet;
 
 //  Dbprintf("received %d bytes, with command: 0x%04x and args: %d %d %d",len,c->cmd,c->arg[0],c->arg[1],c->arg[2]);
 
@@ -1337,9 +1336,11 @@ void UsbPacketReceived(uint8_t *packet, int len) {
 			break;
 
 		case CMD_FPGA_MAJOR_MODE_OFF:       // ## FPGA Control
+			LED_A_ON();
 			FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 			SpinDelay(200);
 			LED_D_OFF(); // LED D indicates field ON or OFF
+			LED_A_OFF();
 			break;
 
 		case CMD_DOWNLOAD_RAW_ADC_SAMPLES_125K:
@@ -1428,7 +1429,7 @@ void UsbPacketReceived(uint8_t *packet, int len) {
 		case CMD_DEVICE_INFO: {
 			uint32_t dev_info = DEVICE_INFO_FLAG_OSIMAGE_PRESENT | DEVICE_INFO_FLAG_CURRENT_MODE_OS;
 			if(common_area.flags.bootrom_present) dev_info |= DEVICE_INFO_FLAG_BOOTROM_PRESENT;
-			cmd_send(CMD_DEVICE_INFO,dev_info,0,0,0,0);
+			cmd_send_old(CMD_DEVICE_INFO,dev_info,0,0,0,0);
 			break;
 		}
 		default:
@@ -1479,13 +1480,12 @@ void  __attribute__((noreturn)) AppMain(void) {
 	LCDInit();
 #endif
 
-	uint8_t rx[sizeof(UsbCommand)];
+	UsbCommand rx;
-	size_t rx_len;
   
 	for(;;) {
 		WDT_HIT();
-		if (usb_poll() && (rx_len = usb_read(rx, sizeof(rx)))) {
+		if (cmd_receive(&rx)) {
-			UsbPacketReceived(rx, rx_len);
+			UsbPacketReceived(&rx);
 		} else {
 #if defined(WITH_LF_StandAlone) && !defined(WITH_ISO14443a_StandAlone)
 			if (BUTTON_HELD(1000) > 0)

armsrc/apps.h

@@ -112,8 +112,4 @@ void    ReaderMifareDES(uint32_t param, uint32_t param2, uint8_t * datain);
 int     DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout);
 size_t  CreateAPDU( uint8_t *datain, size_t len, uint8_t *dataout);
 
-// cmd.h
-bool cmd_receive(UsbCommand* cmd);
-bool cmd_send(uint32_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, size_t len);
-
 #endif

armsrc/epa.c

@@ -15,7 +15,7 @@
 #include "iso14443a.h"
 #include "iso14443b.h"
 #include "epa.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "fpgaloader.h"
 #include "string.h"
 #include "util.h"
@@ -453,20 +453,17 @@ int EPA_PACE_MSE_Set_AT(pace_version_info_t pace_version_info, uint8_t password)
 //-----------------------------------------------------------------------------
 // Perform the PACE protocol by replaying given APDUs
 //-----------------------------------------------------------------------------
-void EPA_PACE_Replay(UsbCommand *c)
+void EPA_PACE_Replay(UsbCommand *c) {
-{
 	uint32_t timings[sizeof(apdu_lengths_replay) / sizeof(apdu_lengths_replay[0])] = {0};
 
-	// if an APDU has been passed, save it
+	// if an APDU has been passed, just save it
 	if (c->arg[0] != 0) {
 		// make sure it's not too big
-		if(c->arg[2] > apdus_replay[c->arg[0] - 1].len)
+		if(c->arg[2] > apdus_replay[c->arg[0] - 1].len) {
-		{
 			cmd_send(CMD_ACK, 1, 0, 0, NULL, 0);
+			return;
 		}
-		memcpy(apdus_replay[c->arg[0] - 1].data + c->arg[1],
+		memcpy(apdus_replay[c->arg[0] - 1].data + c->arg[1], c->d.asBytes, c->arg[2]);
-	           c->d.asBytes,
-	           c->arg[2]);
 		// save/update APDU length
 		if (c->arg[1] == 0) {
 			apdu_lengths_replay[c->arg[0] - 1] = c->arg[2];

armsrc/hfsnoop.c

@@ -14,7 +14,7 @@
 #include "BigBuf.h"
 #include "util.h"
 #include "apps.h"
-#include "usb_cdc.h"	// for usb_poll_validate_length
+#include "usb_cdc.h"
 #include "fpga.h"
 #include "fpgaloader.h"
 

armsrc/hitag2.c

@@ -19,7 +19,7 @@
 #include "hitag2.h"
 
 #include "proxmark3.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "apps.h"
 #include "util.h"
 #include "hitag.h"

armsrc/hitagS.c

@@ -17,6 +17,7 @@
 #include <stdlib.h>
 #include "proxmark3.h"
 #include "apps.h"
+#include "usb_cdc.h"
 #include "util.h"
 #include "hitag.h"
 #include "string.h"

armsrc/i2c.c

@@ -18,6 +18,7 @@
 #include "mifareutil.h" // for MF_DBGLEVEL
 #include "BigBuf.h"
 #include "apps.h"
+#include "usb_cdc.h"
 
 #ifdef WITH_SMARTCARD
 #include "smartcard.h"

armsrc/iclass.c

@@ -24,7 +24,7 @@
 #include "string.h"
 #include "printf.h"
 #include "common.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "iso14443a.h"
 #include "iso15693.h"
 // Needed for CRC in emulation mode;
@@ -34,7 +34,6 @@
 #include "iso15693tools.h"
 #include "protocols.h"
 #include "optimized_cipher.h"
-#include "usb_cdc.h" // for usb_poll_validate_length
 #include "fpgaloader.h"
 
 // iCLASS has a slightly different timing compared to ISO15693. According to the picopass data sheet the tag response is expected 330us after
@@ -734,7 +733,6 @@ void ReaderIClass(uint8_t flags) {
 	if (selectIclassTag(resp, &eof_time)) {
 		result_status = FLAG_ICLASS_READER_CSN;
 		memcpy(card_data, resp, 8);
-	}
 
 		start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
 
@@ -769,6 +767,7 @@ void ReaderIClass(uint8_t flags) {
 				Dbprintf("Failed to read AA block");
 			}
 		}
+	}
 	
 	cmd_send(CMD_ACK, result_status, 0, 0, card_data, sizeof(card_data));
 
@@ -820,9 +819,9 @@ void iClass_ReadBlk(uint8_t blockno) {
 
 	uint8_t readblockdata[10];
 	bool isOK = iClass_ReadBlock(blockno, readblockdata);
-	cmd_send(CMD_ACK, isOK, 0, 0, readblockdata, 8);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LED_D_OFF();
+	cmd_send(CMD_ACK, isOK, 0, 0, readblockdata, 8);
 
 	LED_A_OFF();
 }
@@ -900,11 +899,10 @@ void iClass_WriteBlock(uint8_t blockNo, uint8_t *data) {
 	} else {
 		Dbprintf("Write block [%02x] failed", blockNo);
 	}
-	cmd_send(CMD_ACK, isOK, 0, 0, 0, 0);
-
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LED_D_OFF();
 
+	cmd_send(CMD_ACK, isOK, 0, 0, 0, 0);
 	LED_A_OFF();
 }
 
@@ -935,6 +933,5 @@ void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) {
 	LED_D_OFF();
 
 	cmd_send(CMD_ACK, 1, 0, 0, 0, 0);
-
 	LED_A_OFF();
 }

armsrc/iso14443a.c

@@ -17,7 +17,7 @@
 #include "proxmark3.h"
 #include "apps.h"
 #include "util.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "iso14443crc.h"
 #include "crapto1/crapto1.h"
 #include "mifareutil.h"
@@ -942,7 +942,7 @@ bool prepare_allocated_tag_modulation(tag_response_info_t* response_info, uint8_
 // Main loop of simulated tag: receive commands from reader, decide what
 // response to send, and send it.
 //-----------------------------------------------------------------------------
-void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data) {
+void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, uint8_t* data) {
 
 	uint8_t sak;
 
@@ -1701,13 +1701,13 @@ static int GetATQA(uint8_t *resp, uint8_t *resp_par) {
 // if anticollision is false, then the UID must be provided in uid_ptr[]
 // and num_cascades must be set (1: 4 Byte UID, 2: 7 Byte UID, 3: 10 Byte UID)
 // requests ATS unless no_rats is true
-int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, uint32_t *cuid_ptr, bool anticollision, uint8_t num_cascades, bool no_rats) {
+int iso14443a_select_card(uint8_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, uint32_t *cuid_ptr, bool anticollision, uint8_t num_cascades, bool no_rats) {
 	uint8_t sel_all[]    = { 0x93,0x20 };
 	uint8_t sel_uid[]    = { 0x93,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 	uint8_t rats[]       = { 0xE0,0x80,0x00,0x00 }; // FSD=256, FSDI=8, CID=0
 	uint8_t resp[MAX_FRAME_SIZE]; // theoretically. A usual RATS will be much smaller
 	uint8_t resp_par[MAX_PARITY_SIZE];
-	byte_t uid_resp[4];
+	uint8_t uid_resp[4];
 	size_t uid_resp_len;
 
 	uint8_t sak = 0x04; // cascade uid
@@ -2020,7 +2020,7 @@ void ReaderIso14443a(UsbCommand *c) {
 	size_t lenbits = c->arg[1] >> 16;
 	uint32_t timeout = c->arg[2];
 	uint32_t arg0 = 0;
-	byte_t buf[USB_CMD_DATA_SIZE] = {0};
+	uint8_t buf[USB_CMD_DATA_SIZE] = {0};
 	uint8_t par[MAX_PARITY_SIZE];
 	bool cantSELECT = false;
 
@@ -2048,7 +2048,7 @@ void ReaderIso14443a(UsbCommand *c) {
 			}
 			FpgaDisableTracing();
 			LED_B_ON();
-			cmd_send(CMD_ACK,arg0,card->uidlen,0,buf,sizeof(iso14a_card_select_t));
+			cmd_send(CMD_NACK,arg0,card->uidlen,0,buf,sizeof(iso14a_card_select_t));
 			LED_B_OFF();
 		}
 	}

armsrc/iso14443a.h

@@ -13,6 +13,7 @@
 #ifndef __ISO14443A_H
 #define __ISO14443A_H
 
+#include <stddef.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include "usb_cmd.h"
@@ -31,7 +32,7 @@ extern void GetParity(const uint8_t *pbtCmd, uint16_t len, uint8_t *par);
 extern void AppendCrc14443a(uint8_t *data, int len);
 
 extern void RAMFUNC SnoopIso14443a(uint8_t param);
-extern void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t *data);
+extern void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, uint8_t *data);
 extern void ReaderIso14443a(UsbCommand *c);
 extern void ReaderTransmit(uint8_t *frame, uint16_t len, uint32_t *timing);
 extern void ReaderTransmitBitsPar(uint8_t *frame, uint16_t bits, uint8_t *par, uint32_t *timing);

armsrc/iso14443b.c

@@ -14,6 +14,7 @@
 
 #include "proxmark3.h"
 #include "apps.h"
+#include "usb_cdc.h"
 #include "util.h"
 #include "string.h"
 #include "iso14443crc.h"

armsrc/iso15693.c

@@ -58,7 +58,7 @@
 #include "string.h"
 #include "iso15693tools.h"
 #include "protocols.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "BigBuf.h"
 #include "fpgaloader.h"
 

armsrc/legicrf.c

@@ -14,6 +14,7 @@
 
 #include "proxmark3.h"
 #include "apps.h"
+#include "usb_cdc.h"
 #include "util.h"
 #include "string.h"
 #include "legic_prng.h"

armsrc/lfops.c

@@ -17,7 +17,7 @@
 #include "lfdemod.h"
 #include "lfsampling.h"
 #include "protocols.h"
-#include "usb_cdc.h" // for usb_poll_validate_length
+#include "usb_cdc.h"
 #include "fpgaloader.h"
 
 /**

armsrc/mifarecmd.c

@@ -18,7 +18,7 @@
 #include <stdint.h>
 
 #include "proxmark3.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "crapto1/crapto1.h"
 #include "iso14443a.h"
 #include "BigBuf.h"
@@ -111,11 +111,12 @@ void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 
 	if (MF_DBGLEVEL >= 2)   DbpString("READ BLOCK FINISHED");
 
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
 	LED_B_ON();
 	cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16);
 	LED_B_OFF();
 
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
 
@@ -202,9 +203,10 @@ void MifareUReadBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain)
 		return;
 	}
 
-	cmd_send(CMD_ACK,1,0,0,dataout,16);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LED_D_OFF();
+
+	cmd_send(CMD_ACK,1,0,0,dataout,16);
 	LED_A_OFF();
 }
 
@@ -266,12 +268,13 @@ void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 
 	if (MF_DBGLEVEL >= 2) DbpString("READ SECTOR FINISHED");
 
+	// Thats it...
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
 	LED_B_ON();
 	cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16*NumBlocksPerSector(sectorNo));
 	LED_B_OFF();
 
-	// Thats it...
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
 
@@ -362,10 +365,11 @@ void MifareUReadCard(uint8_t arg0, uint16_t arg1, uint8_t arg2, uint8_t *datain)
 
 	if (MF_DBGLEVEL >= MF_DBG_DEBUG) Dbprintf("Blocks read %d", countblocks);
 
-	cmd_send(CMD_ACK, 1, countblocks*4, BigBuf_max_traceLen(), 0, 0);
-
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LED_D_OFF();
+
+	cmd_send(CMD_ACK, 1, countblocks*4, BigBuf_max_traceLen(), 0, 0);
+
 	BigBuf_free();
 	LED_A_OFF();
 }
@@ -431,13 +435,14 @@ void MifareWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 
 	if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 
+	// Thats it...
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
 	LED_B_ON();
 	cmd_send(CMD_ACK,isOK,0,0,0,0);
 	LED_B_OFF();
 
 
-	// Thats it...
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
 
@@ -475,8 +480,9 @@ void MifareUWriteBlockCompat(uint8_t arg0, uint8_t *datain)
 
 	if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 
-	cmd_send(CMD_ACK,1,0,0,0,0);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
+	cmd_send(CMD_ACK,1,0,0,0,0);
 	LEDsoff();
 }
 */
@@ -544,8 +550,9 @@ void MifareUWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t *datain)
 
 	if (MF_DBGLEVEL >= 2) DbpString("WRITE BLOCK FINISHED");
 
-	cmd_send(CMD_ACK,1,0,0,0,0);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
+	cmd_send(CMD_ACK,1,0,0,0,0);
 	LEDsoff();
 }
 
@@ -613,8 +620,9 @@ void MifareUSetPwd(uint8_t arg0, uint8_t *datain){
 		return;
 	};
 
-	cmd_send(CMD_ACK,1,0,0,0,0);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
+	cmd_send(CMD_ACK,1,0,0,0,0);
 	LEDsoff();
 }
 
@@ -743,16 +751,17 @@ void MifareAcquireEncryptedNonces(uint32_t arg0, uint32_t arg1, uint32_t flags,
 
 	crypto1_destroy(pcs);
 
+	if (field_off) {
+		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+		LEDsoff();
+	}
+
 	LED_B_ON();
 	cmd_send(CMD_ACK, isOK, cuid, num_nonces, buf, sizeof(buf));
 	LED_B_OFF();
 
 	if (MF_DBGLEVEL >= 3)   DbpString("AcquireEncryptedNonces finished");
 
-	if (field_off) {
-		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-		LEDsoff();
-	}
 }
 
 
@@ -978,13 +987,14 @@ void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *dat
 	memcpy(buf+16, &target_ks[1], 4);
 	memcpy(buf+20, &authentication_timeout, 4);
 
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
+	if (MF_DBGLEVEL >= 3)   DbpString("NESTED FINISHED");
+
 	LED_B_ON();
 	cmd_send(CMD_ACK, isOK, 0, targetBlockNo + (targetKeyType * 0x100), buf, sizeof(buf));
 	LED_B_OFF();
 
-	if (MF_DBGLEVEL >= 3)   DbpString("NESTED FINISHED");
-
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
 
@@ -1352,13 +1362,14 @@ void MifareCWipe(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 		break;
 	}
 
+	// reset fpga
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
 	// send USB response
 	LED_B_ON();
 	cmd_send(CMD_ACK,isOK,0,0,NULL,0);
 	LED_B_OFF();
 
-	// reset fpga
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 
 	return;
@@ -1490,15 +1501,16 @@ void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datai
 		break;
 	}
 
+	if ((workFlags & 0x10) || (!isOK)) {
+		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	}
+
 	LED_B_ON();
 	cmd_send(CMD_ACK,isOK,0,0,uid,4);
 	LED_B_OFF();
 
-	if ((workFlags & 0x10) || (!isOK)) {
-		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
-}
 
 
 void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
@@ -1574,6 +1586,10 @@ void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datai
 		break;
 	}
 
+	if ((workFlags & 0x10) || (!isOK)) {
+		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	}
+
 	LED_B_ON();
 	if (workFlags & 0x20) {
 		if (isOK)
@@ -1583,11 +1599,8 @@ void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datai
 		cmd_send(CMD_ACK,isOK,0,0,data,18);
 	LED_B_OFF();
 
-	if ((workFlags & 0x10) || (!isOK)) {
-		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
-}
 
 void MifareCIdent(){
 
@@ -1622,11 +1635,12 @@ void MifareCIdent(){
 	// From iceman1001: removed the if,  since some magic tags misbehavies and send an answer to it.
 	mifare_classic_halt(NULL, 0);
 
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
 	LED_B_ON();
 	cmd_send(CMD_ACK,isOK,0,0,0,0);
 	LED_B_OFF();
 
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
 
@@ -1657,6 +1671,7 @@ void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain){
 	}
 
 	if (MF_DBGLEVEL >= MF_DBG_EXTENDED) DbpString("AUTH 1 FINISHED");
+
 	cmd_send(CMD_ACK, 1, cuid, 0, dataout, sizeof(dataout));
 }
 
@@ -1677,10 +1692,11 @@ void Mifare_DES_Auth2(uint32_t arg0, uint8_t *datain){
 		return;
 	}
 
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+
 	if (MF_DBGLEVEL >= MF_DBG_EXTENDED) DbpString("AUTH 2 FINISHED");
 
 	cmd_send(CMD_ACK, isOK, 0, 0, dataout, sizeof(dataout));
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 	LEDsoff();
 }
 

armsrc/mifaresim.c

@@ -20,7 +20,6 @@
 #include "fpgaloader.h"
 #include "proxmark3.h"
 #include "usb_cdc.h"
-#include "cmd.h"
 #include "protocols.h"
 #include "apps.h"
 

armsrc/mifaresniff.c

@@ -18,7 +18,7 @@
 #include "crapto1/crapto1.h"
 #include "mifareutil.h"
 #include "common.h"
-#include "cmd.h"
+#include "usb_cdc.h"
 #include "BigBuf.h"
 #include "fpgaloader.h"
 
@@ -152,7 +152,7 @@ bool intMfSniffSend() {
 	while (pckLen > 0) {
 		pckSize = MIN(USB_CMD_DATA_SIZE, pckLen);
 		LED_B_ON();
-		cmd_send(CMD_ACK, 1, BigBuf_get_traceLen(), pckSize, trace + BigBuf_get_traceLen() - pckLen, pckSize);
+		cmd_send(CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K, 1, BigBuf_get_traceLen(), pckSize, trace + BigBuf_get_traceLen() - pckLen, pckSize);
 		LED_B_OFF();
 
 		pckLen -= pckSize;
@@ -160,7 +160,7 @@ bool intMfSniffSend() {
 	}
 
 	LED_B_ON();
-	cmd_send(CMD_ACK,2,0,0,0,0);
+	cmd_send(CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K,2,0,0,0,0);
 	LED_B_OFF();
 
 	clear_trace();

armsrc/pcf7931.c

@@ -1,5 +1,6 @@
 #include "proxmark3.h"
 #include "apps.h"
+#include "usb_cdc.h"
 #include "lfsampling.h"
 #include "pcf7931.h"
 #include "util.h"

armsrc/util.h

@@ -8,12 +8,13 @@
 // Utility functions used in many places, not specific to any piece of code.
 //-----------------------------------------------------------------------------
 
-#ifndef __UTIL_H
+#ifndef UTIL_H__
-#define __UTIL_H
+#define UTIL_H__
 
 #include <stddef.h>
 #include <stdint.h>
 #include "common.h"
+#include "at91sam7s512.h"
 
 #define BYTEx(x, n) (((x) >> (n * 8)) & 0xff )
 
@@ -33,40 +34,40 @@
 #define REV32(x) (REV16(x) | (REV16(x >> 16) << 16))
 #define REV64(x) (REV32(x) | (REV32(x >> 32) << 32))
 
-void print_result(char *name, uint8_t *buf, size_t len);
+extern void print_result(char *name, uint8_t *buf, size_t len);
-size_t nbytes(size_t nbits);
+extern size_t nbytes(size_t nbits);
-uint32_t SwapBits(uint32_t value, int nrbits);
+extern uint32_t SwapBits(uint32_t value, int nrbits);
-void num_to_bytes(uint64_t n, size_t len, uint8_t* dest);
+extern void num_to_bytes(uint64_t n, size_t len, uint8_t* dest);
-uint64_t bytes_to_num(uint8_t* src, size_t len);
+extern uint64_t bytes_to_num(uint8_t* src, size_t len);
-void rol(uint8_t *data, const size_t len);
+extern void rol(uint8_t *data, const size_t len);
-void lsl (uint8_t *data, size_t len);
+extern void lsl (uint8_t *data, size_t len);
 
-void LED(int led, int ms);
+extern void LED(int led, int ms);
-void LEDsoff();
+extern void LEDsoff();
-void LEDson();
+extern void LEDson();
-void LEDsinvert();
+extern void LEDsinvert();
-int BUTTON_CLICKED(int ms);
+extern int BUTTON_CLICKED(int ms);
-int BUTTON_HELD(int ms);
+extern int BUTTON_HELD(int ms);
-void FormatVersionInformation(char *dst, int len, const char *prefix, void *version_information);
+extern void FormatVersionInformation(char *dst, int len, const char *prefix, void *version_information);
 
 //iceman's ticks.h
 #ifndef GET_TICKS
 # define GET_TICKS GetTicks()
 #endif
 
-void SpinDelay(int ms);
+extern void SpinDelay(int ms);
-void SpinDelayUs(int us);
+extern void SpinDelayUs(int us);
 
-void StartTickCount();
+extern void StartTickCount();
-uint32_t RAMFUNC GetTickCount();
+extern uint32_t RAMFUNC GetTickCount();
 
-void StartCountUS();
+extern void StartCountUS();
-uint32_t RAMFUNC GetCountUS();
+extern uint32_t RAMFUNC GetCountUS();
-uint32_t RAMFUNC GetDeltaCountUS();
+extern uint32_t RAMFUNC GetDeltaCountUS();
 
-void StartCountSspClk();
+extern void StartCountSspClk();
-void ResetSspClk(void);
+extern void ResetSspClk(void);
-uint32_t GetCountSspClk();
+extern uint32_t GetCountSspClk();
 
 extern void StartTicks(void);
 extern uint32_t GetTicks(void);
@@ -78,6 +79,6 @@ extern void ResetTimer(AT91PS_TC timer);
 extern void StopTicks(void);
 // end iceman's ticks.h
 
-uint32_t prand();
+extern uint32_t prand();
 
 #endif

bootrom/Makefile

@@ -8,7 +8,7 @@
 
 # DO NOT use thumb mode in the phase 1 bootloader since that generates a section with glue code
 ARMSRC = 
-THUMBSRC = cmd.c usb_cdc.c bootrom.c
+THUMBSRC = usb_cdc.c bootrom.c
 ASMSRC = ram-reset.s flash-reset.s
 VERSIONSRC = version.c
 

bootrom/bootrom.c

@@ -6,17 +6,15 @@
 // Main code for the bootloader
 //-----------------------------------------------------------------------------
 
-#include <proxmark3.h>
+#include "proxmark3.h"
 #include "usb_cdc.h"
-#include "cmd.h"
-//#include "usb_hid.h"
 
 void DbpString(char *str) {
-  byte_t len = 0;
+  uint8_t len = 0;
   while (str[len] != 0x00) {
     len++;
   }
-  cmd_send(CMD_DEBUG_PRINT_STRING,len,0,0,(byte_t*)str,len);
+  cmd_send_old(CMD_DEBUG_PRINT_STRING,len,0,0,(uint8_t*)str,len);
 }
 
 struct common_area common_area __attribute__((section(".commonarea")));
@@ -89,26 +87,22 @@ static void Fatal(void)
   for(;;);
 }
 
-void UsbPacketReceived(uint8_t *packet, int len) {
+void UsbPacketReceived(UsbCommand *c) {
   int i, dont_ack=0;
-  UsbCommand* c = (UsbCommand *)packet;
   volatile uint32_t *p;
   
-  if(len != sizeof(UsbCommand)) {
-    Fatal();
-  }
-  
   uint32_t arg0 = (uint32_t)c->arg[0];
   
   switch(c->cmd) {
     case CMD_DEVICE_INFO: {
       dont_ack = 1;
-      arg0 = DEVICE_INFO_FLAG_BOOTROM_PRESENT | DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM |
+      arg0 = DEVICE_INFO_FLAG_BOOTROM_PRESENT 
-      DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH;
+		| DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM
+		| DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH;
       if(common_area.flags.osimage_present) {
         arg0 |= DEVICE_INFO_FLAG_OSIMAGE_PRESENT;
       }
-      cmd_send(CMD_DEVICE_INFO,arg0,1,2,0,0);
+      cmd_send_old(CMD_DEVICE_INFO,arg0,1,2,0,0);
     } break;
       
     case CMD_SETUP_WRITE: {
@@ -134,7 +128,7 @@ void UsbPacketReceived(uint8_t *packet, int len) {
         if( ((flash_address+AT91C_IFLASH_PAGE_SIZE-1) >= end_addr) || (flash_address < start_addr) ) {
           /* Disallow write */
           dont_ack = 1;
-          cmd_send(CMD_NACK,0,0,0,0,0);
+          cmd_send_old(CMD_NACK,0,0,0,0,0);
         } else {
           uint32_t page_n = (flash_address - ((uint32_t)flash_mem)) / AT91C_IFLASH_PAGE_SIZE;
           /* Translate address to flash page and do flash, update here for the 512k part */
@@ -148,7 +142,7 @@ void UsbPacketReceived(uint8_t *packet, int len) {
         while(!((sr = AT91C_BASE_EFC0->EFC_FSR) & AT91C_MC_FRDY));
         if(sr & (AT91C_MC_LOCKE | AT91C_MC_PROGE)) {
           dont_ack = 1;
-          cmd_send(CMD_NACK,0,0,0,0,0);
+          cmd_send_old(CMD_NACK,0,0,0,0,0);
         }
       }
     } break;
@@ -179,7 +173,7 @@ void UsbPacketReceived(uint8_t *packet, int len) {
         } else {
           start_addr = end_addr = 0;
           dont_ack = 1;
-          cmd_send(CMD_NACK,0,0,0,0,0);
+          cmd_send_old(CMD_NACK,0,0,0,0,0);
         }
       }
     } break;
@@ -190,7 +184,7 @@ void UsbPacketReceived(uint8_t *packet, int len) {
   }
   
   if(!dont_ack) {
-    cmd_send(CMD_ACK,arg0,0,0,0,0);
+    cmd_send_old(CMD_ACK,arg0,0,0,0,0);
   }
 }
 
@@ -199,8 +193,7 @@ static void flash_mode(int externally_entered)
 	start_addr = 0;
 	end_addr = 0;
 	bootrom_unlocked = 0;
-  byte_t rx[sizeof(UsbCommand)];
+    UsbCommand rx;
-	size_t rx_len;
 
 	usb_enable();
 	for (volatile size_t i=0; i<0x100000; i++) {};
@@ -208,11 +201,8 @@ static void flash_mode(int externally_entered)
 	for(;;) {
 		WDT_HIT();
 
-    if (usb_poll()) {
+		if (cmd_receive(&rx)) {
-      rx_len = usb_read(rx,sizeof(UsbCommand));
+			UsbPacketReceived(&rx);
-      if (rx_len) {
-        UsbPacketReceived(rx,rx_len);
-      }
 		}
 
 		if(!externally_entered && !BUTTON_PRESS()) {

client/cmdhf14a.c

@@ -51,7 +51,7 @@ int Hf14443_4aGetCardData(iso14a_card_select_t * card) {
 	SendCommand(&c);
 
 	UsbCommand resp;
-	WaitForResponse(CMD_ACK,&resp);
+	WaitForResponse(CMD_NACK, &resp);
 
 	memcpy(card, (iso14a_card_select_t *)resp.d.asBytes, sizeof(iso14a_card_select_t));
 
@@ -156,13 +156,17 @@ int CmdHF14AReader(const char *Cmd) {
 	return 0;
 }
 
-int CmdHF14AInfo(const char *Cmd)
+
-{
+int CmdHF14AInfo(const char *Cmd) {
+
 	UsbCommand c = {CMD_READER_ISO_14443a, {ISO14A_CONNECT | ISO14A_NO_DISCONNECT, 0, 0}};
 	SendCommand(&c);
 
 	UsbCommand resp;
-	WaitForResponse(CMD_ACK,&resp);
+	if (!WaitForResponseTimeout(CMD_NACK, &resp, 500)) {
+		if (Cmd[0] != 's') PrintAndLog("Error: No response from Proxmark.\n");
+		return 0;
+	}
 	
 	iso14a_card_select_t card;
 	memcpy(&card, (iso14a_card_select_t *)resp.d.asBytes, sizeof(iso14a_card_select_t));
@@ -217,7 +221,7 @@ int CmdHF14AInfo(const char *Cmd)
 			SendCommand(&c);
 
 			UsbCommand resp;
-			WaitForResponse(CMD_ACK,&resp);
+			WaitForResponse(CMD_NACK,&resp);
 
 			memcpy(&card, (iso14a_card_select_t *)resp.d.asBytes, sizeof(iso14a_card_select_t));
 
@@ -462,7 +466,7 @@ int CmdHF14ACUIDs(const char *Cmd)
 		SendCommand(&c);
 
 		UsbCommand resp;
-		WaitForResponse(CMD_ACK,&resp);
+		WaitForResponse(CMD_NACK,&resp);
 
 		iso14a_card_select_t *card = (iso14a_card_select_t *) resp.d.asBytes;
 

client/cmdhficlass.c

@@ -333,7 +333,7 @@ int HFiClassReader(bool loop, bool verbose) {
 
 	while (!ukbhit()) {
 		SendCommand(&c);
-		if (WaitForResponseTimeout(CMD_ACK,&resp, 4500)) {
+		if (WaitForResponseTimeout(CMD_ACK, &resp, 1000)) {
 			uint8_t readStatus = resp.arg[0] & 0xff;
 			uint8_t *data = resp.d.asBytes;
 
@@ -368,7 +368,8 @@ int HFiClassReader(bool loop, bool verbose) {
 
 			if (tagFound && !loop) return 1;
 		} else {
-			if (verbose) PrintAndLog("Command execute timeout");
+			if (verbose) PrintAndLog("Error: No response from Proxmark.");
+			break;
 		}
 		if (!loop) break;
 	}

client/cmdhfmf.c

@@ -2672,7 +2672,7 @@ int CmdHF14AMfSniff(const char *Cmd){
 		}
 
 		UsbCommand resp;
-		if (WaitForResponseTimeoutW(CMD_ACK, &resp, 2000, false)) {
+		if (WaitForResponseTimeoutW(CMD_UNKNOWN, &resp, 2000, false)) {
 			res = resp.arg[0] & 0xff;
 			uint16_t traceLen = resp.arg[1];
 			len = resp.arg[2];

client/cmdhfmfu.c

@@ -12,6 +12,7 @@
 
 #include <stdint.h>
 #include <stdio.h>
+#include <string.h>
 #include "comms.h"
 #include "usb_cmd.h"
 #include "cmdmain.h"

client/cmdsmartcard.c

@@ -10,6 +10,7 @@
 #include "cmdsmartcard.h"
 
 #include <ctype.h>
+#include <string.h>
 
 #include "ui.h"
 #include "cmdparser.h"

client/comms.c

@@ -11,10 +11,13 @@
 
 #include "comms.h"
 
+#include <stdio.h>
+#include <stddef.h>
+#include <string.h>
 #include <pthread.h>
-#if defined(__linux__) && !defined(NO_UNLINK)
+#include <inttypes.h>
-#include <unistd.h>		// for unlink()
+#include <time.h>
-#endif
+
 #include "uart.h"
 #include "ui.h"
 #include "common.h"
@@ -31,7 +34,6 @@ static bool offline;
 
 typedef struct {
 	bool run; // If TRUE, continue running the uart_communication thread
-	bool block_after_ACK; // if true, block after receiving an ACK package
 } communication_arg_t;
 
 static communication_arg_t conn;
@@ -45,6 +47,9 @@ static pthread_cond_t txBufferSig = PTHREAD_COND_INITIALIZER;
 
 // Used by UsbReceiveCommand as a ring buffer for messages that are yet to be
 // processed by a command handler (WaitForResponse{,Timeout})
+#define CMD_BUFFER_SIZE 50
+#define CMD_BUFFER_CHECK_TIME 10 // maximum time (in ms) to wait in getCommand()
+
 static UsbCommand rxBuffer[CMD_BUFFER_SIZE];
 
 // Points to the next empty position to write to
@@ -55,6 +60,7 @@ static int cmd_tail = 0;
 
 // to lock rxBuffer operations from different threads
 static pthread_mutex_t rxBufferMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t rxBufferSig = PTHREAD_COND_INITIALIZER;
 
 // These wrappers are required because it is not possible to access a static
 // global variable outside of the context of a single file.
@@ -69,7 +75,7 @@ bool IsOffline() {
 
 void SendCommand(UsbCommand *c) {
 	#ifdef COMMS_DEBUG
-	printf("Sending %04x cmd\n", c->cmd);
+	printf("Sending %04" PRIx64 " cmd\n", c->cmd);
 	#endif
 
 	if (offline) {
@@ -101,8 +107,7 @@ void SendCommand(UsbCommand *c) {
  *  A better method could have been to have explicit command-ACKS, so we can know which ACK goes to which
  *  operation. Right now we'll just have to live with this.
  */
-void clearCommandBuffer()
+void clearCommandBuffer() {
-{
 	//This is a very simple operation
 	pthread_mutex_lock(&rxBufferMutex);
 	cmd_tail = cmd_head;
@@ -113,11 +118,9 @@ void clearCommandBuffer()
  * @brief storeCommand stores a USB command in a circular buffer
  * @param UC
  */
-static void storeCommand(UsbCommand *command)
+static void storeCommand(UsbCommand *command) {
-{
 	pthread_mutex_lock(&rxBufferMutex);
-	if( (cmd_head+1) % CMD_BUFFER_SIZE == cmd_tail)
+	if ((cmd_head + 1) % CMD_BUFFER_SIZE == cmd_tail) {
-	{
 		// If these two are equal, we're about to overwrite in the
 		// circular buffer.
 		PrintAndLog("WARNING: Command buffer about to overwrite command! This needs to be fixed!");
@@ -128,6 +131,7 @@ static void storeCommand(UsbCommand *command)
 	memcpy(destination, command, sizeof(UsbCommand));
 
 	cmd_head = (cmd_head + 1) % CMD_BUFFER_SIZE; //increment head and wrap
+	pthread_cond_signal(&rxBufferSig); // tell main thread that a new command can be retreived
 	pthread_mutex_unlock(&rxBufferMutex);
 }
 
@@ -135,13 +139,24 @@ static void storeCommand(UsbCommand *command)
 /**
  * @brief getCommand gets a command from an internal circular buffer.
  * @param response location to write command
- * @return 1 if response was returned, 0 if nothing has been received
+ * @return 1 if response was returned, 0 if nothing has been received in time
  */
-static int getCommand(UsbCommand* response)
+static int getCommand(UsbCommand* response, uint32_t ms_timeout) {
-{
+
+	struct timespec end_time;
+	clock_gettime(CLOCK_REALTIME, &end_time);
+	end_time.tv_sec += ms_timeout / 1000;
+	end_time.tv_nsec += (ms_timeout % 1000) * 1000000;
+	if (end_time.tv_nsec > 1000000000) {
+		end_time.tv_nsec -= 1000000000;
+		end_time.tv_sec += 1;
+	}
 	pthread_mutex_lock(&rxBufferMutex);
-	//If head == tail, there's nothing to read, or if we just got initialized
+	int res = 0;
-	if (cmd_head == cmd_tail){
+	while (cmd_head == cmd_tail && !res) {
+		res = pthread_cond_timedwait(&rxBufferSig, &rxBufferMutex, &end_time);
+	}
+	if (res) { // timeout
 		pthread_mutex_unlock(&rxBufferMutex);
 		return 0;
 	}
@@ -161,8 +176,7 @@ static int getCommand(UsbCommand* response)
 // Entry point into our code: called whenever we received a packet over USB.
 // Handle debug commands directly, store all other commands in circular buffer.
 //----------------------------------------------------------------------------------
-static void UsbCommandReceived(UsbCommand *UC)
+static void UsbCommandReceived(UsbCommand *UC) {
-{
 	switch (UC->cmd) {
 		// First check if we are handling a debug message
 		case CMD_DEBUG_PRINT_STRING: {
@@ -187,6 +201,24 @@ static void UsbCommandReceived(UsbCommand *UC)
 }
 
 
+static bool receive_from_serial(serial_port sp, uint8_t *rx_buf, size_t len, size_t *received_len) {
+	size_t bytes_read = 0;
+	*received_len = 0;
+	// we eventually need to call uart_receive several times because it may timeout in the middle of a transfer
+	while (uart_receive(sp, rx_buf + *received_len, len - *received_len, &bytes_read) && bytes_read && *received_len < len) {
+		#ifdef COMMS_DEBUG
+		if (bytes_read != len - *received_len) {
+			printf("uart_receive() returned true but not enough bytes could be received. received: %zd, wanted to receive: %zd, already received before: %zd\n",
+				bytes_read, len - *received_len, *received_len);
+		}
+		#endif
+		*received_len += bytes_read;
+		bytes_read = 0;
+	}
+	return (*received_len == len);
+}
+
+
 static void
 #ifdef __has_attribute
 #if __has_attribute(force_align_arg_pointer)
@@ -195,49 +227,70 @@ __attribute__((force_align_arg_pointer))
 #endif
 *uart_communication(void *targ) {
 	communication_arg_t *conn = (communication_arg_t*)targ;
-	size_t rxlen;
+	uint8_t rx[sizeof(UsbCommand)];
-	UsbCommand rx;
+	size_t rxlen = 0;
-	UsbCommand *prx = &rx;
+	uint8_t *prx = rx;
+	UsbCommand *command = (UsbCommand*)rx;
+	UsbResponse *response = (UsbResponse*)rx;
 
 #if defined(__MACH__) && defined(__APPLE__)
 	disableAppNap("Proxmark3 polling UART");
 #endif
 
 	while (conn->run) {
-		rxlen = 0;
 		bool ACK_received = false;
-		if (uart_receive(sp, (uint8_t *)prx, sizeof(UsbCommand) - (prx-&rx), &rxlen) && rxlen) {
+		prx = rx;
+		size_t bytes_to_read = offsetof(UsbResponse, d);  // the fixed part of a new style UsbResponse. Otherwise this will be cmd and arg[0] (64 bit each)
+		if (receive_from_serial(sp, prx, bytes_to_read, &rxlen)) {
 			prx += rxlen;
-			if (prx-&rx < sizeof(UsbCommand)) {
+			if (response->cmd & CMD_VARIABLE_SIZE_FLAG) { // new style response with variable size
-				continue;
+#ifdef COMMS_DEBUG
-			}
+				PrintAndLog("received new style response %04" PRIx16 ", datalen = %zd, arg[0] = %08" PRIx32 ", arg[1] = %08" PRIx32 ", arg[2] = %08" PRIx32,
-			UsbCommandReceived(&rx);
+					response->cmd, response->datalen, response->arg[0], response->arg[1], response->arg[2]);
-			if (rx.cmd == CMD_ACK) {
+#endif
+				bytes_to_read = response->datalen;
+				if (receive_from_serial(sp, prx, bytes_to_read, &rxlen)) {
+					UsbCommand resp;
+					resp.cmd = response->cmd & ~CMD_VARIABLE_SIZE_FLAG;  // remove the flag
+					resp.arg[0] = response->arg[0];
+					resp.arg[1] = response->arg[1];
+					resp.arg[2] = response->arg[2];
+					memcpy(&resp.d.asBytes, &response->d.asBytes, response->datalen);
+					UsbCommandReceived(&resp);
+					if (resp.cmd == CMD_ACK) {
 						ACK_received = true;
 					}
 				}
-		prx = &rx;
+			} else { // old style response uses same data structure as commands. Fixed size.
-
+#ifdef COMMS_DEBUG
+				PrintAndLog("received old style response %016" PRIx64 ", arg[0] = %016" PRIx64, command->cmd, command->arg[0]);
+#endif
+				bytes_to_read = sizeof(UsbCommand) - bytes_to_read;
+				if (receive_from_serial(sp, prx, bytes_to_read, &rxlen)) {
+					UsbCommandReceived(command);
+					if (command->cmd == CMD_ACK) {
+						ACK_received = true;
+					}
+				}
+			}
+		}
 
 		pthread_mutex_lock(&txBufferMutex);
-
+		// if we received an ACK the PM has done its job and waits for another command.
-		if (conn->block_after_ACK) {
+		// We therefore can wait here as well until a new command is to be transmitted.
-			// if we just received an ACK, wait here until a new command is to be transmitted
+		// The advantage is that the next command will be transmitted immediately without the need to wait for a receive timeout
 		if (ACK_received) {
 			while (!txBuffer_pending) {
 				pthread_cond_wait(&txBufferSig, &txBufferMutex);
 			}
 		}
-		}
-				
 		if (txBuffer_pending) {
 			if (!uart_send(sp, (uint8_t*) &txBuffer, sizeof(UsbCommand))) {
 				PrintAndLog("Sending bytes to proxmark failed");
 			}
 			txBuffer_pending = false;
-			pthread_cond_signal(&txBufferSig); // tell main thread that txBuffer is empty
 		}
-
+		pthread_cond_signal(&txBufferSig); // tell main thread that txBuffer is empty
 		pthread_mutex_unlock(&txBufferMutex);
 	}
 
@@ -262,13 +315,13 @@ __attribute__((force_align_arg_pointer))
  * @param show_warning display message after 2 seconds
  * @return true if command was returned, otherwise false
  */
-bool GetFromBigBuf(uint8_t *dest, int bytes, int start_index, UsbCommand *response, size_t ms_timeout, bool show_warning)
+bool GetFromBigBuf(uint8_t *dest, int bytes, int start_index, UsbCommand *response, size_t ms_timeout, bool show_warning) {
-{
-	UsbCommand c = {CMD_DOWNLOAD_RAW_ADC_SAMPLES_125K, {start_index, bytes, 0}};
-	SendCommand(&c);
 
 	uint64_t start_time = msclock();
 
+	UsbCommand c = {CMD_DOWNLOAD_RAW_ADC_SAMPLES_125K, {start_index, bytes, 0}};
+	SendCommand(&c);
+
 	UsbCommand resp;
 	if (response == NULL) {
 		response = &resp;
@@ -276,7 +329,16 @@ bool GetFromBigBuf(uint8_t *dest, int bytes, int start_index, UsbCommand *respon
 
 	int bytes_completed = 0;
 	while (true) {
-		if (getCommand(response)) {
+		if (msclock() - start_time > ms_timeout) {
+			break; // timeout
+		}
+		if (msclock() - start_time > 2000 && show_warning) {
+			// 2 seconds elapsed (but this doesn't mean the timeout was exceeded)
+			PrintAndLog("Waiting for a response from the proxmark...");
+			PrintAndLog("You can cancel this operation by pressing the pm3 button");
+			show_warning = false;
+		}
+		if (getCommand(response, CMD_BUFFER_CHECK_TIME)) {
 			if (response->cmd == CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K) {
 				int copy_bytes = MIN(bytes - bytes_completed, response->arg[1]);
 				memcpy(dest + response->arg[0], response->d.asBytes, copy_bytes);
@@ -285,35 +347,30 @@ bool GetFromBigBuf(uint8_t *dest, int bytes, int start_index, UsbCommand *respon
 				return true;
 			}
 		}
-
-		if (msclock() - start_time > ms_timeout) {
-			break;
-		}
-
-		if (msclock() - start_time > 2000 && show_warning) {
-			PrintAndLog("Waiting for a response from the proxmark...");
-			PrintAndLog("You can cancel this operation by pressing the pm3 button");
-			show_warning = false;
-		}
 	}
 
 	return false;
 }
 
 
-bool GetFromFpgaRAM(uint8_t *dest, int bytes)
+bool GetFromFpgaRAM(uint8_t *dest, int bytes) {
-{
-	UsbCommand c = {CMD_HF_PLOT, {0, 0, 0}};
-	SendCommand(&c);
 
 	uint64_t start_time = msclock();
 
+	UsbCommand c = {CMD_HF_PLOT, {0, 0, 0}};
+	SendCommand(&c);
+
 	UsbCommand response;
 
 	int bytes_completed = 0;
 	bool show_warning = true;
 	while (true) {
-		if (getCommand(&response)) {
+		if (msclock() - start_time > 2000 && show_warning) {
+			PrintAndLog("Waiting for a response from the proxmark...");
+			PrintAndLog("You can cancel this operation by pressing the pm3 button");
+			show_warning = false;
+		}
+		if (getCommand(&response, CMD_BUFFER_CHECK_TIME)) {
 			if (response.cmd == CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K) {
 				int copy_bytes = MIN(bytes - bytes_completed, response.arg[1]);
 				memcpy(dest + response.arg[0], response.d.asBytes, copy_bytes);
@@ -322,19 +379,13 @@ bool GetFromFpgaRAM(uint8_t *dest, int bytes)
 				return true;
 			}
 		}
-
-		if (msclock() - start_time > 2000 && show_warning) {
-			PrintAndLog("Waiting for a response from the proxmark...");
-			PrintAndLog("You can cancel this operation by pressing the pm3 button");
-			show_warning = false;
-		}
 	}
 
 	return false;
 }
 
 
-bool OpenProxmark(void *port, bool wait_for_port, int timeout, bool flash_mode) {
+bool OpenProxmark(void *port, bool wait_for_port, int timeout) {
 	char *portname = (char *)port;
 	if (!wait_for_port) {
 		sp = uart_open(portname);
@@ -366,7 +417,6 @@ bool OpenProxmark(void *port, bool wait_for_port, int timeout, bool flash_mode)
 		// start the USB communication thread
 		serial_port_name = portname;
 		conn.run = true;
-		conn.block_after_ACK = flash_mode;
 		pthread_create(&USB_communication_thread, NULL, &uart_communication, &conn);
 		return true;
 	}
@@ -394,15 +444,6 @@ void CloseProxmark(void) {
 		uart_close(sp);
 	}
 
-#if defined(__linux__) && !defined(NO_UNLINK)
-	// Fix for linux, it seems that it is extremely slow to release the serial port file descriptor /dev/*
-	//
-	// This may be disabled at compile-time with -DNO_UNLINK (used for a JNI-based serial port on Android).
-	if (serial_port_name) {
-		unlink(serial_port_name);
-	}
-#endif
-
 	// Clean up our state
 	sp = NULL;
 	serial_port_name = NULL;
@@ -430,30 +471,28 @@ bool WaitForResponseTimeoutW(uint32_t cmd, UsbCommand* response, size_t ms_timeo
 	printf("Waiting for %04x cmd\n", cmd);
 	#endif
 
+	uint64_t start_time = msclock();
+
 	if (response == NULL) {
 		response = &resp;
 	}
 
-	uint64_t start_time = msclock();
-
 	// Wait until the command is received
 	while (true) {
-		while(getCommand(response)) {
+		if (ms_timeout != -1 && msclock() > start_time + ms_timeout) {
-			if (cmd == CMD_UNKNOWN || response->cmd == cmd) {
+			break; // timeout
-				return true;
 		}
-		}
-
-		if (msclock() - start_time > ms_timeout) {
-			break;
-		}
-
 		if (msclock() - start_time > 2000 && show_warning) {
 			// 2 seconds elapsed (but this doesn't mean the timeout was exceeded)
 			PrintAndLog("Waiting for a response from the proxmark...");
 			PrintAndLog("You can cancel this operation by pressing the pm3 button");
 			show_warning = false;
 		}
+		if (getCommand(response, CMD_BUFFER_CHECK_TIME)) {
+			if (cmd == CMD_UNKNOWN || response->cmd == cmd) {
+				return true;
+			}
+		}
 	}
 	return false;
 }

client/comms.h

@@ -9,32 +9,24 @@
 // Code for communicating with the proxmark3 hardware.
 //-----------------------------------------------------------------------------
 
-#ifndef COMMS_H_
+#ifndef COMMS_H__
-#define COMMS_H_
+#define COMMS_H__
 
+#include <stddef.h>
+#include <stdint.h>
 #include <stdbool.h>
-#include <pthread.h>
-
 #include "usb_cmd.h"
-#include "uart.h"
 
-#ifndef CMD_BUFFER_SIZE
+extern void SetOffline(bool new_offline);
-#define CMD_BUFFER_SIZE 50
+extern bool IsOffline();
-#endif
+extern bool OpenProxmark(void *port, bool wait_for_port, int timeout);
+extern void CloseProxmark(void);
+extern void SendCommand(UsbCommand *c);
+extern void clearCommandBuffer();
+extern bool WaitForResponseTimeoutW(uint32_t cmd, UsbCommand* response, size_t ms_timeout, bool show_warning);
+extern bool WaitForResponseTimeout(uint32_t cmd, UsbCommand* response, size_t ms_timeout);
+extern bool WaitForResponse(uint32_t cmd, UsbCommand* response);
+extern bool GetFromBigBuf(uint8_t *dest, int bytes, int start_index, UsbCommand *response, size_t ms_timeout, bool show_warning);
+extern bool GetFromFpgaRAM(uint8_t *dest, int bytes);
 
-void SetOffline(bool new_offline);
+#endif // COMMS_H__
-bool IsOffline();
-
-bool OpenProxmark(void *port, bool wait_for_port, int timeout, bool flash_mode);
-void CloseProxmark(void);
-
-void SendCommand(UsbCommand *c);
-
-void clearCommandBuffer();
-bool WaitForResponseTimeoutW(uint32_t cmd, UsbCommand* response, size_t ms_timeout, bool show_warning);
-bool WaitForResponseTimeout(uint32_t cmd, UsbCommand* response, size_t ms_timeout);
-bool WaitForResponse(uint32_t cmd, UsbCommand* response);
-bool GetFromBigBuf(uint8_t *dest, int bytes, int start_index, UsbCommand *response, size_t ms_timeout, bool show_warning);
-bool GetFromFpgaRAM(uint8_t *dest, int bytes);
-
-#endif // COMMS_H_

client/flash.c

@@ -185,9 +185,9 @@ static int check_segs(flash_file_t *ctx, int can_write_bl) {
 	return 0;
 }
 
+
 // Load an ELF file and prepare it for flashing
-int flash_load(flash_file_t *ctx, const char *name, bool can_write_bl)
+int flash_load(flash_file_t *ctx, const char *name, bool can_write_bl) {
-{
 	FILE *fd = NULL;
 	Elf32_Ehdr ehdr;
 	Elf32_Phdr *phdrs = NULL;
@@ -267,9 +267,9 @@ fail:
 	return -1;
 }
 
+
 // Get the state of the proxmark, backwards compatible
-static int get_proxmark_state(uint32_t *state)
+static int get_proxmark_state(uint32_t *state) {
-{
 	UsbCommand c = {0};
 	c.cmd = CMD_DEVICE_INFO;
 	SendCommand(&c);
@@ -300,9 +300,9 @@ static int get_proxmark_state(uint32_t *state)
 	return 0;
 }
 
+
 // Enter the bootloader to be able to start flashing
-static int enter_bootloader(char *serial_port_name)
+static int enter_bootloader(char *serial_port_name) {
-{
 	uint32_t state;
 
 	if (get_proxmark_state(&state) < 0)
@@ -336,7 +336,9 @@ static int enter_bootloader(char *serial_port_name)
 		msleep(100);
 		CloseProxmark();
 
-		bool opened = OpenProxmark(serial_port_name, true, 120, true);   // wait for 2 minutes
+		msleep(1000); // wait for OS to detect device disconnect.
+
+		bool opened = OpenProxmark(serial_port_name, true, 120);   // wait for 2 minutes
 		if (opened) {
 			fprintf(stderr," Found.\n");
 			return 0;
@@ -350,6 +352,7 @@ static int enter_bootloader(char *serial_port_name)
 	return -1;
 }
 
+
 static int wait_for_ack(void)
 {
 	UsbCommand ack;
@@ -361,11 +364,11 @@ static int wait_for_ack(void)
 	return 0;
 }
 
+
 // Go into flashing mode
 int flash_start_flashing(int enable_bl_writes,char *serial_port_name)
 {
 	uint32_t state;
-
 	if (enter_bootloader(serial_port_name) < 0)
 		return -1;
 

client/flasher.c

@@ -17,7 +17,7 @@
 #include "flash.h"
 #include "comms.h"
 #include "usb_cmd.h"
-
+#include "uart.h"
 
 void cmd_debug(UsbCommand* UC) {
   //  Debug
@@ -83,7 +83,7 @@ int main(int argc, char **argv)
 
 	char* serial_port_name = argv[1];
 
-	if (!OpenProxmark(serial_port_name, true, 120, true)) {   // wait for 2 minutes
+	if (!OpenProxmark(serial_port_name, true, 120)) {   // wait for 2 minutes
 		fprintf(stderr, "Could not find Proxmark on %s.\n\n", serial_port_name);
 		return -1;
 	} else {

client/mifare/mifarehost.c

@@ -1164,7 +1164,7 @@ int DetectClassicPrng(void){
 
 	clearCommandBuffer();
 	SendCommand(&c);
-	if (!WaitForResponseTimeout(CMD_ACK, &resp, 2000)) {
+	if (!WaitForResponseTimeout(CMD_NACK, &resp, 2000)) {
         PrintAndLog("PRNG UID: Reply timeout.");
 		return -1;
 	}

client/proxmark3.c

@@ -28,7 +28,7 @@
 #include "cmdhw.h"
 #include "whereami.h"
 #include "comms.h"
-
+#include "uart.h"
 
 void
 #ifdef __has_attribute
@@ -286,7 +286,7 @@ int main(int argc, char* argv[]) {
 	set_my_executable_path();
 
 	// try to open USB connection to Proxmark
-	usb_present = OpenProxmark(argv[1], waitCOMPort, 20, false);
+	usb_present = OpenProxmark(argv[1], waitCOMPort, 20);
 
 #ifdef HAVE_GUI
 #ifdef _WIN32
@@ -309,8 +309,10 @@ int main(int argc, char* argv[]) {
 	main_loop(script_cmds_file, script_cmd, usb_present);
 #endif	
 
-	// Clean up the port
+	// Switch off field and clean up the port
 	if (usb_present) {
+		UsbCommand c = {CMD_FPGA_MAJOR_MODE_OFF};
+		SendCommand(&c);
 		CloseProxmark();
 	}
 

client/scripting.c

@@ -14,6 +14,7 @@
 #include <lua.h>
 #include <lualib.h>
 #include <lauxlib.h>
+#include <string.h>
 #include "proxmark3.h"
 #include "comms.h"
 #include "usb_cmd.h"

common/cmd.c

@@ -1,79 +0,0 @@
-/*
- * Proxmark send and receive commands
- *
- * Copyright (c) 2012, Roel Verdult
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * 3. Neither the name of the copyright holders nor the
- * names of its contributors may be used to endorse or promote products
- * derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * @file cmd.c
- * @brief
- */
-
-#include "cmd.h"
-#include "string.h"
-#include "proxmark3.h"
-
-bool cmd_receive(UsbCommand* cmd) {
- 
-  // Check if there is a usb packet available
-  if (!usb_poll()) return false;
-  
-  // Try to retrieve the available command frame
-  size_t rxlen = usb_read((byte_t*)cmd,sizeof(UsbCommand));
-
-  // Check if the transfer was complete
-  if (rxlen != sizeof(UsbCommand)) return false;
-  
-  // Received command successfully
-  return true;
-}
-
-bool cmd_send(uint32_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, size_t len) {
-  UsbCommand txcmd;
-
-  for (size_t i=0; i<sizeof(UsbCommand); i++) {
-    ((byte_t*)&txcmd)[i] = 0x00;
-  }
-  
-  // Compose the outgoing command frame
-  txcmd.cmd = cmd;
-  txcmd.arg[0] = arg0;
-  txcmd.arg[1] = arg1;	
-  txcmd.arg[2] = arg2;
-
-  // Add the (optional) content to the frame, with a maximum size of USB_CMD_DATA_SIZE
-  if (data && len) {
-    len = MIN(len,USB_CMD_DATA_SIZE);
-    for (size_t i=0; i<len; i++) {
-      txcmd.d.asBytes[i] = ((byte_t*)data)[i];
-    }
-  }
-  
-  // Send frame and make sure all bytes are transmitted
-  if (usb_write((byte_t*)&txcmd,sizeof(UsbCommand)) != 0) return false;
-  
-  return true;
-}
-
-

common/cmd.h

@@ -1,44 +0,0 @@
-/*
- * Proxmark send and receive commands
- *
- * Copyright (c) 2010, Roel Verdult
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * 3. Neither the name of the copyright holders nor the
- * names of its contributors may be used to endorse or promote products
- * derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * @file cmd.h
- * @brief
- */
-
-#ifndef _PROXMARK_CMD_H_
-#define _PROXMARK_CMD_H_
-
-#include "common.h"
-#include "usb_cmd.h"
-#include "usb_cdc.h"
-
-bool cmd_receive(UsbCommand* cmd);
-bool cmd_send(uint32_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, size_t len);
-
-#endif // _PROXMARK_CMD_H_
-

common/usb_cdc.c

@@ -33,10 +33,15 @@
  */
 
 #include "usb_cdc.h"
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "common.h"
 #include "at91sam7s512.h"
 #include "config_gpio.h"
 
-
 #define AT91C_EP_CONTROL     0
 #define AT91C_EP_OUT         1
 #define AT91C_EP_IN          2
@@ -49,6 +54,7 @@
 #define STR_MANUFACTURER   0x01
 #define STR_PRODUCT        0x02
 
+
 static const char devDescriptor[] = {
 	/* Device descriptor */
 	0x12,      // bLength
@@ -67,6 +73,7 @@ static const char devDescriptor[] = {
 	0x01       // bNumConfigs
 };
 
+
 static const char cfgDescriptor[] = {
 	/* ============== CONFIGURATION 1 =========== */
 	/* Configuration 1 descriptor */
@@ -157,12 +164,14 @@ static const char cfgDescriptor[] = {
 	0x00    // bInterval
 };
 
+
 static const char StrDescLanguageCodes[] = {
 	4,          // Length
 	0x03,       // Type is string
 	0x09, 0x04  // supported language Code 0 = 0x0409 (English)
 };
 
+
 // Note: ModemManager (Linux) ignores Proxmark3 devices by matching the
 // manufacturer string "proxmark.org". Don't change this.
 static const char StrDescManufacturer[] = {
@@ -182,6 +191,7 @@ static const char StrDescManufacturer[] = {
 	'g', 0x00
 };
 
+
 static const char StrDescProduct[] = {
 	20,         // Length
 	0x03,       // Type is string
@@ -196,8 +206,8 @@ static const char StrDescProduct[] = {
 	'3', 0x00
 };
 
-const char* getStringDescriptor(uint8_t idx)
+
-{
+static const char* getStringDescriptor(uint8_t idx) {
 	switch (idx) {
 		case STR_LANGUAGE_CODES:
 			return StrDescLanguageCodes;
@@ -210,29 +220,31 @@ const char* getStringDescriptor(uint8_t idx)
 	}
 }
 
+
 // Bitmap for all status bits in CSR which must be written as 1 to cause no effect
-#define REG_NO_EFFECT_1_ALL      AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1 \
+#define REG_NO_EFFECT_1_ALL     (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1 | AT91C_UDP_STALLSENT | AT91C_UDP_RXSETUP | AT91C_UDP_TXCOMP)
-                                |AT91C_UDP_STALLSENT   | AT91C_UDP_RXSETUP \
+
-                                |AT91C_UDP_TXCOMP
 
 // Clear flags in the UDP_CSR register
 #define UDP_CLEAR_EP_FLAGS(endpoint, flags) { \
 	volatile unsigned int reg; \
-	reg = pUdp->UDP_CSR[(endpoint)]; \
+	reg = AT91C_BASE_UDP->UDP_CSR[(endpoint)]; \
 	reg |= REG_NO_EFFECT_1_ALL; \
 	reg &= ~(flags); \
-	pUdp->UDP_CSR[(endpoint)] = reg; \
+	AT91C_BASE_UDP->UDP_CSR[(endpoint)] = reg; \
 }
 
+
 // Set flags in the UDP_CSR register
 #define UDP_SET_EP_FLAGS(endpoint, flags) { \
 	volatile unsigned int reg; \
-	reg = pUdp->UDP_CSR[(endpoint)]; \
+	reg = AT91C_BASE_UDP->UDP_CSR[(endpoint)]; \
 	reg |= REG_NO_EFFECT_1_ALL; \
 	reg |= (flags); \
-	pUdp->UDP_CSR[(endpoint)] = reg; \
+	AT91C_BASE_UDP->UDP_CSR[(endpoint)] = reg; \
 }
 
+
 /* USB standard request codes */
 #define STD_GET_STATUS_ZERO           0x0080
 #define STD_GET_STATUS_INTERFACE      0x0081
@@ -260,6 +272,7 @@ const char* getStringDescriptor(uint8_t idx)
 #define SET_LINE_CODING               0x2021
 #define SET_CONTROL_LINE_STATE        0x2221
 
+
 typedef struct {
 	unsigned int dwDTERRate;
 	char bCharFormat;
@@ -267,19 +280,17 @@ typedef struct {
 	char bDataBits;
 } AT91S_CDC_LINE_CODING, *AT91PS_CDC_LINE_CODING;
 
-AT91S_CDC_LINE_CODING line = {
+
+static AT91S_CDC_LINE_CODING line = {
 	115200, // baudrate
 	0,      // 1 Stop Bit
 	0,      // None Parity
 	8};     // 8 Data bits
 
 
-void AT91F_CDC_Enumerate();
+static uint8_t btConfiguration = 0;
-
+static uint8_t btConnection    = 0;
-AT91PS_UDP pUdp = AT91C_BASE_UDP;
+static uint8_t btReceiveBank   = AT91C_UDP_RX_DATA_BK0;
-byte_t btConfiguration = 0;
-byte_t btConnection    = 0;
-byte_t btReceiveBank   = AT91C_UDP_RX_DATA_BK0;
 
 
 //*----------------------------------------------------------------------------
@@ -291,8 +302,8 @@ void usb_disable() {
 	AT91C_BASE_PIOA->PIO_ODR = GPIO_USB_PU;
 
 	// Clear all lingering interrupts
-	if(pUdp->UDP_ISR & AT91C_UDP_ENDBUSRES) {
+	if (AT91C_BASE_UDP->UDP_ISR & AT91C_UDP_ENDBUSRES) {
-		pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES;
+		AT91C_BASE_UDP->UDP_ICR = AT91C_UDP_ENDBUSRES;
 	}
 }
 
@@ -330,34 +341,242 @@ void usb_enable() {
 }
 
 
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_USB_SendZlp
+//* \brief Send zero length packet through an endpoint
+//*----------------------------------------------------------------------------
+static void AT91F_USB_SendZlp(uint8_t endpoint) {
+	UDP_SET_EP_FLAGS(endpoint, AT91C_UDP_TXPKTRDY);
+	while (!(AT91C_BASE_UDP->UDP_CSR[endpoint] & AT91C_UDP_TXCOMP))
+		/* wait */;
+	UDP_CLEAR_EP_FLAGS(endpoint, AT91C_UDP_TXCOMP);
+	while (AT91C_BASE_UDP->UDP_CSR[endpoint] & AT91C_UDP_TXCOMP)
+		/* wait */;
+}
+
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_USB_SendData
+//* \brief Send Data through the control endpoint
+//*----------------------------------------------------------------------------
+static void AT91F_USB_SendData(const char *pData, uint32_t length) {
+	uint32_t cpt = 0;
+	AT91_REG csr;
+
+	do {
+		cpt = MIN(length, 8);
+		length -= cpt;
+
+		while (cpt--)
+			AT91C_BASE_UDP->UDP_FDR[0] = *pData++;
+
+		if (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {
+			UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
+			while (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP)
+				/* wait */;
+		}
+
+		UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY);
+		do {
+			csr = AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL];
+
+			// Data IN stage has been stopped by a status OUT
+			if (csr & AT91C_UDP_RX_DATA_BK0) {
+				UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0);
+				return;
+			}
+		} while (!(csr & AT91C_UDP_TXCOMP));
+
+	} while (length);
+
+	if (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {
+		UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
+		while (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP)
+			/* wait */;
+	}
+}
+
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_USB_SendStall
+//* \brief Stall the control endpoint
+//*----------------------------------------------------------------------------
+static void AT91F_USB_SendStall(void) {
+	UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL);
+	while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_ISOERROR))
+		/* wait */;
+	UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR);
+	while (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR))
+		/* wait */;
+}
+
+
+//*----------------------------------------------------------------------------
+//* \fn    AT91F_CDC_Enumerate
+//* \brief This function is a callback invoked when a SETUP packet is received
+//*----------------------------------------------------------------------------
+static void AT91F_CDC_Enumerate() {
+	uint8_t bmRequestType, bRequest;
+	uint16_t wValue, wIndex, wLength, wStatus;
+
+	if (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP))
+		return;
+
+	bmRequestType = AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL];
+	bRequest      = AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL];
+	wValue        = (AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
+	wValue       |= (AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL] << 8);
+	wIndex        = (AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
+	wIndex       |= (AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL] << 8);
+	wLength       = (AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
+	wLength      |= (AT91C_BASE_UDP->UDP_FDR[AT91C_EP_CONTROL] << 8);
+
+	if (bmRequestType & 0x80) { // Data Phase Transfer Direction Device to Host
+		UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_DIR);
+		while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_DIR))
+			/* wait */;
+	}
+	UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RXSETUP);
+	while (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP)
+		/* wait */;
+
+	// Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1
+	switch ((bRequest << 8) | bmRequestType) {
+	case STD_GET_DESCRIPTOR:
+		if (wValue == 0x100)       // Return Device Descriptor
+			AT91F_USB_SendData(devDescriptor, MIN(sizeof(devDescriptor), wLength));
+		else if (wValue == 0x200)  // Return Configuration Descriptor
+			AT91F_USB_SendData(cfgDescriptor, MIN(sizeof(cfgDescriptor), wLength));
+		else if ((wValue & 0xF00) == 0x300) { // Return String Descriptor
+			const char *strDescriptor = getStringDescriptor(wValue & 0xff);
+			if (strDescriptor != NULL) {
+				AT91F_USB_SendData(strDescriptor, MIN(strDescriptor[0], wLength));
+			} else {
+				AT91F_USB_SendStall();
+			}
+		}
+		else
+			AT91F_USB_SendStall();
+		break;
+	case STD_SET_ADDRESS:
+		AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		AT91C_BASE_UDP->UDP_FADDR = (AT91C_UDP_FEN | wValue);
+		AT91C_BASE_UDP->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_FADDEN : 0;
+		break;
+	case STD_SET_CONFIGURATION:
+		btConfiguration = wValue;
+		AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		AT91C_BASE_UDP->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
+		AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT]    = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) : 0;
+		AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN]     = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN)  : 0;
+		AT91C_BASE_UDP->UDP_CSR[AT91C_EP_NOTIFY] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN)   : 0;
+		break;
+	case STD_GET_CONFIGURATION:
+		AT91F_USB_SendData((char *) &(btConfiguration), sizeof(btConfiguration));
+		break;
+	case STD_GET_STATUS_ZERO:
+		wStatus = 0;    // Device is Bus powered, remote wakeup disabled
+		AT91F_USB_SendData((char *) &wStatus, sizeof(wStatus));
+		break;
+	case STD_GET_STATUS_INTERFACE:
+		wStatus = 0;    // reserved for future use
+		AT91F_USB_SendData((char *) &wStatus, sizeof(wStatus));
+		break;
+	case STD_GET_STATUS_ENDPOINT:
+		wStatus = 0;
+		wIndex &= 0x0F;
+		if ((AT91C_BASE_UDP->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= AT91C_EP_NOTIFY)) {
+			wStatus = (AT91C_BASE_UDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
+			AT91F_USB_SendData((char *) &wStatus, sizeof(wStatus));
+		} else if ((AT91C_BASE_UDP->UDP_GLBSTATE & AT91C_UDP_FADDEN) && (wIndex == AT91C_EP_CONTROL)) {
+			wStatus = (AT91C_BASE_UDP->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
+			AT91F_USB_SendData((char *) &wStatus, sizeof(wStatus));
+		} else
+			AT91F_USB_SendStall();
+		break;
+	case STD_SET_FEATURE_ZERO:
+		AT91F_USB_SendStall();
+		break;
+	case STD_SET_FEATURE_INTERFACE:
+		AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		break;
+	case STD_SET_FEATURE_ENDPOINT:
+		wIndex &= 0x0F;
+		if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) {
+			AT91C_BASE_UDP->UDP_CSR[wIndex] = 0;
+			AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		} else
+			AT91F_USB_SendStall();
+		break;
+	case STD_CLEAR_FEATURE_ZERO:
+		AT91F_USB_SendStall();
+		break;
+	case STD_CLEAR_FEATURE_INTERFACE:
+		AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		break;
+	case STD_CLEAR_FEATURE_ENDPOINT:
+		wIndex &= 0x0F;
+		if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) {
+			if (wIndex == AT91C_EP_OUT)
+				AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT);
+			else if (wIndex == AT91C_EP_IN)
+				AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN);
+			else if (wIndex == AT91C_EP_NOTIFY)
+				AT91C_BASE_UDP->UDP_CSR[AT91C_EP_NOTIFY] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN);
+			AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		}
+		else
+			AT91F_USB_SendStall();
+		break;
+
+	// handle CDC class requests
+	case SET_LINE_CODING:
+		while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RX_DATA_BK0))
+			/* wait */;
+		UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0);
+		AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		break;
+	case GET_LINE_CODING:
+		AT91F_USB_SendData((char *) &line, MIN(sizeof(line), wLength));
+		break;
+	case SET_CONTROL_LINE_STATE:
+		btConnection = wValue;
+		AT91F_USB_SendZlp(AT91C_EP_CONTROL);
+		break;
+	default:
+		AT91F_USB_SendStall();
+		break;
+	}
+}
+
+
 //*----------------------------------------------------------------------------
 //* \fn    usb_check
 //* \brief Test if the device is configured and handle enumeration
 //*----------------------------------------------------------------------------
-bool usb_check() {
+static bool usb_check() {
-	AT91_REG isr = pUdp->UDP_ISR;
+	AT91_REG isr = AT91C_BASE_UDP->UDP_ISR;
 
 	if (isr & AT91C_UDP_ENDBUSRES) {
-		pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES;
+		AT91C_BASE_UDP->UDP_ICR = AT91C_UDP_ENDBUSRES;
 		// reset all endpoints
-		pUdp->UDP_RSTEP  = (unsigned int)-1;
+		AT91C_BASE_UDP->UDP_RSTEP  = (unsigned int)-1;
-		pUdp->UDP_RSTEP  = 0;
+		AT91C_BASE_UDP->UDP_RSTEP  = 0;
 		// Enable the function
-		pUdp->UDP_FADDR = AT91C_UDP_FEN;
+		AT91C_BASE_UDP->UDP_FADDR = AT91C_UDP_FEN;
 		// Configure endpoint 0
-		pUdp->UDP_CSR[AT91C_EP_CONTROL] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
+		AT91C_BASE_UDP->UDP_CSR[AT91C_EP_CONTROL] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
 	} else if (isr & AT91C_UDP_EPINT0) {
-		pUdp->UDP_ICR = AT91C_UDP_EPINT0;
+		AT91C_BASE_UDP->UDP_ICR = AT91C_UDP_EPINT0;
 		AT91F_CDC_Enumerate();
 	}
 	return (btConfiguration) ? true : false;
 }
 
 
-bool usb_poll()
+bool usb_poll() {
-{
 	if (!usb_check()) return false;
-	return (pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank);
+	return (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT] & btReceiveBank);
 }
 
 
@@ -369,30 +588,30 @@ bool usb_poll()
 	that the length available to read is non-zero, thus hopefully fixes the
 	bug.
 **/
-bool usb_poll_validate_length()
+bool usb_poll_validate_length() {
-{
 	if (!usb_check()) return false;
-	if (!(pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank)) return false;
+	if (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT] & btReceiveBank)) return false;
-	return (pUdp->UDP_CSR[AT91C_EP_OUT] >> 16) >  0;
+	return (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT] >> 16) >  0;
 }
 
+
 //*----------------------------------------------------------------------------
 //* \fn    usb_read
 //* \brief Read available data from Endpoint OUT
 //*----------------------------------------------------------------------------
-uint32_t usb_read(byte_t* data, size_t len) {
+static uint32_t usb_read(uint8_t* data, size_t len) {
-	byte_t bank = btReceiveBank;
+	uint8_t bank = btReceiveBank;
 	uint32_t packetSize, nbBytesRcv = 0;
 	uint32_t time_out = 0;
 
 	while (len)  {
 		if (!usb_check()) break;
 
-		if ( pUdp->UDP_CSR[AT91C_EP_OUT] & bank ) {
+		if ( AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT] & bank ) {
-			packetSize = MIN(pUdp->UDP_CSR[AT91C_EP_OUT] >> 16, len);
+			packetSize = MIN(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_OUT] >> 16, len);
 			len -= packetSize;
 			while (packetSize--)
-				data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT];
+				data[nbBytesRcv++] = AT91C_BASE_UDP->UDP_FDR[AT91C_EP_OUT];
 			UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank);
 			if (bank == AT91C_UDP_RX_DATA_BK0) {
 				bank = AT91C_UDP_RX_DATA_BK1;
@@ -412,7 +631,7 @@ uint32_t usb_read(byte_t* data, size_t len) {
 //* \fn    usb_write
 //* \brief Send through endpoint 2
 //*----------------------------------------------------------------------------
-uint32_t usb_write(const byte_t* data, const size_t len) {
+static uint32_t usb_write(const uint8_t* data, const size_t len) {
 	size_t length = len;
 	uint32_t cpt = 0;
 
@@ -423,239 +642,122 @@ uint32_t usb_write(const byte_t* data, const size_t len) {
 	cpt = MIN(length, AT91C_EP_IN_SIZE);
 	length -= cpt;
 	while (cpt--) {
-		pUdp->UDP_FDR[AT91C_EP_IN] = *data++;
+		AT91C_BASE_UDP->UDP_FDR[AT91C_EP_IN] = *data++;
 	}
 	UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
+	while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY))
+		/* wait */;
 
 	while (length) {
 		// Fill the next bank
 		cpt = MIN(length, AT91C_EP_IN_SIZE);
 		length -= cpt;
 		while (cpt--) {
-			pUdp->UDP_FDR[AT91C_EP_IN] = *data++;
+			AT91C_BASE_UDP->UDP_FDR[AT91C_EP_IN] = *data++;
 		}
 		// Wait for the previous bank to be sent
-		while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
+		while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
 			if (!usb_check()) return length;
 		}
-		UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP);
-		while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP);
 		UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
+		while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY))
+			/* wait */;
+		UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP);
+		while (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)
+			/* wait */;
 	}
 
 	// Wait for the end of transfer
-	while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
+	while (!(AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
 		if (!usb_check()) return length;
 	}
-
 	UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP);
-	while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP);
+	while (AT91C_BASE_UDP->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)
+		/* wait */;
+
+	if (len % AT91C_EP_IN_SIZE == 0) { // need to send a zero length packet to complete the transfer
+		AT91F_USB_SendZlp(AT91C_EP_IN);
+	}
 
 	return length;
 }
 
 
-//*----------------------------------------------------------------------------
+//***************************************************************************
-//* \fn    AT91F_USB_SendData
+// Interface to the main program
-//* \brief Send Data through the control endpoint
+//***************************************************************************
-//*----------------------------------------------------------------------------
-unsigned int csrTab[100] = {0x00};
-unsigned char csrIdx = 0;
 
-static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, uint32_t length) {
+// The function to receive a command from the client via USB
-	uint32_t cpt = 0;
+bool cmd_receive(UsbCommand* cmd) {
-	AT91_REG csr;
 
-	do {
+	// Check if there is a usb packet available
-		cpt = MIN(length, 8);
+	if (!usb_poll())
-		length -= cpt;
+		return false;
 
-		while (cpt--)
+	// Try to retrieve the available command frame
-			pUdp->UDP_FDR[0] = *pData++;
+	size_t rxlen = usb_read((uint8_t*)cmd, sizeof(UsbCommand));
 
-		if (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {
+	// Check if the transfer was complete
-			UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
+	if (rxlen != sizeof(UsbCommand))
-			while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP);
+		return false;
-		}
 
-		UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY);
+	// Received command successfully
-		do {
+	return true;
-			csr = pUdp->UDP_CSR[AT91C_EP_CONTROL];
-
-			// Data IN stage has been stopped by a status OUT
-			if (csr & AT91C_UDP_RX_DATA_BK0) {
-				UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0);
-				return;
-			}
-		} while ( !(csr & AT91C_UDP_TXCOMP) );
-
-	} while (length);
-
-	if (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {
-		UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
-		while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP);
-	}
 }
 
 
-//*----------------------------------------------------------------------------
+// The function to send a response to the client via USB
-//* \fn    AT91F_USB_SendZlp
+bool cmd_send(uint16_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, uint16_t datalen) {
-//* \brief Send zero length packet through the control endpoint
+
-//*----------------------------------------------------------------------------
+	UsbResponse txcmd;
-void AT91F_USB_SendZlp(AT91PS_UDP pUdp) {
+
-	UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY);
+	// Compose the outgoing response frame
-	while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) );
+	txcmd.cmd = cmd | CMD_VARIABLE_SIZE_FLAG;
-	UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
+	txcmd.arg[0] = arg0;
-	while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP);
+	txcmd.arg[1] = arg1;
+	txcmd.arg[2] = arg2;
+
+	// Add the (optional) content to the frame, with a maximum size of USB_CMD_DATA_SIZE
+	if (data) {
+		datalen = MIN(datalen, USB_CMD_DATA_SIZE);
+		for (uint16_t i = 0; i < datalen; i++) {
+			txcmd.d.asBytes[i] = ((uint8_t*)data)[i];
 		}
-
+		txcmd.datalen = datalen;
-
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_USB_SendStall
-//* \brief Stall the control endpoint
-//*----------------------------------------------------------------------------
-void AT91F_USB_SendStall(AT91PS_UDP pUdp) {
-	UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL);
-	while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_ISOERROR) );
-	UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR);
-	while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR));
-}
-
-
-//*----------------------------------------------------------------------------
-//* \fn    AT91F_CDC_Enumerate
-//* \brief This function is a callback invoked when a SETUP packet is received
-//*----------------------------------------------------------------------------
-void AT91F_CDC_Enumerate() {
-	byte_t bmRequestType, bRequest;
-	uint16_t wValue, wIndex, wLength, wStatus;
-
-	if ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP) )
-		return;
-
-	bmRequestType = pUdp->UDP_FDR[AT91C_EP_CONTROL];
-	bRequest      = pUdp->UDP_FDR[AT91C_EP_CONTROL];
-	wValue        = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
-	wValue       |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8);
-	wIndex        = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
-	wIndex       |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8);
-	wLength       = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
-	wLength      |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8);
-
-	if (bmRequestType & 0x80) {	// Data Phase Transfer Direction Device to Host
-		UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_DIR);
-		while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_DIR) );
-	}
-	UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RXSETUP);
-	while ( (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP)  );
-
-	// Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1
-	switch ((bRequest << 8) | bmRequestType) {
-	case STD_GET_DESCRIPTOR:
-		if (wValue == 0x100)       // Return Device Descriptor
-			AT91F_USB_SendData(pUdp, devDescriptor, MIN(sizeof(devDescriptor), wLength));
-		else if (wValue == 0x200)  // Return Configuration Descriptor
-			AT91F_USB_SendData(pUdp, cfgDescriptor, MIN(sizeof(cfgDescriptor), wLength));
-		else if ((wValue & 0xF00) == 0x300) { // Return String Descriptor
-			const char *strDescriptor = getStringDescriptor(wValue & 0xff);
-			if (strDescriptor != NULL) {
-				AT91F_USB_SendData(pUdp, strDescriptor, MIN(strDescriptor[0], wLength));
 	} else {
-				AT91F_USB_SendStall(pUdp);
+		txcmd.datalen = 0;
 	}
-		}
-		else
-			AT91F_USB_SendStall(pUdp);
-		break;
-	case STD_SET_ADDRESS:
-		AT91F_USB_SendZlp(pUdp);
-		pUdp->UDP_FADDR = (AT91C_UDP_FEN | wValue);
-		pUdp->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_FADDEN : 0;
-		break;
-	case STD_SET_CONFIGURATION:
-		btConfiguration = wValue;
-		AT91F_USB_SendZlp(pUdp);
-		pUdp->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
-		pUdp->UDP_CSR[AT91C_EP_OUT]    = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) : 0;
-		pUdp->UDP_CSR[AT91C_EP_IN]     = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN)  : 0;
-		pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN)   : 0;
-		break;
-	case STD_GET_CONFIGURATION:
-		AT91F_USB_SendData(pUdp, (char *) &(btConfiguration), sizeof(btConfiguration));
-		break;
-	case STD_GET_STATUS_ZERO:
-		wStatus = 0;	// Device is Bus powered, remote wakeup disabled
-		AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
-		break;
-	case STD_GET_STATUS_INTERFACE:
-		wStatus = 0; 	// reserved for future use
-		AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
-		break;
-	case STD_GET_STATUS_ENDPOINT:
-		wStatus = 0;
-		wIndex &= 0x0F;
-		if ((pUdp->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= AT91C_EP_NOTIFY)) {
-			wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
-			AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
-		}
-		else if ((pUdp->UDP_GLBSTATE & AT91C_UDP_FADDEN) && (wIndex == AT91C_EP_CONTROL)) {
-			wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
-			AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
-		}
-		else
-			AT91F_USB_SendStall(pUdp);
-		break;
-	case STD_SET_FEATURE_ZERO:
-		AT91F_USB_SendStall(pUdp);
-	    break;
-	case STD_SET_FEATURE_INTERFACE:
-		AT91F_USB_SendZlp(pUdp);
-		break;
-	case STD_SET_FEATURE_ENDPOINT:
-		wIndex &= 0x0F;
-		if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) {
-			pUdp->UDP_CSR[wIndex] = 0;
-			AT91F_USB_SendZlp(pUdp);
-		}
-		else
-			AT91F_USB_SendStall(pUdp);
-		break;
-	case STD_CLEAR_FEATURE_ZERO:
-		AT91F_USB_SendStall(pUdp);
-	    break;
-	case STD_CLEAR_FEATURE_INTERFACE:
-		AT91F_USB_SendZlp(pUdp);
-		break;
-	case STD_CLEAR_FEATURE_ENDPOINT:
-		wIndex &= 0x0F;
-		if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) {
-			if (wIndex == AT91C_EP_OUT)
-				pUdp->UDP_CSR[AT91C_EP_OUT] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT);
-			else if (wIndex == AT91C_EP_IN)
-				pUdp->UDP_CSR[AT91C_EP_IN] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN);
-			else if (wIndex == AT91C_EP_NOTIFY)
-				pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN);
-			AT91F_USB_SendZlp(pUdp);
-		}
-		else
-			AT91F_USB_SendStall(pUdp);
-		break;
 
-	// handle CDC class requests
+	// Send frame and make sure all bytes are transmitted
-	case SET_LINE_CODING:
+	size_t tx_size = offsetof(UsbResponse, d) + datalen;
-		while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RX_DATA_BK0) );
+	if (usb_write((uint8_t*)&txcmd, tx_size) != 0) return false;
-		UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0);
+
-		AT91F_USB_SendZlp(pUdp);
+	return true;
-		break;
+}
-	case GET_LINE_CODING:
+
-		AT91F_USB_SendData(pUdp, (char *) &line, MIN(sizeof(line), wLength));
+
-		break;
+// For compatibility only: legacy function to send a response with fixed size to the client via USB
-	case SET_CONTROL_LINE_STATE:
+bool cmd_send_old(uint16_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, uint16_t datalen) {
-		btConnection = wValue;
+
-		AT91F_USB_SendZlp(pUdp);
+	UsbCommand txcmd;
-		break;
+
-	default:
+	// Compose the outgoing response frame
-		AT91F_USB_SendStall(pUdp);
+	txcmd.cmd = cmd;
-	    break;
+	txcmd.arg[0] = arg0;
+	txcmd.arg[1] = arg1;
+	txcmd.arg[2] = arg2;
+
+	// Add the (optional) content to the frame, with a maximum size of USB_CMD_DATA_SIZE
+	if (data) {
+		datalen = MIN(datalen, USB_CMD_DATA_SIZE);
+		for (uint16_t i = 0; i < datalen; i++) {
+			txcmd.d.asBytes[i] = ((uint8_t*)data)[i];
 		}
 	}
+
+	// Send frame and make sure all bytes are transmitted
+	if (usb_write((uint8_t*)&txcmd, sizeof(UsbCommand)) != 0) return false;
+
+	return true;
+}
+

common/usb_cdc.h

@@ -32,18 +32,20 @@
  * @brief
  */
 
-#ifndef _USB_CDC_H_
+#ifndef USB_CDC_H__
-#define _USB_CDC_H_
+#define USB_CDC_H__
 
-#include "common.h"
+#include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include "usb_cmd.h"
 
-void usb_disable();
+extern void usb_disable();
-void usb_enable();
+extern void usb_enable();
-bool usb_check();
+extern bool usb_poll();
-bool usb_poll();
+extern bool usb_poll_validate_length();
-bool usb_poll_validate_length();
+extern bool cmd_receive(UsbCommand* cmd);
-uint32_t usb_read(byte_t* data, size_t len);
+extern bool cmd_send(uint16_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, uint16_t datalen); // new variable sized response
-uint32_t usb_write(const byte_t* data, const size_t len);
+extern bool cmd_send_old(uint16_t cmd, uint32_t arg0, uint32_t arg1, uint32_t arg2, void* data, uint16_t datalen); // old fixed size response
-
-#endif // _USB_CDC_H_
 
+#endif // USB_CDC_H__

include/common.h

@@ -9,13 +9,9 @@
 // Interlib Definitions
 //-----------------------------------------------------------------------------
 
-#ifndef __COMMON_H
+#ifndef COMMON_H__
-#define __COMMON_H
+#define COMMON_H__
 
-#include <stddef.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <at91sam7s512.h>
 typedef unsigned char byte_t;
 
 #ifndef MIN
@@ -28,7 +24,6 @@ typedef unsigned char byte_t;
 # define ABS(a) (((a) < 0) ? -(a) : (a))
 #endif
 
-
 #define RAMFUNC __attribute((long_call, section(".ramfunc")))
 
 #endif

include/usb_cmd.h

@@ -10,13 +10,13 @@
 // own protocol.
 //-----------------------------------------------------------------------------
 
-#ifndef __USB_CMD_H
+#ifndef USB_CMD_H__
-#define __USB_CMD_H
+#define USB_CMD_H__
+
 #ifdef _MSC_VER
 typedef DWORD uint32_t;
 typedef BYTE uint8_t;
 #define PACKED
-// stuff
 #else
 #include <stdint.h>
 #include <stdbool.h>
@@ -25,6 +25,7 @@ typedef BYTE uint8_t;
 
 #define USB_CMD_DATA_SIZE 512
 
+// the packets sent from client to PM3
 typedef struct {
 	uint64_t cmd;
 	uint64_t arg[3];
@@ -34,6 +35,17 @@ typedef struct {
 	} d;
 } PACKED UsbCommand;
 
+// the packets sent from PM3 to client (a smaller version of UsbCommand)
+typedef struct {
+	uint16_t cmd;
+	uint16_t datalen;
+	uint32_t arg[3];
+	union {
+		uint8_t  asBytes[USB_CMD_DATA_SIZE];
+		uint32_t asDwords[USB_CMD_DATA_SIZE/4];
+	} d;
+} PACKED UsbResponse;
+
 // A struct used to send sample-configs over USB
 typedef struct {
 	uint8_t decimation;
@@ -44,6 +56,7 @@ typedef struct{
 	int samples_to_skip;
 } sample_config;
 
+
 // For the bootloader
 #define CMD_DEVICE_INFO                                                   0x0000
 #define CMD_SETUP_WRITE                                                   0x0001
@@ -224,6 +237,7 @@ typedef struct{
 #define CMD_HF_SNIFFER                                                    0x0800
 #define CMD_HF_PLOT                                                       0x0801
 
+#define CMD_VARIABLE_SIZE_FLAG                                            0x8000
 #define CMD_UNKNOWN                                                       0xFFFF
 
 

uart/uart.h

@@ -29,13 +29,10 @@
  * @file uart.h
  */
 
-#ifndef _PM3_UART_H_
+#ifndef PM3_UART_H__
-#define _PM3_UART_H_
+#define PM3_UART_H__
 
-#include <stdio.h>
-#include <string.h>
 #include <stdlib.h>
-
 #include <stdint.h>
 #include <stdbool.h>
 
@@ -69,11 +66,11 @@ typedef void* serial_port;
  *
  * On errors, this method returns INVALID_SERIAL_PORT or CLAIMED_SERIAL_PORT.
  */
-serial_port uart_open(const char* pcPortName);
+extern serial_port uart_open(const char* pcPortName);
 
 /* Closes the given port.
  */
-void uart_close(const serial_port sp);
+extern void uart_close(const serial_port sp);
 
 /* Reads from the given serial port for up to 30ms.
  *   pbtRx: A pointer to a buffer for the returned data to be written to.
@@ -86,13 +83,15 @@ void uart_close(const serial_port sp);
  * partial read may have completed into the buffer by the corresponding
  * implementation, so pszRxLen should be checked to see if any data was written.
  */
-bool uart_receive(const serial_port sp, uint8_t* pbtRx, size_t pszMaxRxLen, size_t* pszRxLen);
+extern bool uart_receive(const serial_port sp, uint8_t* pbtRx, size_t pszMaxRxLen, size_t* pszRxLen);
 
 /* Sends a buffer to a given serial port.
  *   pbtTx: A pointer to a buffer containing the data to send.
  *   szTxLen: The amount of data to be sent.
+ *
+ * Returns TRUE if all data could be sent within 30ms.
  */
-bool uart_send(const serial_port sp, const uint8_t* pbtTx, const size_t szTxLen);
+extern bool uart_send(const serial_port sp, const uint8_t* pbtTx, const size_t szTxLen);
 
 /* Sets the current speed of the serial port, in baud.
  */
@@ -100,7 +99,7 @@ bool uart_set_speed(serial_port sp, const uint32_t uiPortSpeed);
 
 /* Gets the current speed of the serial port, in baud.
  */
-uint32_t uart_get_speed(const serial_port sp);
+extern uint32_t uart_get_speed(const serial_port sp);
 
-#endif // _PM3_UART_H_
+#endif // PM3_UART_H__
 

uart/uart_posix.c

@@ -39,21 +39,21 @@
 
 #include "uart.h"
 
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
 #include <termios.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
-#include <limits.h>
+#include <sys/socket.h>
+#include <netinet/tcp.h>
+#include <netdb.h>
 #include <sys/time.h>
 #include <errno.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-#include <netinet/tcp.h>
-#include <arpa/inet.h>
-#include <netdb.h>
 
 // Fix missing definition on OS X.
 // Taken from https://github.com/unbit/uwsgi/commit/b608eb1772641d525bfde268fe9d6d8d0d5efde7
@@ -69,13 +69,30 @@ typedef struct {
 } serial_port_unix;
 
 // Set time-out on 30 miliseconds
-struct timeval timeout = {
+static struct timeval timeout = {
 	.tv_sec  =     0, // 0 second
 	.tv_usec = 30000  // 30000 micro seconds
 };
 
-serial_port uart_open(const char* pcPortName)
+
-{
+void uart_close(const serial_port sp) {
+	serial_port_unix* spu = (serial_port_unix*)sp;
+	tcflush(spu->fd, TCIOFLUSH);
+	tcsetattr(spu->fd, TCSANOW, &(spu->tiOld));
+	struct flock fl;
+	fl.l_type   = F_UNLCK;
+	fl.l_whence = SEEK_SET;
+	fl.l_start  = 0;
+	fl.l_len    = 0;
+	fl.l_pid    = getpid();
+	fcntl(spu->fd, F_SETLK, &fl);
+	close(spu->fd);
+	free(sp);
+}
+
+
+serial_port uart_open(const char* pcPortName) {
+
 	serial_port_unix* sp = malloc(sizeof(serial_port_unix));
 	if (sp == 0) return INVALID_SERIAL_PORT;
 
@@ -89,14 +106,15 @@ serial_port uart_open(const char* pcPortName)
 		char *colon = strrchr(addrstr, ':');
 		char *portstr;
 
-    // Set time-out to 300 miliseconds only for TCP port
+		// Set time-out to 300 milliseconds only for TCP port
 		timeout.tv_usec = 300000;
 
 		if (colon) {
 			portstr = colon + 1;
 			*colon = '\0';
-    } else
+		} else {
 			portstr = "7901";
+		}
 
 		struct addrinfo info;
 
@@ -112,14 +130,11 @@ serial_port uart_open(const char* pcPortName)
 
 		int sfd;
 		for (rp = addr; rp != NULL; rp = rp->ai_next) {
-      sfd = socket(rp->ai_family, rp->ai_socktype,
+			sfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
-		   rp->ai_protocol);
 			if (sfd == -1)
 				continue;
-
 			if (connect(sfd, rp->ai_addr, rp->ai_addrlen) != -1)
 				break;
-
 			close(sfd);
 		}
 
@@ -170,15 +185,13 @@ serial_port uart_open(const char* pcPortName)
 	sp->tiNew = sp->tiOld;
 
 	// Configure the serial port
-  sp->tiNew.c_cflag = CS8 | CLOCAL | CREAD;
+	sp->tiNew.c_cflag &= ~(CSIZE | PARENB);
-  sp->tiNew.c_iflag = IGNPAR;
+	sp->tiNew.c_cflag |= (CS8 | CLOCAL | CREAD);
-  sp->tiNew.c_oflag = 0;
+	sp->tiNew.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
-  sp->tiNew.c_lflag = 0;
+	sp->tiNew.c_iflag |= IGNPAR;
+	sp->tiNew.c_oflag &= ~OPOST;
+	sp->tiNew.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
 
-  // Block until n bytes are received
-  sp->tiNew.c_cc[VMIN] = 0;
-  // Block until a timer expires (n * 100 mSec.)
-  sp->tiNew.c_cc[VTIME] = 0;
 
 	// Try to set the new terminal info struct
 	if (tcsetattr(sp->fd, TCSANOW, &sp->tiNew) == -1) {
@@ -192,114 +205,81 @@ serial_port uart_open(const char* pcPortName)
 	return sp;
 }
 
-void uart_close(const serial_port sp) {
-  serial_port_unix* spu = (serial_port_unix*)sp;
-  tcflush(spu->fd,TCIOFLUSH);
-  tcsetattr(spu->fd,TCSANOW,&(spu->tiOld));
-  struct flock fl;
-  fl.l_type   = F_UNLCK;
-  fl.l_whence = SEEK_SET;
-  fl.l_start  = 0;
-  fl.l_len    = 0;
-  fl.l_pid    = getpid();
-  fcntl(spu->fd, F_SETLK, &fl);
-  close(spu->fd);
-  free(sp);
-}
 
-bool uart_receive(const serial_port sp, uint8_t* pbtRx, size_t pszMaxRxLen, size_t* pszRxLen) {
+bool uart_receive(const serial_port sp, uint8_t* pbtRx, size_t szMaxRxLen, size_t* pszRxLen) {
-  int res;
-  int byteCount;
-  fd_set rfds;
-  struct timeval tv;
 
-  // Reset the output count
 	*pszRxLen = 0;
 
-  do {
+	if (szMaxRxLen == 0) return true;
-    // Reset file descriptor
+
+	struct timeval t_current;
+	gettimeofday(&t_current, NULL);
+	struct timeval t_end;
+	timeradd(&t_current, &timeout, &t_end);
+
+	while (true) {
+		int res = read(((serial_port_unix*)sp)->fd, pbtRx, szMaxRxLen - *pszRxLen);
+		if (res < 0 && errno != EAGAIN && errno != EWOULDBLOCK) return false;
+		if (res > 0) {
+			*pszRxLen += res;
+			pbtRx += res;
+		}
+		if (*pszRxLen == szMaxRxLen) return true; // we could read all requested bytes in time
+		gettimeofday(&t_current, NULL);
+		if (timercmp(&t_current, &t_end, >)) return true; // timeout
+		// set next select timeout
+		struct timeval t_remains;
+		timersub(&t_end, &t_current, &t_remains);
+		// Set the file descriptor set
+		fd_set rfds;
 		FD_ZERO(&rfds);
 		FD_SET(((serial_port_unix*)sp)->fd, &rfds);
-    tv = timeout;
+		// wait for more bytes available
-    res = select(((serial_port_unix*)sp)->fd+1, &rfds, NULL, NULL, &tv);
+		res = select(((serial_port_unix*)sp)->fd+1, &rfds, NULL, NULL, &t_remains);
-    
-    // Read error
-    if (res < 0) {
-      return false;
-    }
- 
-    // Read time-out
-    if (res == 0) {
-      if (*pszRxLen == 0) {
-        // Error, we received no data
-        return false;
-      } else {
-        // We received some data, but nothing more is available
-        return true;
-      }
-    }
- 
-    // Retrieve the count of the incoming bytes
-    res = ioctl(((serial_port_unix*)sp)->fd, FIONREAD, &byteCount);
 		if (res < 0) return false;
-    
+		if (res == 0) return true; // timeout
-    // Cap the number of bytes, so we don't overrun the buffer
+	}
-    if (pszMaxRxLen - (*pszRxLen) < byteCount) {
+	return true; // should never come here
-    	byteCount = pszMaxRxLen - (*pszRxLen);
 }
 
-    // There is something available, read the data
-    res = read(((serial_port_unix*)sp)->fd, pbtRx+(*pszRxLen), byteCount);
-
-    // Stop if the OS has some troubles reading the data
-    if (res <= 0) return false;
- 
-    *pszRxLen += res;
-
-    if (*pszRxLen == pszMaxRxLen) {
-    	// We have all the data we wanted.
-      return true;
-    }
-    
-  } while (byteCount);
-
-  return true;
-}
 
 bool uart_send(const serial_port sp, const uint8_t* pbtTx, const size_t szTxLen) {
-  int32_t res;
-  size_t szPos = 0;
-  fd_set rfds;
-  struct timeval tv;
 
-  while (szPos < szTxLen) {
+	if (szTxLen == 0) return true;
-    // Reset file descriptor
-    FD_ZERO(&rfds);
-    FD_SET(((serial_port_unix*)sp)->fd,&rfds);
-    tv = timeout;
-    res = select(((serial_port_unix*)sp)->fd+1, NULL, &rfds, NULL, &tv);
 
-    // Write error
+	size_t bytes_written = 0;
-    if (res < 0) {
+
-      return false;
+	struct timeval t_current;
+	gettimeofday(&t_current, NULL);
+	struct timeval t_end;
+	timeradd(&t_current, &timeout, &t_end);
+
+	while (true) {
+		int res = write(((serial_port_unix*)sp)->fd, pbtTx, szTxLen - bytes_written);
+		if (res < 0 && res != EAGAIN && res != EWOULDBLOCK) return false;
+		if (res > 0) {
+			pbtTx += res;
+			bytes_written += res;
 		}
-    
+		if (bytes_written == szTxLen) return true; // we could write all bytes
-    // Write time-out
+		gettimeofday(&t_current, NULL);
-    if (res == 0) {
+		if (timercmp(&t_current, &t_end, >)) return false; // timeout
-      return false;
+		// set next select timeout
-    }
+		struct timeval t_remains;
-    
+		timersub(&t_end, &t_current, &t_remains);
-    // Send away the bytes
+		// Set the file descriptor set
-    res = write(((serial_port_unix*)sp)->fd,pbtTx+szPos,szTxLen-szPos);
+		fd_set wfds;
-    
+		FD_ZERO(&wfds);
-    // Stop if the OS has some troubles sending the data
+		FD_SET(((serial_port_unix*)sp)->fd, &wfds);
-    if (res <= 0) return false;
+		// wait until more bytes can be written
-    
+		res = select(((serial_port_unix*)sp)->fd+1, NULL, &wfds, NULL, &t_remains);
-    szPos += res;
+		if (res < 0) return false;  // error
+		if (res == 0) return false; // timeout
 	}
 	return true;
 }
 
+
 bool uart_set_speed(serial_port sp, const uint32_t uiPortSpeed) {
 	const serial_port_unix* spu = (serial_port_unix*)sp;
 	speed_t stPortSpeed;
@@ -388,4 +368,3 @@ uint32_t uart_get_speed(const serial_port sp) {
 }
 
 #endif
-

uart/uart_win32.c

@@ -38,6 +38,11 @@
 
 #include "uart.h"
 
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+
 // The windows serial port implementation
 #ifdef _WIN32
 #include <windows.h>
@@ -48,6 +53,7 @@ typedef struct {
 	COMMTIMEOUTS ct;  // Serial port time-out configuration
 } serial_port_windows;
 
+
 void upcase(char *p) {
 	while(*p != '\0') {
 		if(*p >= 97 && *p <= 122) {
@@ -57,6 +63,13 @@ void upcase(char *p) {
 	}
 }
 
+
+void uart_close(const serial_port sp) {
+	CloseHandle(((serial_port_windows*)sp)->hPort);
+	free(sp);
+}
+
+
 serial_port uart_open(const char* pcPortName) {
 	char acPortName[255];
 	serial_port_windows* sp = malloc(sizeof(serial_port_windows));
@@ -102,20 +115,18 @@ serial_port uart_open(const char* pcPortName) {
 	return sp;
 }
 
-void uart_close(const serial_port sp) {
-  CloseHandle(((serial_port_windows*)sp)->hPort);
-  free(sp);
-}
 
 bool uart_receive(const serial_port sp, uint8_t *pbtRx, size_t pszMaxRxLen, size_t *pszRxLen) {
 	return ReadFile(((serial_port_windows*)sp)->hPort, pbtRx, pszMaxRxLen, (LPDWORD)pszRxLen, NULL);
 }
 
+
 bool uart_send(const serial_port sp, const uint8_t* pbtTx, const size_t szTxLen) {
 	DWORD dwTxLen = 0;
 	return WriteFile(((serial_port_windows*)sp)->hPort, pbtTx, szTxLen, &dwTxLen, NULL);
 }
 
+
 bool uart_set_speed(serial_port sp, const uint32_t uiPortSpeed) {
 	serial_port_windows* spw;
 	spw = (serial_port_windows*)sp;
@@ -123,6 +134,7 @@ bool uart_set_speed(serial_port sp, const uint32_t uiPortSpeed) {
 	return SetCommState(spw->hPort, &spw->dcb);
 }
 
+
 uint32_t uart_get_speed(const serial_port sp) {
 	const serial_port_windows* spw = (serial_port_windows*)sp;
 	if (!GetCommState(spw->hPort, (serial_port)&spw->dcb)) {