Improve performance of pintoggle

rpio.go

@@ -284,10 +284,10 @@ 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()
@@ -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.

rpio_test.go

@@ -119,3 +119,35 @@ func TestEvent(t *testing.T) {
 	})
 
 }
+
+func BenchmarkToggle(b *testing.B) {
+	src := Pin(3)
+	src.Mode(Output)
+	src.Low()
+
+	pin := Pin(2)
+	pin.Mode(Input)
+	pin.PullDown()
+
+	oldToggle := func(pin Pin) {
+		switch ReadPin(pin) {
+		case Low:
+			pin.High()
+		case High:
+			pin.Low()
+		}
+	}
+
+	b.Run("old toggle", func(b *testing.B) {
+		for i := 0; i < b.N; i++ {
+			oldToggle(src)
+		}
+	})
+
+	b.Run("current toggle", func(b *testing.B) {
+		for i := 0; i < b.N; i++ {
+			TogglePin(src)
+		}
+	})
+
+}