Support AArch64 SIMD/FP registers read/write

This patch adds support in openOCD to read/write AArch64 SIMD/FP registers.
This patch depends on a previous patch which adds support to generation 
of target xml by openOCD with nested architecture defined types. AArch64 
SIMD/FP registers assumes various types and to support all types we 
implement them as architecture defined type aarch64v which in turn 
consists of various architecture defined types. This is compatible with 
AArch64-FPU target xml in GDB. Please refer to 
binutils-gdb/gdb/features/aarch64-fpu.xml

Change-Id: I7ffb0c21b3c2e08f13720b765408b30aab2a9808
Signed-off-by: Omair Javaid <omair.javaid@linaro.org>
Reviewed-on: http://openocd.zylin.com/4373
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>

5 files changed, 411 insertions, 92 deletions

diff --git a/src/target/arm.h b/src/target/arm.h
index eb4a51f..c7963dd 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -225,7 +225,7 @@ struct arm_reg {
 	enum arm_mode mode;
 	struct target *target;
 	struct arm *arm;
-	uint8_t value[8];
+	uint8_t value[16];
 };
 
 struct reg_cache *arm_build_reg_cache(struct target *target, struct arm *arm);
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 1b8e450..93644aa 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -135,6 +135,16 @@ static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regv
 				ARMV8_MRS_DSPSR(0), &value);
 		value_64 = value;
 		break;
+	case ARMV8_FPSR:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_FPSR(0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_FPCR:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_FPCR(0), &value);
+		value_64 = value;
+		break;
 	case ARMV8_ELR_EL1:
 		retval = dpm->instr_read_data_r0_64(dpm,
 				ARMV8_MRS(SYSTEM_ELR_EL1, 0), &value_64);
@@ -184,6 +194,31 @@ static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regv
 
 	if (retval == ERROR_OK && regval != NULL)
 		*regval = value_64;
+	else
+		retval = ERROR_FAIL;
+
+	return retval;
+}
+
+static int armv8_read_reg_simdfp_aarch64(struct armv8_common *armv8, int regnum, uint64_t *lvalue, uint64_t *hvalue)
+{
+	int retval = ERROR_FAIL;
+	struct arm_dpm *dpm = &armv8->dpm;
+
+	switch (regnum) {
+	case ARMV8_V0 ... ARMV8_V31:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MOV_GPR_VFP(0, (regnum - ARMV8_V0), 1), hvalue);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MOV_GPR_VFP(0, (regnum - ARMV8_V0), 0), lvalue);
+		break;
+
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
 
 	return retval;
 }
@@ -216,6 +251,18 @@ static int armv8_write_reg(struct armv8_common *armv8, int regnum, uint64_t valu
 			ARMV8_MSR_DSPSR(0),
 			value);
 		break;
+	case ARMV8_FPSR:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+			ARMV8_MSR_FPSR(0),
+			value);
+		break;
+	case ARMV8_FPCR:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+			ARMV8_MSR_FPCR(0),
+			value);
+		break;
 	/* registers clobbered by taking exception in debug state */
 	case ARMV8_ELR_EL1:
 		retval = dpm->instr_write_data_r0_64(dpm,
@@ -267,6 +314,29 @@ static int armv8_write_reg(struct armv8_common *armv8, int regnum, uint64_t valu
 	return retval;
 }
 
+static int armv8_write_reg_simdfp_aarch64(struct armv8_common *armv8, int regnum, uint64_t lvalue, uint64_t hvalue)
+{
+	int retval = ERROR_FAIL;
+	struct arm_dpm *dpm = &armv8->dpm;
+
+	switch (regnum) {
+	case ARMV8_V0 ... ARMV8_V31:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MOV_VFP_GPR((regnum - ARMV8_V0), 0, 1), hvalue);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MOV_VFP_GPR((regnum - ARMV8_V0), 0, 0), lvalue);
+		break;
+
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	return retval;
+}
+
 static int armv8_read_reg32(struct armv8_common *armv8, int regnum, uint64_t *regval)
 {
 	struct arm_dpm *dpm = &armv8->dpm;
@@ -431,6 +501,9 @@ void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64)
 	if (is_aarch64) {
 		armv8->read_reg_u64 = armv8_read_reg;
 		armv8->write_reg_u64 = armv8_write_reg;
+		armv8->read_reg_u128 = armv8_read_reg_simdfp_aarch64;
+		armv8->write_reg_u128 = armv8_write_reg_simdfp_aarch64;
+
 	} else {
 		armv8->read_reg_u64 = armv8_read_reg32;
 		armv8->write_reg_u64 = armv8_write_reg32;
@@ -897,6 +970,110 @@ int armv8_arch_state(struct target *target)
 	return ERROR_OK;
 }
 
