1/*
2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
3 *
4 * SPDX-License-Identifier: BSD-2-Clause
5 */
6
7#include <sel4vm/guest_vm.h>
8#include <sel4vm/boot.h>
9#include <sel4vm/guest_vm_util.h>
10#include <sel4vm/guest_vcpu_fault.h>
11#include <sel4vm/arch/guest_arm_context.h>
12
13#include <sel4vmmplatsupport/guest_vcpu_util.h>
14#include <sel4vmmplatsupport/arch/guest_boot_init.h>
15
16#include "psci.h"
17#include "smc.h"
18
19static int start_new_vcpu(vm_vcpu_t *vcpu,  uintptr_t entry_address, uintptr_t context_id, int target_cpu)
20{
21    int err;
22    err = vm_assign_vcpu_target(vcpu, target_cpu);
23    if (err) {
24        return -1;
25    }
26    err = vcpu_set_bootargs(vcpu, entry_address, 0, context_id);
27    if (err) {
28        vm_assign_vcpu_target(vcpu, -1);
29        return -1;
30    }
31    err = vcpu_start(vcpu);
32    if (err) {
33        vm_assign_vcpu_target(vcpu, -1);
34        return -1;
35    }
36    return 0;
37}
38
39int handle_psci(vm_vcpu_t *vcpu, seL4_Word fn_number, bool convention)
40{
41    int err;
42    seL4_UserContext regs;
43    err = vm_get_thread_context(vcpu, &regs);
44    if (err) {
45        ZF_LOGE("Failed to get vcpu registers");
46        return -1;
47    }
48    switch (fn_number) {
49    case PSCI_VERSION:
50        smc_set_return_value(&regs, 0x00010000); /* version 1 */
51        break;
52    case PSCI_CPU_ON: {
53        uintptr_t target_cpu = smc_get_arg(&regs, 1);
54        uintptr_t entry_point_address = smc_get_arg(&regs, 2);
55        uintptr_t context_id = smc_get_arg(&regs, 3);
56        vm_vcpu_t *target_vcpu = vm_vcpu_for_target_cpu(vcpu->vm, target_cpu);
57        if (target_vcpu == NULL) {
58            target_vcpu = vm_find_free_unassigned_vcpu(vcpu->vm);
59            if (target_vcpu && start_new_vcpu(target_vcpu, entry_point_address, context_id, target_cpu) == 0) {
60                smc_set_return_value(&regs, PSCI_SUCCESS);
61            } else {
62                smc_set_return_value(&regs, PSCI_INTERNAL_FAILURE);
63            }
64        } else {
65            if (is_vcpu_online(target_vcpu)) {
66                smc_set_return_value(&regs, PSCI_ALREADY_ON);
67            } else {
68                smc_set_return_value(&regs, PSCI_INTERNAL_FAILURE);
69            }
70        }
71
72        break;
73    }
74    case PSCI_MIGRATE_INFO_TYPE:
75        /* trusted OS does not require migration */
76        smc_set_return_value(&regs, 2);
77        break;
78    case PSCI_FEATURES:
79        /* TODO Not sure if required */
80        smc_set_return_value(&regs, PSCI_NOT_SUPPORTED);
81        break;
82    case PSCI_SYSTEM_RESET:
83        smc_set_return_value(&regs, PSCI_SUCCESS);
84        break;
85    default:
86        ZF_LOGE("Unhandled PSCI function id %lu\n", fn_number);
87        return -1;
88    }
89    err = vm_set_thread_context(vcpu, regs);
90    if (err) {
91        ZF_LOGE("Failed to set vcpu context registers");
92        return -1;
93    }
94    advance_vcpu_fault(vcpu);
95    return 0;
96}
97