aboutsummaryrefslogtreecommitdiff
path: root/src/arch/aarch64/hypervisor/handler.c
blob: 0ee37d6adc498cbb0e50bd575315f02401ff2120 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
/*
 * Copyright 2018 The Hafnium Authors.
 *
 * Use of this source code is governed by a BSD-style
 * license that can be found in the LICENSE file or at
 * https://opensource.org/licenses/BSD-3-Clause.
 */

#include <stdnoreturn.h>

#include "hf/arch/barriers.h"
#include "hf/arch/init.h"
#include "hf/arch/mmu.h"
#include "hf/arch/plat/ffa.h"
#include "hf/arch/plat/smc.h"

#include "hf/api.h"
#include "hf/check.h"
#include "hf/cpu.h"
#include "hf/dlog.h"
#include "hf/ffa.h"
#include "hf/ffa_internal.h"
#include "hf/panic.h"
#include "hf/plat/interrupts.h"
#include "hf/vm.h"

#include "vmapi/hf/call.h"

#include "debug_el1.h"
#include "feature_id.h"
#include "msr.h"
#include "perfmon.h"
#include "psci.h"
#include "psci_handler.h"
#include "smc.h"
#include "sysregs.h"

/**
 * Hypervisor Fault Address Register Non-Secure.
 */
#define HPFAR_EL2_NS (UINT64_C(0x1) << 63)

/**
 * Hypervisor Fault Address Register Faulting IPA.
 */
#define HPFAR_EL2_FIPA (UINT64_C(0xFFFFFFFFFF0))

/**
 * Gets the value to increment for the next PC.
 * The ESR encodes whether the instruction is 2 bytes or 4 bytes long.
 */
#define GET_NEXT_PC_INC(esr) (GET_ESR_IL(esr) ? 4 : 2)

/**
 * The Client ID field within X7 for an SMC64 call.
 */
#define CLIENT_ID_MASK UINT64_C(0xffff)

/*
 * Target function IDs for framework messages from the SPMD.
 */
#define SPMD_FWK_MSG_BIT (UINT64_C(1) << 31)
#define SPMD_FWK_MSG_FUNC_MASK UINT64_C(0xFF)
#define SPMD_FWK_MSG_PSCI UINT8_C(0)
#define SPMD_FWK_MSG_FFA_VERSION_REQ UINT8_C(0x8)
#define SPMD_FWK_MSG_FFA_VERSION_RESP UINT8_C(0x9)

/**
 * Returns a reference to the currently executing vCPU.
 */
static struct vcpu *current(void)
{
	// NOLINTNEXTLINE(performance-no-int-to-ptr)
	return (struct vcpu *)read_msr(tpidr_el2);
}

/**
 * Saves the state of per-vCPU peripherals, such as the virtual timer, and
 * informs the arch-independent sections that registers have been saved.
 */
void complete_saving_state(struct vcpu *vcpu)
{
	if (has_vhe_support()) {
		vcpu->regs.peripherals.cntv_cval_el0 =
			read_msr(MSR_CNTV_CVAL_EL02);
		vcpu->regs.peripherals.cntv_ctl_el0 =
			read_msr(MSR_CNTV_CTL_EL02);
	} else {
		vcpu->regs.peripherals.cntv_cval_el0 = read_msr(cntv_cval_el0);
		vcpu->regs.peripherals.cntv_ctl_el0 = read_msr(cntv_ctl_el0);
	}

	api_regs_state_saved(vcpu);

	/*
	 * If switching away from the primary, copy the current EL0 virtual
	 * timer registers to the corresponding EL2 physical timer registers.
	 * This is used to emulate the virtual timer for the primary in case it
	 * should fire while the secondary is running.
	 */
	if (vcpu->vm->id == HF_PRIMARY_VM_ID) {
		/*
		 * Clear timer control register before copying compare value, to
		 * avoid a spurious timer interrupt. This could be a problem if
		 * the interrupt is configured as edge-triggered, as it would
		 * then be latched in.
		 */
		write_msr(cnthp_ctl_el2, 0);

		if (has_vhe_support()) {
			write_msr(cnthp_cval_el2, read_msr(MSR_CNTV_CVAL_EL02));
			write_msr(cnthp_ctl_el2, read_msr(MSR_CNTV_CTL_EL02));
		} else {
			write_msr(cnthp_cval_el2, read_msr(cntv_cval_el0));
			write_msr(cnthp_ctl_el2, read_msr(cntv_ctl_el0));
		}
	}
}

/**
 * Restores the state of per-vCPU peripherals, such as the virtual timer.
 */
void begin_restoring_state(struct vcpu *vcpu)
{
	/*
	 * Clear timer control register before restoring compare value, to avoid
	 * a spurious timer interrupt. This could be a problem if the interrupt
	 * is configured as edge-triggered, as it would then be latched in.
	 */
	if (has_vhe_support()) {
		write_msr(MSR_CNTV_CTL_EL02, 0);
		write_msr(MSR_CNTV_CVAL_EL02,
			  vcpu->regs.peripherals.cntv_cval_el0);
		write_msr(MSR_CNTV_CTL_EL02,
			  vcpu->regs.peripherals.cntv_ctl_el0);
	} else {
		write_msr(cntv_ctl_el0, 0);
		write_msr(cntv_cval_el0, vcpu->regs.peripherals.cntv_cval_el0);
		write_msr(cntv_ctl_el0, vcpu->regs.peripherals.cntv_ctl_el0);
	}

	/*
	 * If we are switching (back) to the primary, disable the EL2 physical
	 * timer which was being used to emulate the EL0 virtual timer, as the
	 * virtual timer is now running for the primary again.
	 */
	if (vcpu->vm->id == HF_PRIMARY_VM_ID) {
		write_msr(cnthp_ctl_el2, 0);
		write_msr(cnthp_cval_el2, 0);
	}
}