+static struct reg_data_type aarch64_vector_base_types[] = {
+	{REG_TYPE_IEEE_DOUBLE, "ieee_double", 0, {NULL} },
+	{REG_TYPE_UINT64, "uint64", 0, {NULL} },
+	{REG_TYPE_INT64, "int64", 0, {NULL} },
+	{REG_TYPE_IEEE_SINGLE, "ieee_single", 0, {NULL} },
+	{REG_TYPE_UINT32, "uint32", 0, {NULL} },
+	{REG_TYPE_INT32, "int32", 0, {NULL} },
+	{REG_TYPE_UINT16, "uint16", 0, {NULL} },
+	{REG_TYPE_INT16, "int16", 0, {NULL} },
+	{REG_TYPE_UINT8, "uint8", 0, {NULL} },
+	{REG_TYPE_INT8, "int8", 0, {NULL} },
+	{REG_TYPE_UINT128, "uint128", 0, {NULL} },
+	{REG_TYPE_INT128, "int128", 0, {NULL} }
+};
+
+static struct reg_data_type_vector aarch64_vector_types[] = {
+	{aarch64_vector_base_types + 0, 2},
+	{aarch64_vector_base_types + 1, 2},
+	{aarch64_vector_base_types + 2, 2},
+	{aarch64_vector_base_types + 3, 4},
+	{aarch64_vector_base_types + 4, 4},
+	{aarch64_vector_base_types + 5, 4},
+	{aarch64_vector_base_types + 6, 8},
+	{aarch64_vector_base_types + 7, 8},
+	{aarch64_vector_base_types + 8, 16},
+	{aarch64_vector_base_types + 9, 16},
+	{aarch64_vector_base_types + 10, 01},
+	{aarch64_vector_base_types + 11, 01},
+};
+
+static struct reg_data_type aarch64_fpu_vector[] = {
+	{REG_TYPE_ARCH_DEFINED, "v2d",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 0} },
+	{REG_TYPE_ARCH_DEFINED, "v2u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 1} },
+	{REG_TYPE_ARCH_DEFINED, "v2i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 2} },
+	{REG_TYPE_ARCH_DEFINED, "v4f",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 3} },
+	{REG_TYPE_ARCH_DEFINED, "v4u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 4} },
+	{REG_TYPE_ARCH_DEFINED, "v4i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 5} },
+	{REG_TYPE_ARCH_DEFINED, "v8u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 6} },
+	{REG_TYPE_ARCH_DEFINED, "v8i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 7} },
+	{REG_TYPE_ARCH_DEFINED, "v16u", REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 8} },
+	{REG_TYPE_ARCH_DEFINED, "v16i", REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 9} },
+	{REG_TYPE_ARCH_DEFINED, "v1u",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 10} },
+	{REG_TYPE_ARCH_DEFINED, "v1i",  REG_TYPE_CLASS_VECTOR, {aarch64_vector_types + 11} },
+};
+
+static struct reg_data_type_union_field aarch64_union_fields_vnd[] = {
+	{"f", aarch64_fpu_vector + 0, aarch64_union_fields_vnd + 1},
+	{"u", aarch64_fpu_vector + 1, aarch64_union_fields_vnd + 2},
+	{"s", aarch64_fpu_vector + 2, NULL},
+};
+
+static struct reg_data_type_union_field aarch64_union_fields_vns[] = {
+	{"f", aarch64_fpu_vector + 3, aarch64_union_fields_vns + 1},
+	{"u", aarch64_fpu_vector + 4, aarch64_union_fields_vns + 2},
+	{"s", aarch64_fpu_vector + 5, NULL},
+};
+
+static struct reg_data_type_union_field aarch64_union_fields_vnh[] = {
+	{"u", aarch64_fpu_vector + 6, aarch64_union_fields_vnh + 1},
+	{"s", aarch64_fpu_vector + 7, NULL},
+};
+
+static struct reg_data_type_union_field aarch64_union_fields_vnb[] = {
+	{"u", aarch64_fpu_vector + 8, aarch64_union_fields_vnb + 1},
+	{"s", aarch64_fpu_vector + 9, NULL},
+};
+
+static struct reg_data_type_union_field aarch64_union_fields_vnq[] = {
+	{"u", aarch64_fpu_vector + 10, aarch64_union_fields_vnq + 1},
+	{"s", aarch64_fpu_vector + 11, NULL},
+};
+
+static struct reg_data_type_union aarch64_union_types[] = {
+	{aarch64_union_fields_vnd},
+	{aarch64_union_fields_vns},
+	{aarch64_union_fields_vnh},
+	{aarch64_union_fields_vnb},
+	{aarch64_union_fields_vnq},
+};
+
+static struct reg_data_type aarch64_fpu_union[] = {
+	{REG_TYPE_ARCH_DEFINED, "vnd", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 0} },
+	{REG_TYPE_ARCH_DEFINED, "vns", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 1} },
+	{REG_TYPE_ARCH_DEFINED, "vnh", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 2} },
+	{REG_TYPE_ARCH_DEFINED, "vnb", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 3} },
+	{REG_TYPE_ARCH_DEFINED, "vnq", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64_union_types + 4} },
+};
+
+static struct reg_data_type_union_field aarch64v_union_fields[] = {
+	{"d", aarch64_fpu_union + 0, aarch64v_union_fields + 1},
+	{"s", aarch64_fpu_union + 1, aarch64v_union_fields + 2},
+	{"h", aarch64_fpu_union + 2, aarch64v_union_fields + 3},
+	{"b", aarch64_fpu_union + 3, aarch64v_union_fields + 4},
+	{"q", aarch64_fpu_union + 4, NULL},
+};
+
+static struct reg_data_type_union aarch64v_union[] = {
+	{aarch64v_union_fields}
+};
+
+static struct reg_data_type aarch64v[] = {
+	{REG_TYPE_ARCH_DEFINED, "aarch64v", REG_TYPE_CLASS_UNION, {.reg_type_union = aarch64v_union} },
+};
+
 static const struct {
 	unsigned id;
 	const char *name;
@@ -905,55 +1082,100 @@ static const struct {
 	enum reg_type type;
 	const char *group;
 	const char *feature;
+	struct reg_data_type *data_type;
 } armv8_regs[] = {
-	{ ARMV8_R0,  "x0",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R1,  "x1",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R2,  "x2",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R3,  "x3",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R4,  "x4",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R5,  "x5",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R6,  "x6",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R7,  "x7",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R8,  "x8",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R9,  "x9",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R10, "x10", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R11, "x11", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R12, "x12", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R13, "x13", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R14, "x14", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R15, "x15", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R16, "x16", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R17, "x17", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R18, "x18", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R19, "x19", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R20, "x20", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R21, "x21", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R22, "x22", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R23, "x23", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R24, "x24", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R25, "x25", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R26, "x26", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R27, "x27", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R28, "x28", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R29, "x29", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R30, "x30", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-
-	{ ARMV8_SP, "sp", 64, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_PC,  "pc", 64, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
-
-	{ ARMV8_xPSR, "CPSR", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" },
-
-	{ ARMV8_ELR_EL1, "ELR_EL1", 64, ARMV8_64_EL1H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
-	{ ARMV8_ESR_EL1, "ESR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
-	{ ARMV8_SPSR_EL1, "SPSR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
-
-	{ ARMV8_ELR_EL2, "ELR_EL2", 64, ARMV8_64_EL2H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
-	{ ARMV8_ESR_EL2, "ESR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
-	{ ARMV8_SPSR_EL2, "SPSR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
-
-	{ ARMV8_ELR_EL3, "ELR_EL3", 64, ARMV8_64_EL3H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
-	{ ARMV8_ESR_EL3, "ESR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
-	{ ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_R0,  "x0",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R1,  "x1",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R2,  "x2",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R3,  "x3",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R4,  "x4",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R5,  "x5",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R6,  "x6",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R7,  "x7",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R8,  "x8",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R9,  "x9",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R10, "x10", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R11, "x11", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R12, "x12", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R13, "x13", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R14, "x14", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R15, "x15", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R16, "x16", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R17, "x17", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R18, "x18", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R19, "x19", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R20, "x20", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R21, "x21", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R22, "x22", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R23, "x23", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R24, "x24", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R25, "x25", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R26, "x26", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R27, "x27", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R28, "x28", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R29, "x29", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_R30, "x30", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core", NULL},
+
+	{ ARMV8_SP, "sp", 64, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core", NULL},
+	{ ARMV8_PC, "pc", 64, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core", NULL},
+
+	{ ARMV8_xPSR, "CPSR", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core", NULL},
+
+	{ ARMV8_V0,  "v0",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V1,  "v1",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V2,  "v2",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V3,  "v3",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V4,  "v4",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V5,  "v5",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V6,  "v6",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V7,  "v7",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V8,  "v8",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V9,  "v9",  128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V10, "v10", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V11, "v11", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V12, "v12", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V13, "v13", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V14, "v14", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V15, "v15", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V16, "v16", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V17, "v17", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V18, "v18", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V19, "v19", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V20, "v20", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V21, "v21", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V22, "v22", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V23, "v23", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V24, "v24", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V25, "v25", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V26, "v26", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V27, "v27", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V28, "v28", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V29, "v29", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V30, "v30", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_V31, "v31", 128, ARM_MODE_ANY, REG_TYPE_ARCH_DEFINED, "simdfp", "org.gnu.gdb.aarch64.fpu", aarch64v},
+	{ ARMV8_FPSR, "fpsr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "simdfp", "org.gnu.gdb.aarch64.fpu", NULL},
+	{ ARMV8_FPCR, "fpcr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "simdfp", "org.gnu.gdb.aarch64.fpu", NULL},
+
+	{ ARMV8_ELR_EL1, "ELR_EL1", 64, ARMV8_64_EL1H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+	{ ARMV8_ESR_EL1, "ESR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+	{ ARMV8_SPSR_EL1, "SPSR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+
+	{ ARMV8_ELR_EL2, "ELR_EL2", 64, ARMV8_64_EL2H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+	{ ARMV8_ESR_EL2, "ESR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+	{ ARMV8_SPSR_EL2, "SPSR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+
+	{ ARMV8_ELR_EL3, "ELR_EL3", 64, ARMV8_64_EL3H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+	{ ARMV8_ESR_EL3, "ESR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
+														NULL},
+	{ ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked",
+														NULL},
 };
 
 static const struct {
@@ -1004,15 +1226,23 @@ static int armv8_set_core_reg(struct reg *reg, uint8_t *buf)
 	struct arm_reg *armv8_reg = reg->arch_info;
 	struct target *target = armv8_reg->target;
 	struct arm *arm = target_to_arm(target);
-	uint64_t value = buf_get_u64(buf, 0, 64);
+	uint64_t value = buf_get_u64(buf, 0, reg->size);
 
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	if (reg == arm->cpsr) {
-		armv8_set_cpsr(arm, (uint32_t)value);
-	} else {
+	if (reg->size <= 64) {
+		if (reg == arm->cpsr)
+			armv8_set_cpsr(arm, (uint32_t)value);
+		else {
+			buf_set_u64(reg->value, 0, reg->size, value);
+			reg->valid = 1;
+		}
+	} else if (reg->size <= 128) {
+		uint64_t hvalue = buf_get_u64(buf + 8, 0, reg->size - 64);
+
 		buf_set_u64(reg->value, 0, 64, value);
+		buf_set_u64(reg->value + 8, 0, reg->size - 64, hvalue);
 		reg->valid = 1;
 	}
 
@@ -1122,11 +1352,15 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 		} else
 			LOG_ERROR("unable to allocate feature list");
 
-		reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
-		if (reg_list[i].reg_data_type)
-			reg_list[i].reg_data_type->type = armv8_regs[i].type;
-		else
-			LOG_ERROR("unable to allocate reg type list");
+		if (armv8_regs[i].data_type == NULL) {
+			reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
+			if (reg_list[i].reg_data_type)
+				reg_list[i].reg_data_type->type = armv8_regs[i].type;
+			else
+				LOG_ERROR("unable to allocate reg type list");
+		} else
+			reg_list[i].reg_data_type =	armv8_regs[i].data_type;
+
 	}
 
 	arm->cpsr = reg_list + ARMV8_xPSR;
@@ -1200,7 +1434,7 @@ int armv8_get_gdb_reg_list(struct target *target,
 
 		switch (reg_class) {
 		case REG_CLASS_GENERAL:
-			*reg_list_size = ARMV8_ELR_EL1;
+			*reg_list_size = ARMV8_V0;
 			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 
 			for (i = 0; i < *reg_list_size; i++)
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 0f3e66f..6525d26 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -63,17 +63,52 @@ enum {
 	ARMV8_PC = 32,
 	ARMV8_xPSR = 33,
 
-	ARMV8_ELR_EL1 = 34,
-	ARMV8_ESR_EL1 = 35,
-	ARMV8_SPSR_EL1 = 36,
-
-	ARMV8_ELR_EL2 = 37,
-	ARMV8_ESR_EL2 = 38,
-	ARMV8_SPSR_EL2 = 39,
-
-	ARMV8_ELR_EL3 = 40,
-	ARMV8_ESR_EL3 = 41,
-	ARMV8_SPSR_EL3 = 42,
+	ARMV8_V0 = 34,
+	ARMV8_V1,
+	ARMV8_V2,
+	ARMV8_V3,
+	ARMV8_V4,
+	ARMV8_V5,
+	ARMV8_V6,
+	ARMV8_V7,
+	ARMV8_V8,
+	ARMV8_V9,
+	ARMV8_V10,
+	ARMV8_V11,
+	ARMV8_V12,
+	ARMV8_V13,
+	ARMV8_V14,
+	ARMV8_V15,
+	ARMV8_V16,
+	ARMV8_V17,
+	ARMV8_V18,
+	ARMV8_V19,
+	ARMV8_V20,
+	ARMV8_V21,
+	ARMV8_V22,
+	ARMV8_V23,
+	ARMV8_V24,
+	ARMV8_V25,
+	ARMV8_V26,
+	ARMV8_V27,
+	ARMV8_V28,
+	ARMV8_V29,
+	ARMV8_V30,
+	ARMV8_V31,
+	ARMV8_FPSR,
+	ARMV8_FPCR,
+
+	ARMV8_ELR_EL1 = 68,
+	ARMV8_ESR_EL1 = 69,
+	ARMV8_SPSR_EL1 = 70,
+
+	ARMV8_ELR_EL2 = 71,
+	ARMV8_ESR_EL2 = 72,
+	ARMV8_SPSR_EL2 = 73,
+
+	ARMV8_ELR_EL3 = 74,
+	ARMV8_ESR_EL3 = 75,
+	ARMV8_SPSR_EL3 = 76,
 
 	ARMV8_LAST_REG,
 };
@@ -179,6 +214,12 @@ struct armv8_common {
 	int (*read_reg_u64)(struct armv8_common *armv8, int num, uint64_t *value);
 	int (*write_reg_u64)(struct armv8_common *armv8, int num, uint64_t value);
 
+	/* SIMD/FPU registers read/write interface */
+	int (*read_reg_u128)(struct armv8_common *armv8, int num,
+			uint64_t *lvalue, uint64_t *hvalue);
+	int (*write_reg_u128)(struct armv8_common *armv8, int num,
+			uint64_t lvalue, uint64_t hvalue);
+
 	int (*examine_debug_reason)(struct target *target);
 	int (*post_debug_entry)(struct target *target);
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index c79b1a0..91b2f51 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -650,21 +650,37 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint64_t value_64;
-	int retval;
+	int retval = ERROR_FAIL;
+
+	if (r->size <= 64) {
+		uint64_t value_64;
+		retval = armv8->read_reg_u64(armv8, regnum, &value_64);
+
+		if (retval == ERROR_OK) {
+			r->valid = true;
+			r->dirty = false;
+			buf_set_u64(r->value, 0, r->size, value_64);
+			if (r->size == 64)
+				LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
+			else
+				LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
+		}
+	} else if (r->size <= 128) {
+		uint64_t lvalue = 0, hvalue = 0;
+		retval = armv8->read_reg_u128(armv8, regnum, &lvalue, &hvalue);
+
+		if (retval == ERROR_OK) {
+			r->valid = true;
+			r->dirty = false;
 
-	retval = armv8->read_reg_u64(armv8, regnum, &value_64);
+			buf_set_u64(r->value, 0, 64, lvalue);
+			buf_set_u64(r->value + 8, 0, r->size - 64, hvalue);
 
-	if (retval == ERROR_OK) {
-		r->valid = true;
-		r->dirty = false;
-		buf_set_u64(r->value, 0, r->size, value_64);
-		if (r->size == 64)
-			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
-		else
-			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
+			LOG_DEBUG("READ: %s, lvalue=%16.8llx", r->name, (unsigned long long) lvalue);
+			LOG_DEBUG("READ: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
+		}
 	}
-	return ERROR_OK;
+	return retval;
 }
 
 /*
@@ -674,20 +690,36 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	int retval = ERROR_FAIL;
-	uint64_t value_64;
 
-	value_64 = buf_get_u64(r->value, 0, r->size);
+	if (r->size <= 64) {
+		uint64_t value_64;
+
+		value_64 = buf_get_u64(r->value, 0, r->size);
+		retval = armv8->write_reg_u64(armv8, regnum, value_64);
 
-	retval = armv8->write_reg_u64(armv8, regnum, value_64);
-	if (retval == ERROR_OK) {
-		r->dirty = false;
-		if (r->size == 64)
-			LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
-		else
-			LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
+		if (retval == ERROR_OK) {
+			r->dirty = false;
+			if (r->size == 64)
+				LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
+			else
+				LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
+		}
+	} else if (r->size <= 128) {
+		uint64_t lvalue, hvalue;
+
+		lvalue = buf_get_u64(r->value, 0, 64);
+		hvalue = buf_get_u64(r->value + 8, 0, r->size - 64);
+		retval = armv8->write_reg_u128(armv8, regnum, lvalue, hvalue);
+
+		if (retval == ERROR_OK) {
+			r->dirty = false;
+
+			LOG_DEBUG("WRITE: %s, lvalue=%16.8llx", r->name, (unsigned long long) lvalue);
+			LOG_DEBUG("WRITE: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
+		}
 	}
 
-	return ERROR_OK;
+	return retval;
 }
 
 /**
@@ -745,6 +777,10 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 		if (r->valid)
 			continue;
 
+		/* Skip reading FP-SIMD registers */
+		if (r->number >= ARMV8_V0 && r->number <= ARMV8_FPCR)
+			continue;
+
 		/*
 		 * Only read registers that are available from the
 		 * current EL (or core mode).
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 987198a..3fda296 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -167,6 +167,14 @@
 #define ARMV8_STRH_IP(Rd, Rn) (0x78002400 | (Rn << 5) | Rd)
 #define ARMV8_STRW_IP(Rd, Rn) (0xb8004400 | (Rn << 5) | Rd)
 
+#define ARMV8_MOV_GPR_VFP(Rd, Rn, Index) (0x4e083c00 | (Index << 20) | (Rn << 5) | Rd)
+#define ARMV8_MOV_VFP_GPR(Rd, Rn, Index) (0x4e081c00 | (Index << 20) | (Rn << 5) | Rd)
+
+#define ARMV8_MRS_FPCR(Rt)	(0xd53b4400 | (Rt))
+#define ARMV8_MRS_FPSR(Rt)	(0xd53b4420 | (Rt))
+#define ARMV8_MSR_FPCR(Rt)	(0xd51b4400 | (Rt))
+#define ARMV8_MSR_FPSR(Rt)	(0xd51b4420 | (Rt))
+
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
 enum armv8_opcode {