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

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>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/staging/pi433/rf69.c b/drivers/staging/pi433/rf69.c
index 340ffa7ccf18f07af09d0eaca01150ff615b9970..4cd16257f0aab57c496b74832cbecbcf537db395 100644 (file)
--- 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)