From b5bd47652d1801727d49e629ac4a05a7617722bd Mon Sep 17 00:00:00 2001
From: Conor <conorpp@vt.edu>
Date: Tue, 16 Aug 2016 22:12:04 -0400
Subject: [PATCH] passing fido HID test program

---
 firmware/.cproject        |   8 +-
 firmware/inc/InitDevice.h |   1 +
 firmware/inc/app.h        |   2 +-
 firmware/inc/u2f_hid.h    |   1 +
 firmware/src/main.c       |  58 +++++++--------
 firmware/src/u2f_hid.c    | 150 +++++++++++++++++++++++++++-----------
 6 files changed, 142 insertions(+), 78 deletions(-)

diff --git a/firmware/.cproject b/firmware/.cproject
index 9a3c19d..5b9d57b 100644
--- a/firmware/.cproject
+++ b/firmware/.cproject
@@ -22,7 +22,7 @@
 					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 				</extensions>
 			</storageModule>
-			<storageModule buildConfig.stockConfigId="com.silabs.ide.si8051.keil.release#com.silabs.ide.si8051.keil:9.53.0" cppBuildConfig.builtinIncludes="studio:/sdk/Device/shared/si8051Base/ studio:/sdk/Device/EFM8UB1/ studio:/sdk/Device/EFM8UB1/inc/ studio:/sdk/Lib/efm8_usb/inc/ studio:/sdk/Device/EFM8UB1/peripheral_driver/inc/ studio:/sdk/Lib/efm8_assert/ studio:/sdk/Device/shared/si8051Base/ studio:/sdk/Device/EFM8UB1/ studio:/sdk/Device/EFM8UB1/inc/ studio:/sdk/Lib/efm8_usb/inc/ studio:/sdk/Device/EFM8UB1/peripheral_driver/inc/ studio:/sdk/Lib/efm8_assert/" cppBuildConfig.builtinLibraryFiles="" cppBuildConfig.builtinLibraryNames="" cppBuildConfig.builtinLibraryObjects="" cppBuildConfig.builtinLibraryPaths="" cppBuildConfig.builtinMacros="" moduleId="com.silabs.ide.project.core" projectCommon.referencedModules="[{&quot;module&quot;:&quot;&lt;project:MModule xmlns:project=\&quot;http://www.silabs.com/sls/Project.ecore\&quot; builtin=\&quot;true\&quot; id=\&quot;com.silabs.sdk.si8051.external.efm8Library.usb\&quot;/&gt;&quot;,&quot;builtinExcludes&quot;:[],&quot;builtin&quot;:true,&quot;builtinSources&quot;:[&quot;lib/efm8_usb/inc/efm8_usb.h&quot;,&quot;lib/efm8_usb/Readme.txt&quot;,&quot;lib/efm8_usb/src/efm8_usbd.c&quot;,&quot;lib/efm8_usb/src/efm8_usbdch9.c&quot;,&quot;lib/efm8_usb/src/efm8_usbdep.c&quot;,&quot;lib/efm8_usb/src/efm8_usbdint.c&quot;]},{&quot;module&quot;:&quot;&lt;project:MModule xmlns:project=\&quot;http://www.silabs.com/sls/Project.ecore\&quot; builtin=\&quot;true\&quot; id=\&quot;com.silabs.sdk.si8051.external.efm8Library.assert\&quot;/&gt;&quot;,&quot;builtinExcludes&quot;:[],&quot;builtin&quot;:true,&quot;builtinSources&quot;:[&quot;lib/efm8_assert/assert.c&quot;,&quot;lib/efm8_assert/assert.h&quot;]},{&quot;module&quot;:&quot;&lt;project:MModule xmlns:project=\&quot;http://www.silabs.com/sls/Project.ecore\&quot; builtin=\&quot;true\&quot; id=\&quot;com.silabs.sdk.si8051.external.efm8PeripheralDriver.usb0\&quot;/&gt;&quot;,&quot;builtinExcludes&quot;:[],&quot;builtin&quot;:true,&quot;builtinSources&quot;:[&quot;lib/efm8ub1/peripheralDrivers/src/usb_0.c&quot;,&quot;lib/efm8ub1/peripheralDrivers/inc/usb_0.h&quot;]}]" projectCommon.toolchainId="com.silabs.ide.si8051.keil:9.53.0"/>
+			<storageModule buildConfig.stockConfigId="com.silabs.ide.si8051.keil.release#com.silabs.ide.si8051.keil:9.53.0" cppBuildConfig.builtinIncludes="studio:/sdk/Device/shared/si8051Base/ studio:/sdk/Device/EFM8UB1/ studio:/sdk/Device/EFM8UB1/inc/ studio:/sdk/Lib/efm8_usb/inc/ studio:/sdk/Device/EFM8UB1/peripheral_driver/inc/ studio:/sdk/Lib/efm8_assert/ studio:/sdk/Device/shared/si8051Base/ studio:/sdk/Device/EFM8UB1/ studio:/sdk/Device/EFM8UB1/inc/ studio:/sdk/Lib/efm8_usb/inc/ studio:/sdk/Device/EFM8UB1/peripheral_driver/inc/ studio:/sdk/Lib/efm8_assert/" cppBuildConfig.builtinLibraryFiles="" cppBuildConfig.builtinLibraryNames="" cppBuildConfig.builtinLibraryObjects="" cppBuildConfig.builtinLibraryPaths="" cppBuildConfig.builtinMacros="" moduleId="com.silabs.ide.project.core" projectCommon.referencedModules="[{&quot;module&quot;:&quot;&lt;project:MModule xmlns:project=\&quot;http://www.silabs.com/sls/Project.ecore\&quot; builtin=\&quot;true\&quot; id=\&quot;com.silabs.sdk.si8051.external.efm8Library.usb\&quot;/&gt;&quot;,&quot;builtinExcludes&quot;:[],&quot;builtin&quot;:true,&quot;builtinSources&quot;:[&quot;lib/efm8_usb/src/efm8_usbdep.c&quot;,&quot;lib/efm8_usb/src/efm8_usbdch9.c&quot;,&quot;lib/efm8_usb/src/efm8_usbd.c&quot;,&quot;lib/efm8_usb/src/efm8_usbdint.c&quot;,&quot;lib/efm8_usb/inc/efm8_usb.h&quot;]},{&quot;module&quot;:&quot;&lt;project:MModule xmlns:project=\&quot;http://www.silabs.com/sls/Project.ecore\&quot; builtin=\&quot;true\&quot; id=\&quot;com.silabs.sdk.si8051.external.efm8Library.assert\&quot;/&gt;&quot;,&quot;builtinExcludes&quot;:[],&quot;builtin&quot;:true,&quot;builtinSources&quot;:[&quot;lib/efm8_assert/assert.h&quot;,&quot;lib/efm8_assert/assert.c&quot;]},{&quot;module&quot;:&quot;&lt;project:MModule xmlns:project=\&quot;http://www.silabs.com/sls/Project.ecore\&quot; builtin=\&quot;true\&quot; id=\&quot;com.silabs.sdk.si8051.external.efm8PeripheralDriver.usb0\&quot;/&gt;&quot;,&quot;builtinExcludes&quot;:[],&quot;builtin&quot;:true,&quot;builtinSources&quot;:[&quot;lib/efm8ub1/peripheralDrivers/src/usb_0.c&quot;,&quot;lib/efm8ub1/peripheralDrivers/inc/usb_0.h&quot;]}]" projectCommon.toolchainId="com.silabs.ide.si8051.keil:9.53.0"/>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
 				<configuration artifactExtension="omf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" errorParsers="org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.CWDLocator;com.silabs.ide.si8051.keil.KeilErrorParser;org.eclipse.cdt.core.GCCErrorParser" id="com.silabs.ide.si8051.keil.release#com.silabs.ide.si8051.keil:9.53.0" name="release" parent="com.silabs.ide.si8051.keil.exe.default">
 					<folderInfo id="com.silabs.ide.si8051.keil.release#com.silabs.ide.si8051.keil:9.53.0." name="/" resourcePath="">
