diff --git a/src/nvme.c b/src/nvme.c index 7d89917b..fee3cee3 100644 --- a/src/nvme.c +++ b/src/nvme.c @@ -401,7 +401,7 @@ out_disable_ctrl: nvme_poll_syslog(); out_shutdown: rtkit_sleep(nvme_rtkit); - pmgr_reset("ANS2"); + pmgr_reset(0, "ANS2"); out_rtkit: rtkit_free(nvme_rtkit); out_sart: @@ -443,7 +443,7 @@ void nvme_shutdown(void) rtkit_sleep(nvme_rtkit); asc_cpu_stop(nvme_asc); - pmgr_reset("ANS2"); + pmgr_reset(0, "ANS2"); rtkit_free(nvme_rtkit); sart_free(nvme_sart); asc_free(nvme_asc); diff --git a/src/pmgr.c b/src/pmgr.c index 9538e328..04585327 100644 --- a/src/pmgr.c +++ b/src/pmgr.c @@ -40,6 +40,7 @@ static int pmgr_initialized = 0; static int pmgr_path[8]; static int pmgr_offset; +static int pmgr_dies; static const u32 *pmgr_ps_regs = NULL; static u32 pmgr_ps_regs_len = 0; @@ -93,17 +94,19 @@ static int pmgr_find_device(u16 id, const struct pmgr_device **device) return -1; } -static uintptr_t pmgr_device_get_addr(const struct pmgr_device *device) +static uintptr_t pmgr_device_get_addr(u8 die, const struct pmgr_device *device) { uintptr_t addr = pmgr_get_psreg(device->psreg_idx); if (addr == 0) return 0; + addr += PMGR_DIE_OFFSET * die; + addr += (device->addr_offset << 3); return addr; } -static int pmgr_set_mode_recursive(u16 id, u8 target_mode, bool recurse) +static int pmgr_set_mode_recursive(u8 die, u16 id, u8 target_mode, bool recurse) { if (!pmgr_initialized) { printf("pmgr: pmgr_set_mode_recursive() called before successful pmgr_init()\n"); @@ -119,7 +122,7 @@ static int pmgr_set_mode_recursive(u16 id, u8 target_mode, bool recurse) return -1; if (!(device->flags & PMGR_FLAG_VIRTUAL)) { - uintptr_t addr = pmgr_device_get_addr(device); + uintptr_t addr = pmgr_device_get_addr(die, device); if (!addr) return -1; if (pmgr_set_mode(addr, target_mode)) @@ -130,7 +133,8 @@ static int pmgr_set_mode_recursive(u16 id, u8 target_mode, bool recurse) for (int i = 0; i < 2; i++) { if (device->parent[i]) { - int ret = pmgr_set_mode_recursive(device->parent[i], target_mode, true); + u16 parent = FIELD_GET(PMGR_DEVICE_ID, device->parent[i]); + int ret = pmgr_set_mode_recursive(die, parent, target_mode, true); if (ret < 0) return ret; } @@ -139,14 +143,18 @@ static int pmgr_set_mode_recursive(u16 id, u8 target_mode, bool recurse) return 0; } -int pmgr_power_enable(u16 id) +int pmgr_power_enable(u32 id) { - return pmgr_set_mode_recursive(id, PMGR_PS_ACTIVE, true); + u16 device = FIELD_GET(PMGR_DEVICE_ID, id); + u8 die = FIELD_GET(PMGR_DIE_ID, id); + return pmgr_set_mode_recursive(die, device, PMGR_PS_ACTIVE, true); } -int pmgr_power_disable(u16 id) +int pmgr_power_disable(u32 id) { - return pmgr_set_mode_recursive(id, PMGR_PS_PWRGATE, false); + u16 device = FIELD_GET(PMGR_DEVICE_ID, id); + u8 die = FIELD_GET(PMGR_DIE_ID, id); + return pmgr_set_mode_recursive(die, device, PMGR_PS_PWRGATE, false); } static int pmgr_adt_find_devices(const char *path, const u32 **devices, u32 *n_devices) @@ -178,7 +186,9 @@ static int pmgr_adt_devices_set_mode(const char *path, u8 target_mode, int recur return -1; for (u32 i = 0; i < n_devices; ++i) { - if (pmgr_set_mode_recursive(devices[i], target_mode, recurse)) + u16 device = FIELD_GET(PMGR_DEVICE_ID, devices[i]); + u8 die = FIELD_GET(PMGR_DIE_ID, devices[i]); + if (pmgr_set_mode_recursive(die, device, target_mode, recurse)) ret = -1; } @@ -196,7 +206,7 @@ int pmgr_adt_power_disable(const char *path) return pmgr_adt_devices_set_mode(path, PMGR_PS_PWRGATE, false); } -int pmgr_reset(const char *name) +int pmgr_reset(int die, const char *name) { const struct pmgr_device *dev = NULL; @@ -212,7 +222,12 @@ int pmgr_reset(const char *name) return -1; } - uintptr_t addr = pmgr_device_get_addr(dev); + if (die < 0 || die > 16) { + printf("pmgr: invalid die id %d for devicece %s\n", die, name); + return -1; + } + + uintptr_t addr = pmgr_device_get_addr(die, dev); u32 reg = read32(addr); if (FIELD_GET(PMGR_PS_ACTUAL, reg) != PMGR_PS_ACTIVE) { @@ -231,6 +246,14 @@ int pmgr_reset(const char *name) int pmgr_init(void) { + int node = adt_path_offset(adt, "/arm-io"); + if (node < 0) { + printf("pmgr: Error getting /arm-io node\n"); + return -1; + } + if (ADT_GETPROP(adt, node, "die-count", &pmgr_dies) < 0) + pmgr_dies = 1; + pmgr_offset = adt_path_offset_trace(adt, "/arm-io/pmgr", pmgr_path); if (pmgr_offset < 0) { printf("pmgr: Error getting /arm-io/pmgr node\n"); @@ -254,49 +277,51 @@ int pmgr_init(void) printf("pmgr: Cleaning up device states...\n"); - for (size_t i = 0; i < pmgr_devices_len; ++i) { - const struct pmgr_device *device = &pmgr_devices[i]; + for (u8 die = 0; die < pmgr_dies; ++die) { + for (size_t i = 0; i < pmgr_devices_len; ++i) { + const struct pmgr_device *device = &pmgr_devices[i]; - if ((device->flags & PMGR_FLAG_VIRTUAL)) - continue; + if ((device->flags & PMGR_FLAG_VIRTUAL)) + continue; - uintptr_t addr = pmgr_device_get_addr(device); - if (!addr) - continue; + uintptr_t addr = pmgr_device_get_addr(die, device); + if (!addr) + continue; - u32 reg = read32(addr); + u32 reg = read32(addr); - if (reg & PMGR_AUTO_ENABLE || FIELD_GET(PMGR_PS_TARGET, reg) == PMGR_PS_ACTIVE) { - for (int j = 0; j < 2; j++) { - if (device->parent[j]) { - const struct pmgr_device *pdevice; - if (pmgr_find_device(device->parent[j], &pdevice)) { - printf("pmgr: Failed to find parent #%d for %s\n", device->parent[j], - device->name); - continue; - } + if (reg & PMGR_AUTO_ENABLE || FIELD_GET(PMGR_PS_TARGET, reg) == PMGR_PS_ACTIVE) { + for (int j = 0; j < 2; j++) { + if (device->parent[j]) { + const struct pmgr_device *pdevice; + if (pmgr_find_device(device->parent[j], &pdevice)) { + printf("pmgr: Failed to find parent #%d for %s\n", device->parent[j], + device->name); + continue; + } - if ((pdevice->flags & PMGR_FLAG_VIRTUAL)) - continue; + if ((pdevice->flags & PMGR_FLAG_VIRTUAL)) + continue; - addr = pmgr_device_get_addr(pdevice); - if (!addr) - continue; + addr = pmgr_device_get_addr(die, pdevice); + if (!addr) + continue; - reg = read32(addr); + reg = read32(addr); - if (!(reg & PMGR_AUTO_ENABLE) && - FIELD_GET(PMGR_PS_TARGET, reg) != PMGR_PS_ACTIVE) { - printf("pmgr: Enabling %s, parent of active device %s\n", pdevice->name, - device->name); - pmgr_set_mode(addr, PMGR_PS_ACTIVE); + if (!(reg & PMGR_AUTO_ENABLE) && + FIELD_GET(PMGR_PS_TARGET, reg) != PMGR_PS_ACTIVE) { + printf("pmgr: Enabling %d.%s, parent of active device %s\n", die, + pdevice->name, device->name); + pmgr_set_mode(addr, PMGR_PS_ACTIVE); + } } } } } } - printf("pmgr: initialized, %d devices found.\n", pmgr_devices_len); + printf("pmgr: initialized, %d devices on %u dies found.\n", pmgr_devices_len, pmgr_dies); return 0; } diff --git a/src/pmgr.h b/src/pmgr.h index c410c655..a86ab7b2 100644 --- a/src/pmgr.h +++ b/src/pmgr.h @@ -5,14 +5,19 @@ #include "types.h" +#define PMGR_DIE_OFFSET 0x2000000000 + +#define PMGR_DEVICE_ID GENMASK(15, 0) +#define PMGR_DIE_ID GENMASK(31, 28) + int pmgr_init(void); -int pmgr_power_enable(u16 id); -int pmgr_power_disable(u16 id); +int pmgr_power_enable(u32 id); +int pmgr_power_disable(u32 id); int pmgr_adt_power_enable(const char *path); int pmgr_adt_power_disable(const char *path); -int pmgr_reset(const char *name); +int pmgr_reset(int die, const char *name); #endif