/**
 * Invalidate all stage 1 TLB entries on the current (physical) CPU for the
 * current VMID.
 */
static void invalidate_vm_tlb(void)
{
	/*
	 * Ensure that the last VTTBR write has taken effect so we invalidate
	 * the right set of TLB entries.
	 */
	isb();

	__asm__ volatile("tlbi vmalle1");

	/*
	 * Ensure that no instructions are fetched for the VM until after the
	 * TLB invalidation has taken effect.
	 */
	isb();

	/*
	 * Ensure that no data reads or writes for the VM happen until after the
	 * TLB invalidation has taken effect. Non-shareable is enough because
	 * the TLB is local to the CPU.
	 */
	dsb(nsh);
}

/**
 * Invalidates the TLB if a different vCPU is being run than the last vCPU of
 * the same VM which was run on the current pCPU.
 *
 * This is necessary because VMs may (contrary to the architecture
 * specification) use inconsistent ASIDs across vCPUs. c.f. KVM's similar
 * workaround:
 * https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=94d0e5980d6791b9
 */
void maybe_invalidate_tlb(struct vcpu *vcpu)
{
	size_t current_cpu_index = cpu_index(vcpu->cpu);
	ffa_vcpu_index_t new_vcpu_index = vcpu_index(vcpu);

	if (vcpu->vm->arch.last_vcpu_on_cpu[current_cpu_index] !=
	    new_vcpu_index) {
		/*
		 * The vCPU has changed since the last time this VM was run on
		 * this pCPU, so we need to invalidate the TLB.
		 */
		invalidate_vm_tlb();

		/* Record the fact that this vCPU is now running on this CPU. */
		vcpu->vm->arch.last_vcpu_on_cpu[current_cpu_index] =
			new_vcpu_index;
	}
}

noreturn void irq_current_exception_noreturn(uintreg_t elr, uintreg_t spsr)
{
	(void)elr;
	(void)spsr;

	panic("IRQ from current exception level.");
}

noreturn void fiq_current_exception_noreturn(uintreg_t elr, uintreg_t spsr)
{
	(void)elr;
	(void)spsr;

	panic("FIQ from current exception level.");
}

noreturn void serr_current_exception_noreturn(uintreg_t elr, uintreg_t spsr)
{
	(void)elr;
	(void)spsr;

	panic("SError from current exception level.");
}

noreturn void sync_current_exception_noreturn(uintreg_t elr, uintreg_t spsr)
{
	uintreg_t esr = read_msr(esr_el2);
	uintreg_t ec = GET_ESR_EC(esr);

	(void)spsr;

	switch (ec) {
	case EC_DATA_ABORT_SAME_EL:
		if (!(esr & (1U << 10))) { /* Check FnV bit. */
			dlog_error(
				"Data abort: pc=%#x, esr=%#x, ec=%#x, "
				"far=%#x\n",
				elr, esr, ec, read_msr(far_el2));
		} else {
			dlog_error(
				"Data abort: pc=%#x, esr=%#x, ec=%#x, "
				"far=invalid\n",
				elr, esr, ec);
		}

		break;

	default:
		dlog_error(
			"Unknown current sync exception pc=%#x, esr=%#x, "
			"ec=%#x\n",
			elr, esr, ec);
		break;
	}

	panic("EL2 exception");
}

/**
 * Sets or clears the VI bit in the HCR_EL2 register saved in the given
 * arch_regs.
 */
static void set_virtual_irq(struct arch_regs *r, bool enable)
{
	if (enable) {
		r->hcr_el2 |= HCR_EL2_VI;
	} else {
		r->hcr_el2 &= ~HCR_EL2_VI;
	}
}

/**
 * Sets or clears the VI bit in the HCR_EL2 register.
 */
static void set_virtual_irq_current(bool enable)
{
	uintreg_t hcr_el2 = current()->regs.hcr_el2;

	if (enable) {
		hcr_el2 |= HCR_EL2_VI;
	} else {
		hcr_el2 &= ~HCR_EL2_VI;
	}
	current()->regs.hcr_el2 = hcr_el2;
}

/**
 * Sets or clears the VF bit in the HCR_EL2 register saved in the given
 * arch_regs.
 */
static void set_virtual_fiq(struct arch_regs *r, bool enable)
{
	if (enable) {
		r->hcr_el2 |= HCR_EL2_VF;
	} else {
		r->hcr_el2 &= ~HCR_EL2_VF;
	}
}

/**
 * Sets or clears the VF bit in the HCR_EL2 register.
 */
static void set_virtual_fiq_current(bool enable)
{
	uintreg_t hcr_el2 = current()->regs.hcr_el2;

	if (enable) {
		hcr_el2 |= HCR_EL2_VF;
	} else {
		hcr_el2 &= ~HCR_EL2_VF;
	}
	current()->regs.hcr_el2 = hcr_el2;
}

#if SECURE_WORLD == 1

/**
 * Handle special direct messages from SPMD to SPMC. For now related to power
 * management only.
 */
