From 270dd5282ab4970025a2618d9dbf3d386b6d0ba4 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 7 Jul 2017 16:03:21 -1000
Subject: [PATCH] mpu9250:Add modify checked register api

   Provide an API for the mag to use read-modify-write
---
 src/drivers/mpu9250/mpu9250.cpp | 11 +++++++++++
 src/drivers/mpu9250/mpu9250.h   | 11 +++++++++++
 2 files changed, 22 insertions(+)

diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 69527a743b..58f98e01e3 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -1013,6 +1013,17 @@ MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	write_reg(reg, val);
 }
 
+void
+MPU9250::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_checked_reg(reg, val);
+}
+
 void
 MPU9250::write_checked_reg(unsigned reg, uint8_t value)
 {
diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h
index da3daf8934..2725896f99 100644
--- a/src/drivers/mpu9250/mpu9250.h
+++ b/src/drivers/mpu9250/mpu9250.h
@@ -462,6 +462,17 @@ private:
 	 */
 	void			write_checked_reg(unsigned reg, uint8_t value);
 
+	/**
+	 * Modify a checked register in the mpu
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
 	/**
 	 * Set the mpu measurement range.
 	 *