diff --git a/arch/arm/include/asm/opcodes.h b/arch/arm/include/asm/opcodes.h
index e796c598513b..751eca1d4e22 100644
--- a/arch/arm/include/asm/opcodes.h
+++ b/arch/arm/include/asm/opcodes.h
@@ -216,6 +216,15 @@ extern asmlinkage unsigned int arm_check_condition(u32 opcode, u32 psr);
 #define __inst_arm_thumb32(arm_opcode, thumb_opcode) __inst_arm(arm_opcode)
 #endif
 
+/*
+ * Macros/defines for extracting register numbers from instruction.
+ */
+#define EXTRACT_REG_NUM(instruction, offset) \
+	(((instruction) & (0xf << (offset))) >> (offset))
+#define RN_OFFSET  16
+#define RT_OFFSET  12
+#define RT2_OFFSET  0
+
 /* Helpers for the helpers.  Don't use these directly. */
 #ifdef __ASSEMBLY__
 #define ___inst_arm(x) .long x
diff --git a/arch/arm/include/uapi/asm/hwcap.h b/arch/arm/include/uapi/asm/hwcap.h
index 20d12f230a2f..4ec061e81d38 100644
--- a/arch/arm/include/uapi/asm/hwcap.h
+++ b/arch/arm/include/uapi/asm/hwcap.h
@@ -27,6 +27,7 @@
 #define HWCAP_IDIV	(HWCAP_IDIVA | HWCAP_IDIVT)
 #define HWCAP_LPAE	(1 << 20)
 #define HWCAP_EVTSTRM	(1 << 21)