static bool spmd_handler(struct ffa_value *args, struct vcpu *current)
{
	ffa_vm_id_t sender = ffa_sender(*args);
	ffa_vm_id_t receiver = ffa_receiver(*args);
	ffa_vm_id_t current_vm_id = current->vm->id;
	uint32_t fwk_msg = ffa_fwk_msg(*args);
	uint8_t fwk_msg_func_id = fwk_msg & SPMD_FWK_MSG_FUNC_MASK;

	/*
	 * Check if direct message request is originating from the SPMD,
	 * directed to the SPMC and the message is a framework message.
	 */
	if (!(sender == HF_SPMD_VM_ID && receiver == HF_SPMC_VM_ID &&
	      current_vm_id == HF_OTHER_WORLD_ID) ||
	    (fwk_msg & SPMD_FWK_MSG_BIT) == 0) {
		return false;
	}

	switch (fwk_msg_func_id) {
	case SPMD_FWK_MSG_PSCI: {
		switch (args->arg3) {
		case PSCI_CPU_OFF: {
			struct vm *vm = vm_get_first_boot();
			struct vcpu *vcpu =
				vm_get_vcpu(vm, vcpu_index(current));

			/*
			 * TODO: the PM event reached the SPMC. In a later
			 * iteration, the PM event can be passed to the SP by
			 * resuming it.
			 */
			*args = (struct ffa_value){
				.func = FFA_MSG_SEND_DIRECT_RESP_32,
				.arg1 = ((uint64_t)HF_SPMC_VM_ID << 16) |
					HF_SPMD_VM_ID,
				.arg2 = 0U};

			dlog_verbose("%s cpu off notification cpuid %#x\n",
				     __func__, vcpu->cpu->id);
			cpu_off(vcpu->cpu);
			break;
		}
		default:
			dlog_verbose("%s PSCI message not handled %#x\n",
				     __func__, args->arg3);
			return false;
		}
	}
	case SPMD_FWK_MSG_FFA_VERSION_REQ: {
		struct ffa_value ret = api_ffa_version(current, args->arg3);
		*args = (struct ffa_value){
			.func = FFA_MSG_SEND_DIRECT_RESP_32,
			.arg1 = ((uint64_t)HF_SPMC_VM_ID << 16) | HF_SPMD_VM_ID,
			/* Set bit 31 since this is a framework message. */
			.arg2 = SPMD_FWK_MSG_BIT |
				SPMD_FWK_MSG_FFA_VERSION_RESP,
			.arg3 = ret.func};
		break;
	}
	default:
		dlog_verbose("%s message not handled %#x\n", __func__, fwk_msg);
		*args = (struct ffa_value){
			.func = FFA_MSG_SEND_DIRECT_RESP_32,
			.arg1 = ((uint64_t)HF_SPMC_VM_ID << 16) | HF_SPMD_VM_ID,
			/* Set bit 31 since this is a framework message. */
			.arg2 = SPMD_FWK_MSG_BIT | fwk_msg_func_id};
	}

	return true;
}

#endif

/**
 * Checks whether to block an SMC being forwarded from a VM.
 */
static bool smc_is_blocked(const struct vm *vm, uint32_t func)
{
	bool block_by_default = !vm->smc_whitelist.permissive;

	for (size_t i = 0; i < vm->smc_whitelist.smc_count; ++i) {
		if (func == vm->smc_whitelist.smcs[i]) {
			return false;
		}
	}

	dlog_notice("SMC %#010x attempted from VM %#x, blocked=%u\n", func,
		    vm->id, block_by_default);

	/* Access is still allowed in permissive mode. */
	return block_by_default;
}

/**
 * Applies SMC access control according to manifest and forwards the call if
 * access is granted.
 */
static void smc_forwarder(const struct vm *vm, struct ffa_value *args)
{
	struct ffa_value ret;
	uint32_t client_id = vm->id;
	uintreg_t arg7 = args->arg7;

	if (smc_is_blocked(vm, args->func)) {
		args->func = SMCCC_ERROR_UNKNOWN;
		return;
	}

	/*
	 * Set the Client ID but keep the existing Secure OS ID and anything
	 * else (currently unspecified) that the client may have passed in the
	 * upper bits.
	 */
	args->arg7 = client_id | (arg7 & ~CLIENT_ID_MASK);
	ret = smc_forward(args->func, args->arg1, args->arg2, args->arg3,
			  args->arg4, args->arg5, args->arg6, args->arg7);

	/*
	 * Preserve the value passed by the caller, rather than the generated
	 * client_id. Note that this would also overwrite any return value that
	 * may be in x7, but the SMCs that we are forwarding are legacy calls
	 * from before SMCCC 1.2 so won't have more than 4 return values anyway.
	 */
	ret.arg7 = arg7;

	plat_smc_post_forward(*args, &ret);

	*args = ret;
}

/**
 * In the normal world, ffa_handler is always called from the virtual FF-A
 * instance (from a VM in EL1). In the secure world, ffa_handler may be called
 * from the virtual (a secure partition in S-EL1) or physical FF-A instance
 * (from the normal world via EL3). The function returns true when the call is
 * handled. The *next pointer is updated to the next vCPU to run, which might be
 * the 'other world' vCPU if the call originated from the virtual FF-A instance
 * and has to be forwarded down to EL3, or left as is to resume the current
 * vCPU.
 */
static bool ffa_handler(struct ffa_value *args, struct vcpu *current,
			struct vcpu **next)
{
	uint32_t func = args->func;

