Implement & format token in libaxl_receive

Signed-off-by: Mattias Andrée <maandree@kth.se>

4 files changed, 132 insertions, 17 deletions

diff --git a/common.h b/common.h
index e0330cb..412a45f 100644
--- a/common.h
+++ b/common.h
@@ -102,16 +102,22 @@ struct libaxl_context {
 	char              *out_buf;
 	size_t             in_buf_size;
 	char              *in_buf;
+	size_t             aux_buf_size;
+	char              *aux_buf;
 };
 
-#define ALIGN(VP, T)\
+#define ALIGN_VAR(VP, ALIGNMENT)\
 	do {\
-		if (*(VP) & (_Alignof(T) - 1)) {\
-			*(VP) |= _Alignof(T) - 1;\
+		if (*(VP) & ((ALIGNMENT) - 1)) {\
+			*(VP) |= (ALIGNMENT) - 1;\
 			*(VP) += 1;\
 		}\
 	} while (0)
 
+#define ALIGN(VP, T) ALIGN_VAR(VP, _Alignof(T))
+
+#define MAX(A, B) ((A) > (B) ? (A) : (B))
+
 #if INT_MIN + 1 == -INT_MAX
 # define TWOS_COMPLEMENT8(VP)  ((void)0)
 # define TWOS_COMPLEMENT16(VP) ((void)0)
diff --git a/libaxl_context_create.c b/libaxl_context_create.c
index da03ecf..92ed94d 100644
--- a/libaxl_context_create.c
+++ b/libaxl_context_create.c
@@ -13,6 +13,8 @@ libaxl_context_create(LIBAXL_CONNECTION *conn)
 		ctx->out_buf      = NULL;
 		ctx->in_buf_size  = 0;
 		ctx->in_buf       = NULL;
+		ctx->aux_buf_size = 0;
+		ctx->aux_buf      = NULL;
 	}
 	return ctx;
 }
diff --git a/libaxl_context_free.c b/libaxl_context_free.c
index c86f9de..91ce159 100644
--- a/libaxl_context_free.c
+++ b/libaxl_context_free.c
@@ -7,6 +7,7 @@ libaxl_context_free(LIBAXL_CONTEXT *ctx)
 	if (ctx && !--ctx->refcount) {
 		free(ctx->out_buf);
 		free(ctx->in_buf);
+		free(ctx->aux_buf);
 		free(ctx);
 	}
 }
