1 files changed, 114 insertions, 0 deletions
diff --git a/drivers/power/pmic/stpmic1.c b/drivers/power/pmic/stpmic1.c
index c962160307..65296c5fc3 100644
--- a/drivers/power/pmic/stpmic1.c
+++ b/drivers/power/pmic/stpmic1.c
@@ -15,6 +15,17 @@
 
 #define STPMIC1_NUM_OF_REGS 0x100
 
+#define STPMIC1_NVM_SIZE 8
+#define STPMIC1_NVM_POLL_TIMEOUT 100000
+#define STPMIC1_NVM_START_ADDRESS 0xf8
+
+enum pmic_nvm_op {
+	SHADOW_READ,
+	SHADOW_WRITE,
+	NVM_READ,
+	NVM_WRITE,
+};
+
 #if CONFIG_IS_ENABLED(DM_REGULATOR)
 static const struct pmic_child_info stpmic1_children_info[] = {
 	{ .prefix = "ldo", .driver = "stpmic1_ldo" },
@@ -101,6 +112,109 @@ U_BOOT_DRIVER(pmic_stpmic1) = {
 	.ops = &stpmic1_ops,
 };
 
+#ifndef CONFIG_SPL_BUILD
+static int stpmic1_nvm_rw(u8 addr, u8 *buf, int buf_len, enum pmic_nvm_op op)
+{
+	struct udevice *dev;
+	unsigned long timeout;
+	u8 cmd = STPMIC1_NVM_CMD_READ;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_PMIC,
+					  DM_GET_DRIVER(pmic_stpmic1), &dev);
+	if (ret)
+		/* No PMIC on power discrete board */
+		return -EOPNOTSUPP;
+
+	if (addr < STPMIC1_NVM_START_ADDRESS)
+		return -EACCES;
+
+	if (op == SHADOW_READ)
+		return pmic_read(dev, addr, buf, buf_len);
+
+	if (op == SHADOW_WRITE)
+		return pmic_write(dev, addr, buf, buf_len);
+
+	if (op == NVM_WRITE) {
+		cmd = STPMIC1_NVM_CMD_PROGRAM;
+
+		ret = pmic_write(dev, addr, buf, buf_len);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = pmic_reg_read(dev, STPMIC1_NVM_CR);
+	if (ret < 0)
+		return ret;
+
+	ret = pmic_reg_write(dev, STPMIC1_NVM_CR, ret | cmd);
+	if (ret < 0)
+		return ret;
+
+	timeout = timer_get_us() + STPMIC1_NVM_POLL_TIMEOUT;
+	for (;;) {
+		ret = pmic_reg_read(dev, STPMIC1_NVM_SR);
+		if (ret < 0)
+			return ret;
+
+		if (!(ret & STPMIC1_NVM_BUSY))
+			break;
+
+		if (time_after(timer_get_us(), timeout))
+			break;
+	}
+
+	if (ret & STPMIC1_NVM_BUSY)
+		return -ETIMEDOUT;
+
+	if (op == NVM_READ) {
+		ret = pmic_read(dev, addr, buf, buf_len);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+int stpmic1_shadow_read_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, SHADOW_READ);
+}
+
+int stpmic1_shadow_write_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, SHADOW_WRITE);
+}
+
+int stpmic1_nvm_read_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, NVM_READ);
+}
+
+int stpmic1_nvm_write_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, NVM_WRITE);
+}
+
+int stpmic1_nvm_read_all(u8 *buf, int buf_len)
+{
+	if (buf_len != STPMIC1_NVM_SIZE)
+		return -EINVAL;
+
+	return stpmic1_nvm_rw(STPMIC1_NVM_START_ADDRESS,
+			     buf, buf_len, NVM_READ);
+}
+
+int stpmic1_nvm_write_all(u8 *buf, int buf_len)
+{
+	if (buf_len != STPMIC1_NVM_SIZE)
+		return -EINVAL;
+
+	return stpmic1_nvm_rw(STPMIC1_NVM_START_ADDRESS,
+			     buf, buf_len, NVM_WRITE);
+}
+#endif /* CONFIG_SPL_BUILD */
+
 #ifdef CONFIG_SYSRESET
 static int stpmic1_sysreset_request(struct udevice *dev, enum sysreset_t type)
 {