Improve buffer realignment, handle missing headroom

A decrypted packet may be aligned correctly, but have too little headroom
to be encrypted using a different method. This led to an assertion
failure, crashing fastd.

Also move the realignment to the receive path, so it is only checked for
forwarded packets.

src/buffer.h

@@ -48,6 +48,31 @@ static inline void fastd_buffer_free(fastd_buffer_t *buffer) {
 	free(buffer);
 }
 
+/**
+   Returns the amount of headroom a buffer has
+   (the number of bytes that can be pushed)
+*/
+static inline size_t fastd_buffer_headroom(const fastd_buffer_t *buffer) {
+	return (const uint8_t *)buffer->data - buffer->base;
+}
+
+/**
+   Realigns a buffer so that it has a given minimal headroom and, subtracting
+   the minimal headroom, is aligned for fastd_block128_t
+
+   Consumes the passed buffer.
+*/
+static inline fastd_buffer_t *fastd_buffer_align(fastd_buffer_t *buffer, size_t min_headroom) {
+	ssize_t surplus = fastd_buffer_headroom(buffer) - min_headroom;
+	if (surplus >= 0 && surplus % sizeof(fastd_block128_t) == 0)
+		return buffer;
+
+	fastd_buffer_t *new_buffer = fastd_buffer_dup(buffer, min_headroom);
+	fastd_buffer_free(buffer);
+
+	return new_buffer;
+}
+
 /** Zeroes the trailing padding of a buffer, aligned to a multiple of 16 bytes */
 static inline void fastd_buffer_zero_pad(fastd_buffer_t *buffer) {
 	uint8_t *end = buffer->data + buffer->len;
@@ -57,7 +82,7 @@ static inline void fastd_buffer_zero_pad(fastd_buffer_t *buffer) {
 
 /** Pushes the data head (decreases the head space) */
 static inline void fastd_buffer_push(fastd_buffer_t *buffer, size_t len) {
-	if (len > (size_t)((uint8_t *)buffer->data - buffer->base))
+	if (len > fastd_buffer_headroom(buffer))
 		exit_bug("tried to push buffer across base");
 
 	buffer->data -= len;

src/receive.c

@@ -293,6 +293,14 @@ void fastd_handle_receive(fastd_peer_t *peer, fastd_buffer_t *buffer, bool reord
 	fastd_iface_write(peer->iface, buffer);
 
 	if (conf.mode == MODE_TAP && conf.forward) {
+		/*
+		  Misaligned buffers come from the null method, as it uses a 1-byte header
+		  rather than (16*n+8)-byte like all other methods. When such a buffer enters
+		  the transmit path again through fastd's forward feature, it will violate
+		  the fastd_block128_t alignment.
+		*/
+		buffer = fastd_buffer_align(buffer, conf.encrypt_headroom);
+
 		fastd_send_data(buffer, peer, NULL);
 		return;
 	}

src/send.c

@@ -214,28 +214,8 @@ static inline bool send_data_tap_single(fastd_buffer_t *buffer, fastd_peer_t *so
 	return true;
 }
 
-/**
-   Realigns a buffer so that its data pointer is aligned to 16 bytes
-   (the alignment of fastd_block128_t)
-
-   Misaligned buffers come from the null method, as it uses a 1-byte header
-   rather than (16*n+8)-byte like all other methods. When such a buffer enters
-   the transmit path again through fastd's forward feature, it will violate
-   the fastd_block128_t alignment.
- */
-static void align_buffer(fastd_buffer_t **buffer) {
-	if (is_aligned((*buffer)->data, sizeof(fastd_block128_t)))
-		return;
-
-	fastd_buffer_t *new_buf = fastd_buffer_dup(*buffer, conf.encrypt_headroom);
-	fastd_buffer_free(*buffer);
-	*buffer = new_buf;
-}
-
 /** Sends a buffer of payload data to other peers */
 void fastd_send_data(fastd_buffer_t *buffer, fastd_peer_t *source, fastd_peer_t *dest) {
-	align_buffer(&buffer);
-
 	if (dest) {
 		conf.protocol->send(dest, buffer);
 		return;

src/util.h

@@ -63,13 +63,6 @@ static inline size_t alignto(size_t l, size_t a) {
 	return block_count(l, a) * a;
 }
 
-/**
-   Checks whether a pointer satisfies a given alignment
- */
-static inline bool is_aligned(const void *p, size_t a) {
-	return !((uintptr_t)p & (a - 1));
-}
-
 /**
    Checks if two strings are equal