zephyr/drivers/nble/rpc_deserialize.c

/*
 * Copyright (c) 2016 Intel Corporation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdlib.h>
#include <string.h>

#ifdef CONFIG_PRINTK
#include <misc/printk.h>
#define PRINTK(...) printk(__VA_ARGS__)
#else
#define PRINTK(...)
#endif /* CONFIG_PRINTK */

#include "rpc.h"

#include "gap_internal.h"
#include "gatt_internal.h"

#include "rpc_functions_to_quark.h"

/* Build the list of prototypes and check that list are made only of matching
 * signatures
 */
#define FN_SIG_NONE(__fn)	void __fn(void);
LIST_FN_SIG_NONE
#undef FN_SIG_NONE

#define FN_SIG_S(__fn, __s)	void __fn(__s p_s);
LIST_FN_SIG_S
#undef FN_SIG_S

#define FN_SIG_P(__fn, __type)	void __fn(__type priv);
LIST_FN_SIG_P
#undef FN_SIG_P

#define FN_SIG_S_B(__fn, __s, __type, __length)				\
	void __fn(__s p_s, __type buf, __length length);
LIST_FN_SIG_S_B
#undef FN_SIG_S_B

#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2,	\
		     __type3)						\
	void __fn(__type1 p_buf1, __length1 length1, __type2 p_buf2,	\
		  __length2 length2, __type3 priv);
LIST_FN_SIG_B_B_P
#undef FN_SIG_B_B_P

#define FN_SIG_S_P(__fn, __s, __type)	void __fn(__s p_s, __type priv);
LIST_FN_SIG_S_P
#undef FN_SIG_S_P

#define FN_SIG_S_B_P(__fn, __s,  __type, __length, __type_ptr)		\
	void __fn(__s p_s, __type buf, __length length, __type_ptr priv);
LIST_FN_SIG_S_B_P
#undef FN_SIG_S_B_P

#define FN_SIG_S_B_B_P(__fn, __s,  __type1, __length1, __type2,		\
		       __length2, __type_ptr)				\
	void __fn(__s p_s, __type1 p_buf1, __length1 length1,		\
		  __type2 p_buf2, __length2 length2, __type_ptr priv);
LIST_FN_SIG_S_B_B_P
#undef FN_SIG_S_B_B_P

/* 1 - define the size check arrays */
#define FN_SIG_NONE(__fn)

#define FN_SIG_S(__fn, __s)				sizeof(*((__s)0)),

#define FN_SIG_P(__fn, __type)

#define FN_SIG_S_B(__fn, __s, __type, __length)		sizeof(*((__s)0)),

#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2,	\
		     __type3)				sizeof(*((__s)0)),

#define FN_SIG_S_P(__fn, __s, __type)			sizeof(*((__s)0)),

#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)		\
							sizeof(*((__s)0)),

#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2,		\
		       __length2, __type3)		sizeof(*((__s)0)),

static uint8_t m_size_s[] = { LIST_FN_SIG_S };
static uint8_t m_size_s_b[] = { LIST_FN_SIG_S_B };
static uint8_t m_size_s_p[] = { LIST_FN_SIG_S_P };
static uint8_t m_size_s_b_p[] = { LIST_FN_SIG_S_B_P };
static uint8_t m_size_s_b_b_p[] = { LIST_FN_SIG_S_B_B_P };

#undef FN_SIG_NONE
#undef FN_SIG_S
#undef FN_SIG_P
#undef FN_SIG_S_B
#undef FN_SIG_B_B_P
#undef FN_SIG_S_P
#undef FN_SIG_S_B_P
#undef FN_SIG_S_B_B_P

/* 2- build the enumerations list */
#define FN_SIG_NONE(__fn)				fn_index_##__fn,
#define FN_SIG_S(__fn, __s)				FN_SIG_NONE(__fn)
#define FN_SIG_P(__fn, __type)				FN_SIG_NONE(__fn)
#define FN_SIG_S_B(__fn, __s, __type, __length)		FN_SIG_NONE(__fn)
#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2,	\
		     __type3)				FN_SIG_NONE(__fn)
#define FN_SIG_S_P(__fn, __s, __type)			FN_SIG_NONE(__fn)
#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)		\
							FN_SIG_NONE(__fn)