diff --git a/libaxl_receive.c b/libaxl_receive.c
index b17cdc9..8455237 100644
--- a/libaxl_receive.c
+++ b/libaxl_receive.c
@@ -64,8 +64,8 @@ static const char *const reply_formats[] = {
 	 * U = STRING16, align output to (void *) and enter (equivalent to "*2/")
 	 * d = data, uses %, align output to (void *) and enter
 	 * * = following are repeated, align output to (void *) and enter
-	 * & = * but requires allocation
-	 * / = end of * or &
+	 * & = * but requires allocation (there is only support for use once) (choose padding after alignment)
+	 * / = end of * or &, for & unless finished realigned output and start over
 	 * p = padding, align input to byte-quad
 	 */
 	[0]                                         = NULL,
@@ -191,6 +191,89 @@ static const char *const reply_formats[] = {
 	[LIBAXL_REQUEST_NO_OPERATION]               = NULL
 };
 
+static size_t
+calculate_struct_size(const char *fmt, size_t *alignmentp)
+{
+	size_t size = 0, i;
+	*alignmentp = 1;
+	for (;; fmt++) {
+		switch (*fmt) {
+		case '?':
+		case '$':
+		case '%':
+		case 'p':
+			break;
+
+		case '.':
+		case '!':
+		case '1':
+		case 'z':
+			size += 1;
+			break;
+
+		case ',':
+		case '2':
+		case 's':
+			*alignmentp = MAX(*alignmentp, 2);
+			size += 2;
+			break;
+
+		case '_':
+			size += 3;
+			break;
+
+		case '-':
+		case '4':
+		case 'S':
+			*alignmentp = MAX(*alignmentp, 4);
+			size += 4;
+			break;
+
+		case '=':
+			*alignmentp = MAX(*alignmentp, 4); /* 2 uint32_t, not 1 uint64_t */
+			size += 8;
+			break;
+
+		case '#':
+			size += 32;
+			break;
+
+		case '@':
+			*alignmentp = MAX(*alignmentp, sizeof(size_t));
+			ALIGN(&size, size_t);
+			size += sizeof(size_t);
+			break;
+
+		case '*':
+			for (i = 0;; fmt++) {
+				if (*fmt == '*') {
+					i += 1;
+				} else if (*fmt == '/') {
+					if (!--i)
+						break;
+				} else if (!*fmt) {
+					abort();
+				}
+			}
+			/* fall through */
+
+		case 'u':
+		case 'U':
+		case 'd':
+			*alignmentp = MAX(*alignmentp, sizeof(void *));
+			ALIGN(&size, void *);
+			size += sizeof(void *);
+			break;
+
+		case '/':
+			return size;
+
+		default:
+			abort();
+		}
+	}
+}
+
 /* TODO make it possible to prefetch pending messages */
 int
 libaxl_receive(LIBAXL_CONTEXT *restrict ctx, union libaxl_input *restrict msgp, int flags)
@@ -209,13 +292,11 @@ libaxl_receive(LIBAXL_CONTEXT *restrict ctx, union libaxl_input *restrict msgp,
 	ssize_t r;
 	uint64_t n, u64;
 	uint8_t code;
-	size_t t, i, j, o, ic = 0, oc = 0;
+	size_t t, i, j, o, ic = 0, oc = 0, size, alignment = 1;
 	int qc = 0;
 #ifdef MSG_TRUNC
 	int flag_trunc;
-#endif
 
-#ifdef MSG_TRUNC
 	flags ^= flag_trunc = flags & MSG_TRUNC;
 #endif
 
@@ -580,6 +661,7 @@ received_reply:
 			break;
 
 		case '*':
+			alignment = 1;
 			ALIGN(&o, void *);
 			*(void **)&msg[o] = data = &inbuf[i];
 			o += sizeof(void *);
@@ -597,20 +679,44 @@ received_reply:
 				goto jump_to_end_of_repeat;
 			break;
 
-		case '&': /* TODO */
-			fprintf(stderr, "libaxl_receive: function not fully implemented: '&' case\n");
-			abort();
-			/*
-			  LIBAXL_REQUEST_LIST_FONTS
-			  LIBAXL_REQUEST_GET_FONT_PATH
-			  LIBAXL_REQUEST_LIST_EXTENSIONS
-			  LIBAXL_REQUEST_LIST_HOSTS
-			 */
+		case '&':
+			size = calculate_struct_size(&fmt[1], &alignment);
+			ALIGN_VAR(&size, alignment);
+			alignment = size;
+			data = NULL;
+			if (counts[oc]) {
+				size *= counts[oc];
+				if (size > ctx->aux_buf_size) {
+					data = liberror_realloc(ctx->aux_buf, size);
+					if (!data)
+						return LIBAXL_ERROR_SYSTEM;
+					ctx->aux_buf = data;
+					ctx->aux_buf_size = size;
+				} else {
+					data = ctx->aux_buf;
+				}
+			}
+			ALIGN(&o, void *);
+			*(void **)&msg[o] = data;
+			o += sizeof(void *);
+			stack[si].fmt = fmt;
+			stack[si].msg = msg;
+			stack[si].ic = ic;
+			stack[si].oc = oc + 1;
+			stack[si].o = o;
+			stack[si].count = count;
+			si += 1;
+			count = counts[oc++];
+			o = 0;
+			msg = data;
+			if (!count)
+				goto jump_to_end_of_repeat;
 			break;
 
 		case '/':
 			if (--count) {
 				fmt = stack[si - 1].fmt;
+				ALIGN_VAR(&o, alignment);
 			} else {
 				if (0) {
 				jump_to_end_of_repeat: