fastrpc: decode input buffers

The format of the returned input buffers is specified in
fastrpc.git/inc/listener_buf.h. Add a decoder for the input buffers so
they can be interpreted by functions on the Application Processor side.

fastrpc/iobuffer.c

@@ -0,0 +1,133 @@
+/*
+ * FastRPC reverse tunnel - argument decoder
+ *
+ * Copyright (C) 2023 Richard Acayan
+ *
+ * This file is part of sensh.
+ *
+ * Sensh is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "iobuffer.h"
+
+// See fastrpc.git/inc/remote.h
+#define REMOTE_SCALARS_INBUFS(sc) (((sc) >> 16) & 0xff)
+
+#define MIN(x, y) (((x) < (y)) ? (x) : (y))
+
+static size_t consume_size(struct fastrpc_decoder_context *ctx, size_t len, char *buf)
+{
+	size_t segment;
+
+	segment = MIN(len, 4 - ctx->size_off);
+	memcpy(&(&ctx->size)[ctx->size_off], buf, segment);
+	ctx->size_off = (ctx->size_off + segment) % 4;
+	ctx->align = (ctx->align + segment) & 0x7;
+
+	return segment;
+}
+
+static void try_populate_inbuf(struct fastrpc_decoder_context *ctx)
+{
+	if (!ctx->size_off) {
+		ctx->inbufs[ctx->idx].s = ctx->size;
+		ctx->inbufs[ctx->idx].p = malloc(ctx->size);
+	}
+}
+
+static size_t consume_alignment(struct fastrpc_decoder_context *ctx, size_t len)
+{
+	size_t segment = 0;
+
+	if (ctx->align) {
+		segment = MIN(len, 8 - ctx->align);
+		ctx->align = (ctx->align + segment) & 0x7;
+	}
+
+	return segment;
+}
+
+static size_t consume_buf(struct fastrpc_decoder_context *ctx, size_t len, char *buf)
+{
+	size_t segment;
+
+	segment = MIN(len, ctx->size - ctx->buf_off);
+	memcpy(&ctx->inbufs[ctx->idx].p[ctx->buf_off], buf, segment);
+
+	if (ctx->buf_off + segment >= ctx->size) {
+		ctx->size = 0;
+		ctx->buf_off = 0;
+		ctx->idx++;
+	} else {
+		ctx->buf_off = ctx->buf_off + segment;
+	}
+
+	ctx->align = (ctx->align + segment) & 0x7;
+
+	return segment;
+}
+
+struct fastrpc_decoder_context *inbuf_decode_start(uint32_t sc)
+{
+	struct fastrpc_decoder_context *ctx;
+	int n_inbufs;
+
+	ctx = malloc(sizeof(*ctx));
+	if (ctx == NULL)
+		return ctx;
+
+	ctx->idx = 0;
+	ctx->size = 0;
+	ctx->n_inbufs = REMOTE_SCALARS_INBUFS(sc);
+
+	ctx->size_off = 0;
+	ctx->buf_off = 0;
+	ctx->align = 0;
+
+	ctx->inbufs = malloc(sizeof(*ctx->inbufs) * ctx->n_inbufs);
+	if (ctx->inbufs == NULL)
+		goto err;
+
+	return ctx;
+
+err:
+	free(ctx);
+	return NULL;
+}
+
+void inbuf_decode_free(struct fastrpc_decoder_context *ctx)
+{
+	free(ctx->inbufs);
+	free(ctx);
+}
+
+void inbuf_decode(struct fastrpc_decoder_context *ctx, size_t len, char *buf)
+{
+	size_t off = 0;
+
+	while (off < len && ctx->idx < ctx->n_inbufs) {
+		if (!ctx->size || ctx->size_off) {
+			off += consume_size(ctx, len - off, &buf[off]);
+			try_populate_inbuf(ctx);
+		} else {
+			off += consume_alignment(ctx, len - off);
+			off += consume_buf(ctx, len - off, &buf[off]);
+		}
+	}
+}

fastrpc/iobuffer.h

@@ -0,0 +1,27 @@
+#ifndef IOBUFFER_H
+#define IOBUFFER_H
+
+#include <stddef.h>
+#include <sys/types.h>
+
+struct fastrpc_io_buffer {
+	size_t s;
+	char *p;
+};
+
+struct fastrpc_decoder_context {
+	struct fastrpc_io_buffer *inbufs;
+	int idx;
+	size_t size;
+	int n_inbufs;
+
+	off_t size_off;
+	off_t buf_off;
+	off_t align;
+};
+
+struct fastrpc_decoder_context *inbuf_decode_start(uint32_t sc);
+void inbuf_decode(struct fastrpc_decoder_context *ctx, size_t len, char *buf);
+void inbuf_decode_free(struct fastrpc_decoder_context *ctx);
+
+#endif

fastrpc/listener.c

@@ -21,6 +21,7 @@
 
 #include "fastrpc.h"
 #include "fastrpc_adsp_listener.h"
+#include "iobuffer.h"
 
 static int adsp_listener_init2(int fd)
 {
@@ -50,6 +51,7 @@ static int adsp_listener_next2(int fd,
 
 int run_fastrpc_listener(int fd)
 {
+	struct fastrpc_decoder_context *ctx;
 	uint32_t result = 0xffffffff;
 	uint32_t handle;
 	uint32_t rctx = 0;
@@ -71,6 +73,18 @@ int run_fastrpc_listener(int fd)
 					  0, outbuf,
 					  &rctx, &handle, &sc,
 					  &inbufs_len, 256, inbufs);
+		if (ret)
+			break;
+
+		ctx = inbuf_decode_start(sc);
+		if (!ctx) {
+			ret = -1;
+			break;
+		}
+
+		inbuf_decode(ctx, inbufs_len, inbufs);
+
+		inbuf_decode_free(ctx);
 	}
 
 	return ret;