#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2,		\
		       __length2, __type3)		FN_SIG_NONE(__fn)

/* Build the list of function indexes in the deserialization array */
enum { LIST_FN_SIG_NONE fn_none_index_max };
enum { LIST_FN_SIG_S fn_s_index_max };
enum { LIST_FN_SIG_P fn_p_index_max };
enum { LIST_FN_SIG_S_B fn_s_b_index_max };
enum { LIST_FN_SIG_B_B_P fn_b_b_p_index_max };
enum { LIST_FN_SIG_S_P fn_s_p_index_max };
enum { LIST_FN_SIG_S_B_P fn_s_b_p_index_max };
enum { LIST_FN_SIG_S_B_B_P fn_s_b_b_p_index_max };

#undef FN_SIG_NONE
#undef FN_SIG_S
#undef FN_SIG_P
#undef FN_SIG_S_B
#undef FN_SIG_B_B_P
#undef FN_SIG_S_P
#undef FN_SIG_S_B_P
#undef FN_SIG_S_B_B_P

/* 3- build the array */
#define FN_SIG_NONE(__fn)			[fn_index_##__fn] =	\
								(void *)__fn,
#define FN_SIG_S(__fn, __s)			FN_SIG_NONE(__fn)
#define FN_SIG_P(__fn, __type)			FN_SIG_NONE(__fn)
#define FN_SIG_S_B(__fn, __s, __type, __length)				\
						FN_SIG_NONE(__fn)
#define FN_SIG_B_B_P(__fn, __type1, __length1, __type2, __length2,	\
		     __type3)			FN_SIG_NONE(__fn)
#define FN_SIG_S_P(__fn, __s, __type)		FN_SIG_NONE(__fn)
#define FN_SIG_S_B_P(__fn, __s, __type, __length, __type_ptr)		\
						FN_SIG_NONE(__fn)
#define FN_SIG_S_B_B_P(__fn, __s, __type1, __length1, __type2,		\
		       __length2, __type3)	FN_SIG_NONE(__fn)

static void (*m_fct_none[])(void) = { LIST_FN_SIG_NONE };
static void (*m_fct_s[])(void *structure) = { LIST_FN_SIG_S };
static void (*m_fct_p[])(void *pointer) = { LIST_FN_SIG_P };
static void (*m_fct_s_b[])(void *structure, void *buffer,
			   uint8_t length) = { LIST_FN_SIG_S_B };
static void (*m_fct_b_b_p[])(void *buffer1, uint8_t length1,
			     void *buffer2, uint8_t length2,
			     void *pointer) = { LIST_FN_SIG_B_B_P };
static void (*m_fct_s_p[])(void *structure,
			   void *pointer) = { LIST_FN_SIG_S_P };
static void (*m_fct_s_b_p[])(void *structure, void *buffer, uint8_t length,
			     void *pointer) = { LIST_FN_SIG_S_B_P };
static void (*m_fct_s_b_b_p[])(void *structure, void *buffer1, uint8_t length1,
			       void *buffer2, uint8_t length2,
			       void *pointer) = { LIST_FN_SIG_S_B_B_P };

static void panic(int err)
{
	PRINTK("panic: errcode %d", err);

	while (true) {
	}
}

static const uint8_t *deserialize_struct(const uint8_t *p,
					 const uint8_t **pp_struct,
					 uint8_t *p_struct_length)
{
	uint8_t struct_length;

	struct_length = *p++;
	*pp_struct = p;
	*p_struct_length = struct_length;

	return p + struct_length;
}

static const uint8_t *deserialize_buf(const uint8_t *p, const uint8_t **pp_buf,
				      uint16_t *p_buflen)
{
	uint8_t b;
	uint16_t buflen;

	/* Get the current byte */
	b = *p++;
	buflen = b & 0x7F;
	if (b & 0x80) {
		/* Get the current byte */
		b = *p++;
		buflen += (uint16_t)b << 7;
	}

	/* Return the values */
	*pp_buf = p;
	*p_buflen = buflen;
	p += buflen;

	return p;
}

