New debug print function

4 files changed, 233 insertions, 122 deletions

diff --git a/src/fastd.c b/src/fastd.c
index fc443a6..bcd6bf6 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -36,6 +36,7 @@
 #include <linux/if_tun.h>
 #include <net/if.h>
 #include <poll.h>
+#include <stdarg.h>
 #include <string.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
@@ -45,6 +46,188 @@
 extern fastd_method fastd_method_null;
 
 
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wformat-security"
+
+void fastd_printf(const fastd_context *ctx, const char *format, ...) {
+	va_list ap;
+	va_start(ap, format);
+
+	char *format_dup = strdup(format);
+	char *str;
+	for (str = format_dup; *str; str++) {
+		if (*str != '%') {
+			fputc(*str, stderr);
+			continue;
+		}
+
+		int len, flag_l = 0, flag_L = 0, flag_j = 0, flag_z = 0, flag_t = 0;
+
+		for(len = 1; str[len]; len++) {
+			char last;
+			bool finished = true;
+			char addr_buf[INET6_ADDRSTRLEN];
+			void *p;
+			fastd_eth_addr *eth_addr;
+
+			switch (str[len]) {
+			case 'l':
+				flag_l++;
+				finished = false;
+				break;
+
+			case 'L':
+				flag_L++;
+				finished = false;
+				break;
+
+			case 'j':
+				flag_j++;
+				finished = false;
+				break;
+
+			case 'z':
+				flag_z++;
+				finished = false;
+				break;
+
+			case 't':
+				flag_t++;
+				finished = false;
+				break;
+
+			case '%':
+				fputc('%', stderr);
+				break;
+
+			case 'd':
+			case 'i':
+			case 'o':
+			case 'u':
+			case 'x':
+			case 'X':
+				last = str[len+1];
+				str[len+1] = 0;
+
+				if (flag_j)
+					fprintf(stderr, str, va_arg(ap, intmax_t));
+				else if (flag_z)
+					fprintf(stderr, str, va_arg(ap, size_t));
+				else if (flag_t)
+					fprintf(stderr, str, va_arg(ap, ptrdiff_t));
+				else if (flag_l == 0)
+					fprintf(stderr, str, va_arg(ap, int));
+				else if (flag_l == 1)
+					fprintf(stderr, str, va_arg(ap, long));
+				else
+					fprintf(stderr, str, va_arg(ap, long long));
+
+				str[len+1] = last;
+				break;
+
+			case 'e':
+			case 'f':
+			case 'F':
+			case 'g':
+			case 'G':
+			case 'a':
+			case 'A':
+				last = str[len+1];
+				str[len+1] = 0;
+
+				if (flag_L)
+					fprintf(stderr, str, va_arg(ap, long double));
+				else
+					fprintf(stderr, str, va_arg(ap, double));
+
+				str[len+1] = last;
+				break;
+
+			case 'c':
+				last = str[len+1];
+				str[len+1] = 0;
+
+				fprintf(stderr, str, va_arg(ap, int));
+
+				str[len+1] = last;
+				break;
+
+			case 's':
+			case 'p':
+				last = str[len+1];
+				str[len+1] = 0;
+
+				fprintf(stderr, str, va_arg(ap, void*));
+
+				str[len+1] = last;
+				break;
+
+			case 'm':
+				last = str[len+1];
+				str[len+1] = 0;
+
+				fprintf(stderr, str);
+
+				str[len+1] = last;
+				break;
+
+			case 'I':
+				p = va_arg(ap, void*);
+
+				if (p) {
+					if (inet_ntop(flag_l ? AF_INET6 : AF_INET, p, addr_buf, sizeof(addr_buf)))
+						fprintf(stderr, "%s", addr_buf);
+				}
+				else {
+					fprintf(stderr, "(null)");
+				}
+				break;
+
+			case 'E':
+				eth_addr = va_arg(ap, fastd_eth_addr*);
+
+				if (eth_addr) {
+					fprintf(stderr, "%02x:%02x:%02x:%02x:%02x:%02x",
+						eth_addr->data[0], eth_addr->data[1], eth_addr->data[2],
+						eth_addr->data[3], eth_addr->data[4], eth_addr->data[5]);
+				}
+				else {
+					fprintf(stderr, "(null)");
+				}
+				break;
+
+			case 'P':
+				p = va_arg(ap, void*);
+
+				if (p) {
+					char* str = ctx->conf->method->peer_str(ctx, (fastd_peer*)p);
+					fprintf(stderr, "%s", str);
+					free(str);
+				}
+				else {
+					fprintf(stderr, "(null)");
+				}
+				break;
+
+			default:
+				finished = false;
+			}
+
+			if (finished) {
+				str += len;
+				break;
+			}
+		}
+	}
+
+	free(format_dup);
+
+	va_end(ap);
+}
+
+#pragma GCC diagnostic pop
+
+
 static void init_tuntap(fastd_context *ctx) {
 	struct ifreq ifr;
 
diff --git a/src/fastd.h b/src/fastd.h
index 9ce15e7..93ebf95 100644
--- a/src/fastd.h
+++ b/src/fastd.h
@@ -59,6 +59,8 @@ struct _fastd_method {
 
 	size_t (*max_packet_size)(fastd_context *ctx);
 
+	char* (*peer_str)(const fastd_context *ctx, const fastd_peer *peer);
+
 	void (*init)(fastd_context *ctx, fastd_peer *peer);
 
 	void (*handle_recv)(fastd_context *ctx, fastd_peer *peer, fastd_buffer buffer);
@@ -104,8 +106,10 @@ struct _fastd_context {
 };
 
 
+void fastd_printf(const fastd_context *ctx, const char *format, ...);
+
 #define pr_log(ctx, level, prefix, args...) if ((ctx)->conf == NULL || (level) <= (ctx)->conf->loglevel) \
-		do { fputs(prefix, stderr); fprintf(stderr, args); fputs("\n", stderr); } while(0)
+		do { fputs(prefix, stderr); fastd_printf(ctx, args); fputs("\n", stderr); } while(0)
 
 #define is_error(ctx) ((ctx)->conf == NULL || LOG_ERROR <= (ctx)->conf->loglevel)
 #define is_warn(ctx) ((ctx)->conf == NULL || LOG_WARN <= (ctx)->conf->loglevel)
diff --git a/src/method_null.c b/src/method_null.c
index 6ed7144..8c144e8 100644
--- a/src/method_null.c
+++ b/src/method_null.c
@@ -24,10 +24,15 @@
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#define _GNU_SOURCE
+
 #include "fastd.h"
 #include "task.h"
 #include "peer.h"
 
+#include <arpa/inet.h>
+
+
 
 static bool null_check_config(fastd_context *ctx, const fastd_config *conf) {
 	if (conf->n_floating > 1) {
@@ -42,6 +47,35 @@ static size_t null_max_packet_size(fastd_context *ctx) {
 	return fastd_max_packet_size(ctx);
 }
 
+static char* null_peer_str(const fastd_context *ctx, const fastd_peer *peer) {
+	char addr_buf[INET6_ADDRSTRLEN] = "";
+	char *ret;
+
+	switch (peer->address.sa.sa_family) {
+	case AF_UNSPEC:
+		return strdup("<floating>");
+
+	case AF_INET:
+		if (inet_ntop(AF_INET, &peer->address.in.sin_addr, addr_buf, sizeof(addr_buf))) {
+			if (asprintf(&ret, "%s:%u", addr_buf, ntohs(peer->address.in.sin_port)) > 0)
+				return ret;
+		}
+		break;
+
+	case AF_INET6:
+		if (inet_ntop(AF_INET6, &peer->address.in6.sin6_addr, addr_buf, sizeof(addr_buf))) {
+			if (asprintf(&ret, "[%s]:%u", addr_buf, ntohs(peer->address.in6.sin6_port)) > 0)
+				return ret;
+		}
+		break;
+
+	default:
+		exit_bug(ctx, "unsupported address family");
+	}
+
+	return NULL;
+}
+
 static void null_init(fastd_context *ctx, fastd_peer *peer) {
 	fastd_task_put_send(ctx, peer, fastd_buffer_alloc(0, 0, 0));
 }
@@ -85,6 +119,8 @@ const fastd_method fastd_method_null = {
 
 	.max_packet_size = null_max_packet_size,
 
+	.peer_str = null_peer_str,
+
 	.init = null_init,
 	.handle_recv = null_handle_recv,
 	.send = null_send,
diff --git a/src/peer.c b/src/peer.c
index 83cc1b0..6fe1aa2 100644
--- a/src/peer.c
+++ b/src/peer.c
@@ -83,21 +83,17 @@ static inline void setup_peer(fastd_context *ctx, fastd_peer *peer) {
 
 void fastd_peer_reset(fastd_context *ctx, fastd_peer *peer) {
 	if (is_debug(ctx)) {
-		char buf[INET6_ADDRSTRLEN] = "";
-
 		switch (peer->address.sa.sa_family) {
 		case AF_UNSPEC:
 			pr_debug(ctx, "resetting peer <floating>");
 			break;
 
 		case AF_INET:
-			if (inet_ntop(AF_INET, &peer->address.in.sin_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "resetting peer %s:%u", buf, ntohs(peer->address.in.sin_port));
+			pr_debug(ctx, "resetting peer %I:%u", &peer->address.in.sin_addr, ntohs(peer->address.in.sin_port));
 			break;
 
 		case AF_INET6:
-			if (inet_ntop(AF_INET6, &peer->address.in6.sin6_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "resetting peer [%s]:%u", buf, ntohs(peer->address.in6.sin6_port));
+			pr_debug(ctx, "resetting peer [%lI]:%u", &peer->address.in6.sin6_addr, ntohs(peer->address.in6.sin6_port));
 			break;
 
 		default:
@@ -125,33 +121,11 @@ static fastd_peer* add_peer(fastd_context *ctx) {
 fastd_peer* fastd_peer_add(fastd_context *ctx, fastd_peer_config *peer_conf) {
 	fastd_peer *peer = add_peer(ctx);
 
-
 	peer->config = peer_conf;
 
 	setup_peer(ctx, peer);
 
-	if (is_debug(ctx)) {
-		char buf[INET6_ADDRSTRLEN] = "";
-
-		switch (peer->address.sa.sa_family) {
-		case AF_UNSPEC:
-			pr_debug(ctx, "adding peer <floating>");
-			break;
-
-		case AF_INET:
-			if (inet_ntop(AF_INET, &peer->address.in.sin_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "adding peer %s:%u", buf, ntohs(peer->address.in.sin_port));
-			break;
-
-		case AF_INET6:
-			if (inet_ntop(AF_INET6, &peer->address.in6.sin6_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "adding peer [%s]:%u", buf, ntohs(peer->address.in6.sin6_port));
-			break;
-
-		default:
-			exit_bug(ctx, "unsupported address family");
-		}
-	}
+	pr_debug(ctx, "adding peer %P", peer);
 
 	return peer;
 }
@@ -167,75 +141,13 @@ fastd_peer* fastd_peer_add_temp(fastd_context *ctx, const fastd_peer_address *ad
 	peer->state = STATE_TEMP;
 	peer->seen = ctx->now;
 
-	if (is_debug(ctx)) {
-		char buf[INET6_ADDRSTRLEN] = "";
-
-		switch (peer->address.sa.sa_family) {
-		case AF_INET:
-			if (inet_ntop(AF_INET, &peer->address.in.sin_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "adding peer %s:%u (temporary)", buf, ntohs(peer->address.in.sin_port));
-			break;
-
-		case AF_INET6:
-			if (inet_ntop(AF_INET6, &peer->address.in6.sin6_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "adding peer [%s]:%u (temporary)", buf, ntohs(peer->address.in6.sin6_port));
-			break;
-
-		default:
-			exit_bug(ctx, "unsupported address family");
-		}
-	}
+	pr_debug(ctx, "adding peer %P (temporary)", peer);
 
 	return peer;
 }
 
 fastd_peer* fastd_peer_merge(fastd_context *ctx, fastd_peer *perm_peer, fastd_peer *temp_peer) {
-	if (is_debug(ctx)) {
-		char buf[INET6_ADDRSTRLEN];
-		char *str_perm = NULL, *str_temp = NULL;
-
-		switch (perm_peer->address.sa.sa_family) {
-		case AF_UNSPEC:
-			str_perm = strdup("<floating>");
-			break;
-
-		case AF_INET:
-			if (inet_ntop(AF_INET, &perm_peer->address.in.sin_addr, buf, sizeof(buf)))
-				asprintf(&str_perm, "%s:%u", buf, ntohs(perm_peer->address.in.sin_port));
-			break;
-
-		case AF_INET6:
-			if (inet_ntop(AF_INET6, &perm_peer->address.in6.sin6_addr, buf, sizeof(buf)))
-				asprintf(&str_perm, "[%s]:%u", buf, ntohs(perm_peer->address.in.sin_port));
-			break;
-
-		default:
-			exit_bug(ctx, "unsupported address family");
-		}
-
-		switch (temp_peer->address.sa.sa_family) {
-		case AF_UNSPEC:
-			str_temp = strdup("<floating>");
-			break;
-
-		case AF_INET:
-			if (inet_ntop(AF_INET, &temp_peer->address.in.sin_addr, buf, sizeof(buf)))
-				asprintf(&str_temp, "%s:%u", buf, ntohs(temp_peer->address.in.sin_port));
-			break;
-
-		case AF_INET6:
-			if (inet_ntop(AF_INET6, &temp_peer->address.in6.sin6_addr, buf, sizeof(buf)))
-				asprintf(&str_temp, "[%s]:%u", buf, ntohs(temp_peer->address.in.sin_port));
-			break;
-
-		default:
-			exit_bug(ctx, "unsupported address family");
-		}
-
-		pr_debug(ctx, "merging peer %s into %s", str_temp, str_perm);
-		free(str_temp);
-		free(str_perm);
-	}
+	pr_debug(ctx, "merging peer %P into %P", temp_peer, perm_peer);
 
 	perm_peer->address = temp_peer->address;
 	perm_peer->state = fastd_peer_is_established(temp_peer) ? STATE_ESTABLISHED : STATE_WAIT;
@@ -254,28 +166,7 @@ fastd_peer* fastd_peer_merge(fastd_context *ctx, fastd_peer *perm_peer, fastd_pe
 }
 
 void fastd_peer_delete(fastd_context *ctx, fastd_peer *peer) {
-	if (is_debug(ctx)) {
-		char buf[INET6_ADDRSTRLEN];
-
-		switch (peer->address.sa.sa_family) {
-		case AF_UNSPEC:
-			pr_debug(ctx, "deleting peer <floating>");
-			break;
-
-		case AF_INET:
-			if (inet_ntop(AF_INET, &peer->address.in.sin_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "deleting peer %s:%u%s", buf, ntohs(peer->address.in.sin_port), fastd_peer_is_temporary(peer) ? " (temporary)" : "");
-			break;
-
-		case AF_INET6:
-			if (inet_ntop(AF_INET6, &peer->address.in6.sin6_addr, buf, sizeof(buf)))
-				pr_debug(ctx, "deleting peer [%s]:%u%s", buf, ntohs(peer->address.in6.sin6_port), fastd_peer_is_temporary(peer) ? " (temporary)" : "");
-			break;
-
-		default:
-			exit_bug(ctx, "unsupported address family");
-		}
-	}
+	pr_debug(ctx, "deleting peer %P", peer);
 
 	reset_peer(ctx, peer);
 
@@ -339,8 +230,7 @@ void fastd_peer_eth_addr_add(fastd_context *ctx, fastd_peer *peer, const fastd_e
 	
 	ctx->eth_addr[min] = (fastd_peer_eth_addr){ *addr, peer, ctx->now };
 
-	pr_debug(ctx, "Learned new MAC address %02x:%02x:%02x:%02x:%02x:%02x",
-		 addr->data[0], addr->data[1], addr->data[2], addr->data[3], addr->data[4], addr->data[5]);
+	pr_debug(ctx, "Learned new MAC address %E", addr);
 }
 
 void fastd_peer_eth_addr_cleanup(fastd_context *ctx) {
@@ -349,10 +239,8 @@ void fastd_peer_eth_addr_cleanup(fastd_context *ctx) {
 	for (i = 0; i < ctx->n_eth_addr; i++) {
 		if (timespec_diff(&ctx->now, &ctx->eth_addr[i].seen) > ctx->conf->eth_addr_stale_time*1000) {
 			deleted++;
-			pr_debug(ctx, "MAC address %02x:%02x:%02x:%02x:%02x:%02x not seen for more than %u seconds, removing",
-				 ctx->eth_addr[i].addr.data[0], ctx->eth_addr[i].addr.data[1], ctx->eth_addr[i].addr.data[2],
-				 ctx->eth_addr[i].addr.data[3], ctx->eth_addr[i].addr.data[4], ctx->eth_addr[i].addr.data[5],
-				 ctx->conf->eth_addr_stale_time);
+			pr_debug(ctx, "MAC address %E not seen for more than %u seconds, removing",
+				 &ctx->eth_addr[i].addr, ctx->conf->eth_addr_stale_time);
 		}
 		else if (deleted) {
 			ctx->eth_addr[i-deleted] = ctx->eth_addr[i];