@@ -40,8 +40,8 @@
 								<option id="com.silabs.ide.si8051.keil.compiler.category.optimization.coloring.1569030790" name="Global register coloring" superClass="com.silabs.ide.si8051.keil.compiler.category.optimization.coloring" value="true" valueType="boolean"/>
 								<option id="com.silabs.ide.si8051.keil.compiler.category.includes.paths.1968864987" name="Include paths (INCDIR(...))" superClass="com.silabs.ide.si8051.keil.compiler.category.includes.paths" valueType="includePath">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/inc/config}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/inc&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/tests&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/inc&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${StudioSdkPath}/Device/shared/si8051Base&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${StudioSdkPath}/Device/EFM8UB1&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${StudioSdkPath}/Device/EFM8UB1/inc&quot;"/>
@@ -59,8 +59,8 @@
 								</option>
 								<option id="com.silabs.ide.si8051.keil.assembler.category.includes.paths.158046198" name="Include paths (INCDIR(...))" superClass="com.silabs.ide.si8051.keil.assembler.category.includes.paths" valueType="includePath">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/inc/config}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/inc&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/tests&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/inc&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${ProjDirPath}/tinyprintf&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${StudioSdkPath}/Device/shared/si8051Base&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${StudioSdkPath}/Device/EFM8UB1&quot;"/>
