From 2faa1290c0634392dfdace39921b10fb47a47b68 Mon Sep 17 00:00:00 2001
From: Mounir IDRASSI <mounir.idrassi@idrix.fr>
Date: Fri, 17 Jun 2016 23:50:44 +0200
Subject: Windows Driver: save FPU state in 32-bit mode before run Whirlpool
 SSE implementation to avoid issues
 (https://msdn.microsoft.com/fr-fr/library/ff565388(v=vs.85).aspx)

---
 src/Common/Pkcs5.c       | 22 ++++++++++++++++++++++
 src/Driver/DriveFilter.c | 12 ++++++++++++
 2 files changed, 34 insertions(+)

diff --git a/src/Common/Pkcs5.c b/src/Common/Pkcs5.c
index 8bc828ef..31716d03 100644
--- a/src/Common/Pkcs5.c
+++ b/src/Common/Pkcs5.c
@@ -19,6 +19,7 @@
 #ifndef TC_WINDOWS_BOOT
 #include "Sha2.h"
 #include "Whirlpool.h"
+#include "cpu.h"
 #include "misc.h"
 #else
 #pragma optimize ("t", on)
@@ -744,6 +745,12 @@ void hmac_whirlpool
 	char* buf = hmac.k;
 	int b;
 	char key[WHIRLPOOL_DIGESTSIZE];
+#if defined (DEVICE_DRIVER) && !defined (_WIN64)
+	KFLOATING_SAVE floatingPointState;
+	NTSTATUS saveStatus = STATUS_SUCCESS;
+	if (HasISSE())
+		saveStatus = KeSaveFloatingPointState (&floatingPointState);
+#endif
     /* If the key is longer than the hash algorithm block size,
 	   let key = whirlpool(key), as per HMAC specifications. */
 	if (lk > WHIRLPOOL_BLOCKSIZE)
@@ -784,6 +791,11 @@ void hmac_whirlpool
 	WHIRLPOOL_add ((unsigned char *) buf, WHIRLPOOL_BLOCKSIZE * 8, ctx);
 
 	hmac_whirlpool_internal(k, lk, d, ld, &hmac);
+
+#if defined (DEVICE_DRIVER) && !defined (_WIN64)
+	if (NT_SUCCESS (saveStatus) && HasISSE())
+		KeRestoreFloatingPointState (&floatingPointState);
+#endif
 	/* Prevent leaks */
 	burn(&hmac, sizeof(hmac));
 }
@@ -821,6 +833,12 @@ void derive_key_whirlpool (char *pwd, int pwd_len, char *salt, int salt_len, uin
 	char* buf = hmac.k;
 	char key[WHIRLPOOL_DIGESTSIZE];
 	int b, l, r;
+#if defined (DEVICE_DRIVER) && !defined (_WIN64)
+	KFLOATING_SAVE floatingPointState;
+	NTSTATUS saveStatus = STATUS_SUCCESS;
+	if (HasISSE())
+		saveStatus = KeSaveFloatingPointState (&floatingPointState);
+#endif
     /* If the password is longer than the hash algorithm block size,
 	   let pwd = whirlpool(pwd), as per HMAC specifications. */
 	if (pwd_len > WHIRLPOOL_BLOCKSIZE)
@@ -883,6 +901,10 @@ void derive_key_whirlpool (char *pwd, int pwd_len, char *salt, int salt_len, uin
 	derive_u_whirlpool (pwd, pwd_len, salt, salt_len, iterations, b, &hmac);
 	memcpy (dk, hmac.u, r);
 
+#if defined (DEVICE_DRIVER) && !defined (_WIN64)
+	if (NT_SUCCESS (saveStatus) && HasISSE())
+		KeRestoreFloatingPointState (&floatingPointState);
+#endif
 
 	/* Prevent possible leaks. */
 	burn (&hmac, sizeof(hmac));
diff --git a/src/Driver/DriveFilter.c b/src/Driver/DriveFilter.c
index 49563592..78cdf254 100644
--- a/src/Driver/DriveFilter.c
+++ b/src/Driver/DriveFilter.c
@@ -27,6 +27,7 @@
 #include "Wipe.h"
 #include "DriveFilter.h"
 #include "Boot/Windows/BootCommon.h"
+#include "cpu.h"
 
 static BOOL DeviceFilterActive = FALSE;
 
@@ -258,6 +259,12 @@ static void ComputeBootLoaderFingerprint(PDEVICE_OBJECT LowerDeviceObject, byte*
 	status = TCReadDevice (LowerDeviceObject, ioBuffer, offset, TC_SECTOR_SIZE_BIOS);
 	if (NT_SUCCESS (status))
 	{
+#if !defined (_WIN64)
+		KFLOATING_SAVE floatingPointState;
+		NTSTATUS saveStatus = STATUS_SUCCESS;
+		if (HasISSE())
+			saveStatus = KeSaveFloatingPointState (&floatingPointState);
+#endif
 		WHIRLPOOL_add (ioBuffer, TC_BOOT_SECTOR_PIM_VALUE_OFFSET * 8, &whirlpool);
 		WHIRLPOOL_add (ioBuffer + TC_BOOT_SECTOR_USER_MESSAGE_OFFSET + TC_BOOT_SECTOR_USER_MESSAGE_MAX_LENGTH, (TC_BOOT_SECTOR_USER_CONFIG_OFFSET - (TC_BOOT_SECTOR_USER_MESSAGE_OFFSET + TC_BOOT_SECTOR_USER_MESSAGE_MAX_LENGTH)) * 8, &whirlpool);
 		WHIRLPOOL_add (ioBuffer + TC_BOOT_SECTOR_USER_CONFIG_OFFSET + 1, (TC_MAX_MBR_BOOT_CODE_SIZE - (TC_BOOT_SECTOR_USER_CONFIG_OFFSET + 1)) * 8, &whirlpool);
@@ -293,6 +300,11 @@ static void ComputeBootLoaderFingerprint(PDEVICE_OBJECT LowerDeviceObject, byte*
 			WHIRLPOOL_finalize (&whirlpool, BootLoaderFingerprint);
 			sha512_end (&BootLoaderFingerprint [WHIRLPOOL_DIGESTSIZE], &sha2);
 		}
+
+#if !defined (_WIN64)
+		if (NT_SUCCESS (saveStatus) && HasISSE())
+			KeRestoreFloatingPointState (&floatingPointState);
+#endif
 	}
 	else
 	{
-- 
cgit v1.2.3