/* Write to the VOUT or one of the VOUT limit / margin registers */
static int set_vxx_reg_to_pmbus(DEVICE_HANDLE_T dev, u8 cmd, u16 value)
{
	int ret;
	/* The data to be sent with the PMBus command PAGE_PLUS_WRITE */
	u8 buffer[5] = { 0x04, PWM_CHANNEL_ALL, cmd, 0, 0 };
	buffer[3] = value & 0xFF;
	buffer[4] = (value & 0xFF00) >> 8;

	/* Write the desired voltage code to the regulator */
	ret = I2C_WRITE(dev, PMBUS_CMD_PAGE_PLUS_WRITE, (void *)&buffer[0],
			sizeof(buffer));
	if (ret) {
		printf("VID: I2C failed to write to the voltage regulator\n");
		return -1;
	}
	return 0;
}
/* VOUT high margin definition */
static int set_vout_margin_high(DEVICE_HANDLE_T dev, u16 value)
{
	return set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_MARGIN_HIGH, value);
}
/* VOUT low margin definition */
static int set_vout_margin_low(DEVICE_HANDLE_T dev, u16 value)
{
	return set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_MARGIN_LOW, value);
}
/* Over-voltage fault limit definition */
static int set_vout_ov_fault_limit(DEVICE_HANDLE_T dev, u16 value)
{
	return set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_OV_FAULT_LIMIT, value);
}
/* Under-voltage fault limit definition */
static int set_vout_uv_fault_limit(DEVICE_HANDLE_T dev, u16 value)
{
	return set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_UV_FAULT_LIMIT, value);
}
/* Over-voltage warning limit definition */
static int set_vout_ov_warn_limit(DEVICE_HANDLE_T dev, u16 value)
{
	return set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_OV_WARN_LIMIT, value);
}
/* Under-voltage warning limit definition */
static int set_vout_uv_warn_limit(DEVICE_HANDLE_T dev, u16 value)
{
	return set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_UV_WARN_LIMIT, value);
}

/* Set target output voltage and wait with timeout for measured output voltage
   to match target voltage  */
static int set_and_wait_vout(DEVICE_HANDLE_T dev, int i2caddress, u16 value, int vdd_target)
{
	int count = MAX_LOOP_WAIT_NEW_VOL, temp = 0, ret, vdd_last;

	ret = set_vxx_reg_to_pmbus(dev, PMBUS_CMD_VOUT_COMMAND, value);
	if (0 != ret)
		return ret;

	/* Wait for the voltage to get to the desired value */
	do {
		vdd_last = read_voltage_from_pmbus(i2caddress);
		if (vdd_last < 0) {
			printf("VID: Couldn't read sensor abort VID adjust\n");
			return -1;
		}
		count--;
		temp = vdd_last - vdd_target;
	} while ((abs(temp) > 2)  && (count > 0));

	return vdd_last;
}


/* Wait with timeout (in milliseconds) while chip is busy or calculations are pending */
static int wait_chip_busy(DEVICE_HANDLE_T dev, int ms_tout)
{
	u8 mfr_common;
	int ret;
	for (int i=0; i<ms_tout; i++)
	{
		udelay(1000);
	    ret = I2C_READ(dev, PMBUS_CMD_MFR_COMMON, &mfr_common, sizeof(mfr_common));
		if ( (0 == ret) && (LTC7132_BUSY_MASK == (mfr_common & LTC7132_BUSY_MASK)) )
			return 0;
	}
	return -1;
}

