+// Check datagram for valid HMAC
+bool sptps_verify_datagram(sptps_t *s, const void *data, size_t len) {
+ if(!s->instate) {
+ return error(s, EIO, "SPTPS state not ready to verify this datagram");
+ }
+
+ if(len < 21) {
+ return error(s, EIO, "Received short packet in sptps_verify_datagram");
+ }
+
+ uint32_t seqno;
+ memcpy(&seqno, data, 4);
+ seqno = ntohl(seqno);
+ // TODO: check whether seqno makes sense, to avoid CPU intensive decrypt
+
+ return chacha_poly1305_verify(s->incipher, seqno, (const char *)data + 4, len - 4);
+}
+
+// Receive incoming data, datagram version.
+static bool sptps_receive_data_datagram(sptps_t *s, const void *vdata, size_t len) {
+ const char *data = vdata;
+
+ if(len < (s->instate ? 21 : 5)) {
+ return error(s, EIO, "Received short packet in sptps_receive_data_datagram");
+ }
+
+ uint32_t seqno;
+ memcpy(&seqno, data, 4);
+ seqno = ntohl(seqno);
+
+ if(!s->instate) {
+ if(seqno != s->inseqno) {
+ return error(s, EIO, "Invalid packet seqno: %d != %d", seqno, s->inseqno);
+ }
+
+ 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);
+ }
+
+ // Decrypt
+
+ if(len > s->decrypted_buffer_len) {
+ s->decrypted_buffer_len *= 2;
+ char *new_buffer = realloc(s->decrypted_buffer, s->decrypted_buffer_len);
+
+ if(!new_buffer) {
+ return error(s, errno, strerror(errno));
+ }
+
+ s->decrypted_buffer = new_buffer;
+ }
+
+ size_t outlen;
+
+ if(!chacha_poly1305_decrypt(s->incipher, seqno, data + 4, len - 4, s->decrypted_buffer, &outlen)) {
+ return error(s, EIO, "Failed to decrypt and verify packet");
+ }
+
+ // 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) {
+ // TODO: Prevent packets that jump far ahead of the queue from causing many others to be dropped.
+ warning(s, "Lost %d packets\n", seqno - s->inseqno);
+ // Mark all packets in the replay window as being late.
+ memset(s->late, 255, 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))) {
+ return error(s, EIO, "Received late or replayed packet, seqno %d, last received %d\n", seqno, s->inseqno);
+ }
+ } else {
+ // We missed some packets. Mark them in the bitmap as being late.
+ for(uint32_t 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);
+ }
+
+ if(seqno >= s->inseqno) {
+ s->inseqno = seqno + 1;
+ }
+
+ if(!s->inseqno) {
+ s->received = 0;
+ } else {
+ s->received++;
+ }
+
+ // Append a NULL byte for safety.
+ s->decrypted_buffer[len - 20] = 0;
+
+ uint8_t type = s->decrypted_buffer[0];
+
+ 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->decrypted_buffer + 1, len - 21)) {
+ abort();
+ }
+ } else if(type == SPTPS_HANDSHAKE) {
+ if(!receive_handshake(s, s->decrypted_buffer + 1, len - 21)) {
+ abort();
+ }
+ } else {
+ return error(s, EIO, "Invalid record type %d", type);
+ }
+
+ return true;
+}
+
projects / meshlink / blobdiff