+#define HWCAP_CPUID	(1 << 22)
 
 /*
  * HWCAP2 flags - for elf_hwcap2 (in kernel) and AT_HWCAP2
diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c
index 84db893dedc2..ea0c1a013e00 100644
--- a/arch/arm/kernel/setup.c
+++ b/arch/arm/kernel/setup.c
@@ -30,6 +30,7 @@
 #include <linux/bug.h>
 #include <linux/compiler.h>
 #include <linux/sort.h>
+#include <linux/perf_event.h>
 
 #include <asm/unified.h>
 #include <asm/cp15.h>
@@ -56,9 +57,11 @@
 #include <asm/unwind.h>
 #include <asm/memblock.h>
 #include <asm/virt.h>
+#include <asm/opcodes.h>
+#include <asm/vfp.h>
 
 #include "atags.h"
-
+#include "../vfp/vfpinstr.h"
 
 #if defined(CONFIG_FPE_NWFPE) || defined(CONFIG_FPE_FASTFPE)
 char fpe_type[8];
@@ -371,6 +374,108 @@ void __init early_print(const char *str, ...)
 	printk("%s", buf);
 }
 
+static u32 arm_id_registers[2 * 8];
+static u32 arm_vfp_id_registers[3];
+
+static void cache_id_registers(void)
+{
+	arm_id_registers[0]  = read_cpuid_ext(CPUID_EXT_PFR0);
+	arm_id_registers[1]  = read_cpuid_ext(CPUID_EXT_PFR1);
+	arm_id_registers[2] = read_cpuid_ext(CPUID_EXT_DFR0);
+	arm_id_registers[3] = read_cpuid_ext(CPUID_EXT_AFR0);
+	arm_id_registers[4] = read_cpuid_ext(CPUID_EXT_MMFR0);
+	arm_id_registers[5] = read_cpuid_ext(CPUID_EXT_MMFR1);
+	arm_id_registers[6] = read_cpuid_ext(CPUID_EXT_MMFR2);
+	arm_id_registers[7] = read_cpuid_ext(CPUID_EXT_MMFR3);
+	arm_id_registers[8] = read_cpuid_ext(CPUID_EXT_ISAR0);
+	arm_id_registers[9] = read_cpuid_ext(CPUID_EXT_ISAR1);
+	arm_id_registers[10] = read_cpuid_ext(CPUID_EXT_ISAR2);
+	arm_id_registers[11] = read_cpuid_ext(CPUID_EXT_ISAR3);
+	arm_id_registers[12] = read_cpuid_ext(CPUID_EXT_ISAR4);
+	arm_id_registers[13] = read_cpuid_ext(CPUID_EXT_ISAR5);
+	arm_vfp_id_registers[0] = fmrx(FPSID);
+	arm_vfp_id_registers[1] = fmrx(MVFR1);
+	arm_vfp_id_registers[2] = fmrx(MVFR0);
+}
+
+static int get_id_trap(struct pt_regs *regs, unsigned int instr)
+{
+	unsigned int destreg, crm, opc2;
+	unsigned int res;
+
+	perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, regs->ARM_pc);
+
+	res = arm_check_condition(instr, regs->ARM_cpsr);
+	if (res == ARM_OPCODE_CONDTEST_FAIL) {
+		regs->ARM_pc += 4;
+		return 0;
+	}
+
+	destreg = EXTRACT_REG_NUM(instr, RT_OFFSET);
+	crm = instr & 0xf;
+	opc2 = (instr >> 5) & 0x7;
+
+	if (crm > 2 || crm == 0)
+		return -EINVAL;
+
+	regs->uregs[destreg] = arm_id_registers[((crm - 1) * 8) + opc2];
+	regs->ARM_pc += 4;
+
+	return 0;
+}
+
+struct undef_hook arm_mrc_id_hook = {
+	.instr_mask	= 0x0f100f10,
+	.instr_val	= 0x0e100f10,
+	.cpsr_mask	= MODE_MASK,
+	.cpsr_val	= USR_MODE,
+	.fn		= get_id_trap,
+};
+
+static int get_vmrs_id_trap(struct pt_regs *regs, unsigned int instr)
+{
+	unsigned int destreg, data, reg;
+	unsigned int res;
+
+	perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, regs->ARM_pc);
+
+	res = arm_check_condition(instr, regs->ARM_cpsr);
+	if (res == ARM_OPCODE_CONDTEST_FAIL) {
+		regs->ARM_pc += 4;
+		return 0;
+	}
+
+	destreg = EXTRACT_REG_NUM(instr, RT_OFFSET);
+	reg = (instr >> 16) & 0xf;
+
+	switch (reg) {
+	case 0:
+		data = arm_vfp_id_registers[0];
+		break;
+	case 6:
+		data = arm_vfp_id_registers[1];
+		break;
+	case 7:
+		data = arm_vfp_id_registers[2];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	regs->uregs[destreg] = data;
+	regs->ARM_pc += 4;
+
+	return 0;
+}
+
+struct undef_hook arm_vmrs_id_hook = {
+	.instr_mask	= 0x0ff00ff0,
+	.instr_val	= 0x0ef00a10,
+	.cpsr_mask	= MODE_MASK,
+	.cpsr_val	= USR_MODE,
+	.fn		= get_vmrs_id_trap,
+};
+
 static void __init cpuid_init_hwcaps(void)
 {
 	unsigned int divide_instrs, vmsa;
@@ -378,6 +483,11 @@ static void __init cpuid_init_hwcaps(void)
 	if (cpu_architecture() < CPU_ARCH_ARMv7)
 		return;
 
+	cache_id_registers();
+	elf_hwcap |= HWCAP_CPUID;
+	register_undef_hook(&arm_mrc_id_hook);
+	register_undef_hook(&arm_vmrs_id_hook);
+
 	divide_instrs = (read_cpuid_ext(CPUID_EXT_ISAR0) & 0x0f000000) >> 24;
 
 	switch (divide_instrs) {
@@ -1009,6 +1119,7 @@ static const char *hwcap_str[] = {
 	"vfpd32",
 	"lpae",
 	"evtstrm",
+	"cpuid",
 	NULL
 };
 
diff --git a/arch/arm/kernel/swp_emulate.c b/arch/arm/kernel/swp_emulate.c
index 67ca8578c6d8..ebdcac50feef 100644
--- a/arch/arm/kernel/swp_emulate.c
+++ b/arch/arm/kernel/swp_emulate.c
@@ -62,14 +62,6 @@
 	__user_swpX_asm(data, addr, res, temp, "b")
 
 /*
- * Macros/defines for extracting register numbers from instruction.
- */
-#define EXTRACT_REG_NUM(instruction, offset) \
-	(((instruction) & (0xf << (offset))) >> (offset))
-#define RN_OFFSET  16
-#define RT_OFFSET  12
-#define RT2_OFFSET  0
-/*
  * Bit 22 of the instruction encoding distinguishes between
  * the SWP and SWPB variants (bit set means SWPB).
  */
