1 files changed, 136 insertions, 169 deletions

diff --git a/drivers/firmware/arm_sdei.c b/drivers/firmware/arm_sdei.c
index b4b9ce97f415..840754dcc6ca 100644
--- a/drivers/firmware/arm_sdei.c
+++ b/drivers/firmware/arm_sdei.c
@@ -78,11 +78,26 @@ struct sdei_crosscall_args {
 	int first_error;
 };
 
-#define CROSSCALL_INIT(arg, event)	(arg.event = event, \
-					 arg.first_error = 0, \
-					 atomic_set(&arg.errors, 0))
+#define CROSSCALL_INIT(arg, event)		\
+	do {					\
+		arg.event = event;		\
+		arg.first_error = 0;		\
+		atomic_set(&arg.errors, 0);	\
+	} while (0)
+
+static inline int sdei_do_local_call(smp_call_func_t fn,
+				     struct sdei_event *event)
+{
+	struct sdei_crosscall_args arg;
+
+	CROSSCALL_INIT(arg, event);
+	fn(&arg);
 
-static inline int sdei_do_cross_call(void *fn, struct sdei_event * event)
+	return arg.first_error;
+}
+
+static inline int sdei_do_cross_call(smp_call_func_t fn,
+				     struct sdei_event *event)
 {
 	struct sdei_crosscall_args arg;
 
@@ -114,26 +129,7 @@ static int sdei_to_linux_errno(unsigned long sdei_err)
 		return -ENOMEM;
 	}
 
