diff --git a/OvmfPkg/XenBusDxe/GrantTable.c b/OvmfPkg/XenBusDxe/GrantTable.c
index 19117fbe0373..6e47483f136f 100644
--- a/OvmfPkg/XenBusDxe/GrantTable.c
+++ b/OvmfPkg/XenBusDxe/GrantTable.c
@@ -35,9 +35,9 @@
 #include <IndustryStandard/Xen/memory.h>
 
 #include <Library/XenHypercallLib.h>
+#include <Library/SynchronizationLib.h>
 
 #include "GrantTable.h"
-#include "InterlockedCompareExchange16.h"
 
 #define NR_RESERVED_ENTRIES 8
 
diff --git a/OvmfPkg/XenBusDxe/Ia32/InterlockedCompareExchange16.nasm b/OvmfPkg/XenBusDxe/Ia32/InterlockedCompareExchange16.nasm
deleted file mode 100644
index d45582dd8109..000000000000
--- a/OvmfPkg/XenBusDxe/Ia32/InterlockedCompareExchange16.nasm
+++ /dev/null
@@ -1,42 +0,0 @@
-;------------------------------------------------------------------------------
-;
-; Copyright (c) 2006, Intel Corporation. All rights reserved.<BR>
-; This program and the accompanying materials
-; are licensed and made available under the terms and conditions of the BSD License
-; which accompanies this distribution.  The full text of the license may be found at
-; http://opensource.org/licenses/bsd-license.php.
-;
-; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
-; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
-;
-; Module Name:
-;
-;   InterlockedCompareExchange16.Asm
-;
-; Abstract:
-;
-;   InterlockedCompareExchange16 function
-;
-; Notes:
-;
-;------------------------------------------------------------------------------
-
-    SECTION .text
-
-;------------------------------------------------------------------------------
-; UINT32
-; EFIAPI
-; InternalSyncCompareExchange16 (
-;   IN      UINT16                    *Value,
-;   IN      UINT16                    CompareValue,
-;   IN      UINT16                    ExchangeValue
-;   );
-;------------------------------------------------------------------------------
-global ASM_PFX(InternalSyncCompareExchange16)
-ASM_PFX(InternalSyncCompareExchange16):
-    mov     ecx, [esp + 4]
-    mov     eax, [esp + 8]
-    mov     edx, [esp + 12]
-    lock    cmpxchg [ecx], dx
-    ret
-
diff --git a/OvmfPkg/XenBusDxe/Ia32/TestAndClearBit.nasm b/OvmfPkg/XenBusDxe/Ia32/TestAndClearBit.nasm
deleted file mode 100644
index d77f74ef249d..000000000000
--- a/OvmfPkg/XenBusDxe/Ia32/TestAndClearBit.nasm
+++ /dev/null
@@ -1,16 +0,0 @@
-SECTION .text
-
-; INT32
-; EFIAPI
-; TestAndClearBit (
-;   IN  INT32 Bit,
-;   IN  volatile VOID* Address
-;   );
-global ASM_PFX(TestAndClearBit)
-ASM_PFX(TestAndClearBit):
-  mov ecx, [esp + 4]
-  mov edx, [esp + 8]
-  lock btr [edx], ecx
-  sbb eax, eax
-  ret
-
diff --git a/OvmfPkg/XenBusDxe/InterlockedCompareExchange16.c b/OvmfPkg/XenBusDxe/InterlockedCompareExchange16.c
deleted file mode 100644
index 2b0fadd88dd1..000000000000
--- a/OvmfPkg/XenBusDxe/InterlockedCompareExchange16.c
+++ /dev/null
@@ -1,33 +0,0 @@
-#include <Library/DebugLib.h>
-#include "InterlockedCompareExchange16.h"
-
-/**
-  Performs an atomic compare exchange operation on a 16-bit unsigned integer.
-
-  Performs an atomic compare exchange operation on the 16-bit unsigned integer
-  specified by Value.  If Value is equal to CompareValue, then Value is set to
-  ExchangeValue and CompareValue is returned.  If Value is not equal to CompareValue,
-  then Value is returned.  The compare exchange operation must be performed using
-  MP safe mechanisms.
-
-  If Value is NULL, then ASSERT().
-
-  @param  Value         A pointer to the 16-bit value for the compare exchange
-                        operation.
-  @param  CompareValue  16-bit value used in compare operation.
-  @param  ExchangeValue 16-bit value used in exchange operation.
-
-  @return The original *Value before exchange.
-
-**/
-UINT16
-EFIAPI
-InterlockedCompareExchange16 (
-  IN OUT  UINT16                    *Value,
-  IN      UINT16                    CompareValue,
-  IN      UINT16                    ExchangeValue
-  )
-{
-  ASSERT (Value != NULL);
-  return InternalSyncCompareExchange16 (Value, CompareValue, ExchangeValue);
-}
diff --git a/OvmfPkg/XenBusDxe/InterlockedCompareExchange16.h b/OvmfPkg/XenBusDxe/InterlockedCompareExchange16.h
deleted file mode 100644
index fd3f15b1c8d6..000000000000
--- a/OvmfPkg/XenBusDxe/InterlockedCompareExchange16.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/**
-  Assembly implementation of InterlockedCompareExchange16.
-
-  Look at the documentation of InterlockedCompareExchange16.
-**/
-UINT16
-EFIAPI
-InternalSyncCompareExchange16 (
-  IN      volatile UINT16           *Value,
-  IN      UINT16                    CompareValue,
-  IN      UINT16                    ExchangeValue
-  );
-
-/**
-  Performs an atomic compare exchange operation on a 16-bit unsigned integer.
-
-  Performs an atomic compare exchange operation on the 16-bit unsigned integer
-  specified by Value.  If Value is equal to CompareValue, then Value is set to
-  ExchangeValue and CompareValue is returned.  If Value is not equal to CompareValue,
-  then Value is returned.  The compare exchange operation must be performed using
-  MP safe mechanisms.
-
-  If Value is NULL, then ASSERT().
-
-  @param  Value         A pointer to the 16-bit value for the compare exchange
-                        operation.
-  @param  CompareValue  16-bit value used in compare operation.
-  @param  ExchangeValue 16-bit value used in exchange operation.
-
-  @return The original *Value before exchange.
-**/
-UINT16
-EFIAPI
-InterlockedCompareExchange16 (
-  IN OUT  UINT16                    *Value,
-  IN      UINT16                    CompareValue,
-  IN      UINT16                    ExchangeValue
-  );
diff --git a/OvmfPkg/XenBusDxe/TestAndClearBit.c b/OvmfPkg/XenBusDxe/TestAndClearBit.c
new file mode 100644
index 000000000000..e971b40a89ce
--- /dev/null
+++ b/OvmfPkg/XenBusDxe/TestAndClearBit.c
@@ -0,0 +1,46 @@
+/** @file
+  Implementation of TestAndClearBit using compare-exchange primitive
+
+  Copyright (C) 2015, Linaro Ltd.
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Base.h>
+#include <Library/SynchronizationLib.h>
+
+INT32
+EFIAPI
+TestAndClearBit (
+  IN INT32            Bit,
+  IN VOID             *Address
+  )
+{
+  UINT16    Word;
+  UINT16    Mask;
+
+  //
+  // Calculate the effective address relative to 'Address' based on the
+  // higher order bits of 'Bit'. Use signed shift instead of division to
+  // ensure we round towards -Inf, and end up with a positive shift in
+  // 'Bit', even if 'Bit' itself is negative.
+  //
+  Address += (Bit >> 4) * sizeof(UINT16);
+
+  Word = *(UINT16 *)Address;
+  Mask = 1U << (Bit & 15);
+
+  while (Word & Mask) {
+    if (Word == InterlockedCompareExchange16 (Address, Word, Word & ~Mask)) {
+      return 1;
+    }
+  }
+  return 0;
+}
diff --git a/OvmfPkg/XenBusDxe/X64/InterlockedCompareExchange16.nasm b/OvmfPkg/XenBusDxe/X64/InterlockedCompareExchange16.nasm
deleted file mode 100644
index 048d1f32f6b9..000000000000
--- a/OvmfPkg/XenBusDxe/X64/InterlockedCompareExchange16.nasm
+++ /dev/null
@@ -1,41 +0,0 @@
-;------------------------------------------------------------------------------
-;
-; Copyright (c) 2006, Intel Corporation. All rights reserved.<BR>
-; This program and the accompanying materials
-; are licensed and made available under the terms and conditions of the BSD License
-; which accompanies this distribution.  The full text of the license may be found at
-; http://opensource.org/licenses/bsd-license.php.
-;
-; THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
-; WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
-;
-; Module Name:
-;
-;   InterlockedCompareExchange16.Asm
-;
-; Abstract:
-;
-;   InterlockedCompareExchange16 function
-;
-; Notes:
-;
-;------------------------------------------------------------------------------
-
-    DEFAULT REL
-    SECTION .text
-
-;------------------------------------------------------------------------------
-; UINT16
-; EFIAPI
-; InterlockedCompareExchange16 (
-;   IN      UINT16                    *Value,
-;   IN      UINT16                    CompareValue,
-;   IN      UINT16                    ExchangeValue
-;   );
-;------------------------------------------------------------------------------
-global ASM_PFX(InternalSyncCompareExchange16)
-ASM_PFX(InternalSyncCompareExchange16):
-    mov     eax, edx
-    lock    cmpxchg [rcx], r8w
-    ret
-
diff --git a/OvmfPkg/XenBusDxe/X64/TestAndClearBit.nasm b/OvmfPkg/XenBusDxe/X64/TestAndClearBit.nasm
deleted file mode 100644
index a4859a62a250..000000000000
--- a/OvmfPkg/XenBusDxe/X64/TestAndClearBit.nasm
+++ /dev/null
@@ -1,15 +0,0 @@
-DEFAULT REL
-SECTION .text
-
-; INT32
-; EFIAPI
-; TestAndClearBit (
-;   IN  INT32 Bit,                // rcx
-;   IN  volatile VOID* Address    // rdx
-;   );
-global ASM_PFX(TestAndClearBit)
-ASM_PFX(TestAndClearBit):
-  lock btr [rdx], ecx
-  sbb eax, eax
-  ret
-
diff --git a/OvmfPkg/XenBusDxe/XenBusDxe.h b/OvmfPkg/XenBusDxe/XenBusDxe.h
index 6c306e017b07..953e4b72e85e 100644
--- a/OvmfPkg/XenBusDxe/XenBusDxe.h
+++ b/OvmfPkg/XenBusDxe/XenBusDxe.h
@@ -122,7 +122,7 @@ INT32
 EFIAPI
 TestAndClearBit (
   IN INT32 Bit,
-  IN volatile VOID *Address
+  IN VOID  *Address
   );
 
 CHAR8*
diff --git a/OvmfPkg/XenBusDxe/XenBusDxe.inf b/OvmfPkg/XenBusDxe/XenBusDxe.inf
index 31553ac5a64a..f0c5db98b1f4 100644
--- a/OvmfPkg/XenBusDxe/XenBusDxe.inf
+++ b/OvmfPkg/XenBusDxe/XenBusDxe.inf
@@ -34,8 +34,6 @@
   DriverBinding.h
   ComponentName.c
   ComponentName.h
-  InterlockedCompareExchange16.c
-  InterlockedCompareExchange16.h
   GrantTable.c
   GrantTable.h
   EventChannel.c
@@ -45,14 +43,7 @@
   XenBus.c
   XenBus.h
   Helpers.c
-
-[Sources.IA32]
-  Ia32/InterlockedCompareExchange16.nasm
-  Ia32/TestAndClearBit.nasm
-
-[Sources.X64]
-  X64/InterlockedCompareExchange16.nasm
-  X64/TestAndClearBit.nasm
+  TestAndClearBit.c
 
 [LibraryClasses]
   UefiDriverEntryPoint
@@ -64,6 +55,7 @@
   DevicePathLib
   DebugLib
   XenHypercallLib
+  SynchronizationLib
 
 [Protocols]
   gEfiDriverBindingProtocolGuid