@@ -87,7 +87,7 @@
 			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
 		</cconfiguration>
 	</storageModule>
-	<storageModule moduleId="com.silabs.ide.project.core" project.generation="6" projectCommon.buildArtifactType="EXE" projectCommon.kitId="com.silabs.kit.si8051.efm8ub1_slstk2000a" projectCommon.partId="com.silabs.mcu.si8051.efm8ub1_g.efm8ub10f16g" projectCommon.sdkId="com.silabs.sdk.si8051.sls:3.0.0"/>
+	<storageModule moduleId="com.silabs.ide.project.core" project.generation="19" projectCommon.buildArtifactType="EXE" projectCommon.kitId="com.silabs.kit.si8051.efm8ub1_slstk2000a" projectCommon.partId="com.silabs.mcu.si8051.efm8ub1_g.efm8ub10f16g" projectCommon.sdkId="com.silabs.sdk.si8051.sls:3.0.0"/>
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
 		<project id="u2f-firmware.com.silabs.ide.project.core.cdt.cdtMbsProjectType.801504117" name="SLS CDT Project" projectType="com.silabs.ide.project.core.cdt.cdtMbsProjectType"/>
 	</storageModule>
diff --git a/firmware/inc/InitDevice.h b/firmware/inc/InitDevice.h
index 356bc5b..2067984 100644
--- a/firmware/inc/InitDevice.h
+++ b/firmware/inc/InitDevice.h
@@ -21,6 +21,7 @@ extern void VREG_0_enter_DefaultMode_from_RESET(void);
 extern void PORTS_0_enter_DefaultMode_from_RESET(void);
 extern void PORTS_1_enter_DefaultMode_from_RESET(void);
 extern void PBCFG_0_enter_DefaultMode_from_RESET(void);
+extern void HFOSC_0_enter_DefaultMode_from_RESET(void);
 extern void CIP51_0_enter_DefaultMode_from_RESET(void);
 extern void CLOCK_0_enter_DefaultMode_from_RESET(void);
 extern void TIMER01_0_enter_DefaultMode_from_RESET(void);
diff --git a/firmware/inc/app.h b/firmware/inc/app.h
index 0c8dbcd..91f30c8 100644
--- a/firmware/inc/app.h
+++ b/firmware/inc/app.h
@@ -39,7 +39,7 @@
 // application settings
 #define U2F_ATTESTATION_KEY_SLOT	15
 //#define ATECC_SETUP_DEVICE
-//#define U2F_PRINT
+#define U2F_PRINT
 //#define U2F_BLINK_ERRORS
 
 // efm8ub1 application eeprom memory mappings
diff --git a/firmware/inc/u2f_hid.h b/firmware/inc/u2f_hid.h
index db9e14d..931e925 100644
--- a/firmware/inc/u2f_hid.h
+++ b/firmware/inc/u2f_hid.h
@@ -153,6 +153,7 @@ extern void set_app_error(uint8_t ec);
 
 // Call from main loop to ensure stale channels get timeout error.
 void u2f_hid_check_timeouts();
+void u2f_print_hid_check_timeouts();
 
 #define U2FHID_IS_INIT(cmd)			((cmd) & 0x80)
 
