diff --git a/arch/arm64/kvm/hyp/nvhe/ffa.c b/arch/arm64/kvm/hyp/nvhe/ffa.c index 81083856d9cf..21fea54b1dc7 100644 --- a/arch/arm64/kvm/hyp/nvhe/ffa.c +++ b/arch/arm64/kvm/hyp/nvhe/ffa.c @@ -1100,6 +1100,29 @@ out_unlock: hyp_spin_unlock(&kvm_ffa_hyp_lock); } +static void do_ffa_direct_msg(struct arm_smccc_res *res, + struct kvm_cpu_context *ctxt, + u64 vm_handle) +{ + DECLARE_REG(u32, func_id, ctxt, 0); + DECLARE_REG(u32, endp, ctxt, 1); + DECLARE_REG(u32, msg_flags, ctxt, 2); + DECLARE_REG(u32, w3, ctxt, 3); + DECLARE_REG(u32, w4, ctxt, 4); + DECLARE_REG(u32, w5, ctxt, 5); + DECLARE_REG(u32, w6, ctxt, 6); + DECLARE_REG(u32, w7, ctxt, 7); + + if (FIELD_GET(FFA_SRC_ENDPOINT_MASK, endp) != vm_handle) { + ffa_to_smccc_res(res, FFA_RET_INVALID_PARAMETERS); + return; + } + + arm_smccc_1_1_smc(func_id, endp, msg_flags, w3, + w4, w5, w6, w7, + res); +} + bool kvm_host_ffa_handler(struct kvm_cpu_context *host_ctxt, u32 func_id) { struct arm_smccc_res res; @@ -1163,6 +1186,13 @@ bool kvm_host_ffa_handler(struct kvm_cpu_context *host_ctxt, u32 func_id) ffa_rx_release(&res); hyp_spin_unlock(&kvm_ffa_hyp_lock); goto out_handled; + case FFA_ID_GET: + ffa_to_smccc_res_prop(&res, FFA_RET_SUCCESS, HOST_FFA_ID); + goto out_handled; + case FFA_MSG_SEND_DIRECT_REQ: + case FFA_FN64_MSG_SEND_DIRECT_REQ: + do_ffa_direct_msg(&res, host_ctxt, HOST_FFA_ID); + goto out_handled; } if (ffa_call_supported(func_id)) @@ -1234,6 +1264,10 @@ bool kvm_guest_ffa_handler(struct pkvm_hyp_vcpu *hyp_vcpu, u64 *exit_code) ffa_rx_release(&res); hyp_spin_unlock(&kvm_ffa_hyp_lock); goto out_guest; + case FFA_MSG_SEND_DIRECT_REQ: + case FFA_FN64_MSG_SEND_DIRECT_REQ: + do_ffa_direct_msg(&res, ctxt, hyp_vcpu_to_ffa_handle(hyp_vcpu)); + goto out_guest; default: ret = -EOPNOTSUPP; break; diff --git a/include/linux/arm_ffa.h b/include/linux/arm_ffa.h index 8224a82f2089..a5447bd617fc 100644 --- a/include/linux/arm_ffa.h +++ b/include/linux/arm_ffa.h @@ -259,6 +259,8 @@ bool ffa_partition_check_property(struct ffa_device *dev, u32 property) #define ffa_partition_supports_direct_recv(dev) \ ffa_partition_check_property(dev, FFA_PARTITION_DIRECT_RECV) +#define FFA_SRC_ENDPOINT_MASK GENMASK(31, 16) + /* For use with FFA_MSG_SEND_DIRECT_{REQ,RESP} which pass data via registers */ struct ffa_send_direct_data { unsigned long data0; /* w3/x3 */