@@ -31,9 +31,22 @@
/* Enable GCS if supported */
mrs_s x1, SYS_ID_AA64PFR1_EL1
ubfx x1, x1, #ID_AA64PFR1_EL1_GCS_SHIFT, #4
- cbz x1, .Lset_hcrx_\@
+ cbz x1, .Lskip_gcs_hcrx_\@
orr x0, x0, #HCRX_EL2_GCSEn
+.Lskip_gcs_hcrx_\@:
+ /* Enable LS64, LS64_V, LS64_ACCDATA if supported */
+ mrs_s x1, SYS_ID_AA64ISAR1_EL1
+ ubfx x1, x1, #ID_AA64ISAR1_EL1_LS64_SHIFT, #4
+ cbz x1, .Lset_hcrx_\@
+ orr x0, x0, #HCRX_EL2_EnALS
+ cmp x1, #ID_AA64ISAR1_EL1_LS64_LS64_V
+ b.lt .Lset_hcrx_\@
+ orr x0, x0, #HCRX_EL2_EnASR
+ cmp x1, #ID_AA64ISAR1_EL1_LS64_LS64_ACCDATA
+ b.lt .Lset_hcrx_\@
+ orr x0, x0, #HCRX_EL2_EnAS0
+
.Lset_hcrx_\@:
msr_s SYS_HCRX_EL2, x0
.Lskip_hcrx_\@:
@@ -211,12 +224,21 @@
/* GCS depends on PIE so we don't check it if PIE is absent */
mrs_s x1, SYS_ID_AA64PFR1_EL1
ubfx x1, x1, #ID_AA64PFR1_EL1_GCS_SHIFT, #4
- cbz x1, .Lset_fgt_\@
+ cbz x1, .Lskip_gcs_fgt_\@
/* Disable traps of access to GCS registers at EL0 and EL1 */
orr x0, x0, #HFGxTR_EL2_nGCS_EL1_MASK
orr x0, x0, #HFGxTR_EL2_nGCS_EL0_MASK
+.Lskip_gcs_fgt_\@:
+ mrs_s x1, SYS_ID_AA64ISAR1_EL1
+ ubfx x1, x1, #ID_AA64ISAR1_EL1_LS64_SHIFT, #4
+ cmp x1, #ID_AA64ISAR1_EL1_LS64_LS64_ACCDATA
+ b.ne .Lset_fgt_\@
+
+ /* Disable the trapping of ACCDATA_EL1 */
+ orr x0, x0, #HFGxTR_EL2_nACCDATA_EL1
+
.Lset_fgt_\@:
msr_s SYS_HFGRTR_EL2, x0
msr_s SYS_HFGWTR_EL2, x0