123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645 |
- #include <errno.h>
- #include <stdio.h>
- #include <string.h>
- #include <libfdt.h>
- #include <arch.h>
- #include <common/debug.h>
- #include <common/fdt_fixup.h>
- #include <common/fdt_wrappers.h>
- #include <drivers/console.h>
- #include <lib/psci/psci.h>
- #include <plat/common/platform.h>
- static int append_psci_compatible(void *fdt, int offs, const char *str)
- {
- return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1);
- }
- #ifdef __aarch64__
- #define PSCI_CPU_SUSPEND_FNID PSCI_CPU_SUSPEND_AARCH64
- #define PSCI_CPU_ON_FNID PSCI_CPU_ON_AARCH64
- #else
- #define PSCI_CPU_SUSPEND_FNID PSCI_CPU_SUSPEND_AARCH32
- #define PSCI_CPU_ON_FNID PSCI_CPU_ON_AARCH32
- #endif
- int dt_add_psci_node(void *fdt)
- {
- int offs;
- if (fdt_path_offset(fdt, "/psci") >= 0) {
- WARN("PSCI Device Tree node already exists!\n");
- return 0;
- }
- offs = fdt_path_offset(fdt, "/");
- if (offs < 0)
- return -1;
- offs = fdt_add_subnode(fdt, offs, "psci");
- if (offs < 0)
- return -1;
- if (append_psci_compatible(fdt, offs, "arm,psci-1.0"))
- return -1;
- if (append_psci_compatible(fdt, offs, "arm,psci-0.2"))
- return -1;
- if (append_psci_compatible(fdt, offs, "arm,psci"))
- return -1;
- if (fdt_setprop_string(fdt, offs, "method", "smc"))
- return -1;
- if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_FNID))
- return -1;
- if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF))
- return -1;
- if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_FNID))
- return -1;
- return 0;
- }
- static int dt_update_one_cpu_node(void *fdt, int offset)
- {
- int offs;
-
- for (offs = fdt_first_subnode(fdt, offset); offs >= 0;
- offs = fdt_next_subnode(fdt, offs)) {
- const char *prop;
- int len;
- int ret;
- prop = fdt_getprop(fdt, offs, "device_type", &len);
- if (prop == NULL)
- continue;
- if ((strcmp(prop, "cpu") != 0) || (len != 4))
- continue;
-
- prop = fdt_getprop(fdt, offs, "enable-method", &len);
- if ((prop != NULL) &&
- (strcmp(prop, "psci") == 0) && (len == 5))
- continue;
- ret = fdt_setprop_string(fdt, offs, "enable-method", "psci");
- if (ret < 0)
- return ret;
-
- return 1;
- }
- if (offs == -FDT_ERR_NOTFOUND)
- return 0;
- return offs;
- }
- int dt_add_psci_cpu_enable_methods(void *fdt)
- {
- int offs, ret;
- do {
- offs = fdt_path_offset(fdt, "/cpus");
- if (offs < 0)
- return offs;
- ret = dt_update_one_cpu_node(fdt, offs);
- } while (ret > 0);
- return ret;
- }
- #define HIGH_BITS(x) ((sizeof(x) > 4) ? ((x) >> 32) : (typeof(x))0)
- int fdt_add_reserved_memory(void *dtb, const char *node_name,
- uintptr_t base, size_t size)
- {
- int offs = fdt_path_offset(dtb, "/reserved-memory");
- int node;
- uint32_t addresses[4];
- int ac, sc;
- unsigned int idx = 0;
- ac = fdt_address_cells(dtb, 0);
- sc = fdt_size_cells(dtb, 0);
- if (offs < 0) {
- offs = fdt_add_subnode(dtb, 0, "reserved-memory");
- if (offs < 0) {
- return offs;
- }
- fdt_setprop_u32(dtb, offs, "#address-cells", ac);
- fdt_setprop_u32(dtb, offs, "#size-cells", sc);
- fdt_setprop(dtb, offs, "ranges", NULL, 0);
- }
-
- fdt_for_each_subnode(node, dtb, offs) {
- uintptr_t c_base;
- size_t c_size;
- int ret;
- ret = fdt_get_reg_props_by_index(dtb, node, 0, &c_base, &c_size);
-
- if (ret != 0) {
- continue;
- }
-
- if (base >= c_base && (base + size) <= (c_base + c_size)) {
- return 0;
- }
- }
- if (ac > 1) {
- addresses[idx] = cpu_to_fdt32(HIGH_BITS(base));
- idx++;
- }
- addresses[idx] = cpu_to_fdt32(base & 0xffffffff);
- idx++;
- if (sc > 1) {
- addresses[idx] = cpu_to_fdt32(HIGH_BITS(size));
- idx++;
- }
- addresses[idx] = cpu_to_fdt32(size & 0xffffffff);
- idx++;
- offs = fdt_add_subnode(dtb, offs, node_name);
- fdt_setprop(dtb, offs, "no-map", NULL, 0);
- fdt_setprop(dtb, offs, "reg", addresses, idx * sizeof(uint32_t));
- return 0;
- }
- static int fdt_add_cpu(void *dtb, int parent, u_register_t mpidr)
- {
- int cpu_offs;
- int err;
- char snode_name[15];
- uint64_t reg_prop;
- reg_prop = mpidr & MPID_MASK & ~MPIDR_MT_MASK;
- snprintf(snode_name, sizeof(snode_name), "cpu@%x",
- (unsigned int)reg_prop);
- cpu_offs = fdt_add_subnode(dtb, parent, snode_name);
- if (cpu_offs < 0) {
- ERROR ("FDT: add subnode \"%s\" failed: %i\n",
- snode_name, cpu_offs);
- return cpu_offs;
- }
- err = fdt_setprop_string(dtb, cpu_offs, "compatible", "arm,armv8");
- if (err < 0) {
- ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
- "compatible", cpu_offs);
- return err;
- }
- err = fdt_setprop_u64(dtb, cpu_offs, "reg", reg_prop);
- if (err < 0) {
- ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
- "reg", cpu_offs);
- return err;
- }
- err = fdt_setprop_string(dtb, cpu_offs, "device_type", "cpu");
- if (err < 0) {
- ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
- "device_type", cpu_offs);
- return err;
- }
- err = fdt_setprop_string(dtb, cpu_offs, "enable-method", "psci");
- if (err < 0) {
- ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
- "enable-method", cpu_offs);
- return err;
- }
- return cpu_offs;
- }
- int fdt_add_cpus_node(void *dtb, unsigned int afflv0,
- unsigned int afflv1, unsigned int afflv2)
- {
- int offs;
- int err;
- unsigned int i, j, k;
- u_register_t mpidr;
- int cpuid;
- if (fdt_path_offset(dtb, "/cpus") >= 0) {
- return -EEXIST;
- }
- offs = fdt_add_subnode(dtb, 0, "cpus");
- if (offs < 0) {
- ERROR ("FDT: add subnode \"cpus\" node to parent node failed");
- return offs;
- }
- err = fdt_setprop_u32(dtb, offs, "#address-cells", 2);
- if (err < 0) {
- ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
- "#address-cells", offs);
- return err;
- }
- err = fdt_setprop_u32(dtb, offs, "#size-cells", 0);
- if (err < 0) {
- ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
- "#size-cells", offs);
- return err;
- }
-
- for (i = afflv2; i > 0U; i--) {
- for (j = afflv1; j > 0U; j--) {
- for (k = afflv0; k > 0U; k--) {
- mpidr = ((i - 1) << MPIDR_AFF2_SHIFT) |
- ((j - 1) << MPIDR_AFF1_SHIFT) |
- ((k - 1) << MPIDR_AFF0_SHIFT) |
- (read_mpidr_el1() & MPIDR_MT_MASK);
- cpuid = plat_core_pos_by_mpidr(mpidr);
- if (cpuid >= 0) {
-
- err = fdt_add_cpu(dtb, offs, mpidr);
- if (err < 0) {
- ERROR ("FDT: %s 0x%08x\n",
- "error adding CPU",
- (uint32_t)mpidr);
- return err;
- }
- }
- }
- }
- }
- return offs;
- }
- int fdt_add_cpu_idle_states(void *dtb, const struct psci_cpu_idle_state *state)
- {
- int cpu_node, cpus_node, idle_states_node, ret;
- uint32_t count, phandle;
- ret = fdt_find_max_phandle(dtb, &phandle);
- phandle++;
- if (ret < 0) {
- return ret;
- }
- cpus_node = fdt_path_offset(dtb, "/cpus");
- if (cpus_node < 0) {
- return cpus_node;
- }
-
- idle_states_node = fdt_add_subnode(dtb, cpus_node, "idle-states");
- if (idle_states_node < 0) {
- return idle_states_node;
- }
- ret = fdt_setprop_string(dtb, idle_states_node, "entry-method", "psci");
- if (ret < 0) {
- return ret;
- }
- for (count = 0U; state->name != NULL; count++, phandle++, state++) {
- int idle_state_node;
- idle_state_node = fdt_add_subnode(dtb, idle_states_node,
- state->name);
- if (idle_state_node < 0) {
- return idle_state_node;
- }
- fdt_setprop_string(dtb, idle_state_node, "compatible",
- "arm,idle-state");
- fdt_setprop_u32(dtb, idle_state_node, "arm,psci-suspend-param",
- state->power_state);
- if (state->local_timer_stop) {
- fdt_setprop_empty(dtb, idle_state_node,
- "local-timer-stop");
- }
- fdt_setprop_u32(dtb, idle_state_node, "entry-latency-us",
- state->entry_latency_us);
- fdt_setprop_u32(dtb, idle_state_node, "exit-latency-us",
- state->exit_latency_us);
- fdt_setprop_u32(dtb, idle_state_node, "min-residency-us",
- state->min_residency_us);
- if (state->wakeup_latency_us) {
- fdt_setprop_u32(dtb, idle_state_node,
- "wakeup-latency-us",
- state->wakeup_latency_us);
- }
- fdt_setprop_u32(dtb, idle_state_node, "phandle", phandle);
- }
- if (count == 0U) {
- return 0;
- }
-
- fdt_for_each_subnode(cpu_node, dtb, cpus_node) {
- const char *device_type;
- fdt32_t *value;
-
- device_type = fdt_getprop(dtb, cpu_node, "device_type", NULL);
- if (device_type == NULL || strcmp(device_type, "cpu") != 0) {
- continue;
- }
-
- ret = fdt_setprop_placeholder(dtb, cpu_node, "cpu-idle-states",
- count * sizeof(phandle),
- (void **)&value);
- if (ret < 0) {
- return ret;
- }
-
- for (uint32_t i = 0U; i < count; ++i) {
- value[i] = cpu_to_fdt32(phandle - count + i);
- }
- }
- return 0;
- }
- int fdt_adjust_gic_redist(void *dtb, unsigned int nr_cores,
- uintptr_t gicr_base, unsigned int gicr_frame_size)
- {
- int offset = fdt_node_offset_by_compatible(dtb, 0, "arm,gic-v3");
- uint64_t reg_64;
- uint32_t reg_32;
- void *val;
- int parent, ret;
- int ac, sc;
- if (offset < 0) {
- return offset;
- }
- parent = fdt_parent_offset(dtb, offset);
- if (parent < 0) {
- return parent;
- }
- ac = fdt_address_cells(dtb, parent);
- sc = fdt_size_cells(dtb, parent);
- if (ac < 0 || sc < 0) {
- return -EINVAL;
- }
- if (gicr_base != INVALID_BASE_ADDR) {
- if (ac == 1) {
- reg_32 = cpu_to_fdt32(gicr_base);
- val = ®_32;
- } else {
- reg_64 = cpu_to_fdt64(gicr_base);
- val = ®_64;
- }
-
- ret = fdt_setprop_inplace_namelen_partial(dtb, offset,
- "reg", 3,
- (ac + sc) * 4,
- val, ac * 4);
- if (ret < 0) {
- return ret;
- }
- }
- if (sc == 1) {
- reg_32 = cpu_to_fdt32(nr_cores * gicr_frame_size);
- val = ®_32;
- } else {
- reg_64 = cpu_to_fdt64(nr_cores * (uint64_t)gicr_frame_size);
- val = ®_64;
- }
-
- return fdt_setprop_inplace_namelen_partial(dtb, offset, "reg", 3,
- (ac + sc + ac) * 4,
- val, sc * 4);
- }
- int fdt_set_mac_address(void *dtb, unsigned int ethernet_idx,
- const uint8_t *mac_addr)
- {
- char eth_alias[12];
- const char *path;
- int node;
- if (ethernet_idx > 9U) {
- return -FDT_ERR_BADVALUE;
- }
- snprintf(eth_alias, sizeof(eth_alias), "ethernet%d", ethernet_idx);
- path = fdt_get_alias(dtb, eth_alias);
- if (path == NULL) {
- return -FDT_ERR_NOTFOUND;
- }
- node = fdt_path_offset(dtb, path);
- if (node < 0) {
- ERROR("Path \"%s\" not found in DT: %d\n", path, node);
- return node;
- }
- return fdt_setprop(dtb, node, "local-mac-address", mac_addr, 6);
- }
|