	/*
	 * NOTE: When adding new methods to this handler update
	 * api_ffa_features accordingly.
	 */
	switch (func) {
	case FFA_VERSION_32:
		*args = api_ffa_version(current, args->arg1);
		return true;
	case FFA_PARTITION_INFO_GET_32: {
		struct ffa_uuid uuid;

		ffa_uuid_init(args->arg1, args->arg2, args->arg3, args->arg4,
			      &uuid);
		*args = api_ffa_partition_info_get(current, &uuid, args->arg5);
		return true;
	}
	case FFA_ID_GET_32:
		*args = api_ffa_id_get(current);
		return true;
	case FFA_SPM_ID_GET_32:
		*args = api_ffa_spm_id_get();
		return true;
	case FFA_FEATURES_32:
		*args = api_ffa_features(args->arg1);
		return true;
	case FFA_RX_RELEASE_32:
		*args = api_ffa_rx_release(ffa_receiver(*args), current, next);
		return true;
	case FFA_RXTX_MAP_64:
		*args = api_ffa_rxtx_map(ipa_init(args->arg1),
					 ipa_init(args->arg2), args->arg3,
					 current);
		return true;
	case FFA_RXTX_UNMAP_32:
		*args = api_ffa_rxtx_unmap(args->arg1, current);
		return true;
	case FFA_RX_ACQUIRE_32:
		*args = api_ffa_rx_acquire(ffa_receiver(*args), current);
		return true;
	case FFA_YIELD_32:
		*args = api_yield(current, next);
		return true;
	case FFA_MSG_SEND_32:
		*args = api_ffa_msg_send(ffa_sender(*args), ffa_receiver(*args),
					 ffa_msg_send_size(*args),
					 ffa_msg_send_attributes(*args),
					 current, next);
		return true;
	case FFA_MSG_SEND2_32:
		*args = api_ffa_msg_send2(ffa_sender(*args),
					  ffa_msg_send2_flags(*args), current);
		return true;
	case FFA_MSG_WAIT_32:
		*args = api_ffa_msg_wait(current, next, args);
		return true;
	case FFA_MSG_POLL_32:
		*args = api_ffa_msg_recv(false, current, next);
		return true;
	case FFA_RUN_32:
		*args = api_ffa_run(ffa_vm_id(*args), ffa_vcpu_index(*args),
				    current, next);
		return true;
	case FFA_MEM_DONATE_32:
	case FFA_MEM_LEND_32:
	case FFA_MEM_SHARE_32:
		*args = api_ffa_mem_send(func, args->arg1, args->arg2,
					 ipa_init(args->arg3), args->arg4,
					 current);
		return true;
	case FFA_MEM_RETRIEVE_REQ_32:
		*args = api_ffa_mem_retrieve_req(args->arg1, args->arg2,
						 ipa_init(args->arg3),
						 args->arg4, current);
		return true;
	case FFA_MEM_RELINQUISH_32:
		*args = api_ffa_mem_relinquish(current);
		return true;
	case FFA_MEM_RECLAIM_32:
		*args = api_ffa_mem_reclaim(
			ffa_assemble_handle(args->arg1, args->arg2), args->arg3,
			current);
		return true;
	case FFA_MEM_FRAG_RX_32:
		*args = api_ffa_mem_frag_rx(ffa_frag_handle(*args), args->arg3,
					    (args->arg4 >> 16) & 0xffff,
					    current);
		return true;
	case FFA_MEM_FRAG_TX_32:
		*args = api_ffa_mem_frag_tx(ffa_frag_handle(*args), args->arg3,
					    (args->arg4 >> 16) & 0xffff,
					    current);
		return true;
	case FFA_MSG_SEND_DIRECT_REQ_64:
	case FFA_MSG_SEND_DIRECT_REQ_32: {
#if SECURE_WORLD == 1
		if (spmd_handler(args, current)) {
			return true;
		}
#endif
		*args = api_ffa_msg_send_direct_req(ffa_sender(*args),
						    ffa_receiver(*args), *args,
						    current, next);
		return true;
	}
	case FFA_MSG_SEND_DIRECT_RESP_64:
	case FFA_MSG_SEND_DIRECT_RESP_32:
		*args = api_ffa_msg_send_direct_resp(ffa_sender(*args),
						     ffa_receiver(*args), *args,
						     current, next);
		return true;
	case FFA_SECONDARY_EP_REGISTER_64:
		/*
		 * DEN0077A FF-A v1.1 Beta0 section 18.3.2.1.1
		 * The callee must return NOT_SUPPORTED if this function is
		 * invoked by a caller that implements version v1.0 of
		 * the Framework.
		 */
		*args = api_ffa_secondary_ep_register(ipa_init(args->arg1),
						      current);
		return true;
	case FFA_NOTIFICATION_BITMAP_CREATE_32:
		*args = api_ffa_notification_bitmap_create(
			(ffa_vm_id_t)args->arg1, (ffa_vcpu_count_t)args->arg2,
			current);
		return true;
	case FFA_NOTIFICATION_BITMAP_DESTROY_32:
		*args = api_ffa_notification_bitmap_destroy(
			(ffa_vm_id_t)args->arg1, current);
		return true;
	case FFA_NOTIFICATION_BIND_32:
		*args = api_ffa_notification_update_bindings(
			ffa_sender(*args), ffa_receiver(*args), args->arg2,
			ffa_notifications_bitmap(args->arg3, args->arg4), true,
			current);
		return true;
	case FFA_NOTIFICATION_UNBIND_32:
		*args = api_ffa_notification_update_bindings(
			ffa_sender(*args), ffa_receiver(*args), 0,
			ffa_notifications_bitmap(args->arg3, args->arg4), false,
			current);
		return true;
	case FFA_MEM_PERM_SET_32:
	case FFA_MEM_PERM_SET_64:
		*args = api_ffa_mem_perm_set(va_init(args->arg1), args->arg2,
					     args->arg3, current);
		return true;
	case FFA_MEM_PERM_GET_32:
	case FFA_MEM_PERM_GET_64:
		*args = api_ffa_mem_perm_get(va_init(args->arg1), current);
		return true;
	case FFA_NOTIFICATION_SET_32:
		*args = api_ffa_notification_set(
			ffa_sender(*args), ffa_receiver(*args), args->arg2,
			ffa_notifications_bitmap(args->arg3, args->arg4),
			current);
		return true;
	case FFA_NOTIFICATION_GET_32:
		*args = api_ffa_notification_get(
			ffa_receiver(*args), ffa_notifications_get_vcpu(*args),
			args->arg2, current);
		return true;
	case FFA_NOTIFICATION_INFO_GET_64:
		*args = api_ffa_notification_info_get(current);
		return true;
	case FFA_INTERRUPT_32:
		*args = plat_ffa_delegate_ffa_interrupt(current, next);
		return true;
	case FFA_CONSOLE_LOG_32:
	case FFA_CONSOLE_LOG_64:
		*args = api_ffa_console_log(*args, current);
		return true;
	}

