3 files changed, 67 insertions, 0 deletions
diff --git a/src/include/device/pnp.h b/src/include/device/pnp.h
index cf809d027c..635876b902 100644
--- a/src/include/device/pnp.h
+++ b/src/include/device/pnp.h
@@ -133,4 +133,37 @@ static inline void pnp_write_index(u16 port, u8 reg, u8 value)
 	outb(value, port + 1);
 }
 
+/*
+ * void pnp_unset_and_set_index(u16 port, u8 reg, u8 unset, u8 set)
+ * Description:
+ *  This routine unsets and sets bits from indexed I/O registers. The
+ *  reg byte is written to the index register at I/O address = port.
+ *  The value byte to update is data register at I/O address = port + 1.
+ *
+ *  Unlike and-then-or style operations, no bitwise negation is necessary
+ *  to specify the bits to unset. Because the bitwise negation implicitly
+ *  promotes operands to int before operating, one may have to explicitly
+ *  downcast the result if the data width is smaller than that of an int.
+ *  Since warnings are errors in coreboot, explicit casting is necessary.
+ *
+ *  Performing said negation inside this routine alleviates this problem,
+ *  while allowing the compiler to warn if the input parameters overflow.
+ *  Casting outside this function would silence valid compiler warnings.
+ *
+ * Parameters:
+ *  @param[in] u16 port   = The address of the port index register.
+ *  @param[in] u8  reg    = The offset within the indexed space.
+ *  @param[in] u8  unset  = Bitmask with ones to the bits to unset from the data register.
+ *  @param[in] u8  set    = Bitmask with ones to the bits to set from the data register.
+ */
+static inline void pnp_unset_and_set_index(u16 port, u8 reg, u8 unset, u8 set)
+{
+	outb(reg, port);
+
+	u8 value = inb(port + 1);
+	value &= (u8)~unset;
+	value |= set;
+	outb(value, port + 1);
+}
+
 #endif /* DEVICE_PNP_H */
diff --git a/src/include/device/pnp_ops.h b/src/include/device/pnp_ops.h
index 15acf367e4..15d31156ed 100644
--- a/src/include/device/pnp_ops.h
+++ b/src/include/device/pnp_ops.h
@@ -21,6 +21,12 @@ static __always_inline uint8_t pnp_read_config(
 	return pnp_read_index(dev >> 8, reg);
 }
 
+static __always_inline void pnp_unset_and_set_config(
+	pnp_devfn_t dev, uint8_t reg, uint8_t unset, uint8_t set)
+{
+	pnp_unset_and_set_index(dev >> 8, reg, unset, set);
+}
+
 static __always_inline
 void pnp_set_logical_device(pnp_devfn_t dev)
 {
diff --git a/src/include/superio/hwm5_conf.h b/src/include/superio/hwm5_conf.h
index 2cf13c60e6..f26a0171f5 100644
--- a/src/include/superio/hwm5_conf.h
+++ b/src/include/superio/hwm5_conf.h
@@ -45,4 +45,32 @@ static inline void pnp_write_hwm5_index(u16 base, u8 reg, u8 value)
 	pnp_write_index(base + 5, reg, value);
 }
 
+/*
+ * void pnp_unset_and_set_hwm5_index(u16 base, u8 reg, u8 unset, u8 set)
+ * Description:
+ *  This routine unsets and sets bits from indexed I/O registers. The
+ *  reg byte is written to the index register at I/O address = base + 5.
+ *  The value byte to update is data register at I/O address = base + 6.
+ *
+ *  Unlike and-then-or style operations, no bitwise negation is necessary
+ *  to specify the bits to unset. Because the bitwise negation implicitly
+ *  promotes operands to int before operating, one may have to explicitly
+ *  downcast the result if the data width is smaller than that of an int.
+ *  Since warnings are errors in coreboot, explicit casting is necessary.
+ *
+ *  Performing said negation inside this routine alleviates this problem,
+ *  while allowing the compiler to warn if the input parameters overflow.
+ *  Casting outside this function would silence valid compiler warnings.
+ *
+ * Parameters:
+ *  @param[in] u16 base   = The address of the base index register.
+ *  @param[in] u8  reg    = The offset within the indexed space.
+ *  @param[in] u8  unset  = Bitmask with ones to the bits to unset from the data register.
+ *  @param[in] u8  set    = Bitmask with ones to the bits to set from the data register.
+ */
+static inline void pnp_unset_and_set_hwm5_index(u16 base, u8 reg, u8 unset, u8 set)
+{
+	pnp_unset_and_set_index(base + 5, reg, unset, set);
+}
+
 #endif /* DEVICE_PNP_HWM5_CONF_H */