fastrpc: listener: add local endpoint

The reverse tunnel has at least some competence in interacting with the
remote processor. Add a local endpoint so the Application Processor can
respond to ADSP requests using this.

fastrpc/listener.c

@@ -24,6 +24,10 @@
 #include "fastrpc.h"
 #include "fastrpc_adsp_listener.h"
 #include "iobuffer.h"
+#include "listener.h"
+
+const struct fastrpc_interface *interfaces[] = {
+};
 
 static int adsp_listener_init2(int fd)
 {
@@ -51,20 +55,94 @@ static int adsp_listener_next2(int fd,
 			inbufs_size, inbufs);
 }
 
+static struct fastrpc_io_buffer *allocate_outbufs(const struct fastrpc_function_def_interp2 *def,
+						  uint32_t *first_inbuf)
+{
+	struct fastrpc_io_buffer *out;
+	size_t out_count;
+	size_t i, j;
+	off_t off;
+	uint32_t *sizes;
+
+	out_count = def->out_bufs + (def->out_nums && 1);
+	out = malloc(sizeof(struct fastrpc_io_buffer) * out_count);
+	if (out == NULL)
+		return NULL;
+
+	out[0].s = def->out_nums * 4;
+	if (out[0].s) {
+		out[0].p = malloc(def->out_nums * 4);
+		if (out[0].p == NULL)
+			goto err_free_out;
+	}
+
+	off = def->out_nums && 1;
+	sizes = &first_inbuf[def->in_nums + def->in_bufs];
+
+	for (i = 0; i < def->out_bufs; i++) {
+		out[off + i].s = sizes[i];
+		out[off + i].p = malloc(sizes[i]);
+		if (out[off + i].p == NULL)
+			goto err_free_prev;
+	}
+
+	return out;
+
+err_free_prev:
+	for (j = 0; j < i; j++)
+		free(out[off + j].p);
+
+err_free_out:
+	free(out);
+	return NULL;
+}
+
+static int check_inbuf_sizes(const struct fastrpc_function_def_interp2 *def,
+			     const struct fastrpc_io_buffer *inbufs)
+{
+	uint8_t i;
+	const uint32_t *sizes = &((const uint32_t *) inbufs[0].p)[def->in_nums];
+
+	if (inbufs[0].s != 4 * (def->in_nums
+			      + def->in_bufs
+			      + def->out_bufs))
+		return -1;
+
+	for (i = 0; i < def->in_bufs; i++) {
+		if (inbufs[i + 1].s != sizes[i])
+			return -1;
+	}
+
+	return 0;
+}
+
 static int return_for_next_invoke(int fd,
 				  uint32_t result,
 				  uint32_t *rctx,
 				  uint32_t *handle,
 				  uint32_t *sc,
+				  const struct fastrpc_io_buffer *returned,
 				  struct fastrpc_io_buffer **decoded)
 {
 	struct fastrpc_decoder_context *ctx;
 	char inbufs[256];
 	char *outbufs = NULL;
 	uint32_t inbufs_len;
-	uint32_t outbufs_len = 0;
+	uint32_t outbufs_len;
 	int ret;
 
+	outbufs_len = outbufs_calculate_size(REMOTE_SCALARS_OUTBUFS(*sc), returned);
+
+	if (outbufs_len) {
+		outbufs = malloc(outbufs_len);
+		if (outbufs == NULL) {
+			perror("Could not allocate encoded output buffer");
+			return -1;
+		}
+
+		outbufs_encode(REMOTE_SCALARS_OUTBUFS(*sc), returned, outbufs);
+	}
+
 	ret = adsp_listener_next2(fd,
 				  *rctx, result,
 				  outbufs_len, outbufs,
@@ -72,18 +150,20 @@ static int return_for_next_invoke(int fd,
 				  &inbufs_len, 256, inbufs);
 	if (ret) {
 		fprintf(stderr, "Could not fetch next FastRPC message: %d\n", ret);
-		return ret;
+		goto err_free_outbufs;
 	}
 
 	if (inbufs_len > 256) {
 		fprintf(stderr, "Large (>256B) input buffers aren't implemented\n");
-		return -1;
+		ret = -1;
+		goto err_free_outbufs;
 	}
 
 	ctx = inbuf_decode_start(*sc);
 	if (!ctx) {
 		perror("Could not start decoding\n");
-		return -1;
+		ret = -1;
+		goto err_free_outbufs;
 	}
 
 	inbuf_decode(ctx, inbufs_len, inbufs);
@@ -96,17 +176,64 @@ static int return_for_next_invoke(int fd,
 
 	*decoded = inbuf_decode_finish(ctx);
 
+err_free_outbufs:
+	free(outbufs);
+	return ret;
+}
+
+static int invoke_requested_procedure(uint32_t handle,
+				      uint32_t sc,
+			              uint32_t *result,
+				      const struct fastrpc_io_buffer *decoded,
+				      struct fastrpc_io_buffer **returned)
+{
+	const struct fastrpc_function_impl *impl;
+	uint8_t in_count;
+	uint8_t out_count;
+	size_t i;
+	int ret;
+
+	if (handle >= sizeof(interfaces) / sizeof(*interfaces)
+	 || REMOTE_SCALARS_METHOD(sc) >= interfaces[handle]->n_procs)
+		return -1;
+
+	impl = &interfaces[handle]->procs[REMOTE_SCALARS_METHOD(sc)];
+
+	if (impl->def == NULL
+	 || impl->impl == NULL)
+		return -1;
+
+	in_count = impl->def->in_bufs + ((impl->def->in_nums
+				       || impl->def->in_bufs
+				       || impl->def->out_bufs) && 1);
+	out_count = impl->def->out_bufs + (impl->def->out_nums && 1);
+
+	if (REMOTE_SCALARS_INBUFS(sc) != in_count
+	 || REMOTE_SCALARS_OUTBUFS(sc) != out_count)
+		return -1;
+
+	ret = check_inbuf_sizes(impl->def, decoded);
+	if (ret)
+		return ret;
+
+	*returned = allocate_outbufs(impl->def, decoded[0].p);
+	if (*returned == NULL && out_count > 0)
+		return -1;
+
+	*result = impl->impl(decoded, *returned);
+
 	return 0;
 }
 
 int run_fastrpc_listener(int fd)
 {
-	struct fastrpc_io_buffer *decoded = NULL;
+	struct fastrpc_io_buffer *decoded = NULL,
+				 *returned = NULL;
 	struct fastrpc_decoder_context *ctx;
 	uint32_t result = 0xffffffff;
 	uint32_t handle;
 	uint32_t rctx = 0;
-	uint32_t sc;
+	uint32_t sc = REMOTE_SCALARS_MAKE(0, 0, 0);
 	int ret;
 
 	ret = adsp_listener_init2(fd);
@@ -118,7 +245,15 @@ int run_fastrpc_listener(int fd)
 	while (!ret) {
 		ret = return_for_next_invoke(fd,
 					     result, &rctx, &handle, &sc,
-					     &decoded);
+					     returned, &decoded);
+		if (ret)
+			break;
+
+		if (returned != NULL)
+			iobuf_free(REMOTE_SCALARS_OUTBUFS(sc), returned);
+
+		ret = invoke_requested_procedure(handle, sc, &result,
+						 decoded, &returned);
 		if (ret)
 			break;
 

fastrpc/listener.h

@@ -1,6 +1,21 @@
 #ifndef LISTENER_H
 #define LISTENER_H
 
+#include "fastrpc.h"
+#include "iobuffer.h"
+
+struct fastrpc_function_impl {
+	const struct fastrpc_function_def_interp2 *def;
+	uint32_t (*impl)(const struct fastrpc_io_buffer *inbufs,
+			 struct fastrpc_io_buffer *outbufs);
+};
+
+struct fastrpc_interface {
+	const char *name;
+	uint8_t n_procs;
+	struct fastrpc_function_impl procs[];
+};
+
 int run_fastrpc_listener(int fd);
 
 #endif