	return false;
}

/**
 * Set or clear VI/VF bits according to pending interrupts.
 */
static void vcpu_update_virtual_interrupts(struct vcpu *next)
{
	struct vcpu_locked vcpu_locked;

	if (next == NULL) {
		if (current()->vm->el0_partition) {
			return;
		}

		/*
		 * Not switching vCPUs, set the bit for the current vCPU
		 * directly in the register.
		 */
		vcpu_locked = vcpu_lock(current());
		set_virtual_irq_current(
			vcpu_interrupt_irq_count_get(vcpu_locked) > 0);
		set_virtual_fiq_current(
			vcpu_interrupt_fiq_count_get(vcpu_locked) > 0);
		vcpu_unlock(&vcpu_locked);
	} else if (vm_id_is_current_world(next->vm->id)) {
		if (next->vm->el0_partition) {
			return;
		}
		/*
		 * About to switch vCPUs, set the bit for the vCPU to which we
		 * are switching in the saved copy of the register.
		 */

		vcpu_locked = vcpu_lock(next);
		set_virtual_irq(&next->regs,
				vcpu_interrupt_irq_count_get(vcpu_locked) > 0);
		set_virtual_fiq(&next->regs,
				vcpu_interrupt_fiq_count_get(vcpu_locked) > 0);
		vcpu_unlock(&vcpu_locked);
	}
}

/**
 * Handles PSCI and FF-A calls and writes the return value back to the registers
 * of the vCPU. This is shared between smc_handler and hvc_handler.
 *
 * Returns true if the call was handled.
 */
static bool hvc_smc_handler(struct ffa_value args, struct vcpu *vcpu,
			    struct vcpu **next)
{
	/* Do not expect PSCI calls emitted from within the secure world. */
#if SECURE_WORLD == 0
	if (psci_handler(vcpu, args.func, args.arg1, args.arg2, args.arg3,
			 &vcpu->regs.r[0], next)) {
		return true;
	}
#endif

	if (ffa_handler(&args, vcpu, next)) {
#if SECURE_WORLD == 1
		/*
		 * If giving back execution to the NWd, check if the Schedule
		 * Receiver Interrupt has been delayed, and trigger it on
		 * current core if so.
		 */
		if ((*next != NULL && (*next)->vm->id == HF_OTHER_WORLD_ID) ||
		    (*next == NULL && vcpu->vm->id == HF_OTHER_WORLD_ID)) {
			plat_ffa_sri_trigger_if_delayed(vcpu->cpu);
		}
#endif
		arch_regs_set_retval(&vcpu->regs, args);
		vcpu_update_virtual_interrupts(*next);
		return true;
	}

	return false;
}

/**
 * Processes SMC instruction calls.
 */
static struct vcpu *smc_handler(struct vcpu *vcpu)
{
	struct ffa_value args = arch_regs_get_args(&vcpu->regs);
	struct vcpu *next = NULL;

	if (hvc_smc_handler(args, vcpu, &next)) {
		return next;
	}

	switch (args.func & ~SMCCC_CONVENTION_MASK) {
	case HF_DEBUG_LOG:
		vcpu->regs.r[0] = api_debug_log(args.arg1, vcpu);
		return NULL;
	}

	smc_forwarder(vcpu->vm, &args);
	arch_regs_set_retval(&vcpu->regs, args);
	return NULL;
}

#if SECURE_WORLD == 1

/**
 * Called from other_world_loop return from SMC.
 * Processes SMC calls originating from the NWd.
 */
struct vcpu *smc_handler_from_nwd(struct vcpu *vcpu)
{
	struct ffa_value args = arch_regs_get_args(&vcpu->regs);
	struct vcpu *next = NULL;

	if (hvc_smc_handler(args, vcpu, &next)) {
		return next;
	}

	/*
	 * If the SMC emitted by the normal world is not handled in the secure
	 * world then return an error stating such ABI is not supported. Only
	 * FF-A calls are supported. We cannot return SMCCC_ERROR_UNKNOWN
	 * directly because the SPMD smc handler would not recognize it as a
	 * standard FF-A call returning from the SPMC.
	 */
	arch_regs_set_retval(&vcpu->regs, ffa_error(FFA_NOT_SUPPORTED));

	return NULL;
}

#endif

/*
 * Exception vector offsets.
 * See Arm Architecture Reference Manual Armv8-A, D1.10.2.
 */

/**
 * Offset for synchronous exceptions at current EL with SPx.
 */
#define OFFSET_CURRENT_SPX UINT64_C(0x200)

/**
 * Offset for synchronous exceptions at lower EL using AArch64.
 */
#define OFFSET_LOWER_EL_64 UINT64_C(0x400)

/**
 * Offset for synchronous exceptions at lower EL using AArch32.
 */
#define OFFSET_LOWER_EL_32 UINT64_C(0x600)

/**
 * Returns the address for the exception handler at EL1.
 */
