firmware: arm_scmi: Fix SCMI genpd domain probing

[ Upstream commit e0f1a30c ]

When, at probe time, an SCMI communication failure inhibits the capacity
to query power domains states, such domains should be skipped.

Registering partially initialized SCMI power domains with genpd will
causes kernel panic.

 arm-scmi timed out in resp(caller: scmi_power_state_get+0xa4/0xd0)
 scmi-power-domain scmi_dev.2: failed to get state for domain 9
 Unable to handle kernel NULL pointer dereference at virtual address 0000000000000000
 Mem abort info:
   ESR = 0x96000006
   EC = 0x25: DABT (current EL), IL = 32 bits
   SET = 0, FnV = 0
   EA = 0, S1PTW = 0
 Data abort info:
   ISV = 0, ISS = 0x00000006
   CM = 0, WnR = 0
 user pgtable: 4k pages, 48-bit VAs, pgdp=00000009f3691000
 [0000000000000000] pgd=00000009f1ca0003, p4d=00000009f1ca0003, pud=00000009f35ea003, pmd=0000000000000000
 Internal error: Oops: 96000006 [#1] PREEMPT SMP
 CPU: 2 PID: 381 Comm: bash Not tainted 5.8.0-rc1-00011-gebd118c2cca8 #2
 Hardware name: ARM LTD ARM Juno Development Platform/ARM Juno Development Platform, BIOS EDK II Jan  3 2020
 Internal error: Oops: 96000006 [#1] PREEMPT SMP
 pstate: 80000005 (Nzcv daif -PAN -UAO BTYPE=--)
 pc : of_genpd_add_provider_onecell+0x98/0x1f8
 lr : of_genpd_add_provider_onecell+0x48/0x1f8
 Call trace:
  of_genpd_add_provider_onecell+0x98/0x1f8
  scmi_pm_domain_probe+0x174/0x1e8
  scmi_dev_probe+0x90/0xe0
  really_probe+0xe4/0x448
  driver_probe_device+0xfc/0x168
  device_driver_attach+0x7c/0x88
  bind_store+0xe8/0x128
  drv_attr_store+0x2c/0x40
  sysfs_kf_write+0x4c/0x60
  kernfs_fop_write+0x114/0x230
  __vfs_write+0x24/0x50
  vfs_write+0xbc/0x1e0
  ksys_write+0x70/0xf8
  __arm64_sys_write+0x24/0x30
  el0_svc_common.constprop.3+0x94/0x160
  do_el0_svc+0x2c/0x98
  el0_sync_handler+0x148/0x1a8
  el0_sync+0x158/0x180

Do not register any power domain that failed to be queried with genpd.

Fixes: 898216c9 ("firmware: arm_scmi: add device power domain support using genpd")
Link: https://lore.kernel.org/r/20200619220330.12217-1-cristian.marussi@arm.comSigned-off-by: Cristian Marussi <cristian.marussi@arm.com>
Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
Signed-off-by: Sasha Levin <sashal@kernel.org>

drivers/firmware/arm_scmi/scmi_pm_domain.c

@@ -85,7 +85,10 @@ static int scmi_pm_domain_probe(struct scmi_device *sdev)
 	for (i = 0; i < num_domains; i++, scmi_pd++) {
 		u32 state;
 
-		domains[i] = &scmi_pd->genpd;
+		if (handle->power_ops->state_get(handle, i, &state)) {
+			dev_warn(dev, "failed to get state for domain %d\n", i);
+			continue;
+		}
 
 		scmi_pd->domain = i;
 		scmi_pd->handle = handle;
@@ -94,13 +97,10 @@ static int scmi_pm_domain_probe(struct scmi_device *sdev)
 		scmi_pd->genpd.power_off = scmi_pd_power_off;
 		scmi_pd->genpd.power_on = scmi_pd_power_on;
 
-		if (handle->power_ops->state_get(handle, i, &state)) {
-			dev_warn(dev, "failed to get state for domain %d\n", i);
-			continue;
-		}
-
 		pm_genpd_init(&scmi_pd->genpd, NULL,
 			      state == SCMI_POWER_STATE_GENERIC_OFF);
+
+		domains[i] = &scmi_pd->genpd;
 	}
 
 	scmi_pd_data->domains = domains;