diff options
Diffstat (limited to 'drivers/staging/pi433/rf69.c')
| -rw-r--r-- | drivers/staging/pi433/rf69.c | 49 |
1 files changed, 40 insertions, 9 deletions
diff --git a/drivers/staging/pi433/rf69.c b/drivers/staging/pi433/rf69.c index e19b9ce794a8..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) @@ -624,9 +657,7 @@ int rf69_set_preamble_length(struct spi_device *spi, u16 preamble_length) retval = rf69_write_reg(spi, REG_PREAMBLE_MSB, msb); if (retval) return retval; - retval = rf69_write_reg(spi, REG_PREAMBLE_LSB, lsb); - - return retval; + return rf69_write_reg(spi, REG_PREAMBLE_LSB, lsb); } int rf69_enable_sync(struct spi_device *spi) |