static uintreg_t get_el1_exception_handler_addr(const struct vcpu *vcpu)
{
	uintreg_t base_addr = has_vhe_support() ? read_msr(MSR_VBAR_EL12)
						: read_msr(vbar_el1);
	uintreg_t pe_mode = vcpu->regs.spsr & PSR_PE_MODE_MASK;
	bool is_arch32 = vcpu->regs.spsr & PSR_ARCH_MODE_32;

	if (pe_mode == PSR_PE_MODE_EL0T) {
		if (is_arch32) {
			base_addr += OFFSET_LOWER_EL_32;
		} else {
			base_addr += OFFSET_LOWER_EL_64;
		}
	} else {
		CHECK(!is_arch32);
		base_addr += OFFSET_CURRENT_SPX;
	}

	return base_addr;
}

/**
 * Injects an exception with the specified Exception Syndrom Register value into
 * the EL1.
 *
 * NOTE: This function assumes that the lazy registers haven't been saved, and
 * writes to the lazy registers of the CPU directly instead of the vCPU.
 */
static void inject_el1_exception(struct vcpu *vcpu, uintreg_t esr_el1_value,
				 uintreg_t far_el1_value)
{
	uintreg_t handler_address = get_el1_exception_handler_addr(vcpu);

	/* Update the CPU state to inject the exception. */
	if (has_vhe_support()) {
		write_msr(MSR_ESR_EL12, esr_el1_value);
		write_msr(MSR_FAR_EL12, far_el1_value);
		write_msr(MSR_ELR_EL12, vcpu->regs.pc);
		write_msr(MSR_SPSR_EL12, vcpu->regs.spsr);
	} else {
		write_msr(esr_el1, esr_el1_value);
		write_msr(far_el1, far_el1_value);
		write_msr(elr_el1, vcpu->regs.pc);
		write_msr(spsr_el1, vcpu->regs.spsr);
	}

	/*
	 * Mask (disable) interrupts and run in EL1h mode.
	 * EL1h mode is used because by default, taking an exception selects the
	 * stack pointer for the target Exception level. The software can change
	 * that later in the handler if needed.
	 */
	vcpu->regs.spsr = PSR_D | PSR_A | PSR_I | PSR_F | PSR_PE_MODE_EL1H;

	/* Transfer control to the exception hander. */
	vcpu->regs.pc = handler_address;
}

/**
 * Injects a Data Abort exception (same exception level).
 */
static void inject_el1_data_abort_exception(struct vcpu *vcpu,
					    uintreg_t esr_el2,
					    uintreg_t far_el2)
{
	/*
	 *  ISS encoding remains the same, but the EC is changed to reflect
	 *  where the exception came from.
	 *  See Arm Architecture Reference Manual Armv8-A, pages D13-2943/2982.
	 */
	uintreg_t esr_el1_value = GET_ESR_ISS(esr_el2) | GET_ESR_IL(esr_el2) |
				  (EC_DATA_ABORT_SAME_EL << ESR_EC_OFFSET);

	dlog_notice("Injecting Data Abort exception into VM %#x.\n",
		    vcpu->vm->id);

	inject_el1_exception(vcpu, esr_el1_value, far_el2);
}

/**
 * Injects a Data Abort exception (same exception level).
 */
static void inject_el1_instruction_abort_exception(struct vcpu *vcpu,
						   uintreg_t esr_el2,
						   uintreg_t far_el2)
{
	/*
	 *  ISS encoding remains the same, but the EC is changed to reflect
	 *  where the exception came from.
	 *  See Arm Architecture Reference Manual Armv8-A, pages D13-2941/2980.
	 */
	uintreg_t esr_el1_value =
		GET_ESR_ISS(esr_el2) | GET_ESR_IL(esr_el2) |
		(EC_INSTRUCTION_ABORT_SAME_EL << ESR_EC_OFFSET);

	dlog_notice("Injecting Instruction Abort exception into VM %#x.\n",
		    vcpu->vm->id);

	inject_el1_exception(vcpu, esr_el1_value, far_el2);
}

/**
 * Injects an exception with an unknown reason into the EL1.
 */
static void inject_el1_unknown_exception(struct vcpu *vcpu, uintreg_t esr_el2)
{
	uintreg_t esr_el1_value =
		GET_ESR_IL(esr_el2) | (EC_UNKNOWN << ESR_EC_OFFSET);

	/*
	 * The value of the far_el2 register is UNKNOWN in this case,
	 * therefore, don't propagate it to avoid leaking sensitive information.
	 */
	uintreg_t far_el1_value = 0;
	char *direction_str;

	direction_str = ISS_IS_READ(esr_el2) ? "read" : "write";
	dlog_notice(
		"Trapped access to system register %s: op0=%d, op1=%d, crn=%d, "
		"crm=%d, op2=%d, rt=%d.\n",
		direction_str, GET_ISS_OP0(esr_el2), GET_ISS_OP1(esr_el2),
		GET_ISS_CRN(esr_el2), GET_ISS_CRM(esr_el2),
		GET_ISS_OP2(esr_el2), GET_ISS_RT(esr_el2));

	dlog_notice("Injecting Unknown Reason exception into VM %#x.\n",
		    vcpu->vm->id);

	inject_el1_exception(vcpu, esr_el1_value, far_el1_value);
}

static struct vcpu *hvc_handler(struct vcpu *vcpu)
{
	struct ffa_value args = arch_regs_get_args(&vcpu->regs);
	struct vcpu *next = NULL;

	if (hvc_smc_handler(args, vcpu, &next)) {
		return next;
	}

