3 files changed, 22 insertions, 35 deletions

diff --git a/payloads/libpayload/drivers/usb/uhci.c b/payloads/libpayload/drivers/usb/uhci.c
index 779098b168..b964ee7a0b 100644
--- a/payloads/libpayload/drivers/usb/uhci.c
+++ b/payloads/libpayload/drivers/usb/uhci.c
@@ -120,7 +120,8 @@ uhci_reset (hci_t *controller)
 	/* reset framelist index */
 	uhci_reg_write16 (controller, FRNUM, 0);
 
-	uhci_reg_mask16 (controller, USBCMD, ~0, 0xc0);	// max packets, configure flag
+	uhci_reg_write16(controller, USBCMD,
+			 uhci_reg_read16(controller, USBCMD) | 0xc0);	// max packets, configure flag
 
 	uhci_start (controller);
 }
@@ -240,7 +241,8 @@ uhci_shutdown (hci_t *controller)
 	detach_controller (controller);
 	UHCI_INST (controller)->roothub->destroy (UHCI_INST (controller)->
 						  roothub);
-	uhci_reg_mask16 (controller, USBCMD, 0, 0);	// stop work
+	uhci_reg_write16(controller, USBCMD,
+			 uhci_reg_read16(controller, USBCMD) & 0);	// stop work
 	free (UHCI_INST (controller)->framelistptr);
 	free (UHCI_INST (controller)->qh_prei);
 	free (UHCI_INST (controller)->qh_intr);
@@ -253,13 +255,15 @@ uhci_shutdown (hci_t *controller)
 static void
 uhci_start (hci_t *controller)
 {
-	uhci_reg_mask16 (controller, USBCMD, ~0, 1);	// start work on schedule
+	uhci_reg_write16(controller, USBCMD,
+			 uhci_reg_read16(controller, USBCMD) | 1);	// start work on schedule
 }
 
 static void
 uhci_stop (hci_t *controller)
 {
-	uhci_reg_mask16 (controller, USBCMD, ~1, 0);	// stop work on schedule
+	uhci_reg_write16(controller, USBCMD,
+			 uhci_reg_read16(controller, USBCMD) & ~1);	// stop work on schedule
 }
 
 #define GET_TD(x) ((void*)(((unsigned int)(x))&~0xf))
@@ -274,7 +278,8 @@ wait_for_completed_qh (hci_t *controller, qh_t *qh)
 			current = GET_TD (qh->elementlinkptr.ptr);
 			timeout = 1000000;
 		}
-		uhci_reg_mask16 (controller, USBSTS, ~0, 0);	// clear resettable registers
+		uhci_reg_write16(controller, USBSTS,
+				 uhci_reg_read16(controller, USBSTS) | 0);	// clear resettable registers
 		udelay (30);
 	}
 	return (GET_TD (qh->elementlinkptr.ptr) ==
@@ -636,24 +641,3 @@ uhci_reg_read8 (hci_t *ctrl, usbreg reg)
 {
 	return inb (ctrl->reg_base + reg);
 }
-
-void
-uhci_reg_mask32 (hci_t *ctrl, usbreg reg, u32 andmask, u32 ormask)
-{
-	uhci_reg_write32 (ctrl, reg,
-			  (uhci_reg_read32 (ctrl, reg) & andmask) | ormask);
-}
-
-void
-uhci_reg_mask16 (hci_t *ctrl, usbreg reg, u16 andmask, u16 ormask)
-{
-	uhci_reg_write16 (ctrl, reg,
-			  (uhci_reg_read16 (ctrl, reg) & andmask) | ormask);
-}
-
-void
-uhci_reg_mask8 (hci_t *ctrl, usbreg reg, u8 andmask, u8 ormask)
-{
-	uhci_reg_write8 (ctrl, reg,
-			 (uhci_reg_read8 (ctrl, reg) & andmask) | ormask);
-}
diff --git a/payloads/libpayload/drivers/usb/uhci_private.h b/payloads/libpayload/drivers/usb/uhci_private.h
index 877592abc8..adcd91c7b3 100644
--- a/payloads/libpayload/drivers/usb/uhci_private.h
+++ b/payloads/libpayload/drivers/usb/uhci_private.h
@@ -104,9 +104,6 @@ typedef struct {
      u16 uhci_reg_read16 (hci_t *ctrl, usbreg reg);
      void uhci_reg_write8 (hci_t *ctrl, usbreg reg, u8 value);
      u8 uhci_reg_read8 (hci_t *ctrl, usbreg reg);
-     void uhci_reg_mask32 (hci_t *ctrl, usbreg reg, u32 andmask, u32 ormask);
-     void uhci_reg_mask16 (hci_t *ctrl, usbreg reg, u16 andmask, u16 ormask);
-     void uhci_reg_mask8 (hci_t *ctrl, usbreg reg, u8 andmask, u8 ormask);
 
      typedef struct uhci {
 	     flistp_t *framelistptr;
diff --git a/payloads/libpayload/drivers/usb/uhci_rh.c b/payloads/libpayload/drivers/usb/uhci_rh.c
index 53b32f6a14..5074099255 100644
--- a/payloads/libpayload/drivers/usb/uhci_rh.c
+++ b/payloads/libpayload/drivers/usb/uhci_rh.c
@@ -53,14 +53,18 @@ uhci_rh_enable_port (usbdev_t *dev, int port)
 		return;
 	}
 
-	uhci_reg_mask16 (controller, port, ~(1 << 12), 0);	/* wakeup */
+	uhci_reg_write16(controller, port,
+			 uhci_reg_read16(controller, port) & ~(1 << 12));	/* wakeup */
 
-	uhci_reg_mask16 (controller, port, ~0, 1 << 9);	/* reset */
+	uhci_reg_write16(controller, port,
+			 uhci_reg_read16(controller, port) | 1 << 9);	/* reset */
 	mdelay (30);		// >10ms
-	uhci_reg_mask16 (controller, port, ~(1 << 9), 0);
+	uhci_reg_write16(controller, port,
+			 uhci_reg_read16(controller, port) & ~(1 << 9));
 	mdelay (1);		// >5.3us per spec, <3ms because some devices make trouble
 
-	uhci_reg_mask16 (controller, port, ~0, 1 << 2);	/* enable */
+	uhci_reg_write16(controller, port,
+			 uhci_reg_read16(controller, port) | 1 << 2);	/* enable */
 	do {
 		value = uhci_reg_read16 (controller, port);
 		mdelay (1);
@@ -75,7 +79,8 @@ uhci_rh_disable_port (usbdev_t *dev, int port)
 	port = PORTSC2;
 	if (port == 1)
 		port = PORTSC1;
-	uhci_reg_mask16 (controller, port, ~4, 0);
+	uhci_reg_write16(controller, port,
+			 uhci_reg_read16(controller, port) & ~4);
 	int value;
 	do {
 		value = uhci_reg_read16 (controller, port);
@@ -102,7 +107,8 @@ uhci_rh_scanport (usbdev_t *dev, int port)
 		usb_detach_device(dev->controller, devno);
 		RH_INST (dev)->port[offset] = -1;
 	}
-	uhci_reg_mask16 (dev->controller, portsc, ~0, (1 << 3) | (1 << 2));	// clear port state change, enable port
+	uhci_reg_write16(dev->controller, portsc,
+			 uhci_reg_read16(dev->controller, portsc) | (1 << 3) | (1 << 2));	// clear port state change, enable port
 
 	mdelay(100); // wait for signal to stabilize