diff options
| -rw-r--r-- | arch/arm/mach-sunxi/mc_smp.c | 126 |
1 files changed, 86 insertions, 40 deletions
diff --git a/arch/arm/mach-sunxi/mc_smp.c b/arch/arm/mach-sunxi/mc_smp.c index 86e90b038555..c0246ec54a0a 100644 --- a/arch/arm/mach-sunxi/mc_smp.c +++ b/arch/arm/mach-sunxi/mc_smp.c @@ -684,11 +684,69 @@ static int __init sunxi_mc_smp_loopback(void) return ret; } +/* + * This holds any device nodes that we requested resources for, + * so that we may easily release resources in the error path. + */ +struct sunxi_mc_smp_nodes { + struct device_node *prcm_node; + struct device_node *cpucfg_node; + struct device_node *sram_node; +}; + +/* This structure holds SoC-specific bits tied to an enable-method string. */ +struct sunxi_mc_smp_data { + const char *enable_method; + int (*get_smp_nodes)(struct sunxi_mc_smp_nodes *nodes); +}; + +static void __init sunxi_mc_smp_put_nodes(struct sunxi_mc_smp_nodes *nodes) +{ + of_node_put(nodes->prcm_node); + of_node_put(nodes->cpucfg_node); + of_node_put(nodes->sram_node); + memset(nodes, 0, sizeof(*nodes)); +} + +static int __init sun9i_a80_get_smp_nodes(struct sunxi_mc_smp_nodes *nodes) +{ + nodes->prcm_node = of_find_compatible_node(NULL, NULL, + "allwinner,sun9i-a80-prcm"); + if (!nodes->prcm_node) { + pr_err("%s: PRCM not available\n", __func__); + return -ENODEV; + } + + nodes->cpucfg_node = of_find_compatible_node(NULL, NULL, + "allwinner,sun9i-a80-cpucfg"); + if (!nodes->cpucfg_node) { + pr_err("%s: CPUCFG not available\n", __func__); + return -ENODEV; + } + + nodes->sram_node = of_find_compatible_node(NULL, NULL, + "allwinner,sun9i-a80-smp-sram"); + if (!nodes->sram_node) { + pr_err("%s: Secure SRAM not available\n", __func__); + return -ENODEV; + } + + return 0; +} + +static const struct sunxi_mc_smp_data sunxi_mc_smp_data[] __initconst = { + { + .enable_method = "allwinner,sun9i-a80-smp", + .get_smp_nodes = sun9i_a80_get_smp_nodes, + }, +}; + static int __init sunxi_mc_smp_init(void) { - struct device_node *cpucfg_node, *sram_node, *node; + struct sunxi_mc_smp_nodes nodes = { 0 }; + struct device_node *node; struct resource res; - int ret; + int i, ret; /* * Don't bother checking the "cpus" node, as an enable-method @@ -706,8 +764,13 @@ static int __init sunxi_mc_smp_init(void) * callbacks in smp_operations, which we would use if we were to * use CPU_METHOD_OF_DECLARE */ - ret = of_property_match_string(node, "enable-method", - "allwinner,sun9i-a80-smp"); + for (i = 0; i < ARRAY_SIZE(sunxi_mc_smp_data); i++) { + ret = of_property_match_string(node, "enable-method", + sunxi_mc_smp_data[i].enable_method); + if (!ret) + break; + } + of_node_put(node); if (ret) return -ENODEV; @@ -720,51 +783,37 @@ static int __init sunxi_mc_smp_init(void) return -ENODEV; } - node = of_find_compatible_node(NULL, NULL, "allwinner,sun9i-a80-prcm"); - if (!node) { - pr_err("%s: PRCM not available\n", __func__); - return -ENODEV; - } + /* Get needed device tree nodes */ + ret = sunxi_mc_smp_data[i].get_smp_nodes(&nodes); + if (ret) + goto err_put_nodes; /* * Unfortunately we can not request the I/O region for the PRCM. * It is shared with the PRCM clock. */ - prcm_base = of_iomap(node, 0); - of_node_put(node); + prcm_base = of_iomap(nodes.prcm_node, 0); if (!prcm_base) { pr_err("%s: failed to map PRCM registers\n", __func__); - return -ENOMEM; - } - - cpucfg_node = of_find_compatible_node(NULL, NULL, - "allwinner,sun9i-a80-cpucfg"); - if (!cpucfg_node) { - ret = -ENODEV; - pr_err("%s: CPUCFG not available\n", __func__); - goto err_unmap_prcm; + ret = -ENOMEM; + goto err_put_nodes; } - cpucfg_base = of_io_request_and_map(cpucfg_node, 0, "sunxi-mc-smp"); + cpucfg_base = of_io_request_and_map(nodes.cpucfg_node, 0, + "sunxi-mc-smp"); if (IS_ERR(cpucfg_base)) { ret = PTR_ERR(cpucfg_base); pr_err("%s: failed to map CPUCFG registers: %d\n", __func__, ret); - goto err_put_cpucfg_node; - } - - sram_node = of_find_compatible_node(NULL, NULL, - "allwinner,sun9i-a80-smp-sram"); - if (!sram_node) { - ret = -ENODEV; - goto err_unmap_release_cpucfg; + goto err_unmap_prcm; } - sram_b_smp_base = of_io_request_and_map(sram_node, 0, "sunxi-mc-smp"); + sram_b_smp_base = of_io_request_and_map(nodes.sram_node, 0, + "sunxi-mc-smp"); if (IS_ERR(sram_b_smp_base)) { ret = PTR_ERR(sram_b_smp_base); pr_err("%s: failed to map secure SRAM\n", __func__); - goto err_put_sram_node; + goto err_unmap_release_cpucfg; } /* Configure CCI-400 for boot cluster */ @@ -775,9 +824,8 @@ static int __init sunxi_mc_smp_init(void) goto err_unmap_release_secure_sram; } - /* We don't need the CPUCFG and SRAM device nodes anymore */ - of_node_put(cpucfg_node); - of_node_put(sram_node); + /* We don't need the device nodes anymore */ + sunxi_mc_smp_put_nodes(&nodes); /* Set the hardware entry point address */ writel(__pa_symbol(sunxi_mc_smp_secondary_startup), @@ -792,18 +840,16 @@ static int __init sunxi_mc_smp_init(void) err_unmap_release_secure_sram: iounmap(sram_b_smp_base); - of_address_to_resource(sram_node, 0, &res); + of_address_to_resource(nodes.sram_node, 0, &res); release_mem_region(res.start, resource_size(&res)); -err_put_sram_node: - of_node_put(sram_node); err_unmap_release_cpucfg: iounmap(cpucfg_base); - of_address_to_resource(cpucfg_node, 0, &res); + of_address_to_resource(nodes.cpucfg_node, 0, &res); release_mem_region(res.start, resource_size(&res)); -err_put_cpucfg_node: - of_node_put(cpucfg_node); err_unmap_prcm: iounmap(prcm_base); +err_put_nodes: + sunxi_mc_smp_put_nodes(&nodes); return ret; } |