diff --git a/rpio.go b/rpio.go
index 155766c..e453c80 100644
--- a/rpio.go
+++ b/rpio.go
@@ -284,19 +284,19 @@ func PinMode(pin Pin, mode Mode) {
 func WritePin(pin Pin, state State) {
 	p := uint8(pin)
 
-	// Clear register, 10 / 11 depending on bank
 	// Set register, 7 / 8 depending on bank
-	clearReg := p/32 + 10
+	// Clear register, 10 / 11 depending on bank
 	setReg := p/32 + 7
+	clearReg := p/32 + 10
 
 	memlock.Lock()
-	defer memlock.Unlock()
 
 	if state == Low {
 		gpioMem[clearReg] = 1 << (p & 31)
 	} else {
 		gpioMem[setReg] = 1 << (p & 31)
 	}
+	memlock.Unlock() // not deferring saves ~600ns
 }
 
 // Read the state of a pin
@@ -312,14 +312,23 @@ func ReadPin(pin Pin) State {
 }
 
 // Toggle a pin state (high -> low -> high)
-// TODO: probably possible to do this much faster without read
 func TogglePin(pin Pin) {
-	switch ReadPin(pin) {
-	case Low:
-		pin.High()
-	case High:
-		pin.Low()
+	p := uint8(pin)
+
+	setReg := p/32 + 7
+	clearReg := p/32 + 10
+	levelReg := p/32 + 13
+
+	bit := uint32(1 << (p & 31))
+
+	memlock.Lock()
+
+	if (gpioMem[levelReg] & bit) != 0 {
+		gpioMem[clearReg] = bit
+	} else {
+		gpioMem[setReg] = bit
 	}
+	memlock.Unlock()
 }
 
 // Enable edge event detection on pin.
diff --git a/rpio_test.go b/rpio_test.go
index 23e40fa..d534f5f 100644
--- a/rpio_test.go
+++ b/rpio_test.go
@@ -117,4 +117,77 @@ func TestEvent(t *testing.T) {
 		}
 
 	})
+
+}
+
+func BenchmarkGpio(b *testing.B) {
+	src := Pin(3)
+	src.Mode(Output)
+	src.Low()
+
+	pin := Pin(2)
+	pin.Mode(Input)
+	pin.PullDown()
+
+	oldWrite := func(pin Pin, state State) {
+		p := uint8(pin)
+
+		setReg := p/32 + 7
+		clearReg := p/32 + 10
+
+		memlock.Lock()
+		defer memlock.Unlock()
+
+		if state == Low {
+			gpioMem[clearReg] = 1 << (p & 31)
+		} else {
+			gpioMem[setReg] = 1 << (p & 31)
+		}
+	}
+
+	oldToggle := func(pin Pin) {
+		switch ReadPin(pin) {
+		case Low:
+			oldWrite(pin, High)
+		case High:
+			oldWrite(pin, Low)
+		}
+	}
+
+	b.Run("write", func(b *testing.B) {
+		b.Run("old", func(b *testing.B) {
+			for i := 0; i < b.N; i++ {
+				if i%2 == 0 {
+					oldWrite(src, High)
+				} else {
+					oldWrite(src, Low)
+				}
+			}
+		})
+
+		b.Run("new", func(b *testing.B) {
+			for i := 0; i < b.N; i++ {
+				if i%2 == 0 {
+					WritePin(src, High)
+				} else {
+					WritePin(src, Low)
+				}
+			}
+		})
+	})
+
+	b.Run("toggle", func(b *testing.B) {
+		b.Run("old", func(b *testing.B) {
+			for i := 0; i < b.N; i++ {
+				oldToggle(src)
+			}
+		})
+
+		b.Run("new", func(b *testing.B) {
+			for i := 0; i < b.N; i++ {
+				TogglePin(src)
+			}
+		})
+	})
+
 }