[PATCH] staging: pi433: add dependency to PA0,1,2 setting for output power level

[Sidong Yang <realwakka@gmail.com>]

When setting output power level called, the power level should be
checked by power amplifier level register and high power option. There
was todo about it. Add some variables for checking power level range.
The values that used for checking high power or minimum power are from
rf69 datasheets. The maximum power level is always same regardless of
mode.

Signed-off-by: Sidong Yang <realwakka@gmail.com>
---
 drivers/staging/pi433/rf69.c | 45 ++++++++++++++++++++++++++++++++++++++------
 1 file changed, 39 insertions(+), 6 deletions(-)

diff --git a/drivers/staging/pi433/rf69.c b/drivers/staging/pi433/rf69.c
index 340ffa7ccf18..4cd16257f0aa 100644
--- a/drivers/staging/pi433/rf69.c
+++ b/drivers/staging/pi433/rf69.c
@@ -349,18 +349,51 @@ int rf69_disable_amplifier(struct spi_device *spi, u8 amplifier_mask)
 
 int rf69_set_output_power_level(struct spi_device *spi, u8 power_level)
 {
-	// TODO: Dependency to PA0,1,2 setting
-	power_level += 18;
+	u8 pa_level, ocp, test_pa1, test_pa2;
+	bool pa0, pa1, pa2, high_power;
+	u8 min_power_level;
+
+	// check register pa_level
+	pa_level = rf69_read_reg(spi, REG_PALEVEL);
+	pa0 = pa_level & MASK_PALEVEL_PA0;
+	pa1 = pa_level & MASK_PALEVEL_PA1;
+	pa2 = pa_level & MASK_PALEVEL_PA2;
+
+	// check high power mode
+	ocp = rf69_read_reg(spi, REG_OCP);
+	test_pa1 = rf69_read_reg(spi, REG_TESTPA1);
+	test_pa2 = rf69_read_reg(spi, REG_TESTPA2);
+	high_power = (ocp == 0x0f) && (test_pa1 == 0x5d) && (test_pa2 == 0x7c);
+
+	if (pa0 && !pa1 && !pa2) {
+		power_level += 18;
+		min_power_level = 0;
+	} else if (!pa0 && pa1 && !pa2) {
+		power_level += 18;
+		min_power_level = 16;
+	} else if (!pa0 && pa1 && pa2) {
+		if (high_power)
+			power_level += 11;
+		else
+			power_level += 14;
+		min_power_level = 16;
+	} else {
+		goto failed;
+	}
 
 	// check input value
-	if (power_level > 0x1f) {
-		dev_dbg(&spi->dev, "set: illegal input param");
-		return -EINVAL;
-	}
+	if (power_level > 0x1f)
+		goto failed;
+
+	if (power_level < min_power_level)
+		goto failed;
 
 	// write value
 	return rf69_read_mod_write(spi, REG_PALEVEL, MASK_PALEVEL_OUTPUT_POWER,
 				   power_level);
+failed:
+	dev_dbg(&spi->dev, "set: illegal input param");
+	return -EINVAL;
 }
 
 int rf69_set_pa_ramp(struct spi_device *spi, enum pa_ramp pa_ramp)
-- 
2.11.0