-	/* Not an error value ... */
-	return sdei_err;
-}
-
-/*
- * If x0 is any of these values, then the call failed, use sdei_to_linux_errno()
- * to translate.
- */
-static int sdei_is_err(struct arm_smccc_res *res)
-{
-	switch (res->a0) {
-	case SDEI_NOT_SUPPORTED:
-	case SDEI_INVALID_PARAMETERS:
-	case SDEI_DENIED:
-	case SDEI_PENDING:
-	case SDEI_OUT_OF_RESOURCE:
-		return true;
-	}
-
-	return false;
+	return 0;
 }
 
 static int invoke_sdei_fn(unsigned long function_id, unsigned long arg0,
@@ -141,14 +137,13 @@ static int invoke_sdei_fn(unsigned long function_id, unsigned long arg0,
 			  unsigned long arg3, unsigned long arg4,
 			  u64 *result)
 {
-	int err = 0;
+	int err;
 	struct arm_smccc_res res;
 
 	if (sdei_firmware_call) {
 		sdei_firmware_call(function_id, arg0, arg1, arg2, arg3, arg4,
 				   &res);
-		if (sdei_is_err(&res))
-			err = sdei_to_linux_errno(res.a0);
+		err = sdei_to_linux_errno(res.a0);
 	} else {
 		/*
 		 * !sdei_firmware_call means we failed to probe or called
@@ -210,36 +205,34 @@ static struct sdei_event *sdei_event_create(u32 event_num,
 	lockdep_assert_held(&sdei_events_lock);
 
 	event = kzalloc(sizeof(*event), GFP_KERNEL);
-	if (!event)
-		return ERR_PTR(-ENOMEM);
+	if (!event) {
+		err = -ENOMEM;
+		goto fail;
+	}
 
 	INIT_LIST_HEAD(&event->list);
 	event->event_num = event_num;
 
 	err = sdei_api_event_get_info(event_num, SDEI_EVENT_INFO_EV_PRIORITY,
 				      &result);
-	if (err) {
-		kfree(event);
-		return ERR_PTR(err);
-	}
+	if (err)
+		goto fail;
 	event->priority = result;
 
 	err = sdei_api_event_get_info(event_num, SDEI_EVENT_INFO_EV_TYPE,
 				      &result);
-	if (err) {
-		kfree(event);
-		return ERR_PTR(err);
-	}
+	if (err)
+		goto fail;
 	event->type = result;
 
 	if (event->type == SDEI_EVENT_TYPE_SHARED) {
 		reg = kzalloc(sizeof(*reg), GFP_KERNEL);
 		if (!reg) {
-			kfree(event);
-			return ERR_PTR(-ENOMEM);
+			err = -ENOMEM;
+			goto fail;
 		}
 
-		reg->event_num = event_num;
+		reg->event_num = event->event_num;
 		reg->priority = event->priority;
 
 		reg->callback = cb;
@@ -251,8 +244,8 @@ static struct sdei_event *sdei_event_create(u32 event_num,
 
 		regs = alloc_percpu(struct sdei_registered_event);
 		if (!regs) {
-			kfree(event);
-			return ERR_PTR(-ENOMEM);
+			err = -ENOMEM;
+			goto fail;
 		}
 
 		for_each_possible_cpu(cpu) {
@@ -272,6 +265,10 @@ static struct sdei_event *sdei_event_create(u32 event_num,
 	spin_unlock(&sdei_list_lock);
 
 	return event;
+
+fail:
+	kfree(event);
+	return ERR_PTR(err);
 }
 
 static void sdei_event_destroy_llocked(struct sdei_event *event)
@@ -490,16 +487,6 @@ static void _local_event_unregister(void *data)
 	sdei_cross_call_return(arg, err);
 }
 
-static int _sdei_event_unregister(struct sdei_event *event)
-{
-	lockdep_assert_held(&sdei_events_lock);
-
-	if (event->type == SDEI_EVENT_TYPE_SHARED)
-		return sdei_api_event_unregister(event->event_num);
-
-	return sdei_do_cross_call(_local_event_unregister, event);
-}
-
 int sdei_event_unregister(u32 event_num)
 {
 	int err;
@@ -509,24 +496,27 @@ int sdei_event_unregister(u32 event_num)
 
 	mutex_lock(&sdei_events_lock);
 	event = sdei_event_find(event_num);
-	do {
-		if (!event) {
-			pr_warn("Event %u not registered\n", event_num);
-			err = -ENOENT;
-			break;
-		}
+	if (!event) {
+		pr_warn("Event %u not registered\n", event_num);
+		err = -ENOENT;
+		goto unlock;
+	}
 
-		spin_lock(&sdei_list_lock);
-		event->reregister = false;
-		event->reenable = false;
-		spin_unlock(&sdei_list_lock);
+	spin_lock(&sdei_list_lock);
+	event->reregister = false;
+	event->reenable = false;
+	spin_unlock(&sdei_list_lock);
 
-		err = _sdei_event_unregister(event);
-		if (err)
-			break;
+	if (event->type == SDEI_EVENT_TYPE_SHARED)
+		err = sdei_api_event_unregister(event->event_num);
+	else
+		err = sdei_do_cross_call(_local_event_unregister, event);
 
-		sdei_event_destroy(event);
-	} while (0);
+	if (err)
+		goto unlock;
+
+	sdei_event_destroy(event);
+unlock:
 	mutex_unlock(&sdei_events_lock);
 
 	return err;
@@ -547,7 +537,7 @@ static int sdei_unregister_shared(void)
 		if (event->type != SDEI_EVENT_TYPE_SHARED)
 			continue;
 
-		err = _sdei_event_unregister(event);
+		err = sdei_api_event_unregister(event->event_num);
 		if (err)
 			break;
 	}
@@ -581,25 +571,6 @@ static void _local_event_register(void *data)
 	sdei_cross_call_return(arg, err);
 }
 
-static int _sdei_event_register(struct sdei_event *event)
-{
-	int err;
-
-	lockdep_assert_held(&sdei_events_lock);
-
-	if (event->type == SDEI_EVENT_TYPE_SHARED)
-		return sdei_api_event_register(event->event_num,
-					       sdei_entry_point,
-					       event->registered,
-					       SDEI_EVENT_REGISTER_RM_ANY, 0);
-
-	err = sdei_do_cross_call(_local_event_register, event);
-	if (err)
-		sdei_do_cross_call(_local_event_unregister, event);
-
-	return err;
-}
-
 int sdei_event_register(u32 event_num, sdei_event_callback *cb, void *arg)
 {
 	int err;
@@ -608,63 +579,44 @@ int sdei_event_register(u32 event_num, sdei_event_callback *cb, void *arg)
 	WARN_ON(in_nmi());
 
 	mutex_lock(&sdei_events_lock);
-	do {
-		if (sdei_event_find(event_num)) {
-			pr_warn("Event %u already registered\n", event_num);
-			err = -EBUSY;
-			break;
-		}
-
-		event = sdei_event_create(event_num, cb, arg);
-		if (IS_ERR(event)) {
-			err = PTR_ERR(event);
-			pr_warn("Failed to create event %u: %d\n", event_num,
-				err);
-			break;
-		}
-
-		cpus_read_lock();
-		err = _sdei_event_register(event);
-		if (err) {
-			sdei_event_destroy(event);
-			pr_warn("Failed to register event %u: %d\n", event_num,
-				err);
-		} else {
-			spin_lock(&sdei_list_lock);
-			event->reregister = true;
-			spin_unlock(&sdei_list_lock);
-		}
-		cpus_read_unlock();
-	} while (0);
-	mutex_unlock(&sdei_events_lock);
-
-	return err;
-}
-
-static int sdei_reregister_event_llocked(struct sdei_event *event)
-{
-	int err;
-
-	lockdep_assert_held(&sdei_events_lock);
-	lockdep_assert_held(&sdei_list_lock);
+	if (sdei_event_find(event_num)) {
+		pr_warn("Event %u already registered\n", event_num);
+		err = -EBUSY;
+		goto unlock;
+	}
 
-	err = _sdei_event_register(event);
-	if (err) {
-		pr_err("Failed to re-register event %u\n", event->event_num);
-		sdei_event_destroy_llocked(event);
-		return err;
+	event = sdei_event_create(event_num, cb, arg);
+	if (IS_ERR(event)) {
+		err = PTR_ERR(event);
+		pr_warn("Failed to create event %u: %d\n", event_num, err);
+		goto unlock;
 	}
 
-	if (event->reenable) {
-		if (event->type == SDEI_EVENT_TYPE_SHARED)
-			err = sdei_api_event_enable(event->event_num);
-		else
-			err = sdei_do_cross_call(_local_event_enable, event);
+	cpus_read_lock();
+	if (event->type == SDEI_EVENT_TYPE_SHARED) {
+		err = sdei_api_event_register(event->event_num,
+					      sdei_entry_point,
+					      event->registered,
+					      SDEI_EVENT_REGISTER_RM_ANY, 0);
+	} else {
+		err = sdei_do_cross_call(_local_event_register, event);
+		if (err)
+			sdei_do_cross_call(_local_event_unregister, event);
 	}
 
-	if (err)
-		pr_err("Failed to re-enable event %u\n", event->event_num);
+	if (err) {
+		sdei_event_destroy(event);
+		pr_warn("Failed to register event %u: %d\n", event_num, err);
+		goto cpu_unlock;
+	}
 
+	spin_lock(&sdei_list_lock);
+	event->reregister = true;
+	spin_unlock(&sdei_list_lock);
+cpu_unlock:
+	cpus_read_unlock();
+unlock:
+	mutex_unlock(&sdei_events_lock);
 	return err;
 }
 
@@ -680,9 +632,24 @@ static int sdei_reregister_shared(void)
 			continue;
 
 		if (event->reregister) {
-			err = sdei_reregister_event_llocked(event);
-			if (err)
+			err = sdei_api_event_register(event->event_num,
+					sdei_entry_point, event->registered,
+					SDEI_EVENT_REGISTER_RM_ANY, 0);
+			if (err) {
+				pr_err("Failed to re-register event %u\n",
+				       event->event_num);
+				sdei_event_destroy_llocked(event);
 				break;
+			}
+		}
+
+		if (event->reenable) {
+			err = sdei_api_event_enable(event->event_num);
+			if (err) {
+				pr_err("Failed to re-enable event %u\n",
+				       event->event_num);
+				break;
+			}
 		}
 	}
 	spin_unlock(&sdei_list_lock);
@@ -694,7 +661,7 @@ static int sdei_reregister_shared(void)
 static int sdei_cpuhp_down(unsigned int cpu)
 {
 	struct sdei_event *event;
-	struct sdei_crosscall_args arg;
+	int err;
 
 	/* un-register private events */
 	spin_lock(&sdei_list_lock);
@@ -702,12 +669,11 @@ static int sdei_cpuhp_down(unsigned int cpu)
 		if (event->type == SDEI_EVENT_TYPE_SHARED)
 			continue;
 
-		CROSSCALL_INIT(arg, event);
-		/* call the cross-call function locally... */
-		_local_event_unregister(&arg);
-		if (arg.first_error)
+		err = sdei_do_local_call(_local_event_unregister, event);
+		if (err) {
 			pr_err("Failed to unregister event %u: %d\n",
-			       event->event_num, arg.first_error);
+			       event->event_num, err);
+		}
 	}
 	spin_unlock(&sdei_list_lock);
 
@@ -717,7 +683,7 @@ static int sdei_cpuhp_down(unsigned int cpu)
 static int sdei_cpuhp_up(unsigned int cpu)
 {
 	struct sdei_event *event;
-	struct sdei_crosscall_args arg;
+	int err;
 
 	/* re-register/enable private events */
 	spin_lock(&sdei_list_lock);
@@ -726,20 +692,19 @@ static int sdei_cpuhp_up(unsigned int cpu)
 			continue;
 
 		if (event->reregister) {
-			CROSSCALL_INIT(arg, event);
-			/* call the cross-call function locally... */
-			_local_event_register(&arg);
-			if (arg.first_error)
+			err = sdei_do_local_call(_local_event_register, event);
+			if (err) {
 				pr_err("Failed to re-register event %u: %d\n",
-				       event->event_num, arg.first_error);
+				       event->event_num, err);
+			}
 		}
 
 		if (event->reenable) {
-			CROSSCALL_INIT(arg, event);
-			_local_event_enable(&arg);
-			if (arg.first_error)
+			err = sdei_do_local_call(_local_event_enable, event);
+			if (err) {
 				pr_err("Failed to re-enable event %u: %d\n",
-				       event->event_num, arg.first_error);
+				       event->event_num, err);
+			}
 		}
 	}
 	spin_unlock(&sdei_list_lock);
