Fix whitespace.
[tinc] / src / sptps.c
index ac710f5..2a298ff 100644 (file)
@@ -1,6 +1,7 @@
 /*
     sptps.c -- Simple Peer-to-Peer Security
-    Copyright (C) 2011 Guus Sliepen <guus@tinc-vpn.org>,
+    Copyright (C) 2011-2012 Guus Sliepen <guus@tinc-vpn.org>,
+                  2010      Brandon L. Black <blblack@gmail.com>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
 #include "prf.h"
 #include "sptps.h"
 
-char *logfilename;
-#include "utils.c"
+unsigned int sptps_replaywin = 16;
 
+/*
+   Nonce MUST be exchanged first (done)
+   Signatures MUST be done over both nonces, to guarantee the signature is fresh
+   Otherwise: if ECDHE key of one side is compromised, it can be reused!
+
+   Add explicit tag to beginning of structure to distinguish the client and server when signing. (done)
+
+   Sign all handshake messages up to ECDHE kex with long-term public keys. (done)
+
+   HMACed KEX finished message to prevent downgrade attacks and prove you have the right key material (done by virtue of ECDSA over the whole ECDHE exchange?)
+
+   Explicit close message needs to be added.
+
+   Maybe do add some alert messages to give helpful error messages? Not more than TLS sends.
+
+   Use counter mode instead of OFB. (done)
+
+   Make sure ECC operations are fixed time (aka prevent side-channel attacks).
+*/
+
+// Log an error message.
 static bool error(sptps_t *s, int s_errno, const char *msg) {
        fprintf(stderr, "SPTPS error: %s\n", msg);
        errno = s_errno;
        return false;
 }
 
