svn commit: r297390 - in head/sys: arm/arm arm64/arm64

[Andrew Turner]

Author: andrew
Date: Tue Mar 29 13:51:26 2016
New Revision: 297390
URL: https://svnweb.freebsd.org/changeset/base/297390

Log:
  Read the CPU ID for the current CPU from the GIC. The GIC may have a
  different ID space than the kernel. Because of this we need to read the
  ID from the hardware. The hardware will provide this value to the CPU by
  reading any of the first 8 Interrupt Processor Targets Registers.
  
  Obtained from:	ABT Systems Ltd
  Sponsored by:	The FreeBSD Foundation
  Differential Revision:	https://reviews.freebsd.org/D5706

Modified:
  head/sys/arm/arm/gic.c
  head/sys/arm64/arm64/gic.c

Modified: head/sys/arm/arm/gic.c
==============================================================================
--- head/sys/arm/arm/gic.c	Tue Mar 29 13:31:09 2016	(r297389)
+++ head/sys/arm/arm/gic.c	Tue Mar 29 13:51:26 2016	(r297390)
@@ -376,7 +376,7 @@ arm_gic_attach(device_t dev)
 {
 	struct		arm_gic_softc *sc;
 	int		i;
-	uint32_t	icciidr;
+	uint32_t	icciidr, mask;
 #ifdef ARM_INTRNG
 	phandle_t	pxref;
 	intptr_t	xref = gic_xref(dev);
@@ -437,10 +437,28 @@ arm_gic_attach(device_t dev)
 		gic_d_write_4(sc, GICD_ICENABLER(i >> 5), 0xFFFFFFFF);
 	}
 
+	/* Read the current cpuid mask by reading ITARGETSR{0..7} */
+	for (i = 0; i < 8; i++) {
+		mask = gic_d_read_4(sc, GICD_ITARGETSR(i));
+		if (mask != 0)
+			break;
+	}
+	/* No mask found, assume we are on CPU interface 0 */
+	if (mask == 0)
+		mask = 1;
+
+	/* Collect the mask in the lower byte */
+	mask |= mask >> 16;
+	mask |= mask >> 8;
+	/* Distribute this back to the upper bytes */
+	mask |= mask << 8;
+	mask |= mask << 16;
+
 	for (i = 0; i < sc->nirqs; i += 4) {
 		gic_d_write_4(sc, GICD_IPRIORITYR(i >> 2), 0);
-		gic_d_write_4(sc, GICD_ITARGETSR(i >> 2),
-		    1 << 0 | 1 << 8 | 1 << 16 | 1 << 24);
+		if (i > 32) {
+			gic_d_write_4(sc, GICD_ITARGETSR(i >> 2), mask);
+		}
 	}
 
 	/* Set all the interrupts to be in Group 0 (secure) */

Modified: head/sys/arm64/arm64/gic.c
==============================================================================
--- head/sys/arm64/arm64/gic.c	Tue Mar 29 13:31:09 2016	(r297389)
+++ head/sys/arm64/arm64/gic.c	Tue Mar 29 13:51:26 2016	(r297390)
@@ -162,7 +162,7 @@ arm_gic_attach(device_t dev)
 {
 	struct		arm_gic_softc *sc;
 	int		i;
-	uint32_t	icciidr;
+	uint32_t	icciidr, mask;
 
 	if (arm_gic_sc)
 		return (ENXIO);
@@ -212,10 +212,28 @@ arm_gic_attach(device_t dev)
 		gic_d_write_4(sc, GICD_ICENABLER(i >> 5), 0xFFFFFFFF);
 	}
 
+	/* Read the current cpuid mask by reading ITARGETSR{0..7} */
+	for (i = 0; i < 8; i++) {
+		mask = gic_d_read_4(sc, GICD_ITARGETSR(i));
+		if (mask != 0)
+			break;
+	}
+	/* No mask found, assume we are on CPU interface 0 */
+	if (mask == 0)
+		mask = 1;
+
+	/* Collect the mask in the lower byte */
+	mask |= mask >> 16;
+	mask |= mask >> 8;
+	/* Distribute this back to the upper bytes */
+	mask |= mask << 8;
+	mask |= mask << 16;
+
 	for (i = 0; i < sc->nirqs; i += 4) {
 		gic_d_write_4(sc, GICD_IPRIORITYR(i >> 2), 0);
-		gic_d_write_4(sc, GICD_ITARGETSR(i >> 2),
-		    1 << 0 | 1 << 8 | 1 << 16 | 1 << 24);
+		if (i > 32) {
+			gic_d_write_4(sc, GICD_ITARGETSR(i >> 2), mask);
+		}
 	}
 
 	/* Set all the interrupts to be in Group 0 (secure) */