static int set_voltage_to_pmbus(int i2caddress, int vdd)
{
	int ret, vdd_last, vdd_target = vdd;
	int vlow_margin, vhigh_margin, vuv_fault, vov_fault, vuv_warn, vov_warn;
	int multiplier;
	unsigned char value;
	DEVICE_HANDLE_T dev;

	/* Open device handle */
	ret = vid_get_device(i2caddress, &dev);
	if (ret)
		return ret;

	/* Scale up to the proper value for the VOUT command, little endian */
	multiplier = get_pmbus_multiplier(dev);
	vdd += board_vdd_drop_compensation();
	vlow_margin = vdd - VDD_MV_MARGIN;
	vhigh_margin = vdd + VDD_MV_MARGIN;
	vuv_fault = vdd  - VDD_MV_FAULT_LIMIT;
	vov_fault = vdd  + VDD_MV_FAULT_LIMIT;
	vuv_warn = vdd  - VDD_MV_WARNING_LIMIT;
	vov_warn = vdd  + VDD_MV_WARNING_LIMIT;

	if (multiplier != MV_PER_V)
	{
		vdd = DIV_ROUND_UP(vdd * multiplier, MV_PER_V);
		vlow_margin = DIV_ROUND_UP(vlow_margin * multiplier, MV_PER_V);
		vhigh_margin = DIV_ROUND_UP(vhigh_margin * multiplier, MV_PER_V);
		vuv_fault = DIV_ROUND_UP(vuv_fault * multiplier, MV_PER_V);
		vov_fault = DIV_ROUND_UP(vov_fault * multiplier, MV_PER_V);
		vuv_warn = DIV_ROUND_UP(vuv_warn * multiplier, MV_PER_V);
		vov_warn = DIV_ROUND_UP(vov_warn * multiplier, MV_PER_V);
	}

	vdd_last = read_voltage_from_pmbus(i2caddress);
	if (vdd_last < 0) {
		printf("VID: Couldn't read sensor abort VID adjust\n");
		return -1;
	}

	/* Check write protect state */
	ret = I2C_READ(dev, PMBUS_CMD_WRITE_PROTECT, (void *)&value, sizeof(value));
	if (ret)
		return vdd_last;

	if (value != EN_WRITE_ALL_CMD) {
		value = EN_WRITE_ALL_CMD;
		ret = I2C_WRITE(dev, PMBUS_CMD_WRITE_PROTECT,
				(void *)&value, sizeof(value));
		if (ret)
			return vdd_last;
	}

	ret = 0;
	if (vdd_target > vdd_last)
	{
		int r;
		/* Adjusting to higher vdd */
		r = set_vout_ov_fault_limit(dev, vov_fault);
		ret = r < 0 ? r : ret;
		r = set_vout_ov_warn_limit(dev, vov_warn);
		ret = r < 0 ? r : ret;
		r = set_vout_margin_high(dev, vhigh_margin);
		ret = r < 0 ? r : ret;
		r = wait_chip_busy(dev, 11);
		ret = r < 0 ? r : ret;
		vdd_last = set_and_wait_vout(dev, i2caddress, vdd, vdd_target);
		ret = vdd_last < 0 ? vdd_last : ret;
		r = set_vout_margin_low(dev, vlow_margin);
		ret = r < 0 ? r : ret;
		r = set_vout_uv_warn_limit(dev ,vuv_warn);
		ret = r < 0 ? r : ret;
		r = set_vout_uv_fault_limit(dev, vuv_fault);
		ret = r < 0 ? r : ret;
	}
	else
	{
		int r;
		/* Adjusting to lower vdd */
		r = set_vout_uv_fault_limit(dev, vuv_fault);
		ret = r < 0 ? r : ret;
		r = set_vout_uv_warn_limit(dev ,vuv_warn);
		ret = r < 0 ? r : ret;
		r = set_vout_margin_low(dev, vlow_margin);
		ret = r < 0 ? r : ret;
		r = wait_chip_busy(dev, 11);
		ret = r < 0 ? r : ret;
		vdd_last = set_and_wait_vout(dev, i2caddress, vdd, vdd_target);
		ret = vdd_last < 0 ? vdd_last : ret;
		r = set_vout_margin_high(dev, vhigh_margin);
		ret = r < 0 ? r : ret;
		r = set_vout_ov_warn_limit(dev, vov_warn);
		ret = r < 0 ? r : ret;
		r = set_vout_ov_fault_limit(dev, vov_fault);
		ret = r < 0 ? r : ret;
	}

	if (ret < 0 )
		return ret;
	else
		return vdd_last;
}