+// Send a record (datagram version, accepts all record types, handles encryption and authentication).
+static bool send_record_priv_datagram(sptps_t *s, uint8_t type, const char *data, uint16_t len) {
+       char buffer[len + 23UL];
+
+       // Create header with sequence number, length and record type
+       uint32_t seqno = htonl(s->outseqno++);
+       uint16_t netlen = htons(len);
+
+       memcpy(buffer, &netlen, 2);
+       memcpy(buffer + 2, &seqno, 4);
+       buffer[6] = type;
+
+       // Add plaintext (TODO: avoid unnecessary copy)
+       memcpy(buffer + 7, data, len);
+
+       if(s->outstate) {
+               // If first handshake has finished, encrypt and HMAC
+               cipher_set_counter(&s->outcipher, &seqno, sizeof seqno);
+               if(!cipher_counter_xor(&s->outcipher, buffer + 6, len + 1UL, buffer + 6))
+                       return false;
+
+               if(!digest_create(&s->outdigest, buffer, len + 7UL, buffer + 7UL + len))
+                       return false;
+
+               return s->send_data(s->handle, type, buffer + 2, len + 21UL);
+       } else {
+               // Otherwise send as plaintext
+               return s->send_data(s->handle, type, buffer + 2, len + 5UL);
+       }
+}
+// Send a record (private version, accepts all record types, handles encryption and authentication).
 static bool send_record_priv(sptps_t *s, uint8_t type, const char *data, uint16_t len) {
-       char plaintext[len + 23];
-       char ciphertext[len + 19];
+       if(s->datagram)
+               return send_record_priv_datagram(s, type, data, len);
+
+       char buffer[len + 23UL];
 
        // Create header with sequence number, length and record type
        uint32_t seqno = htonl(s->outseqno++);
        uint16_t netlen = htons(len);
 
-       memcpy(plaintext, &seqno, 4);
-       memcpy(plaintext + 4, &netlen, 2);
-       plaintext[6] = type;
+       memcpy(buffer, &seqno, 4);
+       memcpy(buffer + 4, &netlen, 2);
+       buffer[6] = type;
 
        // Add plaintext (TODO: avoid unnecessary copy)
-       memcpy(plaintext + 7, data, len);
+       memcpy(buffer + 7, data, len);
 
-       if(s->state) {
+       if(s->outstate) {
                // If first handshake has finished, encrypt and HMAC
-               if(!digest_create(&s->outdigest, plaintext, len + 7, plaintext + 7 + len))
+               if(!cipher_counter_xor(&s->outcipher, buffer + 4, len + 3UL, buffer + 4))
                        return false;
 
-               if(!cipher_encrypt(&s->outcipher, plaintext + 4, sizeof ciphertext, ciphertext, NULL, false))
+               if(!digest_create(&s->outdigest, buffer, len + 7UL, buffer + 7UL + len))
                        return false;
 
-               return s->send_data(s->handle, ciphertext, len + 19);
+               return s->send_data(s->handle, type, buffer + 4, len + 19UL);
        } else {
                // Otherwise send as plaintext
-               return s->send_data(s->handle, plaintext + 4, len + 3);
+               return s->send_data(s->handle, type, buffer + 4, len + 3UL);
        }
 }
 
-bool send_record(sptps_t *s, uint8_t type, const char *data, uint16_t len) {
+// Send an application record.
+bool sptps_send_record(sptps_t *s, uint8_t type, const char *data, uint16_t len) {
        // Sanity checks: application cannot send data before handshake is finished,
        // and only record types 0..127 are allowed.
-       if(!s->state)
+       if(!s->outstate)
                return error(s, EINVAL, "Handshake phase not finished yet");
 
-       if(type & 128)
+       if(type >= SPTPS_HANDSHAKE)
                return error(s, EINVAL, "Invalid application record type");
 
        return send_record_priv(s, type, data, len);
 }
 
+// Send a Key EXchange record, containing a random nonce and an ECDHE public key.
 static bool send_kex(sptps_t *s) {
        size_t keylen = ECDH_SIZE;
-       size_t siglen = ecdsa_size(&s->mykey);
-       char data[32 + keylen + siglen];
 
-       // Create a random nonce.
-       s->myrandom = realloc(s->myrandom, 32);
-       if(!s->myrandom)
+       // Make room for our KEX message, which we will keep around since send_sig() needs it.
+       if(s->mykex)
+               abort();
+       s->mykex = realloc(s->mykex, 1 + 32 + keylen);
+       if(!s->mykex)
                return error(s, errno, strerror(errno));
 
-       randomize(s->myrandom, 32);
-       memcpy(data, s->myrandom, 32);
+       // Set version byte to zero.
+       s->mykex[0] = SPTPS_VERSION;
+
+       // Create a random nonce.
+       randomize(s->mykex + 1, 32);
 
        // Create a new ECDH public key.
-       if(!ecdh_generate_public(&s->ecdh, data + 32))
+       if(!ecdh_generate_public(&s->ecdh, s->mykex + 1 + 32))
                return false;
 
-       // Sign the former.
-       if(!ecdsa_sign(&s->mykey, data, 32 + keylen, data + 32 + keylen))
+       return send_record_priv(s, SPTPS_HANDSHAKE, s->mykex, 1 + 32 + keylen);
+}
+
+// Send a SIGnature record, containing an ECDSA signature over both KEX records.
+static bool send_sig(sptps_t *s) {
+       size_t keylen = ECDH_SIZE;
+       size_t siglen = ecdsa_size(&s->mykey);
+
+       // Concatenate both KEX messages, plus tag indicating if it is from the connection originator, plus label
+       char msg[(1 + 32 + keylen) * 2 + 1 + s->labellen];
+       char sig[siglen];
+
+       msg[0] = s->initiator;
+       memcpy(msg + 1, s->mykex, 1 + 32 + keylen);
+       memcpy(msg + 1 + 33 + keylen, s->hiskex, 1 + 32 + keylen);
+       memcpy(msg + 1 + 2 * (33 + keylen), s->label, s->labellen);
+
+       // Sign the result.
+       if(!ecdsa_sign(&s->mykey, msg, sizeof msg, sig))
                return false;
 
-       // Send the handshake record.
-       return send_record_priv(s, 128, data, sizeof data);
+       // Send the SIG exchange record.
+       return send_record_priv(s, SPTPS_HANDSHAKE, sig, sizeof sig);
 }
 
-static bool generate_key_material(sptps_t *s, const char *shared, size_t len, const char *hisrandom) {
+// Generate key material from the shared secret created from the ECDHE key exchange.
+static bool generate_key_material(sptps_t *s, const char *shared, size_t len) {
        // Initialise cipher and digest structures if necessary
-       if(!s->state) {
+       if(!s->outstate) {
                bool result
-                       =  cipher_open_by_name(&s->incipher, "aes-256-ofb")
-                       && cipher_open_by_name(&s->outcipher, "aes-256-ofb")
+                       =  cipher_open_by_name(&s->incipher, "aes-256-ecb")
+                       && cipher_open_by_name(&s->outcipher, "aes-256-ecb")
                        && digest_open_by_name(&s->indigest, "sha256", 16)
                        && digest_open_by_name(&s->outdigest, "sha256", 16);
                if(!result)
@@ -126,13 +203,13 @@ static bool generate_key_material(sptps_t *s, const char *shared, size_t len, co
        char seed[s->labellen + 64 + 13];
        strcpy(seed, "key expansion");
        if(s->initiator) {
-               memcpy(seed + 13, hisrandom, 32);
-               memcpy(seed + 45, s->myrandom, 32);
+               memcpy(seed + 13, s->mykex + 1, 32);
+               memcpy(seed + 45, s->hiskex + 1, 32);
        } else {
-               memcpy(seed + 13, s->myrandom, 32);
-               memcpy(seed + 45, hisrandom, 32);
+               memcpy(seed + 13, s->hiskex + 1, 32);
+               memcpy(seed + 45, s->mykex + 1, 32);
        }
-       memcpy(seed + 78, s->label, s->labellen);
+       memcpy(seed + 77, s->label, s->labellen);
 
        // Use PRF to generate the key material
        if(!prf(shared, len, seed, s->labellen + 64 + 13, s->key, keylen))
@@ -141,59 +218,108 @@ static bool generate_key_material(sptps_t *s, const char *shared, size_t len, co
        return true;
 }
 
+// Send an ACKnowledgement record.
 static bool send_ack(sptps_t *s) {
-       return send_record_priv(s, 128, "", 0);
+       return send_record_priv(s, SPTPS_HANDSHAKE, "", 0);
 }
 
+// Receive an ACKnowledgement record.
 static bool receive_ack(sptps_t *s, const char *data, uint16_t len) {
        if(len)
-               return false;
+               return error(s, EIO, "Invalid ACK record length");
 
-       // TODO: set cipher/digest keys
-       return error(s, ENOSYS, "receive_ack() not completely implemented yet");
+       if(s->initiator) {
+               bool result
+                       = cipher_set_counter_key(&s->incipher, s->key)
+                       && digest_set_key(&s->indigest, s->key + cipher_keylength(&s->incipher), digest_keylength(&s->indigest));
+               if(!result)
+                       return false;
+       } else {
+               bool result
+                       = cipher_set_counter_key(&s->incipher, s->key + cipher_keylength(&s->outcipher) + digest_keylength(&s->outdigest))
+                       && digest_set_key(&s->indigest, s->key + cipher_keylength(&s->outcipher) + digest_keylength(&s->outdigest) + cipher_keylength(&s->incipher), digest_keylength(&s->indigest));
+               if(!result)
+                       return false;
+       }
+
+       free(s->key);
+       s->key = NULL;
+       s->instate = true;
+
+       return true;
 }
 
+// Receive a Key EXchange record, respond by sending a SIG record.
 static bool receive_kex(sptps_t *s, const char *data, uint16_t len) {
+       // Verify length of the HELLO record
+       if(len != 1 + 32 + ECDH_SIZE)
+               return error(s, EIO, "Invalid KEX record length");
+
+       // Ignore version number for now.
+
+       // Make a copy of the KEX message, send_sig() and receive_sig() need it
+       if(s->hiskex)
+               abort();
+       s->hiskex = realloc(s->hiskex, len);
+       if(!s->hiskex)
+               return error(s, errno, strerror(errno));
+
+       memcpy(s->hiskex, data, len);
+
+       return send_sig(s);
+}
+
+// Receive a SIGnature record, verify it, if it passed, compute the shared secret and calculate the session keys.
+static bool receive_sig(sptps_t *s, const char *data, uint16_t len) {
        size_t keylen = ECDH_SIZE;
        size_t siglen = ecdsa_size(&s->hiskey);
 
        // Verify length of KEX record.
-       if(len != 32 + keylen + siglen)
+       if(len != siglen)
                return error(s, EIO, "Invalid KEX record length");
 
+       // Concatenate both KEX messages, plus tag indicating if it is from the connection originator
+       char msg[(1 + 32 + keylen) * 2 + 1 + s->labellen];
+
+       msg[0] = !s->initiator;
+       memcpy(msg + 1, s->hiskex, 1 + 32 + keylen);
+       memcpy(msg + 1 + 33 + keylen, s->mykex, 1 + 32 + keylen);
+       memcpy(msg + 1 + 2 * (33 + keylen), s->label, s->labellen);
+
        // Verify signature.
-       if(!ecdsa_verify(&s->hiskey, data, 32 + keylen, data + 32 + keylen))
+       if(!ecdsa_verify(&s->hiskey, msg, sizeof msg, data))
                return false;
 
        // Compute shared secret.
        char shared[ECDH_SHARED_SIZE];
-       if(!ecdh_compute_shared(&s->ecdh, data + 32, shared))
+       if(!ecdh_compute_shared(&s->ecdh, s->hiskex + 1 + 32, shared))
                return false;
 
        // Generate key material from shared secret.
-       if(!generate_key_material(s, shared, sizeof shared, data))
+       if(!generate_key_material(s, shared, sizeof shared))
                return false;
 
-       // Send cipher change record if necessary
-       if(s->state)
-               if(!send_ack(s))
-                       return false;
+       free(s->mykex);
+       free(s->hiskex);
 
-       // TODO: set cipher/digest keys
+       s->mykex = NULL;
+       s->hiskex = NULL;
+
+       // Send cipher change record
+       if(s->outstate && !send_ack(s))
+               return false;
+
+       // TODO: only set new keys after ACK has been set/received
        if(s->initiator) {
                bool result
-                       =  cipher_set_key(&s->incipher, s->key, false)
-                       && digest_set_key(&s->indigest, s->key + cipher_keylength(&s->incipher), digest_keylength(&s->indigest))
-                       && cipher_set_key(&s->outcipher, s->key + cipher_keylength(&s->incipher) + digest_keylength(&s->indigest), true)
+                       = cipher_set_counter_key(&s->outcipher, s->key + cipher_keylength(&s->incipher) + digest_keylength(&s->indigest))
                        && digest_set_key(&s->outdigest, s->key + cipher_keylength(&s->incipher) + digest_keylength(&s->indigest) + cipher_keylength(&s->outcipher), digest_keylength(&s->outdigest));
                if(!result)
                        return false;
        } else {
                bool result
-                       =  cipher_set_key(&s->outcipher, s->key, true)
-                       && digest_set_key(&s->outdigest, s->key + cipher_keylength(&s->outcipher), digest_keylength(&s->outdigest))
-                       && cipher_set_key(&s->incipher, s->key + cipher_keylength(&s->outcipher) + digest_keylength(&s->outdigest), false)
-                       && digest_set_key(&s->indigest, s->key + cipher_keylength(&s->outcipher) + digest_keylength(&s->outdigest) + cipher_keylength(&s->incipher), digest_keylength(&s->indigest));
+                       =  cipher_set_counter_key(&s->outcipher, s->key)
+                       && digest_set_key(&s->outdigest, s->key + cipher_keylength(&s->outcipher), digest_keylength(&s->outdigest));
                if(!result)
                        return false;
        }
@@ -201,37 +327,175 @@ static bool receive_kex(sptps_t *s, const char *data, uint16_t len) {
        return true;
 }
 
+// Force another Key EXchange (for testing purposes).
+bool sptps_force_kex(sptps_t *s) {
+       if(!s->outstate || s->state != SPTPS_SECONDARY_KEX)
+               return error(s, EINVAL, "Cannot force KEX in current state");
+
+       s->state = SPTPS_KEX;
+       return send_kex(s);
+}
+
+// Receive a handshake record.
 static bool receive_handshake(sptps_t *s, const char *data, uint16_t len) {
        // Only a few states to deal with handshaking.
        switch(s->state) {
-               case 0:
-                       // We have sent our public ECDH key, we expect our peer to sent one as well.
+               case SPTPS_SECONDARY_KEX:
+                       // We receive a secondary KEX request, first respond by sending our own.
+                       if(!send_kex(s))
+                               return false;
+               case SPTPS_KEX:
+                       // We have sent our KEX request, we expect our peer to sent one as well.
                        if(!receive_kex(s, data, len))
                                return false;
-                       s->state = 1;
+                       s->state = SPTPS_SIG;
                        return true;
-               case 1:
-                       // We receive a secondary key exchange request, first respond by sending our own public ECDH key.
-                       if(!send_kex(s))
-                               return false;
-               case 2:
+               case SPTPS_SIG:
                        // If we already sent our secondary public ECDH key, we expect the peer to send his.
-                       if(!receive_kex(s, data, len))
+                       if(!receive_sig(s, data, len))
                                return false;
-                       s->state = 3;
+                       if(s->outstate)
+                               s->state = SPTPS_ACK;
+                       else {
+                               s->outstate = true;
+                               if(!receive_ack(s, NULL, 0))
+                                       return false;
+                               s->receive_record(s->handle, SPTPS_HANDSHAKE, NULL, 0);
+                               s->state = SPTPS_SECONDARY_KEX;
+                       }
+
                        return true;
-               case 3:
-                       // We expect an empty handshake message to indicate transition to the new keys.
+               case SPTPS_ACK:
+                       // We expect a handshake message to indicate transition to the new keys.
                        if(!receive_ack(s, data, len))
                                return false;
-                       s->state = 1;
+                       s->receive_record(s->handle, SPTPS_HANDSHAKE, NULL, 0);
+                       s->state = SPTPS_SECONDARY_KEX;
                        return true;
+               // TODO: split ACK into a VERify and ACK?
                default:
                        return error(s, EIO, "Invalid session state");
        }
 }
 
-bool receive_data(sptps_t *s, const char *data, size_t len) {
+// Check datagram for valid HMAC
+bool sptps_verify_datagram(sptps_t *s, const char *data, size_t len) {
+       if(!s->instate || len < 21)
+               return false;
+
+       char buffer[len + 23];
+       uint16_t netlen = htons(len - 21);
+
+       memcpy(buffer, &netlen, 2);
+       memcpy(buffer + 2, data, len);
+
+       return digest_verify(&s->indigest, buffer, len - 14, buffer + len - 14);
+}
+
+// Receive incoming data, datagram version.
+static bool sptps_receive_data_datagram(sptps_t *s, const char *data, size_t len) {
+       if(len < (s->instate ? 21 : 5))
+               return error(s, EIO, "Received short packet");
+
+       uint32_t seqno;
+       memcpy(&seqno, data, 4);
+       seqno = ntohl(seqno);
+
+       if(!s->instate) {
+               if(seqno != s->inseqno) {
+                       fprintf(stderr, "Received invalid packet seqno: %d != %d\n", seqno, s->inseqno);
+                       return error(s, EIO, "Invalid packet seqno");
+               }
+
+               s->inseqno = seqno + 1;
+
+               uint8_t type = data[4];
+
+               if(type != SPTPS_HANDSHAKE)
+                       return error(s, EIO, "Application record received before handshake finished");
+
+               return receive_handshake(s, data + 5, len - 5);
+       }
+
+       // Replay protection using a sliding window of configurable size.
+       // s->inseqno is expected sequence number
+       // seqno is received sequence number
+       // s->late[] is a circular buffer, a 1 bit means a packet has not been received yet
+       // The circular buffer contains bits for sequence numbers from s->inseqno - s->replaywin * 8 to (but excluding) s->inseqno.
+       if(s->replaywin) {
+               if(seqno != s->inseqno) {
+                       if(seqno >= s->inseqno + s->replaywin * 8) {
+                               // Prevent packets that jump far ahead of the queue from causing many others to be dropped.
+                               if(s->farfuture++ < s->replaywin >> 2) {
+                                       fprintf(stderr, "Packet is %d seqs in the future, dropped (%u)\n", seqno - s->inseqno, s->farfuture);
+                                       return false;
+                               }
+                               // Unless we have seen lots of them, in which case we consider the others lost.
+                               fprintf(stderr, "Lost %d packets\n", seqno - s->inseqno);
+                               memset(s->late, 0, s->replaywin);
+                       } else if (seqno < s->inseqno) {
+                               // If the sequence number is farther in the past than the bitmap goes, or if the packet was already received, drop it.
+                               if((s->inseqno >= s->replaywin * 8 && seqno < s->inseqno - s->replaywin * 8) || !(s->late[(seqno / 8) % s->replaywin] & (1 << seqno % 8))) {
+                                       fprintf(stderr, "Received late or replayed packet, seqno %d, last received %d", seqno, s->inseqno);
+                                       return false;
+                               }
+                       } else {
+                               // We missed some packets. Mark them in the bitmap as being late.
+                               for(int i = s->inseqno; i < seqno; i++)
+                                       s->late[(i / 8) % s->replaywin] |= 1 << i % 8;
+                       }
+               }
+
+               // Mark the current packet as not being late.
+               s->late[(seqno / 8) % s->replaywin] &= ~(1 << seqno % 8);
+               s->farfuture = 0;
+       }
+
+       if(seqno > s->inseqno)
+               s->inseqno = seqno + 1;
+
+       uint16_t netlen = htons(len - 21);
+
+       char buffer[len + 23];
+
+       memcpy(buffer, &netlen, 2);
+       memcpy(buffer + 2, data, len);
+
+       memcpy(&seqno, buffer + 2, 4);
+
+       // Check HMAC and decrypt.
+       if(!digest_verify(&s->indigest, buffer, len - 14, buffer + len - 14))
+               return error(s, EIO, "Invalid HMAC");
+
+       cipher_set_counter(&s->incipher, &seqno, sizeof seqno);
+       if(!cipher_counter_xor(&s->incipher, buffer + 6, len - 4, buffer + 6))
+               return false;
+
+       // Append a NULL byte for safety.
+       buffer[len - 14] = 0;
+
+       uint8_t type = buffer[6];
+
+       if(type < SPTPS_HANDSHAKE) {
+               if(!s->instate)
+                       return error(s, EIO, "Application record received before handshake finished");
+               if(!s->receive_record(s->handle, type, buffer + 7, len - 21))
+                       return false;
+       } else if(type == SPTPS_HANDSHAKE) {
+               if(!receive_handshake(s, buffer + 7, len - 21))
+                       return false;
+       } else {
+               return error(s, EIO, "Invalid record type");
+       }
+
+       return true;
+}
+
+// Receive incoming data. Check if it contains a complete record, if so, handle it.
+bool sptps_receive_data(sptps_t *s, const char *data, size_t len) {
+       if(s->datagram)
+               return sptps_receive_data_datagram(s, data, len);
+
        while(len) {
                // First read the 2 length bytes.
                if(s->buflen < 6) {
@@ -239,12 +503,7 @@ bool receive_data(sptps_t *s, const char *data, size_t len) {
                        if(toread > len)
                                toread = len;
 
-                       if(s->state) {
-                               if(!cipher_decrypt(&s->incipher, data, toread, s->inbuf + s->buflen, NULL, false))
-                                       return false;
-                       } else {
-                               memcpy(s->inbuf + s->buflen, data, toread);
-                       }
+                       memcpy(s->inbuf + s->buflen, data, toread);
 
                        s->buflen += toread;
                        len -= toread;
@@ -254,11 +513,19 @@ bool receive_data(sptps_t *s, const char *data, size_t len) {
                        if(s->buflen < 6)
                                return true;
 
+                       // Decrypt the length bytes
+
+                       if(s->instate) {
+                               if(!cipher_counter_xor(&s->incipher, s->inbuf + 4, 2, &s->reclen))
+                                       return false;
+                       } else {
+                               memcpy(&s->reclen, s->inbuf + 4, 2);
+                       }
+
+                       s->reclen = ntohs(s->reclen);
+
                        // If we have the length bytes, ensure our buffer can hold the whole request.
-                       uint16_t reclen;
-                       memcpy(&reclen, s->inbuf + 4, 2);
-                       reclen = htons(reclen);
-                       s->inbuf = realloc(s->inbuf, reclen + 23UL);
+                       s->inbuf = realloc(s->inbuf, s->reclen + 23UL);
                        if(!s->inbuf)
                                return error(s, errno, strerror(errno));
 
@@ -272,41 +539,40 @@ bool receive_data(sptps_t *s, const char *data, size_t len) {
                }
 
                // Read up to the end of the record.
-               uint16_t reclen;
-               memcpy(&reclen, s->inbuf + 4, 2);
-               reclen = htons(reclen);
-               size_t toread = reclen + (s->state ? 23UL : 7UL) - s->buflen;
+               size_t toread = s->reclen + (s->instate ? 23UL : 7UL) - s->buflen;
                if(toread > len)
                        toread = len;
 
-               if(s->state) {
-                       if(!cipher_decrypt(&s->incipher, data, toread, s->inbuf + s->buflen, NULL, false))
-                               return false;
-               } else {
-                       memcpy(s->inbuf + s->buflen, data, toread);
-               }
-
+               memcpy(s->inbuf + s->buflen, data, toread);
                s->buflen += toread;
                len -= toread;
                data += toread;
 
                // If we don't have a whole record, exit.
-               if(s->buflen < reclen + (s->state ? 23UL : 7UL))
+               if(s->buflen < s->reclen + (s->instate ? 23UL : 7UL))
                        return true;
 
-               // Check HMAC.
-               if(s->state)
-                       if(!digest_verify(&s->indigest, s->inbuf, reclen + 7UL, s->inbuf + reclen + 7UL))
-                               error(s, EIO, "Invalid HMAC");
+               // Check HMAC and decrypt.
+               if(s->instate) {
+                       if(!digest_verify(&s->indigest, s->inbuf, s->reclen + 7UL, s->inbuf + s->reclen + 7UL))
+                               return error(s, EIO, "Invalid HMAC");
+
+                       if(!cipher_counter_xor(&s->incipher, s->inbuf + 6UL, s->reclen + 1UL, s->inbuf + 6UL))
+                               return false;
+               }
+
+               // Append a NULL byte for safety.
+               s->inbuf[s->reclen + 7UL] = 0;
 
                uint8_t type = s->inbuf[6];
 
-               // Handle record.
-               if(type < 128) {
-                       if(!s->receive_record(s->handle, type, s->inbuf + 7, reclen))
+               if(type < SPTPS_HANDSHAKE) {
+                       if(!s->instate)
+                               return error(s, EIO, "Application record received before handshake finished");
+                       if(!s->receive_record(s->handle, type, s->inbuf + 7, s->reclen))
                                return false;
-               } else if(type == 128) {
-                       if(!receive_handshake(s, s->inbuf + 7, reclen))
+               } else if(type == SPTPS_HANDSHAKE) {
+                       if(!receive_handshake(s, s->inbuf + 7, s->reclen))
                                return false;
                } else {
                        return error(s, EIO, "Invalid record type");
@@ -318,24 +584,34 @@ bool receive_data(sptps_t *s, const char *data, size_t len) {
        return true;
 }
 
-bool start_sptps(sptps_t *s, void *handle, bool initiator, ecdsa_t mykey, ecdsa_t hiskey, const char *label, size_t labellen, send_data_t send_data, receive_record_t receive_record) {
+// Start a SPTPS session.
+bool sptps_start(sptps_t *s, void *handle, bool initiator, bool datagram, ecdsa_t mykey, ecdsa_t hiskey, const char *label, size_t labellen, send_data_t send_data, receive_record_t receive_record) {
        // Initialise struct sptps
        memset(s, 0, sizeof *s);
 
        s->handle = handle;
        s->initiator = initiator;
+       s->datagram = datagram;
        s->mykey = mykey;
        s->hiskey = hiskey;
+       s->replaywin = sptps_replaywin;
+       if(s->replaywin) {
+               s->late = malloc(s->replaywin);
+               if(!s->late)
+                       return error(s, errno, strerror(errno));
+       }
 
        s->label = malloc(labellen);
        if(!s->label)
                return error(s, errno, strerror(errno));
 
-       s->inbuf = malloc(7);
-       if(!s->inbuf)
-               return error(s, errno, strerror(errno));
-       s->buflen = 4;
-       memset(s->inbuf, 0, 4);
+       if(!datagram) {
+               s->inbuf = malloc(7);
+               if(!s->inbuf)
+                       return error(s, errno, strerror(errno));
+               s->buflen = 4;
+               memset(s->inbuf, 0, 4);
+       }
 
        memcpy(s->label, label, labellen);
        s->labellen = labellen;
@@ -344,15 +620,24 @@ bool start_sptps(sptps_t *s, void *handle, bool initiator, ecdsa_t mykey, ecdsa_
        s->receive_record = receive_record;
 
        // Do first KEX immediately
+       s->state = SPTPS_KEX;
        return send_kex(s);
 }
 
-bool stop_sptps(sptps_t *s) {
+// Stop a SPTPS session.
+bool sptps_stop(sptps_t *s) {
        // Clean up any resources.
+       cipher_close(&s->incipher);
+       cipher_close(&s->outcipher);
+       digest_close(&s->indigest);
+       digest_close(&s->outdigest);
        ecdh_free(&s->ecdh);
        free(s->inbuf);
-       free(s->myrandom);
+       free(s->mykex);
+       free(s->hiskex);
        free(s->key);
        free(s->label);
+       free(s->late);
+       memset(s, 0, sizeof *s);
        return true;
 }