static void deserialize_none(uint8_t fn_index, const uint8_t *buf,
			     uint16_t length)
{
	(void)buf;

	if (length != 0) {
		panic(-1);
	}

	m_fct_none[fn_index]();
}

static void deserialize_s(uint8_t fn_index, const uint8_t *buf, uint16_t length)
{
	const uint8_t *p_struct_data;
	uint8_t struct_length;
	const uint8_t *p;

	p = deserialize_struct(buf, &p_struct_data, &struct_length);

	if ((length != (p - buf)) || (struct_length != m_size_s[fn_index])) {
		panic(-1);
	} else {
		/* Always align structures on word boundary */
		uintptr_t struct_data[(struct_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];

		memcpy(struct_data, p_struct_data, struct_length);

		m_fct_s[fn_index](struct_data);
	}
}

static void deserialize_p(uint8_t fn_index, const uint8_t *buf, uint16_t length)
{
	uintptr_t priv;

	if (length != 4) {
		panic(-1);
	}

	/* little endian conversion */
	priv = buf[0] | (buf[1] << 8) | (buf[2] << 16) | (buf[3] << 24);

	m_fct_p[fn_index]((void *)priv);
}

static void deserialize_s_b(uint8_t fn_index, const uint8_t *buf,
			    uint16_t length)
{
	const uint8_t *p_struct_data;
	uint8_t struct_length;
	const uint8_t *p_vbuf;
	uint16_t vbuf_length;
	const uint8_t *p;

	p = deserialize_struct(buf, &p_struct_data, &struct_length);
	p = deserialize_buf(p, &p_vbuf, &vbuf_length);

	if ((length != (p - buf)) || (struct_length != m_size_s_b[fn_index])) {
		panic(-1);
	} else {
		/* Always align structures on word boundary */
		uintptr_t struct_data[(struct_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		uintptr_t vbuf[(vbuf_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		void *buf = NULL;

		memcpy(struct_data, p_struct_data, struct_length);

		if (vbuf_length) {
			memcpy(vbuf, p_vbuf, vbuf_length);
			buf = vbuf;
		}

		m_fct_s_b[fn_index](struct_data, buf, vbuf_length);
	}
}

static void deserialize_b_b_p(uint8_t fn_index, const uint8_t *buf,
			      uint16_t length)
{
	const uint8_t *p_vbuf1;
	uint16_t vbuf1_length;
	const uint8_t *p_vbuf2;
	uint16_t vbuf2_length;
	uintptr_t priv;
	const uint8_t *p;

	p = deserialize_buf(buf, &p_vbuf1, &vbuf1_length);
	p = deserialize_buf(p, &p_vbuf2, &vbuf2_length);
	p += 4;

	if (length != (p - buf)) {
		panic(-1);
	} else {
		/* Always align structures on word boundary */
		uintptr_t vbuf1[(vbuf1_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		uintptr_t vbuf2[(vbuf2_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		void *buf1 = NULL;
		void *buf2 = NULL;

		if (vbuf1_length) {
			memcpy(vbuf1, p_vbuf1, vbuf1_length);
			buf1 = vbuf1;
		}

		if (vbuf2_length) {
			memcpy(vbuf2, p_vbuf2, vbuf2_length);
			buf2 = vbuf2;
		}

		p = p_vbuf2 + vbuf2_length;

		/* little endian conversion */
		priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);

		m_fct_b_b_p[fn_index](buf1, vbuf1_length, buf2, vbuf2_length,
				      (void *)priv);
	}
}

static void deserialize_s_p(uint8_t fn_index, const uint8_t *buf,
			    uint16_t length)
{
	const uint8_t *p_struct_data;
	uint8_t struct_length;
	uintptr_t priv;
	const uint8_t *p;

	p = deserialize_struct(buf, &p_struct_data, &struct_length);
	p += 4;

	if ((length != (p - buf)) || (struct_length != m_size_s_p[fn_index])) {
		panic(-1);
	} else {
		/* Always align structures on word boundary */
		uintptr_t struct_data[(struct_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];

		memcpy(struct_data, p_struct_data, struct_length);
		p = p_struct_data + struct_length;

		/* little endian conversion */
		priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);

		m_fct_s_p[fn_index](struct_data, (void *)priv);
	}
}

static void deserialize_s_b_p(uint8_t fn_index, const uint8_t *buf,
			      uint16_t length)
{
	const uint8_t *p_struct_data;
	uint8_t struct_length;
	const uint8_t *p_vbuf;
	uint16_t vbuf_length;
	uintptr_t priv;
	const uint8_t *p;

	p = deserialize_struct(buf, &p_struct_data, &struct_length);
	p = deserialize_buf(p, &p_vbuf, &vbuf_length);
	p += 4;

	if ((length != (p - buf)) || (struct_length != m_size_s_b_p[fn_index])) {
		panic(-1);
	} else {
		/* Always align structures on word boundary */
		uintptr_t struct_data[(struct_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		uintptr_t vbuf[(vbuf_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		void *buf = NULL;

		memcpy(struct_data, p_struct_data, struct_length);

		if (vbuf_length) {
			memcpy(vbuf, p_vbuf, vbuf_length);
			buf = vbuf;
		}
		p = p_vbuf + vbuf_length;

		/* little endian conversion */
		priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);

		m_fct_s_b_p[fn_index](struct_data, buf, vbuf_length,
				      (void *)priv);
	}
}

static void deserialize_s_b_b_p(uint8_t fn_index, const uint8_t *buf,
				uint16_t length)
{
	const uint8_t *p_struct_data;
	uint8_t struct_length;
	const uint8_t *p_vbuf1;
	uint16_t vbuf1_length;
	const uint8_t *p_vbuf2;
	uint16_t vbuf2_length;
	uintptr_t priv;
	const uint8_t *p;

	p = deserialize_struct(buf, &p_struct_data, &struct_length);
	p = deserialize_buf(p, &p_vbuf1, &vbuf1_length);
	p = deserialize_buf(p, &p_vbuf2, &vbuf2_length);
	p += 4;

	if ((length != (p - buf)) ||
	    (struct_length != m_size_s_b_b_p[fn_index])) {
		panic(-1);
	} else {
		/* Always align structures on word boundary */
		uintptr_t struct_data[(struct_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		uintptr_t vbuf1[(vbuf1_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		uintptr_t vbuf2[(vbuf2_length +
				(sizeof(uintptr_t) - 1))/(sizeof(uintptr_t))];
		void *buf1 = NULL;
		void *buf2 = NULL;

		memcpy(struct_data, p_struct_data, struct_length);

		if (vbuf1_length) {
			memcpy(vbuf1, p_vbuf1, vbuf1_length);
			buf1 = vbuf1;
		}

		if (vbuf2_length) {
			memcpy(vbuf2, p_vbuf2, vbuf2_length);
			buf2 = vbuf2;
		}

		p = p_vbuf2 + vbuf2_length;

		/* little endian conversion */
		priv = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);

		m_fct_s_b_b_p[fn_index](struct_data, buf1, vbuf1_length, buf2,
					vbuf2_length, (void *)priv);
	}
}

void rpc_deserialize(const uint8_t *buf, uint16_t length)
{

	uint8_t fn_index;
	uint8_t sig_type;

	if (buf) {
		sig_type = buf[0];
		fn_index = buf[1];

		buf += 2;
		length -= 2;

		switch (sig_type) {
		case SIG_TYPE_NONE:
			if (sizeof(m_fct_none)) {
				deserialize_none(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_S:
			if (sizeof(m_fct_s)) {
				deserialize_s(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_P:
			if (sizeof(m_fct_p)) {
				deserialize_p(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_S_B:
			if (sizeof(m_fct_s_b)) {
				deserialize_s_b(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_B_B_P:
			if (sizeof(m_fct_b_b_p)) {
				deserialize_b_b_p(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_S_P:
			if (sizeof(m_fct_s_p)) {
				deserialize_s_p(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_S_B_P:
			if (sizeof(m_fct_s_b_p)) {
				deserialize_s_b_p(fn_index, buf, length);
			}
			break;
		case SIG_TYPE_S_B_B_P:
			if (sizeof(m_fct_s_b_b_p)) {
				deserialize_s_b_b_p(fn_index, buf, length);
			}
			break;
		default:
			panic(-1);
			break;
		}
	}
}