@@ -976,7 +941,7 @@ static int sdei_get_conduit(struct platform_device *pdev)
 		}
 
 		pr_warn("invalid \"method\" property: %s\n", method);
-	} else if (IS_ENABLED(CONFIG_ACPI) && !acpi_disabled) {
+	} else if (!acpi_disabled) {
 		if (acpi_psci_use_hvc()) {
 			sdei_firmware_call = &sdei_smccc_hvc;
 			return SMCCC_CONDUIT_HVC;
@@ -1000,8 +965,6 @@ static int sdei_probe(struct platform_device *pdev)
 		return 0;
 
 	err = sdei_api_get_version(&ver);
-	if (err == -EOPNOTSUPP)
-		pr_err("advertised but not implemented in platform firmware\n");
 	if (err) {
 		pr_err("Failed to get SDEI version: %d\n", err);
 		sdei_mark_interface_broken();
@@ -1099,16 +1062,20 @@ static bool __init sdei_present_acpi(void)
 
 static int __init sdei_init(void)
 {
-	int ret = platform_driver_register(&sdei_driver);
-
-	if (!ret && sdei_present_acpi()) {
-		struct platform_device *pdev;
-
-		pdev = platform_device_register_simple(sdei_driver.driver.name,
-						       0, NULL, 0);
-		if (IS_ERR(pdev))
-			pr_info("Failed to register ACPI:SDEI platform device %ld\n",
-				PTR_ERR(pdev));
+	struct platform_device *pdev;
+	int ret;
+
+	ret = platform_driver_register(&sdei_driver);
+	if (ret || !sdei_present_acpi())
+		return ret;
+
+	pdev = platform_device_register_simple(sdei_driver.driver.name,
+					       0, NULL, 0);
+	if (IS_ERR(pdev)) {
+		ret = PTR_ERR(pdev);
+		platform_driver_unregister(&sdei_driver);
+		pr_info("Failed to register ACPI:SDEI platform device %d\n",
+			ret);
 	}
 
 	return ret;