diff --git a/drivers/nble/Makefile b/drivers/nble/Makefile index a9d8829a3ab..c16f9363620 100644 --- a/drivers/nble/Makefile +++ b/drivers/nble/Makefile @@ -1 +1 @@ -obj-$(CONFIG_NBLE) += gap.o conn.o gatt.o uart.o rpc_serialize.o +obj-$(CONFIG_NBLE) += gap.o conn.o gatt.o uart.o rpc_serialize.o rpc_deserialize.o diff --git a/drivers/nble/rpc_deserialize.c b/drivers/nble/rpc_deserialize.c new file mode 100644 index 00000000000..f24edfaf254 --- /dev/null +++ b/drivers/nble/rpc_deserialize.c @@ -0,0 +1,521 @@ +/* + * 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 +#include + +#ifdef CONFIG_PRINTK +#include +#define PRINTK(...) printk(__VA_ARGS__) +#else +#define PRINTK(...) +#endif /* CONFIG_PRINTK */ + +#include "rpc.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; + } + } +}