Help with `chacha20poly1305@openssh` transport protection in swift-nio-ssh

I'm trying to build an app to be able to connect to OpenWRT routers using ssh.

I decided to use swift-no-ssh to accomplish that but on the router server the encryption algorithms are chacha20-poly1305@openssh.com, aes128-ctr and aes256-ctr while on the client swift-nio-ssh implements aes128-gcm@openssh.com and aes256-gcm@openssh.com. So following the comment of @lukasa on another thread I tried to implement my custom CHACHA20Poly1305OpenSSHTransportProtection to inject in the SSHClientConfiguration. The problem is that I just can't make sense of it, I'm not able to properly decrypt and encrypt messages. Could some kind soul lend me a hand and help me or address me towards a valid alternative? I have also read that this implementation would have be released in different packages, but I didn't find anything.

Here my surely wrong version of the implementation. I just slightly changed the default aesxxx-gcm@openssh and I was hoping the errors would point me towards a correct implementation but they were very non-descriptive so this didn't help. Also, in another thread that I don't find anymore @lukasa said swift-crypto ChaChaPoly is not suitable for this.

final class CHACHA20Poly1305OpenSSHTransportProtection: NIOSSHTransportProtection {

    private var outboundEncryptionKey: SymmetricKey
    private var inboundEncryptionKey: SymmetricKey
    private var outboundNonce: ChaChaPoly.Nonce
    private var inboundNonce: ChaChaPoly.Nonce

    static var cipherName: String {
        "chacha20-poly1305@openssh.com"
    }

    static var macName: String? {
        nil
    }

    static var cipherBlockSize: Int {
        64
    }

    static var keySizes: NIOSSH.ExpectedKeySizes {
        .init(ivSize: 12, encryptionKeySize: 32, macKeySize: 32)
    }

    var macBytes: Int {
        16
    }

    var lengthEncrypted: Bool {
        true
    }

    init(initialKeys: NIOSSH.NIOSSHSessionKeys) throws {
        guard initialKeys.outboundEncryptionKey.bitCount == Self.keySizes.encryptionKeySize * 8,
            initialKeys.inboundEncryptionKey.bitCount == Self.keySizes.encryptionKeySize * 8 else {
            struct InvalidKeySize: Error {}
            throw InvalidKeySize()
        }

        self.outboundEncryptionKey = initialKeys.outboundEncryptionKey
        self.inboundEncryptionKey = initialKeys.inboundEncryptionKey
        self.outboundNonce = try ChaChaPoly.Nonce(data: initialKeys.initialOutboundIV)
        self.inboundNonce = try ChaChaPoly.Nonce(data: initialKeys.initialOutboundIV)
    }

    func updateKeys(_ newKeys: NIOSSH.NIOSSHSessionKeys) throws {
        guard newKeys.outboundEncryptionKey.bitCount == Self.keySizes.encryptionKeySize * 8,
              newKeys.inboundEncryptionKey.bitCount == Self.keySizes.encryptionKeySize * 8 else {
            struct InvalidKeySize: Error {}
            throw InvalidKeySize()
        }

        self.outboundEncryptionKey = newKeys.outboundEncryptionKey
        self.inboundEncryptionKey = newKeys.inboundEncryptionKey
        self.outboundNonce = try ChaChaPoly.Nonce(data: newKeys.initialOutboundIV)
        self.inboundNonce = try ChaChaPoly.Nonce(data: newKeys.initialInboundIV)
    }
    
    func decryptFirstBlock(_ source: inout NIOCore.ByteBuffer) throws {
        // For ChaCha20-Poly1305, the length is encrypted along with the data, so there is no need to decrypt the first block separately.
    }
    
    func decryptAndVerifyRemainingPacket(_ source: inout NIOCore.ByteBuffer, sequenceNumber: UInt32) throws -> NIOCore.ByteBuffer {
        var plaintext: Data

        // Establish a nested scope here to avoid the byte buffer views causing an accidental CoW.
        do {
            // The first 4 bytes are the length. The last 16 are the tag. Everything else is ciphertext. We expect
            // that the ciphertext is a clean multiple of the block size, and to be non-zero.
            guard let lengthView = source.readSlice(length: 4)?.readableBytesView,
                let ciphertextView = source.readSlice(length: source.readableBytes - 16)?.readableBytesView,
                let tagView = source.readSlice(length: 16)?.readableBytesView,
                ciphertextView.count > 0, ciphertextView.count % Self.cipherBlockSize == 0 else {
                // The only way this fails is if the payload doesn't match this encryption scheme.
                struct InvalidEncryptedPacketLength: Error {}
                throw InvalidEncryptedPacketLength()
            }

            // Ok, let's try to decrypt this data.
            let sealedBox = try ChaChaPoly.SealedBox(nonce: self.inboundNonce, ciphertext: ciphertextView, tag: tagView)
            plaintext = try ChaChaPoly.open(sealedBox, using: self.inboundEncryptionKey, authenticating: lengthView)

            // All good! A quick soundness check to verify that the length of the plaintext is ok.
            guard plaintext.count % Self.cipherBlockSize == 0, plaintext.count == ciphertextView.count else {
                struct InvalidDecryptedPlaintextLength: Error {}
                throw InvalidDecryptedPlaintextLength()
            }
        }

        // Don't forget to increment the inbound nonce.
        //self.inboundNonce.increment()

        // Ok, we want to write the plaintext back into the buffer. This contains the padding length byte and the padding
        // bytes, so we want to strip those. We write back into the buffer and then slice the return value out because
        // it's highly likely that the source buffer is held uniquely, which means we can avoid an allocation.
        try plaintext.removePaddingBytes()
        source.prependData(plaintext)

        // This slice read must succeed, as we just wrote in that many bytes.
        return source.readSlice(length: plaintext.count)!
    }
    
    func encryptPacket(_ destination: inout NIOCore.ByteBuffer, sequenceNumber: UInt32) throws {
        // Keep track of where the length is going to be written.
        let packetLengthIndex = destination.readerIndex
        let packetLengthLength = MemoryLayout<UInt32>.size
        let packetPaddingIndex = destination.readerIndex

        // We encrypte everything, except the length
        let encryptedBufferSize = destination.readableBytes

        // Now we need to encrypt the data. We pass the length field as additional authenticated data, and the encrypted
        // payload portion as the data to encrypt. We know these views will be valid, so we forcibly unwrap them: if they're invalid,
        // our math was wrong and we cannot recover.
        let sealedBox = try ChaChaPoly.seal(destination.viewBytes(at: packetPaddingIndex, length: encryptedBufferSize)!,
                                         using: self.outboundEncryptionKey,
                                         nonce: self.outboundNonce,
                                         authenticating: destination.viewBytes(at: packetLengthIndex, length: packetLengthLength)!)

        assert(sealedBox.ciphertext.count == encryptedBufferSize)

        // We now want to overwrite the portion of the bytebuffer that contains the plaintext with the ciphertext,
        // and then append the tag.
        destination.setContiguousBytes(sealedBox.ciphertext, at: packetPaddingIndex)
        let tagLength = destination.writeContiguousBytes(sealedBox.tag)
        precondition(tagLength == self.macBytes, "Unexpected short tag")

        // Now we increment the Nonce for the next use, and then we're done!
        //self.outboundNonce.increment()
    }
}