diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index ff346f9df..6bb66a02b 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -105,6 +105,16 @@ void gicv3_spis_config_defaults(uintptr_t gicd_base)
 	/* Maximum SPI INTID is 32 * (GICD_TYPER.ITLinesNumber + 1) - 1 */
 	num_ints = ((typer_reg & TYPER_IT_LINES_NO_MASK) + 1U) << 5;
 
+	/*
+	 * The GICv3 architecture allows GICD_TYPER.ITLinesNumber to be 31, so
+	 * the maximum possible value for num_ints is 1024. Limit the value to
+	 * MAX_SPI_ID + 1 to avoid getting wrong address in GICD_OFFSET() macro.
+	 */
+	if (num_ints > MAX_SPI_ID + 1U) {
+		num_ints = MAX_SPI_ID + 1U;
+	}
+	INFO("Maximum SPI INTID supported: %u\n", num_ints - 1);
+
 	/* Treat all (E)SPIs as G1NS by default. We do 32 at a time. */
 	for (i = MIN_SPI_ID; i < num_ints; i += (1U << IGROUPR_SHIFT)) {
 		gicd_write_igroupr(gicd_base, i, ~0U);
@@ -117,7 +127,8 @@ void gicv3_spis_config_defaults(uintptr_t gicd_base)
 		 * Maximum ESPI INTID is 32 * (GICD_TYPER.ESPI_range + 1) + 4095
 		 */
 		num_eints = ((((typer_reg >> TYPER_ESPI_RANGE_SHIFT) &
-			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID - 1;
+			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID;
+		INFO("Maximum ESPI INTID supported: %u\n", num_eints - 1);
 
 		for (i = MIN_ESPI_ID; i < num_eints;
 					i += (1U << IGROUPR_SHIFT)) {
@@ -125,6 +136,7 @@ void gicv3_spis_config_defaults(uintptr_t gicd_base)
 		}
 	} else {
 		num_eints = 0U;
+		INFO("ESPI range is not implemented.\n");
 	}
 #endif
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index 22efd458d..5a49b4f5e 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -70,7 +70,8 @@ static bool is_sgi_ppi(unsigned int id);
 		for (unsigned int int_id = MIN_ESPI_ID; int_id < (intr_num);\
 				int_id += (1U << REG##R_SHIFT)) {	\
 			gicd_write_##reg((base), int_id,		\
-			(ctx)->gicd_##reg[(int_id - (MIN_ESPI_ID - MIN_SPI_ID))\
+			(ctx)->gicd_##reg[(int_id - (MIN_ESPI_ID -	\
+			round_up(TOTAL_SPI_INTR_NUM, 1U << REG##R_SHIFT)))\
 						>> REG##R_SHIFT]);	\
 		}							\
 	} while (false)
@@ -79,7 +80,8 @@ static bool is_sgi_ppi(unsigned int id);
 	do {								\
 		for (unsigned int int_id = MIN_ESPI_ID; int_id < (intr_num);\
 				int_id += (1U << REG##R_SHIFT)) {	\
-			(ctx)->gicd_##reg[(int_id - (MIN_ESPI_ID - MIN_SPI_ID))\
+			(ctx)->gicd_##reg[(int_id - (MIN_ESPI_ID -	\
+			round_up(TOTAL_SPI_INTR_NUM, 1U << REG##R_SHIFT)))\
 			>> REG##R_SHIFT] = gicd_read_##reg((base), int_id);\
 		}							\
 	} while (false)
@@ -755,7 +757,7 @@ void gicv3_distif_save(gicv3_dist_ctx_t * const dist_ctx)
 		 * Maximum ESPI INTID is 32 * (GICD_TYPER.ESPI_range + 1) + 4095
 		 */
 		num_eints = ((((typer_reg >> TYPER_ESPI_RANGE_SHIFT) &
-			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID - 1;
+			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID;
 	} else {
 		num_eints = 0U;
 	}
@@ -879,7 +881,7 @@ void gicv3_distif_init_restore(const gicv3_dist_ctx_t * const dist_ctx)
 		 * Maximum ESPI INTID is 32 * (GICD_TYPER.ESPI_range + 1) + 4095
 		 */
 		num_eints = ((((typer_reg >> TYPER_ESPI_RANGE_SHIFT) &
-			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID - 1;
+			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID;
 	} else {
 		num_eints = 0U;
 	}
@@ -1299,8 +1301,8 @@ unsigned int gicv3_set_pmr(unsigned int mask)
  ******************************************************************************/
 int gicv3_rdistif_probe(const uintptr_t gicr_frame)
 {
-	u_register_t mpidr;
-	unsigned int proc_num, proc_self;
+	u_register_t mpidr, mpidr_self;
+	unsigned int proc_num;
 	uint64_t typer_val;
 	uintptr_t rdistif_base;
 	bool gicr_frame_found = false;
@@ -1314,18 +1316,18 @@ int gicv3_rdistif_probe(const uintptr_t gicr_frame)
 	assert((read_sctlr_el3() & SCTLR_C_BIT) != 0U);
 #endif /* !__aarch64__ */
 
-	proc_self = gicv3_driver_data->mpidr_to_core_pos(read_mpidr_el1());
+	mpidr_self = read_mpidr_el1() & MPIDR_AFFINITY_MASK;
 	rdistif_base = gicr_frame;
 	do {
 		typer_val = gicr_read_typer(rdistif_base);
+		mpidr = mpidr_from_gicr_typer(typer_val);
 		if (gicv3_driver_data->mpidr_to_core_pos != NULL) {
-			mpidr = mpidr_from_gicr_typer(typer_val);
 			proc_num = gicv3_driver_data->mpidr_to_core_pos(mpidr);
 		} else {
 			proc_num = (unsigned int)(typer_val >>
 				TYPER_PROC_NUM_SHIFT) & TYPER_PROC_NUM_MASK;
 		}
-		if (proc_num == proc_self) {
+		if (mpidr == mpidr_self) {
 			/* The base address doesn't need to be initialized on
 			 * every warm boot.
 			 */
diff --git a/drivers/arm/gic/v3/gicv3_private.h b/drivers/arm/gic/v3/gicv3_private.h
index c5d027da2..416cdd018 100644
--- a/drivers/arm/gic/v3/gicv3_private.h
+++ b/drivers/arm/gic/v3/gicv3_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -48,7 +48,8 @@
 #define	GICD_OFFSET_64(REG, id)						\
 	(((id) <= MAX_SPI_ID) ?						\
 	GICD_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 3) :	\
-	GICD_##REG##RE + (((uintptr_t)(id) - MIN_ESPI_ID) << 3))
+	GICD_##REG##RE + ((((uintptr_t)(id) - MIN_ESPI_ID) >>		\
+					REG##R_SHIFT) << 3))
 
 #else	/* GICv3 */
 #define	GICD_OFFSET_8(REG, id)	\