diff --git a/firmware/src/main.c b/firmware/src/main.c
index f7742ed..4fd2ebe 100644
--- a/firmware/src/main.c
+++ b/firmware/src/main.c
@@ -230,35 +230,35 @@ int16_t main(void) {
 				if (state == APP_HID_MSG)
 					state = APP_NOTHING;
 				break;
-			case APP_WINK:
-				// Do wink pattern for USB HID wink request
-				rgb_hex(winkc);
-				light = 1;
-				ms_wink = get_ms();
-				state = _APP_WINK;
-				break;
-			case _APP_WINK:
-
-				if (ms_since(ms_wink,150))
-				{
-					if (light)
-					{
-						light = 0;
-						rgb_hex(winkc);
-					}
-					else
-					{
-						light = 1;
-						rgb_hex(0);
-					}
-					winks++;
-				}
-				if (winks == 5)
-				{
-					winks = 0;
-					state = APP_NOTHING;
-				}
-				break;
+//			case APP_WINK:
+//				// Do wink pattern for USB HID wink request
+//				rgb_hex(winkc);
+//				light = 1;
+//				ms_wink = get_ms();
+//				state = _APP_WINK;
+//				break;
+//			case _APP_WINK:
+//
+//				if (ms_since(ms_wink,150))
+//				{
+//					if (light)
+//					{
+//						light = 0;
+//						rgb_hex(winkc);
+//					}
+//					else
+//					{
+//						light = 1;
+//						rgb_hex(0);
+//					}
+//					winks++;
+//				}
+//				if (winks == 5)
+//				{
+//					winks = 0;
+//					state = APP_NOTHING;
+//				}
+//				break;
 		}
 
 		if (error)
diff --git a/firmware/src/u2f_hid.c b/firmware/src/u2f_hid.c
index a49b413..019afa0 100644
--- a/firmware/src/u2f_hid.c
+++ b/firmware/src/u2f_hid.c
@@ -45,7 +45,8 @@
 
 #ifndef U2F_HID_DISABLE
 
-#define CID_MAX (sizeof(CIDS)/sizeof(uint32_t))
+#define CID_MAX (sizeof(CIDS)/sizeof(struct CID))
+#define BROADCAST_CID (CIDS[CID_MAX-1])
 
 
 typedef enum
@@ -57,7 +58,7 @@ typedef enum
 struct CID
 {
 	uint32_t cid;
-	uint16_t last_used;
+	uint32_t last_used;
 	uint8_t busy;
 	uint8_t last_cmd;
 };
@@ -88,6 +89,7 @@ uint32_t _hid_lockt = 0;
 uint32_t _hid_lock_cid = 0;
 
 static struct CID CIDS[5];
+
 static uint8_t CID_NUM = 0;
 
 static uint8_t _hid_pkt[HID_PACKET_SIZE];
@@ -186,10 +188,7 @@ void u2f_hid_writeback(uint8_t * payload, uint16_t len)
 
 }
 