	switch (args.func) {
	case HF_MAILBOX_WRITABLE_GET:
		vcpu->regs.r[0] = api_mailbox_writable_get(vcpu);
		break;

	case HF_MAILBOX_WAITER_GET:
		vcpu->regs.r[0] = api_mailbox_waiter_get(args.arg1, vcpu);
		break;

	case HF_INTERRUPT_ENABLE:
		vcpu->regs.r[0] = api_interrupt_enable(args.arg1, args.arg2,
						       args.arg3, vcpu);
		break;

	case HF_INTERRUPT_GET:
		vcpu->regs.r[0] = api_interrupt_get(vcpu);
		break;

	case HF_INTERRUPT_INJECT:
		vcpu->regs.r[0] = api_interrupt_inject(args.arg1, args.arg2,
						       args.arg3, vcpu, &next);
		break;

	case HF_DEBUG_LOG:
		vcpu->regs.r[0] = api_debug_log(args.arg1, vcpu);
		break;

#if SECURE_WORLD == 1
	case HF_INTERRUPT_DEACTIVATE:
		vcpu->regs.r[0] = plat_ffa_interrupt_deactivate(
			args.arg1, args.arg2, vcpu);
		break;
#endif

	default:
		vcpu->regs.r[0] = SMCCC_ERROR_UNKNOWN;
	}

	vcpu_update_virtual_interrupts(next);

	return next;
}

struct vcpu *irq_lower(void)
{
#if SECURE_WORLD == 1
	struct vcpu *next = NULL;

	plat_ffa_secure_interrupt(current(), &next);

	/*
	 * Since we are in interrupt context, set the bit for the
	 * next vCPU directly in the register.
	 */
	vcpu_update_virtual_interrupts(next);

	return next;
#else
	/*
	 * Switch back to primary VM, interrupts will be handled there.
	 *
	 * If the VM has aborted, this vCPU will be aborted when the scheduler
	 * tries to run it again. This means the interrupt will not be delayed
	 * by the aborted VM.
	 *
	 * TODO: Only switch when the interrupt isn't for the current VM.
	 */
	return api_preempt(current());
#endif
}

struct vcpu *fiq_lower(void)
{
#if SECURE_WORLD == 1
	struct vcpu_locked current_locked;
	struct vcpu *current_vcpu = current();
	int64_t ret;

	if (plat_ffa_vm_managed_exit_supported(current_vcpu->vm)) {
		uint8_t pmr = plat_interrupts_get_priority_mask();

		/* Mask all interrupts */
		plat_interrupts_set_priority_mask(0x0);

		current_locked = vcpu_lock(current_vcpu);
		current_vcpu->priority_mask = pmr;
		ret = api_interrupt_inject_locked(current_locked,
						  HF_MANAGED_EXIT_INTID,
						  current_vcpu, NULL);
		if (ret != 0) {
			panic("Failed to inject managed exit interrupt\n");
		}

		/* Entering managed exit sequence. */
		current_vcpu->processing_managed_exit = true;

		vcpu_unlock(&current_locked);

		/*
		 * Since we are in interrupt context, set the bit for the
		 * current vCPU directly in the register.
		 */
		vcpu_update_virtual_interrupts(NULL);

		/* Resume current vCPU. */
		return NULL;
	}
	/*
	 * SP does not support managed exit. It is pre-empted and execution
	 * handed back to the normal world through the FFA_INTERRUPT ABI. The
	 * api_preempt() call is equivalent to calling api_switch_to_other_world
	 * for current vCPU passing FFA_INTERRUPT. The SP can be resumed later
	 * by FFA_RUN.
	 */
	return api_preempt(current_vcpu);

#else
	return irq_lower();
#endif
}

noreturn struct vcpu *serr_lower(void)
{
	/*
	 * SError exceptions should be isolated and handled by the responsible
	 * VM/exception level. Getting here indicates a bug, that isolation is
	 * not working, or a processor that does not support ARMv8.2-IESB, in
	 * which case Hafnium routes SError exceptions to EL2 (here).
	 */
	panic("SError from a lower exception level.");
}

/**
 * Initialises a fault info structure. It assumes that an FnV bit exists at
 * bit offset 10 of the ESR, and that it is only valid when the bottom 6 bits of
 * the ESR (the fault status code) are 010000; this is the case for both
 * instruction and data aborts, but not necessarily for other exception reasons.
 */
static struct vcpu_fault_info fault_info_init(uintreg_t esr,
					      const struct vcpu *vcpu,
					      uint32_t mode)
{
	uint32_t fsc = esr & 0x3f;
	struct vcpu_fault_info r;
	uint64_t hpfar_el2_val;
	uint64_t hpfar_el2_fipa;

	r.mode = mode;
	r.pc = va_init(vcpu->regs.pc);

	/* Get Hypervisor IPA Fault Address value. */
	hpfar_el2_val = read_msr(hpfar_el2);

	/* Extract Faulting IPA. */
	hpfar_el2_fipa = (hpfar_el2_val & HPFAR_EL2_FIPA) << 8;

#if SECURE_WORLD == 1

	/**
	 * Determine if faulting IPA targets NS space.
	 * At NS-EL2 hpfar_el2 bit 63 is RES0. At S-EL2, this bit determines if
	 * the faulting Stage-1 address output is a secure or non-secure IPA.
	 */
	if ((hpfar_el2_val & HPFAR_EL2_NS) != 0) {
		r.mode |= MM_MODE_NS;
	}

#endif

	/*
	 * Check the FnV bit, which is only valid if dfsc/ifsc is 010000. It
	 * indicates that we cannot rely on far_el2.
	 */
	if (fsc == 0x10 && esr & (1U << 10)) {
		r.vaddr = va_init(0);
		r.ipaddr = ipa_init(hpfar_el2_fipa);
	} else {
		r.vaddr = va_init(read_msr(far_el2));
		r.ipaddr = ipa_init(hpfar_el2_fipa |
				    (read_msr(far_el2) & (PAGE_SIZE - 1)));
	}

	return r;
}

