diff --git a/src/pmgr.c b/src/pmgr.c index b7d8e67b..cd004fba 100644 --- a/src/pmgr.c +++ b/src/pmgr.c @@ -6,22 +6,28 @@ #include "types.h" #include "utils.h" +#define PMGR_RESET BIT(31) +#define PMGR_AUTO_ENABLE BIT(28) +#define PMGR_PS_AUTO GENMASK(27, 24) +#define PMGR_PARENT_OFF BIT(11) +#define PMGR_DEV_DISABLE BIT(10) +#define PMGR_WAS_CLKGATED BIT(9) +#define PMGR_WAS_PWRGATED BIT(8) +#define PMGR_PS_ACTUAL GENMASK(7, 4) +#define PMGR_PS_TARGET GENMASK(3, 0) + +#define PMGR_PS_ACTIVE 0xf +#define PMGR_PS_CLKGATE 0x4 +#define PMGR_PS_PWRGATE 0x0 + #define PMGR_POLL_TIMEOUT 10000 -#define PMGR_TARGET_MODE_MASK 0x0f -#define PMGR_TARGET_MODE(m) (((m)&0xf)) -#define PMGR_ACTUAL_MODE_MASK 0xf0 -#define PMGR_ACTUAL_MODE(m) (((m)&0xf) << 4) - -#define PMGR_MODE_ENABLE 0xf -#define PMGR_MODE_DISABLE 0 - #define PMGR_FLAG_VIRTUAL 0x10 struct pmgr_device { u32 flags; - u16 parent; - u8 unk1[4]; + u16 parent[2]; + u8 unk1[2]; u8 addr_offset; u8 psreg_idx; u8 unk2[14]; @@ -41,34 +47,6 @@ static u32 pmgr_ps_regs_len = 0; static const struct pmgr_device *pmgr_devices = NULL; static u32 pmgr_devices_len = 0; -int pmgr_init(void) -{ - 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"); - return -1; - } - - pmgr_ps_regs = adt_getprop(adt, pmgr_offset, "ps-regs", &pmgr_ps_regs_len); - if (pmgr_ps_regs == NULL || pmgr_ps_regs_len == 0) { - printf("pmgr: Error getting /arm-io/pmgr ps-regs\n."); - return -1; - } - - pmgr_devices = adt_getprop(adt, pmgr_offset, "devices", &pmgr_devices_len); - if (pmgr_devices == NULL || pmgr_devices_len == 0) { - printf("pmgr: Error getting /arm-io/pmgr devices.\n"); - return -1; - } - - pmgr_devices_len /= sizeof(*pmgr_devices); - pmgr_initialized = 1; - - printf("pmgr: initialized, %d devices found.\n", pmgr_devices_len); - - return 0; -} - static uintptr_t pmgr_get_psreg(u8 idx) { if (idx * 12 >= pmgr_ps_regs_len) { @@ -90,8 +68,9 @@ static uintptr_t pmgr_get_psreg(u8 idx) static int pmgr_set_mode(uintptr_t addr, u8 target_mode) { - mask32(addr, PMGR_TARGET_MODE_MASK, PMGR_TARGET_MODE(target_mode)); - if (poll32(addr, PMGR_ACTUAL_MODE_MASK, PMGR_ACTUAL_MODE(target_mode), PMGR_POLL_TIMEOUT) < 0) { + mask32(addr, PMGR_PS_TARGET, FIELD_PREP(PMGR_PS_TARGET, target_mode)); + if (poll32(addr, PMGR_PS_ACTUAL, FIELD_PREP(PMGR_PS_ACTUAL, target_mode), PMGR_POLL_TIMEOUT) < + 0) { printf("pmgr: timeout while trying to set mode %x for device at 0x%lx: %x\n", target_mode, addr, read32(addr)); return -1; @@ -124,7 +103,7 @@ static uintptr_t pmgr_device_get_addr(const struct pmgr_device *device) return addr; } -static int pmgr_set_mode_recursive(u16 id, u8 target_mode, int recurse) +static int pmgr_set_mode_recursive(u16 id, u8 target_mode, bool recurse) { if (!pmgr_initialized) { printf("pmgr: pmgr_set_mode_recursive() called before successful pmgr_init()\n"); @@ -134,24 +113,27 @@ static int pmgr_set_mode_recursive(u16 id, u8 target_mode, int recurse) if (id == 0) return -1; - while (id != 0) { - const struct pmgr_device *device; + const struct pmgr_device *device; - if (pmgr_find_device(id, &device)) + if (pmgr_find_device(id, &device)) + return -1; + + if (!(device->flags & PMGR_FLAG_VIRTUAL)) { + uintptr_t addr = pmgr_device_get_addr(device); + if (!addr) return -1; + if (pmgr_set_mode(addr, target_mode)) + return -1; + } + if (!recurse) + return 0; - if (!(device->flags & PMGR_FLAG_VIRTUAL)) { - uintptr_t addr = pmgr_device_get_addr(device); - if (!addr) - return -1; - if (pmgr_set_mode(addr, target_mode)) - return -1; - - if (!recurse) - return 0; + for (int i = 0; i < 2; i++) { + if (device->parent[i]) { + int ret = pmgr_set_mode_recursive(device->parent[i], target_mode, true); + if (ret < 0) + return ret; } - - id = device->parent; } return 0; @@ -159,12 +141,12 @@ static int pmgr_set_mode_recursive(u16 id, u8 target_mode, int recurse) int pmgr_clock_enable(u16 id) { - return pmgr_set_mode_recursive(id, PMGR_MODE_ENABLE, 1); + return pmgr_set_mode_recursive(id, PMGR_PS_ACTIVE, true); } int pmgr_clock_disable(u16 id) { - return pmgr_set_mode_recursive(id, PMGR_MODE_DISABLE, 0); + return pmgr_set_mode_recursive(id, PMGR_PS_PWRGATE, false); } static int pmgr_adt_find_clocks(const char *path, const u32 **clocks, u32 *n_clocks) @@ -205,11 +187,79 @@ static int pmgr_adt_clocks_set_mode(const char *path, u8 target_mode, int recurs int pmgr_adt_clocks_enable(const char *path) { - int ret = pmgr_adt_clocks_set_mode(path, PMGR_MODE_ENABLE, 1); + int ret = pmgr_adt_clocks_set_mode(path, PMGR_PS_ACTIVE, true); return ret; } int pmgr_adt_clocks_disable(const char *path) { - return pmgr_adt_clocks_set_mode(path, PMGR_MODE_DISABLE, 0); + return pmgr_adt_clocks_set_mode(path, PMGR_PS_PWRGATE, false); +} + +int pmgr_init(void) +{ + 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"); + return -1; + } + + pmgr_ps_regs = adt_getprop(adt, pmgr_offset, "ps-regs", &pmgr_ps_regs_len); + if (pmgr_ps_regs == NULL || pmgr_ps_regs_len == 0) { + printf("pmgr: Error getting /arm-io/pmgr ps-regs\n."); + return -1; + } + + pmgr_devices = adt_getprop(adt, pmgr_offset, "devices", &pmgr_devices_len); + if (pmgr_devices == NULL || pmgr_devices_len == 0) { + printf("pmgr: Error getting /arm-io/pmgr devices.\n"); + return -1; + } + + pmgr_devices_len /= sizeof(*pmgr_devices); + pmgr_initialized = 1; + + 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]; + + if ((device->flags & PMGR_FLAG_VIRTUAL)) + continue; + + uintptr_t addr = pmgr_device_get_addr(device); + if (!addr) + continue; + + 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; + } + addr = pmgr_device_get_addr(pdevice); + if (!addr) + continue; + + 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); + } + } + } + } + } + + printf("pmgr: initialized, %d devices found.\n", pmgr_devices_len); + + return 0; }