-static uint8_t is_cid_free(struct CID* c)
-{
-	return (c->cid == 0 || get_ms() - c->last_used > 1000);
-}
+
 
 static void refresh_cid(struct CID* c)
 {
@@ -201,9 +200,9 @@ static uint32_t get_new_cid()
 {
 	static uint32_t base = 0xcafebabe;
 	int i;
-	for(i = 0; i < CID_MAX; i++)
+	for(i = 0; i < CID_MAX-1; i++)
 	{
-		if (is_cid_free(CIDS+i))
+		if (!CIDS[i].busy)
 		{
 			goto newcid;
 		}
@@ -224,9 +223,9 @@ static uint32_t get_new_cid()
 static int8_t add_new_cid(uint32_t cid)
 {
 	int i;
-	for(i = 0; i < CID_MAX; i++)
+	for(i = 0; i < CID_MAX-1; i++)
 	{
-		if (is_cid_free(CIDS+i))
+		if (!CIDS[i].busy)
 		{
 			CIDS[i].cid = cid;
 			return 0;
@@ -337,12 +336,17 @@ static uint8_t hid_u2f_parse(struct u2f_hid_msg* req)
 			init_res->version_build = 0;
 			init_res->cflags = CAPABILITY_WINK | CAPABILITY_LOCK;
 
+			u2f_printlx("replied to cid ",1,hid_layer.current_cid);
 			// write back the same data nonce
 			u2f_hid_writeback(req->pkt.init.payload, 8);
 			u2f_hid_writeback((uint8_t *)init_res, 9);
 			u2f_hid_flush();
+
 			hid_layer.current_cid = init_res->cid;
 
+
+			u2f_printlx("init'd to cid ",1,hid_layer.current_cid);
+
 			break;
 		case U2FHID_MSG:
 
@@ -378,7 +382,7 @@ static uint8_t hid_u2f_parse(struct u2f_hid_msg* req)
 			break;
 		case U2FHID_PING:
 
-			hid_layer.current_cid = req->cid;
+			u2f_prints("PING\r\n");
 
 			if (hid_layer.bytes_buffered == 0)
 			{
@@ -387,6 +391,7 @@ static uint8_t hid_u2f_parse(struct u2f_hid_msg* req)
 				if (hid_layer.bytes_buffered >= U2FHID_LEN(req))
 				{
 					u2f_hid_writeback(hid_layer.buffer,hid_layer.bytes_buffered);
+					u2f_printd("PING DONE ",1,hid_layer.bytes_buffered + hid_layer.bytes_written);
 					u2f_hid_flush();
 				}
 				else
@@ -400,12 +405,15 @@ static uint8_t hid_u2f_parse(struct u2f_hid_msg* req)
 				{
 					u2f_hid_writeback(hid_layer.buffer,hid_layer.bytes_buffered);
 					hid_layer.bytes_buffered = 0;
+					u2f_prints("PING WB\r\n");
+
 				}
 
 				buffer_request(req);
-				if (hid_layer.bytes_buffered >= hid_layer.req_len)
+				if (hid_layer.bytes_buffered + hid_layer.bytes_written >= hid_layer.req_len)
 				{
 					u2f_hid_writeback(hid_layer.buffer,hid_layer.bytes_buffered);
+					u2f_printd("PING D0N3 ",1,hid_layer.bytes_buffered + hid_layer.bytes_written);
 					u2f_hid_flush();
 				}
 				else
@@ -414,6 +422,7 @@ static uint8_t hid_u2f_parse(struct u2f_hid_msg* req)
 				}
 			}
 
+
 			break;
 
 		case U2FHID_WINK:
@@ -456,6 +465,7 @@ static uint8_t hid_u2f_parse(struct u2f_hid_msg* req)
 		default:
 			set_app_error(ERROR_HID_INVALID_CMD);
 			stamp_error(hid_layer.current_cid, ERR_INVALID_CMD);
+			u2f_printb("invalid cmd: ",1,hid_layer.current_cmd);
 	}
 
 	return u2f_hid_busy();
@@ -470,81 +480,130 @@ void u2f_hid_check_timeouts()
 	uint8_t i;
 	for(i = 0; i < CID_MAX; i++)
 	{
-		if (CIDS[i].busy && (((uint16_t)get_ms() - (CIDS[i].last_used)) >= 700) && CIDS[i].last_cmd == U2FHID_PING)
+		if (CIDS[i].busy && ((get_ms() - CIDS[i].last_used) >= 500))
 		{
+
+
+			u2f_printlx("timeout cid ",2,CIDS[i].cid,get_ms());
 			stamp_error(CIDS[i].cid, ERR_MSG_TIMEOUT);
-			CIDS[i].busy = 0;
+			del_cid(CIDS[i].cid);
+			u2f_hid_reset_packet();
 		}
 	}
+
 }
 
+
 void u2f_hid_request(struct u2f_hid_msg* req)
 {
 	uint8_t* payload;
 	static int8_t last_seq;
-	struct CID* cid;
+	struct CID* cid = NULL;
 
 	restart:
 
 	payload = req->pkt.init.payload;
-	last_seq = -1;
 	cid = get_cid(req->cid);
 
 	// Error checking
-	if (!(U2FHID_IS_INIT(req->pkt.init.cmd)))
+	if ((U2FHID_IS_INIT(req->pkt.init.cmd)))
 	{
+		if (U2FHID_LEN(req) > 7609)
+		{
+			stamp_error(req->cid, ERR_INVALID_LEN);
+			return;
+		}
+		if (req->pkt.init.cmd != U2FHID_INIT && req->cid != hid_layer.current_cid && u2f_hid_busy())
+		{
+			stamp_error(req->cid, ERR_CHANNEL_BUSY);
+			return;
+		}
+	}
+	else
+	{
+		// do i need this?..
 		if (U2FHID_LEN(req) <= U2FHID_INIT_PAYLOAD_SIZE)
 		{
 			return;
 		}
+
+	}
+	if (!req->cid)
+	{
+		stamp_error(req->cid, ERR_SYNC_FAIL);
+		return;
 	}
 
 
-	if (cid != NULL)
+
+	if (req->cid == U2FHID_BROADCAST)
+	{
+		if (!(req->pkt.init.cmd == U2FHID_INIT))
+		{
+			stamp_error(req->cid, ERR_SYNC_FAIL);
+			return;
+		}
+		cid = &BROADCAST_CID;
+		BROADCAST_CID.cid = U2FHID_BROADCAST;
+	}
+	else if (U2FHID_IS_INIT(req->pkt.init.cmd) && cid == NULL)
+	{
+		add_new_cid(req->cid);
+		cid = get_cid(req->cid);
+		if (cid == NULL)
+		{
+			u2f_prints("out of cids\r\n");
+			return;
+		}
+		cid->busy = 0;
+	}
+	else if (cid == NULL)
+	{
+		// ignore random cont packets
+		u2f_prints("ignoring random cont\r\n");
+		return;
+	}
+
+
+
+	// Reset init packets
+	if (req->pkt.init.cmd == U2FHID_INIT)
 	{
 		cid->busy = 0;
-	}
-	else if (req->cid == U2FHID_BROADCAST)
-	{
-
 	}
 	else
 	{
-		if (U2FHID_IS_INIT(req->pkt.init.cmd))
-		{
-			add_new_cid(req->cid);
-			cid = get_cid(req->cid);
-			cid->busy = 0;
-		}
-		else
-		{
-			// ignore random cont packets
-			return;
-		}
 
 	}
 
+	u2f_printlx("got cid ",1,req->cid);
+
 	hid_layer.current_cid = req->cid;
-	hid_layer.current_cmd = req->pkt.init.cmd;
 	hid_layer.last_buffered = get_ms();
 
-	cid->last_used = (uint16_t)get_ms();
-	cid->last_cmd = req->pkt.init.cmd;
+
+	cid->last_used = get_ms();
+
+
+
 
 	// ignore if we locked to a different cid
 	if(hid_is_locked() && req->pkt.init.cmd != U2FHID_INIT)
 	{
-		if (!hid_is_lock_cid(cid->cid))
+		if (!hid_is_lock_cid(req->cid))
 		{
-			stamp_error(cid->cid, ERR_CHANNEL_BUSY);
+			stamp_error(req->cid, ERR_CHANNEL_BUSY);
 			return;
 		}
 	}
 
-	if (req->pkt.init.cmd & TYPE_INIT)
+	if ((req->pkt.init.cmd & TYPE_INIT) && !cid->busy)
 	{
-		cid->busy = hid_u2f_parse(req);
-		return;
+		u2f_prints("type init\r\n");
+		cid->last_cmd = req->pkt.init.cmd;
+		hid_layer.current_cmd = req->pkt.init.cmd;
+		last_seq = -1;
+
 	}
 	else
 	{
@@ -554,16 +613,19 @@ void u2f_hid_request(struct u2f_hid_msg* req)
 		hid_layer.last_buffered = get_ms();
 		if (last_seq + 1 != req->pkt.cont.seq)
 		{
-			u2f_hid_reset_packet();
 			stamp_error(hid_layer.current_cid, ERR_INVALID_SEQ);
+			u2f_hid_reset_packet();
+			u2f_prints("invalid seq!\r\n");
 			return;
 		}
 		last_seq = req->pkt.cont.seq;
 
-		cid->busy = hid_u2f_parse(req);
-		return;
+		hid_layer.current_cmd = cid->last_cmd;
+
 	}
 
+	cid->busy = hid_u2f_parse(req);
+
 
 
 //	hid_layer.state = (u2f_hid_busy()) ? HID_BUSY : HID_READY;