First draft

doc/examples/OBC+TTC/shared/incp/service.c

@@ -0,0 +1,177 @@
+#define _INCP_INTERNAL
+
+#include <string.h>
+
+#include "incp2.h"
+#include "service.h"
+#include "messaging.h"
+#include "asn1/asn1_wrapper.h"
+
+
+
+static err_t handle_incoming(endpoint_t source, BitStream *in_bs);
+
+
+
+void incp_receiver_task(void *args)
+{
+	(void)args; //Stfu
+
+	while (1)
+	{
+		//LATER: Blocking wait for RTOS queue to fetch bus / radio message (go wild on that routing)
+		BitStream in_bs = { 0 };						// <-- Populate from transport
+		endpoint_t source = INCP_ENDPOINT_NVAL;			// <-- Populate from transport
+
+		handle_incoming(source, &in_bs);
+	}
+}
+
+
+
+err_t incp_async_datareq(endpoint_t subsystem, U32 id)
+{
+	err_t err; //Don't init on purpose
+	BitStream out_bs;
+	uint8_t buf[DEFAULT_MSGBUF_SIZE];
+	BitStream_Init(&out_bs, buf, sizeof(buf));
+
+	IncpMsg msg;
+	msg.kind = dataReq_PRESENT;
+	msg.u.dataReq.id = id;
+
+	err = incp_encode_header(&out_bs, &msg);
+	if (err != ERR_SUCCESS) goto _exit;
+
+	err = impl_transmit(subsystem, buf, BitStream_GetLength(&out_bs));
+
+_exit:
+	return err;
+}
+
+err_t incp_async_ping(endpoint_t subsystem, uint8_t *bytes)
+{
+	err_t err; //Don't init on purpose
+	BitStream out_bs;
+	uint8_t buf[DEFAULT_MSGBUF_SIZE];
+	BitStream_Init(&out_bs, buf, sizeof(buf));
+
+	IncpMsg msg;
+	msg.kind = ping_PRESENT;
+	memcpy(&msg.u.ping.payload, bytes != NULL ? bytes : "I am a very fancy ping payload!", 32);
+	
+	err = incp_encode_header(&out_bs, &msg);
+	if (err != ERR_SUCCESS) goto _exit;
+
+	err = impl_transmit(subsystem, buf, BitStream_GetLength(&out_bs));
+
+_exit:
+	return err;
+}
+
+
+
+static inline err_t handle_incoming_ping(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
+{
+	(void)in_bs;
+	err_t err; //Don't init on purpose
+	BitStream out_bs;
+	uint8_t buf[DEFAULT_MSGBUF_SIZE];
+	BitStream_Init(&out_bs, buf, sizeof(buf));
+
+	IncpMsg msg = *in_msg;
+	msg.kind = pong_PRESENT;
+
+	err = incp_encode_header(&out_bs, &msg);
+	if (err != ERR_SUCCESS) goto _exit;
+
+	err = impl_transmit(source, buf, BitStream_GetLength(&out_bs)); //"Destroy a Ghast with a fireball"
+
+_exit:
+	return err;
+}
+
+static inline err_t handle_incoming_pong(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
+{
+	(void)in_bs;
+	err_t err; //Don't init on purpose
+
+	impl_callback_pong(source, in_msg);
+	err = ERR_SUCCESS;
+
+_exit:
+	return err;
+}
+
+static inline err_t handle_incoming_datareq(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
+{
+	err_t err; //Don't init on purpose
+	BitStream out_bs;
+	uint8_t buf[DEFAULT_MSGBUF_SIZE];
+	BitStream_Init(&out_bs, buf, sizeof(buf));
+
+	AnyData data = { 0 };
+	data.kind = in_msg->u.dataReq.id; //REVIEW: Dubious types
+
+	err = impl_populate_dataret(&data);
+	if (err != ERR_SUCCESS) goto _exit;
+
+	err = incp_encode_dataret(&out_bs, &data);
+	if (err != ERR_SUCCESS) goto _exit;
+
+	err = impl_transmit(source, buf, BitStream_GetLength(&out_bs));
+
+_exit:
+	return err;
+}
+
+static inline err_t handle_incoming_dataret(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
+{
+	err_t err; //Don't init on purpose
+	AnyData data; //Careful with lifetime of this! Rust my beloved (never tried it)
+
+	err = incp_decode_dataret(in_bs, &data);
+	if (err != ERR_SUCCESS) goto _exit;
+
+	impl_callback_dataret(source, &data);
+	err = ERR_SUCCESS;
+
+_exit:
+	return err;
+}
+
+static err_t handle_incoming(endpoint_t source, BitStream *in_bs)
+{
+	err_t err; //Don't init on purpose
+	IncpMsg msg;
+
+
+	err = incp_decode_header(in_bs, &msg);
+	if (err != ERR_SUCCESS) goto _exit;
+	
+	switch (msg.kind)
+	{
+	case ping_PRESENT:
+		err = handle_incoming_ping(source, &msg, in_bs);
+		break;
+
+	case pong_PRESENT:
+		err = handle_incoming_pong(source, &msg, in_bs);
+		break;
+
+	case dataReq_PRESENT:
+		err = handle_incoming_datareq(source, &msg, in_bs);
+		break;
+
+	case dataRet_PRESENT:
+		err = handle_incoming_dataret(source, &msg, in_bs);
+		break;
+	
+	default:
+		err = ERR_NOT_IMPL;
+		break;
+	}
+
+_exit:
+	return err;
+}