p2p: fix disconnect at capacity

With the introduction of static/trusted nodes, the peer count
can go above MaxPeers. Update the capacity check to handle this.
While here, decouple the trusted nodes check from the handshake
by passing a function instead.

p2p/handshake_test.go

@@ -141,9 +141,10 @@ func TestSetupConn(t *testing.T) {
 	fd0, fd1 := net.Pipe()
 
 	done := make(chan struct{})
+	keepalways := func(discover.NodeID) bool { return true }
 	go func() {
 		defer close(done)
-		conn0, err := setupConn(fd0, prv0, hs0, node1, false, nil)
+		conn0, err := setupConn(fd0, prv0, hs0, node1, keepalways)
 		if err != nil {
 			t.Errorf("outbound side error: %v", err)
 			return
@@ -156,7 +157,7 @@ func TestSetupConn(t *testing.T) {
 		}
 	}()
 
-	conn1, err := setupConn(fd1, prv1, hs1, nil, false, nil)
+	conn1, err := setupConn(fd1, prv1, hs1, nil, keepalways)
 	if err != nil {
 		t.Fatalf("inbound side error: %v", err)
 	}