struct vcpu *sync_lower_exception(uintreg_t esr, uintreg_t far)
{
	struct vcpu *vcpu = current();
	struct vcpu_fault_info info;
	struct vcpu *new_vcpu = NULL;
	uintreg_t ec = GET_ESR_EC(esr);
	bool is_el0_partition = vcpu->vm->el0_partition;
	bool resume = false;

	switch (ec) {
	case EC_WFI_WFE:
		/* Skip the instruction. */
		vcpu->regs.pc += GET_NEXT_PC_INC(esr);

		/*
		 * For EL0 partitions, treat both WFI and WFE the same way so
		 * that FFA_RUN can be called on the partition to resume it. If
		 * we treat WFI using api_wait_for_interrupt, the VCPU will be
		 * in blocked waiting for interrupt but we cannot inject
		 * interrupts into EL0 partitions.
		 */
		if (is_el0_partition) {
			api_yield(vcpu, &new_vcpu);
			return new_vcpu;
		}

		/* Check TI bit of ISS, 0 = WFI, 1 = WFE. */
		if (esr & 1) {
			/* WFE */
			/*
			 * TODO: consider giving the scheduler more context,
			 * somehow.
			 */
			api_yield(vcpu, &new_vcpu);
			return new_vcpu;
		}
		/* WFI */
		return api_wait_for_interrupt(vcpu);

	case EC_DATA_ABORT_LOWER_EL:
		info = fault_info_init(
			esr, vcpu, (esr & (1U << 6)) ? MM_MODE_W : MM_MODE_R);

		resume = vcpu_handle_page_fault(vcpu, &info);
		if (is_el0_partition) {
			dlog_warning("Data abort on EL0 partition\n");
			/*
			 * Abort EL0 context if we should not resume the
			 * context, or it is an alignment fault.
			 * vcpu_handle_page_fault() only checks the mode of the
			 * page in an architecture agnostic way but alignment
			 * faults on aarch64 can happen on a correctly mapped
			 * page.
			 */
			if (!resume || ((esr & 0x3f) == 0x21)) {
				return api_abort(vcpu);
			}
		}

		if (resume) {
			return NULL;
		}

		/* Inform the EL1 of the data abort. */
		inject_el1_data_abort_exception(vcpu, esr, far);

		/* Schedule the same VM to continue running. */
		return NULL;

	case EC_INSTRUCTION_ABORT_LOWER_EL:
		info = fault_info_init(esr, vcpu, MM_MODE_X);

		if (vcpu_handle_page_fault(vcpu, &info)) {
			return NULL;
		}

		if (is_el0_partition) {
			dlog_warning("Instruction abort on EL0 partition\n");
			return api_abort(vcpu);
		}

		/* Inform the EL1 of the instruction abort. */
		inject_el1_instruction_abort_exception(vcpu, esr, far);

		/* Schedule the same VM to continue running. */
		return NULL;
	case EC_SVC:
		CHECK(is_el0_partition);
		return hvc_handler(vcpu);
	case EC_HVC:
		if (is_el0_partition) {
			dlog_warning("Unexpected HVC Trap on EL0 partition\n");
			return api_abort(vcpu);
		}
		return hvc_handler(vcpu);

	case EC_SMC: {
		uintreg_t smc_pc = vcpu->regs.pc;
		struct vcpu *next = smc_handler(vcpu);

		/* Skip the SMC instruction. */
		vcpu->regs.pc = smc_pc + GET_NEXT_PC_INC(esr);

		return next;
	}

	case EC_MSR:
		/*
		 * NOTE: This should never be reached because it goes through a
		 * separate path handled by handle_system_register_access().
		 */
		panic("Handled by handle_system_register_access().");

	default:
		dlog_notice(
			"Unknown lower sync exception pc=%#x, esr=%#x, "
			"ec=%#x\n",
			vcpu->regs.pc, esr, ec);
		break;
	}

	if (is_el0_partition) {
		return api_abort(vcpu);
	}

	/*
	 * The exception wasn't handled. Inject to the VM to give it chance to
	 * handle as an unknown exception.
	 */
	inject_el1_unknown_exception(vcpu, esr);

	/* Schedule the same VM to continue running. */
	return NULL;
}

/**
 * Handles EC = 011000, MSR, MRS instruction traps.
 * Returns non-null ONLY if the access failed and the vCPU is changing.
 */
void handle_system_register_access(uintreg_t esr_el2)
{
	struct vcpu *vcpu = current();
	ffa_vm_id_t vm_id = vcpu->vm->id;
	uintreg_t ec = GET_ESR_EC(esr_el2);

	CHECK(ec == EC_MSR);
	/*
	 * Handle accesses to debug and performance monitor registers.
	 * Inject an exception for unhandled/unsupported registers.
	 */
	if (debug_el1_is_register_access(esr_el2)) {
		if (!debug_el1_process_access(vcpu, vm_id, esr_el2)) {
			inject_el1_unknown_exception(vcpu, esr_el2);
			return;
		}
	} else if (perfmon_is_register_access(esr_el2)) {
		if (!perfmon_process_access(vcpu, vm_id, esr_el2)) {
			inject_el1_unknown_exception(vcpu, esr_el2);
			return;
		}
	} else if (feature_id_is_register_access(esr_el2)) {
		if (!feature_id_process_access(vcpu, esr_el2)) {
			inject_el1_unknown_exception(vcpu, esr_el2);
			return;
		}
	} else {
		inject_el1_unknown_exception(vcpu, esr_el2);
		return;
	}

	/* Instruction was fulfilled. Skip it and run the next one. */
	vcpu->regs.pc += GET_NEXT_PC_INC(esr_el2);
}