diff options
Diffstat (limited to 'src/aclk/mqtt_websockets')
25 files changed, 6376 insertions, 0 deletions
diff --git a/src/aclk/mqtt_websockets/.github/workflows/run-tests.yaml b/src/aclk/mqtt_websockets/.github/workflows/run-tests.yaml new file mode 100644 index 000000000..da5dde821 --- /dev/null +++ b/src/aclk/mqtt_websockets/.github/workflows/run-tests.yaml @@ -0,0 +1,14 @@ +name: run-tests +on: + push: + schedule: + - cron: '5 3 * * 0' + pull_request: +jobs: + run-tests: + runs-on: ubuntu-latest + steps: + - name: Install ruby and deps + run: sudo apt-get install ruby ruby-dev mosquitto + - name: Checkout + uses: actions/checkout@v2 diff --git a/src/aclk/mqtt_websockets/.gitignore b/src/aclk/mqtt_websockets/.gitignore new file mode 100644 index 000000000..9f1a0d89a --- /dev/null +++ b/src/aclk/mqtt_websockets/.gitignore @@ -0,0 +1,10 @@ +build/* +!build/.keep +test +.vscode +mqtt/mqtt.c +mqtt/include/mqtt.h +libmqttwebsockets.* +*.o +.dirstamp +.deps diff --git a/src/aclk/mqtt_websockets/README.md b/src/aclk/mqtt_websockets/README.md new file mode 100644 index 000000000..b159686df --- /dev/null +++ b/src/aclk/mqtt_websockets/README.md @@ -0,0 +1,7 @@ +# mqtt_websockets + +Library to connect MQTT client over Websockets Secure (WSS). + +## License + +The Project is released under GPL v3 license. See [License](LICENSE) diff --git a/src/aclk/mqtt_websockets/c-rbuf/cringbuffer.c b/src/aclk/mqtt_websockets/c-rbuf/cringbuffer.c new file mode 100644 index 000000000..8950c6906 --- /dev/null +++ b/src/aclk/mqtt_websockets/c-rbuf/cringbuffer.c @@ -0,0 +1,203 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include "cringbuffer.h" +#include "cringbuffer_internal.h" + +#include <stdlib.h> +#include <assert.h> +#include <string.h> + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +// this allows user to use their own +// custom memory allocation functions +#ifdef RBUF_CUSTOM_MALLOC +#include "../../helpers/ringbuffer_pal.h" +#else +#define crbuf_malloc(...) malloc(__VA_ARGS__) +#define crbuf_free(...) free(__VA_ARGS__) +#endif + +rbuf_t rbuf_create(size_t size) +{ + rbuf_t buffer = crbuf_malloc(sizeof(struct rbuf_t) + size); + if (!buffer) + return NULL; + + memset(buffer, 0, sizeof(struct rbuf_t)); + + buffer->data = ((char*)buffer) + sizeof(struct rbuf_t); + + buffer->head = buffer->data; + buffer->tail = buffer->data; + buffer->size = size; + buffer->end = buffer->data + size; + + return buffer; +} + +void rbuf_free(rbuf_t buffer) +{ + crbuf_free(buffer); +} + +void rbuf_flush(rbuf_t buffer) +{ + buffer->head = buffer->data; + buffer->tail = buffer->data; + buffer->size_data = 0; +} + +char *rbuf_get_linear_insert_range(rbuf_t buffer, size_t *bytes) +{ + *bytes = 0; + if (buffer->head == buffer->tail && buffer->size_data) + return NULL; + + *bytes = ((buffer->head >= buffer->tail) ? buffer->end : buffer->tail) - buffer->head; + return buffer->head; +} + +char *rbuf_get_linear_read_range(rbuf_t buffer, size_t *bytes) +{ + *bytes = 0; + if(buffer->head == buffer->tail && !buffer->size_data) + return NULL; + + *bytes = ((buffer->tail >= buffer->head) ? buffer->end : buffer->head) - buffer->tail; + + return buffer->tail; +} + +int rbuf_bump_head(rbuf_t buffer, size_t bytes) +{ + size_t free_bytes = rbuf_bytes_free(buffer); + if (bytes > free_bytes) + return 0; + int i = buffer->head - buffer->data; + buffer->head = &buffer->data[(i + bytes) % buffer->size]; + buffer->size_data += bytes; + return 1; +} + +int rbuf_bump_tail(rbuf_t buffer, size_t bytes) +{ + if(!rbuf_bump_tail_noopt(buffer, bytes)) + return 0; + + // if tail catched up with head + // start writing buffer from beggining + // this is not necessary (rbuf must work well without it) + // but helps to optimize big writes as rbuf_get_linear_insert_range + // will return bigger continuous region + if(buffer->tail == buffer->head) { + assert(buffer->size_data == 0); + rbuf_flush(buffer); + } + + return 1; +} + +size_t rbuf_get_capacity(rbuf_t buffer) +{ + return buffer->size; +} + +size_t rbuf_bytes_available(rbuf_t buffer) +{ + return buffer->size_data; +} + +size_t rbuf_bytes_free(rbuf_t buffer) +{ + return buffer->size - buffer->size_data; +} + +size_t rbuf_push(rbuf_t buffer, const char *data, size_t len) +{ + size_t to_cpy; + char *w_ptr = rbuf_get_linear_insert_range(buffer, &to_cpy); + if(!to_cpy) + return to_cpy; + + to_cpy = MIN(to_cpy, len); + memcpy(w_ptr, data, to_cpy); + rbuf_bump_head(buffer, to_cpy); + if(to_cpy < len) + to_cpy += rbuf_push(buffer, &data[to_cpy], len - to_cpy); + return to_cpy; +} + +size_t rbuf_pop(rbuf_t buffer, char *data, size_t len) +{ + size_t to_cpy; + const char *r_ptr = rbuf_get_linear_read_range(buffer, &to_cpy); + if(!to_cpy) + return to_cpy; + + to_cpy = MIN(to_cpy, len); + memcpy(data, r_ptr, to_cpy); + rbuf_bump_tail(buffer, to_cpy); + if(to_cpy < len) + to_cpy += rbuf_pop(buffer, &data[to_cpy], len - to_cpy); + return to_cpy; +} + +static inline void rbuf_ptr_inc(rbuf_t buffer, const char **ptr) +{ + (*ptr)++; + if(*ptr >= buffer->end) + *ptr = buffer->data; +} + +int rbuf_memcmp(rbuf_t buffer, const char *haystack, const char *needle, size_t needle_bytes) +{ + const char *end = needle + needle_bytes; + + // as head==tail can mean 2 things here + if (haystack == buffer->head && buffer->size_data) { + if (*haystack != *needle) + return (*haystack - *needle); + rbuf_ptr_inc(buffer, &haystack); + needle++; + } + + while (haystack != buffer->head && needle != end) { + if (*haystack != *needle) + return (*haystack - *needle); + rbuf_ptr_inc(buffer, &haystack); + needle++; + } + return 0; +} + +int rbuf_memcmp_n(rbuf_t buffer, const char *to_cmp, size_t to_cmp_bytes) +{ + return rbuf_memcmp(buffer, buffer->tail, to_cmp, to_cmp_bytes); +} + +char *rbuf_find_bytes(rbuf_t buffer, const char *needle, size_t needle_bytes, int *found_idx) +{ + const char *ptr = buffer->tail; + *found_idx = 0; + + if (!rbuf_bytes_available(buffer)) + return NULL; + + if (buffer->head == buffer->tail && buffer->size_data) { + if(!rbuf_memcmp(buffer, ptr, needle, needle_bytes)) + return (char *)ptr; + rbuf_ptr_inc(buffer, &ptr); + (*found_idx)++; + } + + while (ptr != buffer->head) + { + if(!rbuf_memcmp(buffer, ptr, needle, needle_bytes)) + return (char *)ptr; + rbuf_ptr_inc(buffer, &ptr); + (*found_idx)++; + } + return NULL; +} diff --git a/src/aclk/mqtt_websockets/c-rbuf/cringbuffer.h b/src/aclk/mqtt_websockets/c-rbuf/cringbuffer.h new file mode 100644 index 000000000..eb98035a9 --- /dev/null +++ b/src/aclk/mqtt_websockets/c-rbuf/cringbuffer.h @@ -0,0 +1,47 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#ifndef CRINGBUFFER_H +#define CRINGBUFFER_H + +#include <stddef.h> + +typedef struct rbuf_t *rbuf_t; + +rbuf_t rbuf_create(size_t size); +void rbuf_free(rbuf_t buffer); +void rbuf_flush(rbuf_t buffer); + +/* /param bytes how much bytes can be copied into pointer returned + * /return pointer where data can be copied to or NULL if buffer full + */ +char *rbuf_get_linear_insert_range(rbuf_t buffer, size_t *bytes); +char *rbuf_get_linear_read_range(rbuf_t buffer, size_t *bytes); + +int rbuf_bump_head(rbuf_t buffer, size_t bytes); +int rbuf_bump_tail(rbuf_t buffer, size_t bytes); + +/* @param buffer related buffer instance + * @returns total capacity of buffer in bytes (not free/used) + */ +size_t rbuf_get_capacity(rbuf_t buffer); + +/* @param buffer related buffer instance + * @returns count of bytes stored in the buffer + */ +size_t rbuf_bytes_available(rbuf_t buffer); + +/* @param buffer related buffer instance + * @returns count of bytes available/free in the buffer (how many more bytes you can store in this buffer) + */ +size_t rbuf_bytes_free(rbuf_t buffer); + +/* writes as many bytes from `data` into the `buffer` as possible + * but maximum `len` bytes + */ +size_t rbuf_push(rbuf_t buffer, const char *data, size_t len); +size_t rbuf_pop(rbuf_t buffer, char *data, size_t len); + +char *rbuf_find_bytes(rbuf_t buffer, const char *needle, size_t needle_bytes, int *found_idx); +int rbuf_memcmp_n(rbuf_t buffer, const char *to_cmp, size_t to_cmp_bytes); + +#endif diff --git a/src/aclk/mqtt_websockets/c-rbuf/cringbuffer_internal.h b/src/aclk/mqtt_websockets/c-rbuf/cringbuffer_internal.h new file mode 100644 index 000000000..d32de187c --- /dev/null +++ b/src/aclk/mqtt_websockets/c-rbuf/cringbuffer_internal.h @@ -0,0 +1,37 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#ifndef CRINGBUFFER_INTERNAL_H +#define CRINGBUFFER_INTERNAL_H + +struct rbuf_t { + char *data; + + // points to next byte where we can write + char *head; + // points to oldest (next to be poped) readable byte + char *tail; + + // to avoid calculating data + size + // all the time + char *end; + + size_t size; + size_t size_data; +}; + +/* this exists so that it can be tested by unit tests + * without optimization that resets head and tail to + * beginning if buffer empty + */ +inline static int rbuf_bump_tail_noopt(rbuf_t buffer, size_t bytes) +{ + if (bytes > buffer->size_data) + return 0; + int i = buffer->tail - buffer->data; + buffer->tail = &buffer->data[(i + bytes) % buffer->size]; + buffer->size_data -= bytes; + + return 1; +} + +#endif diff --git a/src/aclk/mqtt_websockets/c-rbuf/ringbuffer_test.c b/src/aclk/mqtt_websockets/c-rbuf/ringbuffer_test.c new file mode 100644 index 000000000..6a17c9956 --- /dev/null +++ b/src/aclk/mqtt_websockets/c-rbuf/ringbuffer_test.c @@ -0,0 +1,485 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include "ringbuffer.h" + +// to be able to access internals +// never do this from app +#include "../src/ringbuffer_internal.h" + +#include <stdio.h> +#include <string.h> + +#define KNRM "\x1B[0m" +#define KRED "\x1B[31m" +#define KGRN "\x1B[32m" +#define KYEL "\x1B[33m" +#define KBLU "\x1B[34m" +#define KMAG "\x1B[35m" +#define KCYN "\x1B[36m" +#define KWHT "\x1B[37m" + +#define UNUSED(x) (void)(x) + +int total_fails = 0; +int total_tests = 0; +int total_checks = 0; + +#define CHECK_EQ_RESULT(x, y) \ + while (s_len--) \ + putchar('.'); \ + printf("%s%s " KNRM "\n", (((x) == (y)) ? KGRN : KRED), (((x) == (y)) ? " PASS " : " FAIL ")); \ + if ((x) != (y)) \ + total_fails++; \ + total_checks++; + +#define CHECK_EQ_PREFIX(x, y, prefix, subtest_name, ...) \ + { \ + int s_len = \ + 100 - \ + printf(("Checking: " KWHT "%s %s%2d " subtest_name " " KNRM), __func__, prefix, subtest_no, ##__VA_ARGS__); \ + CHECK_EQ_RESULT(x, y) \ + } + +#define CHECK_EQ(x, y, subtest_name, ...) \ + { \ + int s_len = \ + 100 - printf(("Checking: " KWHT "%s %2d " subtest_name " " KNRM), __func__, subtest_no, ##__VA_ARGS__); \ + CHECK_EQ_RESULT(x, y) \ + } + +#define TEST_DECL() \ + int subtest_no = 0; \ + printf(KYEL "TEST SUITE: %s\n" KNRM, __func__); \ + total_tests++; + +static void test_rbuf_get_linear_insert_range() +{ + TEST_DECL(); + + // check empty buffer behaviour + rbuf_t buff = rbuf_create(5); + char *to_write; + size_t ret; + to_write = rbuf_get_linear_insert_range(buff, &ret); + CHECK_EQ(ret, 5, "empty size"); + CHECK_EQ(to_write, buff->head, "empty write ptr"); + rbuf_free(buff); + + // check full buffer behaviour + subtest_no++; + buff = rbuf_create(5); + ret = rbuf_bump_head(buff, 5); + CHECK_EQ(ret, 1, "ret"); + to_write = rbuf_get_linear_insert_range(buff, &ret); + CHECK_EQ(to_write, NULL, "writable NULL"); + CHECK_EQ(ret, 0, "writable count = 0"); + + // check buffer flush + subtest_no++; + rbuf_flush(buff); + CHECK_EQ(rbuf_bytes_free(buff), 5, "size_free"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check behaviour head > tail + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 3); + to_write = rbuf_get_linear_insert_range(buff, &ret); + CHECK_EQ(to_write, buff->head, "write location"); + CHECK_EQ(ret, 2, "availible to linear write"); + + // check behaviour tail > head + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 5); + rbuf_bump_tail(buff, 3); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 3, "tail_ptr"); + to_write = rbuf_get_linear_insert_range(buff, &ret); + CHECK_EQ(to_write, buff->head, "write location"); + CHECK_EQ(ret, 3, "availible to linear write"); + +/* // check behaviour tail and head at last element + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 4); + rbuf_bump_tail(buff, 4); + CHECK_EQ(buff->head, buff->end - 1, "head_ptr"); + CHECK_EQ(buff->tail, buff->end - 1, "tail_ptr"); + to_write = rbuf_get_linear_insert_range(buff, &ret); + CHECK_EQ(to_write, buff->head, "write location"); + CHECK_EQ(ret, 1, "availible to linear write");*/ + + // check behaviour tail and head at last element + // after rbuf_bump_tail optimisation that restarts buffer + // in case tail catches up with head + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 4); + rbuf_bump_tail(buff, 4); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + to_write = rbuf_get_linear_insert_range(buff, &ret); + CHECK_EQ(to_write, buff->head, "write location"); + CHECK_EQ(ret, 5, "availible to linear write"); +} + +#define _CHECK_EQ(x, y, subtest_name, ...) CHECK_EQ_PREFIX(x, y, prefix, subtest_name, ##__VA_ARGS__) +#define _PREFX "(size = %5zu) " +static void test_rbuf_bump_head_bsize(size_t size) +{ + char prefix[16]; + snprintf(prefix, 16, _PREFX, size); + int subtest_no = 0; + rbuf_t buff = rbuf_create(size); + _CHECK_EQ(rbuf_bytes_free(buff), size, "size_free"); + + subtest_no++; + int ret = rbuf_bump_head(buff, size); + _CHECK_EQ(buff->data, buff->head, "loc"); + _CHECK_EQ(ret, 1, "ret"); + _CHECK_EQ(buff->size_data, buff->size, "size"); + _CHECK_EQ(rbuf_bytes_free(buff), 0, "size_free"); + + subtest_no++; + ret = rbuf_bump_head(buff, 1); + _CHECK_EQ(buff->data, buff->head, "loc no move"); + _CHECK_EQ(ret, 0, "ret error"); + _CHECK_EQ(buff->size_data, buff->size, "size"); + _CHECK_EQ(rbuf_bytes_free(buff), 0, "size_free"); + rbuf_free(buff); + + subtest_no++; + buff = rbuf_create(size); + ret = rbuf_bump_head(buff, size - 1); + _CHECK_EQ(buff->head, buff->end-1, "loc end"); + rbuf_free(buff); +} +#undef _CHECK_EQ + +static void test_rbuf_bump_head() +{ + TEST_DECL(); + UNUSED(subtest_no); + + size_t test_sizes[] = { 1, 2, 3, 5, 6, 7, 8, 100, 99999, 0 }; + for (int i = 0; test_sizes[i]; i++) + test_rbuf_bump_head_bsize(test_sizes[i]); +} + +static void test_rbuf_bump_tail_noopt(int subtest_no) +{ + rbuf_t buff = rbuf_create(10); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + + subtest_no++; + int ret = rbuf_bump_head(buff, 5); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_free(buff), 5, "size_free"); + CHECK_EQ(rbuf_bytes_available(buff), 5, "size_avail"); + CHECK_EQ(buff->head, buff->data + 5, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 2); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 3, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 7, "size_free"); + CHECK_EQ(buff->head, buff->data + 5, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 2, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 3); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(buff->head, buff->data + 5, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 5, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 1); + CHECK_EQ(ret, 0, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(buff->head, buff->data + 5, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 5, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_head(buff, 7); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 7, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 3, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 5, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 5); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 2, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 8, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check tail can't overrun head + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 3); + CHECK_EQ(ret, 0, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 2, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 8, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check head can't overrun tail + subtest_no++; + ret = rbuf_bump_head(buff, 9); + CHECK_EQ(ret, 0, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 2, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 8, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check head can fill the buffer + subtest_no++; + ret = rbuf_bump_head(buff, 8); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 10, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 0, "size_free"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check can empty the buffer + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 10); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); +} + +static void test_rbuf_bump_tail_opt(int subtest_no) +{ + subtest_no++; + rbuf_t buff = rbuf_create(10); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + + subtest_no++; + int ret = rbuf_bump_head(buff, 5); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_free(buff), 5, "size_free"); + CHECK_EQ(rbuf_bytes_available(buff), 5, "size_avail"); + CHECK_EQ(buff->head, buff->data + 5, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail(buff, 2); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 3, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 7, "size_free"); + CHECK_EQ(buff->head, buff->data + 5, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 2, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail(buff, 3); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail_noopt(buff, 1); + CHECK_EQ(ret, 0, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_head(buff, 6); + ret = rbuf_bump_tail(buff, 5); + ret = rbuf_bump_head(buff, 6); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 7, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 3, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data + 5, "tail_ptr"); + + subtest_no++; + ret = rbuf_bump_tail(buff, 5); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 2, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 8, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check tail can't overrun head + subtest_no++; + ret = rbuf_bump_tail(buff, 3); + CHECK_EQ(ret, 0, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 2, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 8, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check head can't overrun tail + subtest_no++; + ret = rbuf_bump_head(buff, 9); + CHECK_EQ(ret, 0, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 2, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 8, "size_free"); + CHECK_EQ(buff->head, buff->data + 2, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check head can fill the buffer + subtest_no++; + ret = rbuf_bump_head(buff, 8); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 10, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 0, "size_free"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + + // check can empty the buffer + subtest_no++; + ret = rbuf_bump_tail(buff, 10); + CHECK_EQ(ret, 1, "ret"); + CHECK_EQ(rbuf_bytes_available(buff), 0, "size_avail"); + CHECK_EQ(rbuf_bytes_free(buff), 10, "size_free"); + CHECK_EQ(buff->head, buff->data, "head_ptr"); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); +} + +static void test_rbuf_bump_tail() +{ + TEST_DECL(); + test_rbuf_bump_tail_noopt(subtest_no); + test_rbuf_bump_tail_opt(subtest_no); +} + +#define ASCII_A 0x61 +#define ASCII_Z 0x7A +#define TEST_DATA_SIZE ASCII_Z-ASCII_A+1 +static void test_rbuf_push() +{ + TEST_DECL(); + rbuf_t buff = rbuf_create(10); + int i; + char test_data[TEST_DATA_SIZE]; + + for (int i = 0; i <= TEST_DATA_SIZE; i++) + test_data[i] = i + ASCII_A; + + int ret = rbuf_push(buff, test_data, 10); + CHECK_EQ(ret, 10, "written 10 bytes"); + CHECK_EQ(rbuf_bytes_free(buff), 0, "empty size == 0"); + for (i = 0; i < 10; i++) + CHECK_EQ(buff->data[i], i + ASCII_A, "Check data"); + + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 5); + rbuf_bump_tail_noopt(buff, 5); //to not reset both pointers to beginning + ret = rbuf_push(buff, test_data, 10); + CHECK_EQ(ret, 10, "written 10 bytes"); + for (i = 0; i < 10; i++) + CHECK_EQ(buff->data[i], ((i+5)%10) + ASCII_A, "Check Data"); + + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 9); + rbuf_bump_tail_noopt(buff, 9); + ret = rbuf_push(buff, test_data, 10); + CHECK_EQ(ret, 10, "written 10 bytes"); + for (i = 0; i < 10; i++) + CHECK_EQ(buff->data[i], ((i + 1) % 10) + ASCII_A, "Check data"); + + // let tail > head + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 9); + rbuf_bump_tail_noopt(buff, 9); + rbuf_bump_head(buff, 1); + ret = rbuf_push(buff, test_data, 9); + CHECK_EQ(ret, 9, "written 9 bytes"); + CHECK_EQ(buff->head, buff->end - 1, "head_ptr"); + CHECK_EQ(buff->tail, buff->head, "tail_ptr"); + rbuf_bump_tail(buff, 1); + //TODO push byte can be usefull optimisation + ret = rbuf_push(buff, &test_data[9], 1); + CHECK_EQ(ret, 1, "written 1 byte"); + CHECK_EQ(rbuf_bytes_free(buff), 0, "empty size == 0"); + for (i = 0; i < 10; i++) + CHECK_EQ(buff->data[i], i + ASCII_A, "Check data"); + + subtest_no++; + rbuf_flush(buff); + rbuf_bump_head(buff, 9); + rbuf_bump_tail_noopt(buff, 7); + rbuf_bump_head(buff, 1); + ret = rbuf_push(buff, test_data, 7); + CHECK_EQ(ret, 7, "written 7 bytes"); + CHECK_EQ(buff->head, buff->data + 7, "head_ptr"); + CHECK_EQ(buff->tail, buff->head, "tail_ptr"); + rbuf_bump_tail(buff, 3); + CHECK_EQ(buff->tail, buff->data, "tail_ptr"); + //TODO push byte can be usefull optimisation + ret = rbuf_push(buff, &test_data[7], 3); + CHECK_EQ(ret, 3, "written 3 bytes"); + CHECK_EQ(rbuf_bytes_free(buff), 0, "empty size == 0"); + for (i = 0; i < 10; i++) + CHECK_EQ(buff->data[i], i + ASCII_A, "Check data"); + + // test can't overfill the buffer + subtest_no++; + rbuf_flush(buff); + rbuf_push(buff, test_data, TEST_DATA_SIZE); + CHECK_EQ(ret, 3, "written 10 bytes"); + for (i = 0; i < 10; i++) + CHECK_EQ(buff->data[i], i + ASCII_A, "Check data"); +} + +#define TEST_RBUF_FIND_BYTES_SIZE 10 +void test_rbuf_find_bytes() +{ + TEST_DECL(); + rbuf_t buff = rbuf_create(TEST_RBUF_FIND_BYTES_SIZE); + char *filler_3 = " "; + char *needle = "needle"; + int idx; + char *ptr; + + // make sure needle is wrapped aroung in the buffer + // to test we still can find it + // target "edle ne" + rbuf_bump_head(buff, TEST_RBUF_FIND_BYTES_SIZE / 2); + rbuf_push(buff, filler_3, strlen(filler_3)); + rbuf_bump_tail(buff, TEST_RBUF_FIND_BYTES_SIZE / 2); + rbuf_push(buff, needle, strlen(needle)); + ptr = rbuf_find_bytes(buff, needle, strlen(needle), &idx); + CHECK_EQ(ptr, buff->data + (TEST_RBUF_FIND_BYTES_SIZE / 2) + strlen(filler_3), "Pointer to needle correct"); + CHECK_EQ(idx, ptr - buff->tail, "Check needle index"); +} + +int main() +{ + test_rbuf_bump_head(); + test_rbuf_bump_tail(); + test_rbuf_get_linear_insert_range(); + test_rbuf_push(); + test_rbuf_find_bytes(); + + printf( + KNRM "Total Tests %d, Total Checks %d, Successful Checks %d, Failed Checks %d\n", + total_tests, total_checks, total_checks - total_fails, total_fails); + if (total_fails) + printf(KRED "!!!Some test(s) Failed!!!\n"); + else + printf(KGRN "ALL TESTS PASSED\n"); + + return total_fails; +} diff --git a/src/aclk/mqtt_websockets/c_rhash/c_rhash.c b/src/aclk/mqtt_websockets/c_rhash/c_rhash.c new file mode 100644 index 000000000..a71b500e2 --- /dev/null +++ b/src/aclk/mqtt_websockets/c_rhash/c_rhash.c @@ -0,0 +1,264 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include "c_rhash_internal.h" + +#include <stdlib.h> +#include <string.h> + +#ifdef DEBUG_VERBOSE +#include <stdio.h> +#endif + +#define c_rmalloc(...) malloc(__VA_ARGS__) +#define c_rcalloc(...) calloc(__VA_ARGS__) +#define c_rfree(...) free(__VA_ARGS__) + +static inline uint32_t simple_hash(const char *name) { + unsigned char *s = (unsigned char *) name; + uint32_t hval = 0x811c9dc5; + while (*s) { + hval *= 16777619; + hval ^= (uint32_t) *s++; + } + return hval; +} + +c_rhash c_rhash_new(size_t bin_count) { + if (!bin_count) + bin_count = 1000; + + c_rhash hash = c_rcalloc(1, sizeof(struct c_rhash_s) + (bin_count * sizeof(struct bin_ll*)) ); + if (hash == NULL) + return NULL; + + hash->bin_count = bin_count; + hash->bins = (c_rhash_bin *)((char*)hash + sizeof(struct c_rhash_s)); + + return hash; +} + +static size_t get_itemtype_len(uint8_t item_type, const void* item_data) { + switch (item_type) { + case ITEMTYPE_STRING: + return strlen(item_data) + 1; + case ITEMTYPE_UINT64: + return sizeof(uint64_t); + case ITEMTYPE_UINT8: + return 1; + case ITEMTYPE_OPAQUE_PTR: + return sizeof(void*); + default: + return 0; + } +} + +static int compare_bin_item(struct bin_item *item, uint8_t key_type, const void *key) { + if (item->key_type != key_type) + return 1; + + size_t key_value_len = get_itemtype_len(key_type, key); + + if(key_type == ITEMTYPE_STRING) { + size_t new_key_value_len = get_itemtype_len(item->key_type, item->key); + if (new_key_value_len != key_value_len) + return 1; + } + + if(memcmp(item->key, key, key_value_len) == 0) { + return 0; + } + + return 1; +} + +static int insert_into_bin(c_rhash_bin *bin, uint8_t key_type, const void *key, uint8_t value_type, const void *value) { + struct bin_item *prev = NULL; + while (*bin != NULL) { + if (!compare_bin_item(*bin, key_type, key)) { +#ifdef DEBUG_VERBOSE + printf("Key already present! Updating value!\n"); +#endif +// TODO: optimize here if the new value is of different kind compared to the old one +// in case it is not crazily bigger we can reuse the memory and avoid malloc and free + c_rfree((*bin)->value); + (*bin)->value_type = value_type; + (*bin)->value = c_rmalloc(get_itemtype_len(value_type, value)); + if ((*bin)->value == NULL) + return 1; + memcpy((*bin)->value, value, get_itemtype_len(value_type, value)); + return 0; + } + prev = *bin; + bin = &(*bin)->next; + } + + if (*bin == NULL) + *bin = c_rcalloc(1, sizeof(struct bin_item)); + if (prev != NULL) + prev->next = *bin; + + (*bin)->key_type = key_type; + size_t len = get_itemtype_len(key_type, key); + (*bin)->key = c_rmalloc(len); + memcpy((*bin)->key, key, len); + + (*bin)->value_type = value_type; + len = get_itemtype_len(value_type, value); + (*bin)->value = c_rmalloc(len); + memcpy((*bin)->value, value, len); + return 0; +} + +static inline uint32_t get_bin_idx_str(c_rhash hash, const char *key) { + uint32_t nhash = simple_hash(key); + return nhash % hash->bin_count; +} + +static inline c_rhash_bin *get_binptr_by_str(c_rhash hash, const char *key) { + return &hash->bins[get_bin_idx_str(hash, key)]; +} + +int c_rhash_insert_str_ptr(c_rhash hash, const char *key, void *value) { + c_rhash_bin *bin = get_binptr_by_str(hash, key); + +#ifdef DEBUG_VERBOSE + if (bin != NULL) + printf("COLLISION. There will be more than one item in bin idx=%d\n", nhash); +#endif + + return insert_into_bin(bin, ITEMTYPE_STRING, key, ITEMTYPE_OPAQUE_PTR, &value); +} + +int c_rhash_insert_str_uint8(c_rhash hash, const char *key, uint8_t value) { + c_rhash_bin *bin = get_binptr_by_str(hash, key); + +#ifdef DEBUG_VERBOSE + if (bin != NULL) + printf("COLLISION. There will be more than one item in bin idx=%d\n", nhash); +#endif + + return insert_into_bin(bin, ITEMTYPE_STRING, key, ITEMTYPE_UINT8, &value); +} + +int c_rhash_insert_uint64_ptr(c_rhash hash, uint64_t key, void *value) { + c_rhash_bin *bin = &hash->bins[key % hash->bin_count]; + +#ifdef DEBUG_VERBOSE + if (bin != NULL) + printf("COLLISION. There will be more than one item in bin idx=%d\n", nhash); +#endif + + return insert_into_bin(bin, ITEMTYPE_UINT64, &key, ITEMTYPE_OPAQUE_PTR, &value); +} + +int c_rhash_get_uint8_by_str(c_rhash hash, const char *key, uint8_t *ret_val) { + uint32_t nhash = get_bin_idx_str(hash, key); + + struct bin_item *bin = hash->bins[nhash]; + + while (bin) { + if (bin->key_type == ITEMTYPE_STRING) { + if (!strcmp(bin->key, key)) { + *ret_val = *(uint8_t*)bin->value; + return 0; + } + } + bin = bin->next; + } + return 1; +} + +int c_rhash_get_ptr_by_str(c_rhash hash, const char *key, void **ret_val) { + uint32_t nhash = get_bin_idx_str(hash, key); + + struct bin_item *bin = hash->bins[nhash]; + + while (bin) { + if (bin->key_type == ITEMTYPE_STRING) { + if (!strcmp(bin->key, key)) { + *ret_val = *((void**)bin->value); + return 0; + } + } + bin = bin->next; + } + *ret_val = NULL; + return 1; +} + +int c_rhash_get_ptr_by_uint64(c_rhash hash, uint64_t key, void **ret_val) { + uint32_t nhash = key % hash->bin_count; + + struct bin_item *bin = hash->bins[nhash]; + + while (bin) { + if (bin->key_type == ITEMTYPE_UINT64) { + if (*((uint64_t *)bin->key) == key) { + *ret_val = *((void**)bin->value); + return 0; + } + } + bin = bin->next; + } + *ret_val = NULL; + return 1; +} + +static void c_rhash_destroy_bin(c_rhash_bin bin) { + struct bin_item *next; + do { + next = bin->next; + c_rfree(bin->key); + c_rfree(bin->value); + c_rfree(bin); + bin = next; + } while (bin != NULL); +} + +int c_rhash_iter_uint64_keys(c_rhash hash, c_rhash_iter_t *iter, uint64_t *key) { + while (iter->bin < hash->bin_count) { + if (iter->item != NULL) + iter->item = iter->item->next; + if (iter->item == NULL) { + if (iter->initialized) + iter->bin++; + else + iter->initialized = 1; + if (iter->bin < hash->bin_count) + iter->item = hash->bins[iter->bin]; + } + if (iter->item != NULL && iter->item->key_type == ITEMTYPE_UINT64) { + *key = *(uint64_t*)iter->item->key; + return 0; + } + } + return 1; +} + +int c_rhash_iter_str_keys(c_rhash hash, c_rhash_iter_t *iter, const char **key) { + while (iter->bin < hash->bin_count) { + if (iter->item != NULL) + iter->item = iter->item->next; + if (iter->item == NULL) { + if (iter->initialized) + iter->bin++; + else + iter->initialized = 1; + if (iter->bin < hash->bin_count) + iter->item = hash->bins[iter->bin]; + } + if (iter->item != NULL && iter->item->key_type == ITEMTYPE_STRING) { + *key = (const char*)iter->item->key; + return 0; + } + } + return 1; +} + +void c_rhash_destroy(c_rhash hash) { + for (size_t i = 0; i < hash->bin_count; i++) { + if (hash->bins[i] != NULL) + c_rhash_destroy_bin(hash->bins[i]); + } + c_rfree(hash); +} diff --git a/src/aclk/mqtt_websockets/c_rhash/c_rhash.h b/src/aclk/mqtt_websockets/c_rhash/c_rhash.h new file mode 100644 index 000000000..37addd161 --- /dev/null +++ b/src/aclk/mqtt_websockets/c_rhash/c_rhash.h @@ -0,0 +1,61 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include <sys/types.h> +#include <stdint.h> +#include <stddef.h> + +#ifndef DEFAULT_BIN_COUNT + #define DEFAULT_BIN_COUNT 1000 +#endif + +#define ITEMTYPE_UNSET (0x0) +#define ITEMTYPE_STRING (0x1) +#define ITEMTYPE_UINT8 (0x2) +#define ITEMTYPE_UINT64 (0x3) +#define ITEMTYPE_OPAQUE_PTR (0x4) + +typedef struct c_rhash_s *c_rhash; + +c_rhash c_rhash_new(size_t bin_count); + +void c_rhash_destroy(c_rhash hash); + +// # Insert +// ## Insert where key is string +int c_rhash_insert_str_ptr(c_rhash hash, const char *key, void *value); +int c_rhash_insert_str_uint8(c_rhash hash, const char *key, uint8_t value); +// ## Insert where key is uint64 +int c_rhash_insert_uint64_ptr(c_rhash hash, uint64_t key, void *value); + +// # Get +// ## Get where key is string +int c_rhash_get_ptr_by_str(c_rhash hash, const char *key, void **ret_val); +int c_rhash_get_uint8_by_str(c_rhash hash, const char *key, uint8_t *ret_val); +// ## Get where key is uint64 +int c_rhash_get_ptr_by_uint64(c_rhash hash, uint64_t key, void **ret_val); + +typedef struct { + size_t bin; + struct bin_item *item; + int initialized; +} c_rhash_iter_t; + +#define C_RHASH_ITER_T_INITIALIZER { .bin = 0, .item = NULL, .initialized = 0 } + +#define c_rhash_iter_t_initialize(p_iter) memset(p_iter, 0, sizeof(c_rhash_iter_t)) + +/* + * goes trough whole hash map and returns every + * type uint64 key present/stored + * + * it is not necessary to finish iterating and iterator can be reinitialized + * there are no guarantees on the order in which the keys will come + * behavior here is implementation dependent and can change any time + * + * returns: + * 0 for every key and stores the key in *key + * 1 on error or when all keys of this type has been already iterated over + */ +int c_rhash_iter_uint64_keys(c_rhash hash, c_rhash_iter_t *iter, uint64_t *key); + +int c_rhash_iter_str_keys(c_rhash hash, c_rhash_iter_t *iter, const char **key); diff --git a/src/aclk/mqtt_websockets/c_rhash/c_rhash_internal.h b/src/aclk/mqtt_websockets/c_rhash/c_rhash_internal.h new file mode 100644 index 000000000..20f741076 --- /dev/null +++ b/src/aclk/mqtt_websockets/c_rhash/c_rhash_internal.h @@ -0,0 +1,19 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include "c_rhash.h" + +struct bin_item { + uint8_t key_type:4; + void *key; + uint8_t value_type:4; + void *value; + + struct bin_item *next; +}; + +typedef struct bin_item *c_rhash_bin; + +struct c_rhash_s { + size_t bin_count; + c_rhash_bin *bins; +}; diff --git a/src/aclk/mqtt_websockets/c_rhash/tests.c b/src/aclk/mqtt_websockets/c_rhash/tests.c new file mode 100644 index 000000000..909c5562d --- /dev/null +++ b/src/aclk/mqtt_websockets/c_rhash/tests.c @@ -0,0 +1,273 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include <stdio.h> +#include <string.h> + +#include "c_rhash.h" + +// terminal color codes +#define KNRM "\x1B[0m" +#define KRED "\x1B[31m" +#define KGRN "\x1B[32m" +#define KYEL "\x1B[33m" +#define KBLU "\x1B[34m" +#define KMAG "\x1B[35m" +#define KCYN "\x1B[36m" +#define KWHT "\x1B[37m" + +#define KEY_1 "key1" +#define KEY_2 "keya" + +#define PRINT_ERR(str, ...) fprintf(stderr, "└─╼ ❌ " KRED str KNRM "\n" __VA_OPT__(,) __VA_ARGS__) + +#define ASSERT_RETVAL(fnc, comparator, expected_retval, ...) \ +{ int rval; \ +if(!((rval = fnc(__VA_ARGS__)) comparator expected_retval)) { \ + PRINT_ERR("Failed test. Value returned by \"%s\" in fnc:\"%s\",line:%d is not equal to expected value. Expected:%d, Got:%d", #fnc, __FUNCTION__, __LINE__, expected_retval, rval); \ + rc = 1; \ + goto test_cleanup; \ +} passed_subtest_count++;}; + +#define ASSERT_VAL_UINT8(returned, expected) \ +if(returned != expected) { \ + PRINT_ERR("Failed test. Value returned (%d) doesn't match expected (%d)! fnc:\"%s\",line:%d", returned, expected, __FUNCTION__, __LINE__); \ + rc = 1; \ + goto test_cleanup; \ +} passed_subtest_count++; + +#define ASSERT_VAL_PTR(returned, expected) \ +if((void*)returned != (void*)expected) { \ + PRINT_ERR("Failed test. Value returned(%p) doesn't match expected(%p)! fnc:\"%s\",line:%d", (void*)returned, (void*)expected, __FUNCTION__, __LINE__); \ + rc = 1; \ + goto test_cleanup; \ +} passed_subtest_count++; + +#define ALL_SUBTESTS_PASS() printf("└─╼ ✅" KGRN " Test \"%s\" DONE. All of %zu subtests PASS. (line:%d)\n" KNRM, __FUNCTION__, passed_subtest_count, __LINE__); + +#define TEST_START() size_t passed_subtest_count = 0; int rc = 0; printf("╒═ Starting test \"%s\"\n", __FUNCTION__); + +int test_str_uint8() { + c_rhash hash = c_rhash_new(100); + uint8_t val; + + TEST_START(); + // function should fail on empty hash + ASSERT_RETVAL(c_rhash_get_uint8_by_str, !=, 0, hash, KEY_1, &val); + + ASSERT_RETVAL(c_rhash_insert_str_uint8, ==, 0, hash, KEY_1, 5); + ASSERT_RETVAL(c_rhash_get_uint8_by_str, ==, 0, hash, KEY_1, &val); + ASSERT_VAL_UINT8(5, val); + + ASSERT_RETVAL(c_rhash_insert_str_uint8, ==, 0, hash, KEY_2, 8); + ASSERT_RETVAL(c_rhash_get_uint8_by_str, ==, 0, hash, KEY_1, &val); + ASSERT_VAL_UINT8(5, val); + ASSERT_RETVAL(c_rhash_get_uint8_by_str, ==, 0, hash, KEY_2, &val); + ASSERT_VAL_UINT8(8, val); + ASSERT_RETVAL(c_rhash_get_uint8_by_str, !=, 0, hash, "sndnskjdf", &val); + + // test update of key + ASSERT_RETVAL(c_rhash_insert_str_uint8, ==, 0, hash, KEY_1, 100); + ASSERT_RETVAL(c_rhash_get_uint8_by_str, ==, 0, hash, KEY_1, &val); + ASSERT_VAL_UINT8(100, val); + + ALL_SUBTESTS_PASS(); +test_cleanup: + c_rhash_destroy(hash); + return rc; +} + +int test_uint64_ptr() { + c_rhash hash = c_rhash_new(100); + void *val; + + TEST_START(); + + // function should fail on empty hash + ASSERT_RETVAL(c_rhash_get_ptr_by_uint64, !=, 0, hash, 0, &val); + + ASSERT_RETVAL(c_rhash_insert_uint64_ptr, ==, 0, hash, 0, &hash); + ASSERT_RETVAL(c_rhash_get_ptr_by_uint64, ==, 0, hash, 0, &val); + ASSERT_VAL_PTR(&hash, val); + + ASSERT_RETVAL(c_rhash_insert_uint64_ptr, ==, 0, hash, 1, &val); + ASSERT_RETVAL(c_rhash_get_ptr_by_uint64, ==, 0, hash, 0, &val); + ASSERT_VAL_PTR(&hash, val); + ASSERT_RETVAL(c_rhash_get_ptr_by_uint64, ==, 0, hash, 1, &val); + ASSERT_VAL_PTR(&val, val); + ASSERT_RETVAL(c_rhash_get_ptr_by_uint64, !=, 0, hash, 2, &val); + + ALL_SUBTESTS_PASS(); +test_cleanup: + c_rhash_destroy(hash); + return rc; +} + +#define UINT64_PTR_INC_ITERATION_COUNT 5000 +int test_uint64_ptr_incremental() { + c_rhash hash = c_rhash_new(100); + void *val; + + TEST_START(); + + char a = 0x20; + char *ptr = &a; + while(ptr < &a + UINT64_PTR_INC_ITERATION_COUNT) { + ASSERT_RETVAL(c_rhash_insert_uint64_ptr, ==, 0, hash, (ptr-&a), ptr); + ptr++; + } + + ptr = &a; + char *retptr; + for(int i = 0; i < UINT64_PTR_INC_ITERATION_COUNT; i++) { + ASSERT_RETVAL(c_rhash_get_ptr_by_uint64, ==, 0, hash, i, (void**)&retptr); + ASSERT_VAL_PTR(retptr, (&a+i)); + } + + ALL_SUBTESTS_PASS(); +test_cleanup: + c_rhash_destroy(hash); + return rc; +} + +struct test_string { + const char *str; + int counter; +}; + +struct test_string test_strings[] = { + { .str = "Cillum reprehenderit eiusmod elit nisi aliquip esse exercitation commodo Lorem voluptate esse.", .counter = 0 }, + { .str = "Ullamco eiusmod tempor occaecat ad.", .counter = 0 }, + { .str = "Esse aliquip tempor sint tempor ullamco duis aute incididunt ad.", .counter = 0 }, + { .str = "Cillum Lorem labore cupidatat commodo proident adipisicing.", .counter = 0 }, + { .str = "Quis ad cillum officia exercitation.", .counter = 0 }, + { .str = "Ipsum enim dolor ullamco amet sint nisi ut occaecat sint non.", .counter = 0 }, + { .str = "Id duis officia ipsum cupidatat velit fugiat.", .counter = 0 }, + { .str = "Aliqua non occaecat voluptate reprehenderit reprehenderit veniam minim exercitation ea aliquip enim aliqua deserunt qui.", .counter = 0 }, + { .str = "Ullamco elit tempor laboris reprehenderit quis deserunt duis quis tempor reprehenderit magna dolore reprehenderit exercitation.", .counter = 0 }, + { .str = "Culpa do dolor quis incididunt et labore in ex.", .counter = 0 }, + { .str = "Aliquip velit cupidatat qui incididunt ipsum nostrud eiusmod ut proident nisi magna fugiat excepteur.", .counter = 0 }, + { .str = "Aliqua qui dolore tempor id proident ullamco sunt magna.", .counter = 0 }, + { .str = "Labore eiusmod ut fugiat dolore reprehenderit mollit magna.", .counter = 0 }, + { .str = "Veniam aliquip dolor excepteur minim nulla esse cupidatat esse.", .counter = 0 }, + { .str = "Do quis dolor irure nostrud occaecat aute proident anim.", .counter = 0 }, + { .str = "Enim veniam non nulla ad quis sit amet.", .counter = 0 }, + { .str = "Cillum reprehenderit do enim esse do ullamco consectetur ea.", .counter = 0 }, + { .str = "Sit et duis sint anim qui ad anim labore exercitation sunt cupidatat.", .counter = 0 }, + { .str = "Dolor officia adipisicing sint pariatur in dolor occaecat officia reprehenderit magna.", .counter = 0 }, + { .str = "Aliquip dolore qui occaecat eiusmod sunt incididunt reprehenderit minim et.", .counter = 0 }, + { .str = "Aute fugiat laboris cillum tempor consequat tempor do non laboris culpa officia nisi.", .counter = 0 }, + { .str = "Et excepteur do aliquip fugiat nisi velit tempor officia enim quis elit incididunt.", .counter = 0 }, + { .str = "Eu officia adipisicing incididunt occaecat officia cupidatat enim sit sit officia.", .counter = 0 }, + { .str = "Do amet cillum duis pariatur commodo nulla cillum magna nulla Lorem veniam cupidatat.", .counter = 0 }, + { .str = "Dolor adipisicing voluptate laboris occaecat culpa aliquip ipsum ut consequat aliqua aliquip commodo sunt velit.", .counter = 0 }, + { .str = "Nulla proident ipsum quis nulla.", .counter = 0 }, + { .str = "Laborum adipisicing nulla do aute aliqua est quis sint culpa pariatur laborum voluptate qui.", .counter = 0 }, + { .str = "Proident eiusmod sunt et nulla elit pariatur dolore irure ex voluptate excepteur adipisicing consectetur.", .counter = 0 }, + { .str = "Consequat ex voluptate officia excepteur aute deserunt proident commodo et.", .counter = 0 }, + { .str = "Velit sit cupidatat dolor dolore.", .counter = 0 }, + { .str = "Sunt enim do non anim nostrud exercitation ullamco ex proident commodo.", .counter = 0 }, + { .str = "Id ex officia cillum ad.", .counter = 0 }, + { .str = "Laboris in sunt eiusmod veniam laboris nostrud.", .counter = 0 }, + { .str = "Ex magna occaecat ea ea incididunt aliquip.", .counter = 0 }, + { .str = "Sunt eiusmod ex nostrud eu pariatur sit cupidatat ea adipisicing cillum culpa esse consequat aliquip.", .counter = 0 }, + { .str = "Excepteur commodo qui incididunt enim culpa sunt non excepteur Lorem adipisicing.", .counter = 0 }, + { .str = "Quis officia est ullamco reprehenderit incididunt occaecat pariatur ex reprehenderit nisi.", .counter = 0 }, + { .str = "Culpa irure proident proident et eiusmod irure aliqua ipsum cupidatat minim sit.", .counter = 0 }, + { .str = "Qui cupidatat aliquip est velit magna veniam.", .counter = 0 }, + { .str = "Pariatur ad ad mollit nostrud non irure minim veniam anim aliquip quis eu.", .counter = 0 }, + { .str = "Nisi ex minim eu adipisicing tempor Lorem nisi do ad exercitation est non eu.", .counter = 0 }, + { .str = "Cupidatat do mollit ad commodo cupidatat ut.", .counter = 0 }, + { .str = "Est non excepteur eiusmod nostrud et eu.", .counter = 0 }, + { .str = "Cupidatat mollit nisi magna officia ut elit eiusmod.", .counter = 0 }, + { .str = "Est aliqua consectetur laboris ex consequat est ut dolor.", .counter = 0 }, + { .str = "Duis eu laboris laborum ut id Lorem nostrud qui ad velit proident fugiat minim ullamco.", .counter = 0 }, + { .str = "Pariatur esse excepteur anim amet excepteur irure sint quis esse ex cupidatat ut.", .counter = 0 }, + { .str = "Esse reprehenderit amet qui excepteur aliquip amet.", .counter = 0 }, + { .str = "Ullamco laboris elit labore adipisicing aute nulla qui laborum tempor officia ut dolor aute.", .counter = 0 }, + { .str = "Commodo sunt cillum velit minim laborum Lorem aliqua tempor ad id eu.", .counter = 0 }, + { .str = NULL, .counter = 0 } +}; + +uint32_t test_strings_contain_element(const char *str) { + struct test_string *str_desc = test_strings; + while(str_desc->str) { + if (!strcmp(str, str_desc->str)) + return str_desc - test_strings; + str_desc++; + } + return -1; +} + +#define TEST_INCREMENT_STR_KEYS_HASH_SIZE 20 +int test_increment_str_keys() { + c_rhash hash; + const char *key; + + TEST_START(); + + hash = c_rhash_new(TEST_INCREMENT_STR_KEYS_HASH_SIZE); // less than element count of test_strings + + c_rhash_iter_t iter = C_RHASH_ITER_T_INITIALIZER; + + // check iter on empty hash + ASSERT_RETVAL(c_rhash_iter_str_keys, !=, 0, hash, &iter, &key); + + int32_t element_count = 0; + while (test_strings[element_count].str) { + ASSERT_RETVAL(c_rhash_insert_str_ptr, ==, 0, hash, test_strings[element_count].str, NULL); + test_strings[element_count].counter++; // we want to test we got each key exactly once + element_count++; + } + + if (element_count <= TEST_INCREMENT_STR_KEYS_HASH_SIZE * 2) { + // verify we are actually test also iteration trough single bin (when 2 keys have same hash pointing them to same bin) + PRINT_ERR("For this test to properly test all the hash size needs to be much smaller than all test key count."); + rc = 1; + goto test_cleanup; + } + + // we insert another type of key as iterator should skip it + // in case is another type + ASSERT_RETVAL(c_rhash_insert_uint64_ptr, ==, 0, hash, 5, NULL); + + c_rhash_iter_t_initialize(&iter); + while(!c_rhash_iter_str_keys(hash, &iter, &key)) { + element_count--; + int i; + if ( (i = test_strings_contain_element(key)) < 0) { + PRINT_ERR("Key \"%s\" is not present in test_strings array! (Fnc: %s, Line: %d)", key, __FUNCTION__, __LINE__); + rc = 1; + goto test_cleanup; + } + passed_subtest_count++; + + test_strings[i].counter--; + } + ASSERT_VAL_UINT8(element_count, 0); // we added also same non string keys + + // check each key was present exactly once + struct test_string *str_desc = test_strings; + while (str_desc->str) { + ASSERT_VAL_UINT8(str_desc->counter, 0); + str_desc++; + } + + ALL_SUBTESTS_PASS(); +test_cleanup: + c_rhash_destroy(hash); + return rc; +} + +#define RUN_TEST(fnc) \ +if(fnc()) \ + return 1; + +int main(int argc, char *argv[]) { + RUN_TEST(test_str_uint8); + RUN_TEST(test_uint64_ptr); + RUN_TEST(test_uint64_ptr_incremental); + RUN_TEST(test_increment_str_keys); + // TODO hash with mixed key tests + // TODO iterator test + return 0; +} diff --git a/src/aclk/mqtt_websockets/common_internal.h b/src/aclk/mqtt_websockets/common_internal.h new file mode 100644 index 000000000..2be1c45b8 --- /dev/null +++ b/src/aclk/mqtt_websockets/common_internal.h @@ -0,0 +1,27 @@ +// SPDX-License-Identifier: GPL-3.0-only + +#ifndef COMMON_INTERNAL_H +#define COMMON_INTERNAL_H + +#include "endian_compat.h" + +#ifdef MQTT_WSS_CUSTOM_ALLOC +#include "../helpers/mqtt_wss_pal.h" +#else +#define mw_malloc(...) malloc(__VA_ARGS__) +#define mw_calloc(...) calloc(__VA_ARGS__) +#define mw_free(...) free(__VA_ARGS__) +#define mw_strdup(...) strdup(__VA_ARGS__) +#define mw_realloc(...) realloc(__VA_ARGS__) +#endif + +#ifndef MQTT_WSS_FRAG_MEMALIGN +#define MQTT_WSS_FRAG_MEMALIGN (8) +#endif + +#define OPENSSL_VERSION_095 0x00905100L +#define OPENSSL_VERSION_097 0x00907000L +#define OPENSSL_VERSION_110 0x10100000L +#define OPENSSL_VERSION_111 0x10101000L + +#endif /* COMMON_INTERNAL_H */ diff --git a/src/aclk/mqtt_websockets/common_public.c b/src/aclk/mqtt_websockets/common_public.c new file mode 100644 index 000000000..7991b0c23 --- /dev/null +++ b/src/aclk/mqtt_websockets/common_public.c @@ -0,0 +1,9 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include "common_public.h" + +// this dummy exists to have a special pointer with special meaning +// other than NULL +void _caller_responsibility(void *ptr) { + (void)(ptr); +} diff --git a/src/aclk/mqtt_websockets/common_public.h b/src/aclk/mqtt_websockets/common_public.h new file mode 100644 index 000000000..a855737f9 --- /dev/null +++ b/src/aclk/mqtt_websockets/common_public.h @@ -0,0 +1,33 @@ +#ifndef MQTT_WEBSOCKETS_COMMON_PUBLIC_H +#define MQTT_WEBSOCKETS_COMMON_PUBLIC_H + +#include <stddef.h> + +/* free_fnc_t in general (in whatever function or struct it is used) + * decides how the related data will be handled. + * - If NULL the data are copied internally (causing malloc and later free) + * - If pointer provided the free function pointed will be called when data are no longer needed + * to free associated memory. This is effectively transfering ownership of that pointer to the library. + * This also allows caller to provide custom free function other than system one. + * - If == CALLER_RESPONSIBILITY the library will not copy the data pointed to and will not call free + * at the end. This is usefull to avoid copying memory (and associated malloc/free) when data are for + * example static. In this case caller has to guarantee the memory pointed to will be valid for entire duration + * it is needed. For example by freeing the data after PUBACK is received or by data being static. + */ +typedef void (*free_fnc_t)(void *ptr); +void _caller_responsibility(void *ptr); +#define CALLER_RESPONSIBILITY ((free_fnc_t)&_caller_responsibility) + +struct mqtt_ng_stats { + size_t tx_bytes_queued; + int tx_messages_queued; + int tx_messages_sent; + int rx_messages_rcvd; + size_t tx_buffer_used; + size_t tx_buffer_free; + size_t tx_buffer_size; + // part of transaction buffer that containes mesages we can free alredy during the garbage colleciton step + size_t tx_buffer_reclaimable; +}; + +#endif /* MQTT_WEBSOCKETS_COMMON_PUBLIC_H */ diff --git a/src/aclk/mqtt_websockets/endian_compat.h b/src/aclk/mqtt_websockets/endian_compat.h new file mode 100644 index 000000000..b36d2c858 --- /dev/null +++ b/src/aclk/mqtt_websockets/endian_compat.h @@ -0,0 +1,31 @@ +// SPDX-License-Identifier: GPL-3.0-only + +#ifndef MQTT_WSS_ENDIAN_COMPAT_H +#define MQTT_WSS_ENDIAN_COMPAT_H + +#ifdef __APPLE__ + #include <libkern/OSByteOrder.h> + + #define htobe16(x) OSSwapHostToBigInt16(x) + #define htole16(x) OSSwapHostToLittleInt16(x) + #define be16toh(x) OSSwapBigToHostInt16(x) + #define le16toh(x) OSSwapLittleToHostInt16(x) + + #define htobe32(x) OSSwapHostToBigInt32(x) + #define htole32(x) OSSwapHostToLittleInt32(x) + #define be32toh(x) OSSwapBigToHostInt32(x) + #define le32toh(x) OSSwapLittleToHostInt32(x) + + #define htobe64(x) OSSwapHostToBigInt64(x) + #define htole64(x) OSSwapHostToLittleInt64(x) + #define be64toh(x) OSSwapBigToHostInt64(x) + #define le64toh(x) OSSwapLittleToHostInt64(x) +#else +#ifdef __FreeBSD__ + #include <sys/endian.h> +#else + #include <endian.h> +#endif +#endif + +#endif /* MQTT_WSS_ENDIAN_COMPAT_H */ diff --git a/src/aclk/mqtt_websockets/mqtt_constants.h b/src/aclk/mqtt_websockets/mqtt_constants.h new file mode 100644 index 000000000..3d6a2aa4a --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_constants.h @@ -0,0 +1,103 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#ifndef MQTT_CONSTANTS_H +#define MQTT_CONSTANTS_H + +#define MQTT_MAX_QOS 0x02 + +#define MQTT_VERSION_5_0 0x5 + +/* [MQTT-1.5.5] most significant bit + of MQTT Variable Byte Integer signifies + there are more bytes following */ +#define MQTT_VBI_CONTINUATION_FLAG 0x80 +#define MQTT_VBI_DATA_MASK 0x7F +#define MQTT_VBI_MAXBYTES 4 + +/* MQTT control packet types as defined in + 2.1.2 MQTT Control Packet type */ +#define MQTT_CPT_CONNECT 0x1 +#define MQTT_CPT_CONNACK 0x2 +#define MQTT_CPT_PUBLISH 0x3 +#define MQTT_CPT_PUBACK 0x4 +#define MQTT_CPT_PUBREC 0x5 +#define MQTT_CPT_PUBREL 0x6 +#define MQTT_CPT_PUBCOMP 0x7 +#define MQTT_CPT_SUBSCRIBE 0x8 +#define MQTT_CPT_SUBACK 0x9 +#define MQTT_CPT_UNSUBSCRIBE 0xA +#define MQTT_CPT_UNSUBACK 0xB +#define MQTT_CPT_PINGREQ 0xC +#define MQTT_CPT_PINGRESP 0xD +#define MQTT_CPT_DISCONNECT 0xE +#define MQTT_CPT_AUTH 0xF + +// MQTT CONNECT FLAGS (spec:3.1.2.3) +#define MQTT_CONNECT_FLAG_USERNAME 0x80 +#define MQTT_CONNECT_FLAG_PASSWORD 0x40 +#define MQTT_CONNECT_FLAG_LWT_RETAIN 0x20 +#define MQTT_CONNECT_FLAG_LWT 0x04 +#define MQTT_CONNECT_FLAG_CLEAN_START 0x02 + +#define MQTT_CONNECT_FLAG_QOS_MASK 0x18 +#define MQTT_CONNECT_FLAG_QOS_BITSHIFT 3 + +#define MQTT_MAX_CLIENT_ID 23 /* [MQTT-3.1.3-5] */ + +// MQTT Property identifiers [MQTT-2.2.2.2] +#define MQTT_PROP_PAYLOAD_FMT_INDICATOR 0x01 +#define MQTT_PROP_PAYLOAD_FMT_INDICATOR_NAME "Payload Format Indicator" +#define MQTT_PROP_MSG_EXPIRY_INTERVAL 0x02 +#define MQTT_PROP_MSG_EXPIRY_INTERVAL_NAME "Message Expiry Interval" +#define MQTT_PROP_CONTENT_TYPE 0x03 +#define MQTT_PROP_CONTENT_TYPE_NAME "Content Type" +#define MQTT_PROP_RESPONSE_TOPIC 0x08 +#define MQTT_PROP_RESPONSE_TOPIC_NAME "Response Topic" +#define MQTT_PROP_CORRELATION_DATA 0x09 +#define MQTT_PROP_CORRELATION_DATA_NAME "Correlation Data" +#define MQTT_PROP_SUB_IDENTIFIER 0x0B +#define MQTT_PROP_SUB_IDENTIFIER_NAME "Subscription Identifier" +#define MQTT_PROP_SESSION_EXPIRY_INTERVAL 0x11 +#define MQTT_PROP_SESSION_EXPIRY_INTERVAL_NAME "Session Expiry Interval" +#define MQTT_PROP_ASSIGNED_CLIENT_ID 0x12 +#define MQTT_PROP_ASSIGNED_CLIENT_ID_NAME "Assigned Client Identifier" +#define MQTT_PROP_SERVER_KEEP_ALIVE 0x13 +#define MQTT_PROP_SERVER_KEEP_ALIVE_NAME "Server Keep Alive" +#define MQTT_PROP_AUTH_METHOD 0x15 +#define MQTT_PROP_AUTH_METHOD_NAME "Authentication Method" +#define MQTT_PROP_AUTH_DATA 0x16 +#define MQTT_PROP_AUTH_DATA_NAME "Authentication Data" +#define MQTT_PROP_REQ_PROBLEM_INFO 0x17 +#define MQTT_PROP_REQ_PROBLEM_INFO_NAME "Request Problem Information" +#define MQTT_PROP_WILL_DELAY_INTERVAL 0x18 +#define MQTT_PROP_WIIL_DELAY_INTERVAL_NAME "Will Delay Interval" +#define MQTT_PROP_REQ_RESP_INFORMATION 0x19 +#define MQTT_PROP_REQ_RESP_INFORMATION_NAME "Request Response Information" +#define MQTT_PROP_RESP_INFORMATION 0x1A +#define MQTT_PROP_RESP_INFORMATION_NAME "Response Information" +#define MQTT_PROP_SERVER_REF 0x1C +#define MQTT_PROP_SERVER_REF_NAME "Server Reference" +#define MQTT_PROP_REASON_STR 0x1F +#define MQTT_PROP_REASON_STR_NAME "Reason String" +#define MQTT_PROP_RECEIVE_MAX 0x21 +#define MQTT_PROP_RECEIVE_MAX_NAME "Receive Maximum" +#define MQTT_PROP_TOPIC_ALIAS_MAX 0x22 +#define MQTT_PROP_TOPIC_ALIAS_MAX_NAME "Topic Alias Maximum" +#define MQTT_PROP_TOPIC_ALIAS 0x23 +#define MQTT_PROP_TOPIC_ALIAS_NAME "Topic Alias" +#define MQTT_PROP_MAX_QOS 0x24 +#define MQTT_PROP_MAX_QOS_NAME "Maximum QoS" +#define MQTT_PROP_RETAIN_AVAIL 0x25 +#define MQTT_PROP_RETAIN_AVAIL_NAME "Retain Available" +#define MQTT_PROP_USR 0x26 +#define MQTT_PROP_USR_NAME "User Property" +#define MQTT_PROP_MAX_PKT_SIZE 0x27 +#define MQTT_PROP_MAX_PKT_SIZE_NAME "Maximum Packet Size" +#define MQTT_PROP_WILDCARD_SUB_AVAIL 0x28 +#define MQTT_PROP_WILDCARD_SUB_AVAIL_NAME "Wildcard Subscription Available" +#define MQTT_PROP_SUB_ID_AVAIL 0x29 +#define MQTT_PROP_SUB_ID_AVAIL_NAME "Subscription Identifier Available" +#define MQTT_PROP_SHARED_SUB_AVAIL 0x2A +#define MQTT_PROP_SHARED_SUB_AVAIL_NAME "Shared Subscription Available" + +#endif /* MQTT_CONSTANTS_H */ diff --git a/src/aclk/mqtt_websockets/mqtt_ng.c b/src/aclk/mqtt_websockets/mqtt_ng.c new file mode 100644 index 000000000..f570fde71 --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_ng.c @@ -0,0 +1,2237 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif + +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <pthread.h> +#include <inttypes.h> + +#include "c_rhash/c_rhash.h" + +#include "common_internal.h" +#include "mqtt_constants.h" +#include "mqtt_wss_log.h" +#include "mqtt_ng.h" + +#define UNIT_LOG_PREFIX "mqtt_client: " +#define FATAL(fmt, ...) mws_fatal(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define ERROR(fmt, ...) mws_error(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define WARN(fmt, ...) mws_warn (client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define INFO(fmt, ...) mws_info (client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define DEBUG(fmt, ...) mws_debug(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) + +#define SMALL_STRING_DONT_FRAGMENT_LIMIT 128 + +#define MIN(a,b) (((a)<(b))?(a):(b)) + +#define LOCK_HDR_BUFFER(buffer) pthread_mutex_lock(&((buffer)->mutex)) +#define UNLOCK_HDR_BUFFER(buffer) pthread_mutex_unlock(&((buffer)->mutex)) + +#define BUFFER_FRAG_GARBAGE_COLLECT 0x01 +// some packets can be marked for garbage collection +// immediately when they are sent (e.g. sent PUBACK on QoS1) +#define BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND 0x02 +// as buffer fragment can point to both +// external data and data in the same buffer +// we mark the former case with BUFFER_FRAG_DATA_EXTERNAL +#define BUFFER_FRAG_DATA_EXTERNAL 0x04 +// as single MQTT Packet can be stored into multiple +// buffer fragments (depending on copy requirements) +// this marks this fragment to be the first/last +#define BUFFER_FRAG_MQTT_PACKET_HEAD 0x10 +#define BUFFER_FRAG_MQTT_PACKET_TAIL 0x20 + +typedef uint16_t buffer_frag_flag_t; +struct buffer_fragment { + size_t len; + size_t sent; + buffer_frag_flag_t flags; + void (*free_fnc)(void *ptr); + unsigned char *data; + + uint16_t packet_id; + + struct buffer_fragment *next; +}; + +typedef struct buffer_fragment *mqtt_msg_data; + +// buffer used for MQTT headers only +// not for actual data sent +struct header_buffer { + size_t size; + unsigned char *data; + unsigned char *tail; + struct buffer_fragment *tail_frag; +}; + +struct transaction_buffer { + struct header_buffer hdr_buffer; + // used while building new message + // to be able to revert state easily + // in case of error mid processing + struct header_buffer state_backup; + pthread_mutex_t mutex; + struct buffer_fragment *sending_frag; +}; + +enum mqtt_client_state { + RAW = 0, + CONNECT_PENDING, + CONNECTING, + CONNECTED, + ERROR, + DISCONNECTED +}; + +enum parser_state { + MQTT_PARSE_FIXED_HEADER_PACKET_TYPE = 0, + MQTT_PARSE_FIXED_HEADER_LEN, + MQTT_PARSE_VARIABLE_HEADER, + MQTT_PARSE_MQTT_PACKET_DONE +}; + +enum varhdr_parser_state { + MQTT_PARSE_VARHDR_INITIAL = 0, + MQTT_PARSE_VARHDR_OPTIONAL_REASON_CODE, + MQTT_PARSE_VARHDR_PROPS, + MQTT_PARSE_VARHDR_TOPICNAME, + MQTT_PARSE_VARHDR_POST_TOPICNAME, + MQTT_PARSE_VARHDR_PACKET_ID, + MQTT_PARSE_REASONCODES, + MQTT_PARSE_PAYLOAD +}; + +struct mqtt_vbi_parser_ctx { + char data[MQTT_VBI_MAXBYTES]; + uint8_t bytes; + uint32_t result; +}; + +enum mqtt_datatype { + MQTT_TYPE_UNKNOWN = 0, + MQTT_TYPE_UINT_8, + MQTT_TYPE_UINT_16, + MQTT_TYPE_UINT_32, + MQTT_TYPE_VBI, + MQTT_TYPE_STR, + MQTT_TYPE_STR_PAIR, + MQTT_TYPE_BIN +}; + +struct mqtt_property { + uint8_t id; + enum mqtt_datatype type; + union { + char *strings[2]; + void *bindata; + uint8_t uint8; + uint16_t uint16; + uint32_t uint32; + } data; + size_t bindata_len; + struct mqtt_property *next; +}; + +enum mqtt_properties_parser_state { + PROPERTIES_LENGTH = 0, + PROPERTY_CREATE, + PROPERTY_ID, + PROPERTY_TYPE_UINT8, + PROPERTY_TYPE_UINT16, + PROPERTY_TYPE_UINT32, + PROPERTY_TYPE_STR_BIN_LEN, + PROPERTY_TYPE_STR, + PROPERTY_TYPE_BIN, + PROPERTY_TYPE_VBI, + PROPERTY_NEXT +}; + +struct mqtt_properties_parser_ctx { + enum mqtt_properties_parser_state state; + struct mqtt_property *head; + struct mqtt_property *tail; + uint32_t properties_length; + uint32_t vbi_length; + struct mqtt_vbi_parser_ctx vbi_parser_ctx; + size_t bytes_consumed; + int str_idx; +}; + +struct mqtt_connack { + uint8_t flags; + uint8_t reason_code; +}; +struct mqtt_puback { + uint16_t packet_id; + uint8_t reason_code; +}; + +struct mqtt_suback { + uint16_t packet_id; + uint8_t *reason_codes; + uint8_t reason_code_count; + uint8_t reason_codes_pending; +}; + +struct mqtt_publish { + uint16_t topic_len; + char *topic; + uint16_t packet_id; + size_t data_len; + char *data; + uint8_t qos; +}; + +struct mqtt_disconnect { + uint8_t reason_code; +}; + +struct mqtt_ng_parser { + rbuf_t received_data; + + uint8_t mqtt_control_packet_type; + uint32_t mqtt_fixed_hdr_remaining_length; + size_t mqtt_parsed_len; + + struct mqtt_vbi_parser_ctx vbi_parser; + struct mqtt_properties_parser_ctx properties_parser; + + enum parser_state state; + enum varhdr_parser_state varhdr_state; + + struct mqtt_property *varhdr_properties; + + union { + struct mqtt_connack connack; + struct mqtt_puback puback; + struct mqtt_suback suback; + struct mqtt_publish publish; + struct mqtt_disconnect disconnect; + } mqtt_packet; +}; + +struct topic_alias_data { + uint16_t idx; + uint32_t usage_count; +}; + +struct topic_aliases_data { + c_rhash stoi_dict; + uint32_t idx_max; + uint32_t idx_assigned; + pthread_rwlock_t rwlock; +}; + +struct mqtt_ng_client { + struct transaction_buffer main_buffer; + + enum mqtt_client_state client_state; + + mqtt_msg_data connect_msg; + + mqtt_wss_log_ctx_t log; + + mqtt_ng_send_fnc_t send_fnc_ptr; + void *user_ctx; + + // time when last fragment of MQTT message was sent + time_t time_of_last_send; + + struct mqtt_ng_parser parser; + + size_t max_mem_bytes; + + void (*puback_callback)(uint16_t packet_id); + void (*connack_callback)(void* user_ctx, int connack_reply); + void (*msg_callback)(const char *topic, const void *msg, size_t msglen, int qos); + + unsigned int ping_pending:1; + + struct mqtt_ng_stats stats; + pthread_mutex_t stats_mutex; + + struct topic_aliases_data tx_topic_aliases; + c_rhash rx_aliases; + + size_t max_msg_size; +}; + +unsigned char pingreq[] = { MQTT_CPT_PINGREQ << 4, 0x00 }; + +struct buffer_fragment ping_frag = { + .data = pingreq, + .flags = BUFFER_FRAG_MQTT_PACKET_HEAD | BUFFER_FRAG_MQTT_PACKET_TAIL, + .free_fnc = NULL, + .len = sizeof(pingreq), + .next = NULL, + .sent = 0, + .packet_id = 0 +}; + +int uint32_to_mqtt_vbi(uint32_t input, unsigned char *output) { + int i = 1; + *output = 0; + + /* MQTT 5 specs allows max 4 bytes of output + making it 0xFF, 0xFF, 0xFF, 0x7F + representing number 268435455 decimal + see 1.5.5. Variable Byte Integer */ + if(input >= 256 * 1024 * 1024) + return 0; + + if(!input) { + *output = 0; + return 1; + } + + while(input) { + output[i-1] = input & MQTT_VBI_DATA_MASK; + input >>= 7; + if (input) + output[i-1] |= MQTT_VBI_CONTINUATION_FLAG; + i++; + } + return i - 1; +} + +int mqtt_vbi_to_uint32(char *input, uint32_t *output) { + // dont want to operate directly on output + // as I want it to be possible for input and output + // pointer to be the same + uint32_t result = 0; + uint32_t multiplier = 1; + + do { + result += (uint32_t)(*input & MQTT_VBI_DATA_MASK) * multiplier; + if (multiplier > 128*128*128) + return 1; + multiplier <<= 7; + } while (*input++ & MQTT_VBI_CONTINUATION_FLAG); + *output = result; + return 0; +} + +#ifdef TESTS +#include <stdio.h> +#define MQTT_VBI_MAXLEN 4 +// we add extra byte to check we dont write out of bounds +// in case where 4 bytes are supposed to be written +static const char _mqtt_vbi_0[MQTT_VBI_MAXLEN + 1] = { 0x00, 0x00, 0x00, 0x00, 0x00 }; +static const char _mqtt_vbi_127[MQTT_VBI_MAXLEN + 1] = { 0x7F, 0x00, 0x00, 0x00, 0x00 }; +static const char _mqtt_vbi_128[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x01, 0x00, 0x00, 0x00 }; +static const char _mqtt_vbi_16383[MQTT_VBI_MAXLEN + 1] = { 0xFF, 0x7F, 0x00, 0x00, 0x00 }; +static const char _mqtt_vbi_16384[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x80, 0x01, 0x00, 0x00 }; +static const char _mqtt_vbi_2097151[MQTT_VBI_MAXLEN + 1] = { 0xFF, 0xFF, 0x7F, 0x00, 0x00 }; +static const char _mqtt_vbi_2097152[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x80, 0x80, 0x01, 0x00 }; +static const char _mqtt_vbi_268435455[MQTT_VBI_MAXLEN + 1] = { 0xFF, 0xFF, 0xFF, 0x7F, 0x00 }; +static const char _mqtt_vbi_999999999[MQTT_VBI_MAXLEN + 1] = { 0x80, 0x80, 0x80, 0x80, 0x01 }; + +#define MQTT_VBI_TESTCASE(case, expected_len) \ + { \ + memset(buf, 0, MQTT_VBI_MAXLEN + 1); \ + int len; \ + if ((len=uint32_to_mqtt_vbi(case, buf)) != expected_len) { \ + fprintf(stderr, "uint32_to_mqtt_vbi(case:%d, line:%d): Incorrect length returned. Expected %d, Got %d\n", case, __LINE__, expected_len, len); \ + return 1; \ + } \ + if (memcmp(buf, _mqtt_vbi_ ## case, MQTT_VBI_MAXLEN + 1 )) { \ + fprintf(stderr, "uint32_to_mqtt_vbi(case:%d, line:%d): Wrong output\n", case, __LINE__); \ + return 1; \ + } } + + +int test_uint32_mqtt_vbi() { + char buf[MQTT_VBI_MAXLEN + 1]; + + MQTT_VBI_TESTCASE(0, 1) + MQTT_VBI_TESTCASE(127, 1) + MQTT_VBI_TESTCASE(128, 2) + MQTT_VBI_TESTCASE(16383, 2) + MQTT_VBI_TESTCASE(16384, 3) + MQTT_VBI_TESTCASE(2097151, 3) + MQTT_VBI_TESTCASE(2097152, 4) + MQTT_VBI_TESTCASE(268435455, 4) + + memset(buf, 0, MQTT_VBI_MAXLEN + 1); + int len; + if ((len=uint32_to_mqtt_vbi(268435456, buf)) != 0) { + fprintf(stderr, "uint32_to_mqtt_vbi(case:268435456, line:%d): Incorrect length returned. Expected 0, Got %d\n", __LINE__, len); + return 1; + } + + return 0; +} + +#define MQTT_VBI2UINT_TESTCASE(case, expected_error) \ + { \ + uint32_t result; \ + int ret = mqtt_vbi_to_uint32(_mqtt_vbi_ ## case, &result); \ + if (ret && !(expected_error)) { \ + fprintf(stderr, "mqtt_vbi_to_uint(case:%d, line:%d): Unexpectedly Errored\n", (case), __LINE__); \ + return 1; \ + } \ + if (!ret && (expected_error)) { \ + fprintf(stderr, "mqtt_vbi_to_uint(case:%d, line:%d): Should return error but didnt\n", (case), __LINE__); \ + return 1; \ + } \ + if (!ret && result != (case)) { \ + fprintf(stderr, "mqtt_vbi_to_uint(case:%d, line:%d): Returned wrong result %d\n", (case), __LINE__, result); \ + return 1; \ + }} + + +int test_mqtt_vbi_to_uint32() { + MQTT_VBI2UINT_TESTCASE(0, 0) + MQTT_VBI2UINT_TESTCASE(127, 0) + MQTT_VBI2UINT_TESTCASE(128, 0) + MQTT_VBI2UINT_TESTCASE(16383, 0) + MQTT_VBI2UINT_TESTCASE(16384, 0) + MQTT_VBI2UINT_TESTCASE(2097151, 0) + MQTT_VBI2UINT_TESTCASE(2097152, 0) + MQTT_VBI2UINT_TESTCASE(268435455, 0) + MQTT_VBI2UINT_TESTCASE(999999999, 1) + return 0; +} +#endif /* TESTS */ + +// this helps with switch statements +// as they have to use integer type (not pointer) +enum memory_mode { + MEMCPY, + EXTERNAL_FREE_AFTER_USE, + CALLER_RESPONSIBLE +}; + +static inline enum memory_mode ptr2memory_mode(void * ptr) { + if (ptr == NULL) + return MEMCPY; + if (ptr == CALLER_RESPONSIBILITY) + return CALLER_RESPONSIBLE; + return EXTERNAL_FREE_AFTER_USE; +} + +#define frag_is_marked_for_gc(frag) ((frag->flags & BUFFER_FRAG_GARBAGE_COLLECT) || ((frag->flags & BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND) && frag->sent == frag->len)) +#define FRAG_SIZE_IN_BUFFER(frag) (sizeof(struct buffer_fragment) + ((frag->flags & BUFFER_FRAG_DATA_EXTERNAL) ? 0 : frag->len)) + +static void buffer_frag_free_data(struct buffer_fragment *frag) +{ + if ( frag->flags & BUFFER_FRAG_DATA_EXTERNAL && frag->data != NULL) { + switch (ptr2memory_mode(frag->free_fnc)) { + case MEMCPY: + mw_free(frag->data); + break; + case EXTERNAL_FREE_AFTER_USE: + frag->free_fnc(frag->data); + break; + case CALLER_RESPONSIBLE: + break; + } + frag->data = NULL; + } +} + +#define HEADER_BUFFER_SIZE 1024*1024 +#define GROWTH_FACTOR 1.25 + +#define BUFFER_BYTES_USED(buf) ((size_t)((buf)->tail - (buf)->data)) +#define BUFFER_BYTES_AVAILABLE(buf) ((buf)->size - BUFFER_BYTES_USED(buf)) +#define BUFFER_FIRST_FRAG(buf) ((struct buffer_fragment *)((buf)->tail_frag ? (buf)->data : NULL)) +static void buffer_purge(struct header_buffer *buf) { + struct buffer_fragment *frag = BUFFER_FIRST_FRAG(buf); + while (frag) { + buffer_frag_free_data(frag); + frag = frag->next; + } + buf->tail = buf->data; + buf->tail_frag = NULL; +} + +#define FRAG_PADDING(addr) ((MQTT_WSS_FRAG_MEMALIGN - ((uintptr_t)addr % MQTT_WSS_FRAG_MEMALIGN)) % MQTT_WSS_FRAG_MEMALIGN) +static struct buffer_fragment *buffer_new_frag(struct header_buffer *buf, buffer_frag_flag_t flags) +{ + uint8_t padding = FRAG_PADDING(buf->tail); + + if (BUFFER_BYTES_AVAILABLE(buf) < sizeof(struct buffer_fragment) + padding) + return NULL; + + struct buffer_fragment *frag = (struct buffer_fragment *)(buf->tail + padding); + + memset(frag, 0, sizeof(*frag)); + buf->tail += sizeof(*frag) + padding; + + if (/*!((frag)->flags & BUFFER_FRAG_MQTT_PACKET_HEAD) &&*/ buf->tail_frag) + buf->tail_frag->next = frag; + + buf->tail_frag = frag; + + frag->data = buf->tail; + + frag->flags = flags; + + return frag; +} + +static void buffer_rebuild(struct header_buffer *buf) +{ + struct buffer_fragment *frag = (struct buffer_fragment*)buf->data; + do { + buf->tail = (unsigned char *) frag + sizeof(struct buffer_fragment); + buf->tail_frag = frag; + if (!(frag->flags & BUFFER_FRAG_DATA_EXTERNAL)) { + buf->tail_frag->data = buf->tail; + buf->tail += frag->len; + } + if (frag->next != NULL) + frag->next = (struct buffer_fragment*)(buf->tail + FRAG_PADDING(buf->tail)); + frag = frag->next; + } while(frag); +} + +static void buffer_garbage_collect(struct header_buffer *buf, mqtt_wss_log_ctx_t log_ctx) +{ +#if !defined(MQTT_DEBUG_VERBOSE) && !defined(ADDITIONAL_CHECKS) + (void) log_ctx; +#endif +#ifdef MQTT_DEBUG_VERBOSE + mws_debug(log_ctx, "Buffer Garbage Collection!"); +#endif + + struct buffer_fragment *frag = BUFFER_FIRST_FRAG(buf); + while (frag) { + if (!frag_is_marked_for_gc(frag)) + break; + + buffer_frag_free_data(frag); + + frag = frag->next; + } + + if (frag == BUFFER_FIRST_FRAG(buf)) { +#ifdef MQTT_DEBUG_VERBOSE + mws_debug(log_ctx, "Buffer Garbage Collection! No Space Reclaimed!"); +#endif + return; + } + + if (!frag) { + buf->tail_frag = NULL; + buf->tail = buf->data; + return; + } + +#ifdef ADDITIONAL_CHECKS + if (!(frag->flags & BUFFER_FRAG_MQTT_PACKET_HEAD)) { + mws_error(log_ctx, "Expected to find end of buffer (NULL) or next packet head!"); + return; + } +#endif + + memmove(buf->data, frag, buf->tail - (unsigned char *) frag); + buffer_rebuild(buf); +} + +static void transaction_buffer_garbage_collect(struct transaction_buffer *buf, mqtt_wss_log_ctx_t log_ctx) +{ +#ifdef MQTT_DEBUG_VERBOSE + mws_debug(log_ctx, "Transaction Buffer Garbage Collection! %s", buf->sending_frag == NULL ? "NULL" : "in flight message"); +#endif + + // Invalidate the cached sending fragment + // as we will move data around + if (buf->sending_frag != &ping_frag) + buf->sending_frag = NULL; + + buffer_garbage_collect(&buf->hdr_buffer, log_ctx); +} + +static int transaction_buffer_grow(struct transaction_buffer *buf, mqtt_wss_log_ctx_t log_ctx, float rate, size_t max) +{ + if (buf->hdr_buffer.size >= max) + return 0; + + // Invalidate the cached sending fragment + // as we will move data around + if (buf->sending_frag != &ping_frag) + buf->sending_frag = NULL; + + buf->hdr_buffer.size *= rate; + if (buf->hdr_buffer.size > max) + buf->hdr_buffer.size = max; + + void *ret = mw_realloc(buf->hdr_buffer.data, buf->hdr_buffer.size); + if (ret == NULL) { + mws_warn(log_ctx, "Buffer growth failed (realloc)"); + return 1; + } + + mws_debug(log_ctx, "Message metadata buffer was grown"); + + buf->hdr_buffer.data = ret; + buffer_rebuild(&buf->hdr_buffer); + return 0; +} + +inline static int transaction_buffer_init(struct transaction_buffer *to_init, size_t size) +{ + pthread_mutex_init(&to_init->mutex, NULL); + + to_init->hdr_buffer.size = size; + to_init->hdr_buffer.data = mw_malloc(size); + if (to_init->hdr_buffer.data == NULL) + return 1; + + to_init->hdr_buffer.tail = to_init->hdr_buffer.data; + to_init->hdr_buffer.tail_frag = NULL; + return 0; +} + +static void transaction_buffer_destroy(struct transaction_buffer *to_init) +{ + buffer_purge(&to_init->hdr_buffer); + pthread_mutex_destroy(&to_init->mutex); + mw_free(to_init->hdr_buffer.data); +} + +// Creates transaction +// saves state of buffer before any operation was done +// allowing for rollback if things go wrong +#define transaction_buffer_transaction_start(buf) \ + { LOCK_HDR_BUFFER(buf); \ + memcpy(&(buf)->state_backup, &(buf)->hdr_buffer, sizeof((buf)->hdr_buffer)); } + +#define transaction_buffer_transaction_commit(buf) UNLOCK_HDR_BUFFER(buf); + +void transaction_buffer_transaction_rollback(struct transaction_buffer *buf, struct buffer_fragment *frag) +{ + memcpy(&buf->hdr_buffer, &buf->state_backup, sizeof(buf->hdr_buffer)); + if (buf->hdr_buffer.tail_frag != NULL) + buf->hdr_buffer.tail_frag->next = NULL; + + while(frag) { + buffer_frag_free_data(frag); + // we are not actually freeing the structure itself + // just the data it manages + // structure itself is in permanent buffer + // which is locked by HDR_BUFFER lock + frag = frag->next; + } + + UNLOCK_HDR_BUFFER(buf); +} + +#define TX_ALIASES_INITIALIZE() c_rhash_new(0) +#define RX_ALIASES_INITIALIZE() c_rhash_new(UINT16_MAX >> 8) +struct mqtt_ng_client *mqtt_ng_init(struct mqtt_ng_init *settings) +{ + struct mqtt_ng_client *client = mw_calloc(1, sizeof(struct mqtt_ng_client)); + if (client == NULL) + return NULL; + + if (transaction_buffer_init(&client->main_buffer, HEADER_BUFFER_SIZE)) + goto err_free_client; + + client->rx_aliases = RX_ALIASES_INITIALIZE(); + if (client->rx_aliases == NULL) + goto err_free_trx_buf; + + if (pthread_mutex_init(&client->stats_mutex, NULL)) + goto err_free_rx_alias; + + client->tx_topic_aliases.stoi_dict = TX_ALIASES_INITIALIZE(); + if (client->tx_topic_aliases.stoi_dict == NULL) + goto err_free_stats_mutex; + client->tx_topic_aliases.idx_max = UINT16_MAX; + + if (pthread_rwlock_init(&client->tx_topic_aliases.rwlock, NULL)) + goto err_free_tx_alias; + + // TODO just embed the struct into mqtt_ng_client + client->parser.received_data = settings->data_in; + client->send_fnc_ptr = settings->data_out_fnc; + client->user_ctx = settings->user_ctx; + + client->log = settings->log; + + client->puback_callback = settings->puback_callback; + client->connack_callback = settings->connack_callback; + client->msg_callback = settings->msg_callback; + + return client; + +err_free_tx_alias: + c_rhash_destroy(client->tx_topic_aliases.stoi_dict); +err_free_stats_mutex: + pthread_mutex_destroy(&client->stats_mutex); +err_free_rx_alias: + c_rhash_destroy(client->rx_aliases); +err_free_trx_buf: + transaction_buffer_destroy(&client->main_buffer); +err_free_client: + mw_free(client); + return NULL; +} + +static inline uint8_t get_control_packet_type(uint8_t first_hdr_byte) +{ + return first_hdr_byte >> 4; +} + +static void mqtt_ng_destroy_rx_alias_hash(c_rhash hash) +{ + c_rhash_iter_t i = C_RHASH_ITER_T_INITIALIZER; + uint64_t stored_key; + void *to_free; + while(!c_rhash_iter_uint64_keys(hash, &i, &stored_key)) { + c_rhash_get_ptr_by_uint64(hash, stored_key, &to_free); + mw_free(to_free); + } + c_rhash_destroy(hash); +} + +static void mqtt_ng_destroy_tx_alias_hash(c_rhash hash) +{ + c_rhash_iter_t i = C_RHASH_ITER_T_INITIALIZER; + const char *stored_key; + void *to_free; + while(!c_rhash_iter_str_keys(hash, &i, &stored_key)) { + c_rhash_get_ptr_by_str(hash, stored_key, &to_free); + mw_free(to_free); + } + c_rhash_destroy(hash); +} + +void mqtt_ng_destroy(struct mqtt_ng_client *client) +{ + transaction_buffer_destroy(&client->main_buffer); + pthread_mutex_destroy(&client->stats_mutex); + + mqtt_ng_destroy_tx_alias_hash(client->tx_topic_aliases.stoi_dict); + pthread_rwlock_destroy(&client->tx_topic_aliases.rwlock); + mqtt_ng_destroy_rx_alias_hash(client->rx_aliases); + + mw_free(client); +} + +int frag_set_external_data(mqtt_wss_log_ctx_t log, struct buffer_fragment *frag, void *data, size_t data_len, free_fnc_t data_free_fnc) +{ + if (frag->len) { + // TODO?: This could potentially be done in future if we set rule + // external data always follows in buffer data + // could help reduce fragmentation in some messages but + // currently not worth it considering time is tight + mws_fatal(log, UNIT_LOG_PREFIX "INTERNAL ERROR: Cannot set external data to fragment already containing in buffer data!"); + return 1; + } + + switch (ptr2memory_mode(data_free_fnc)) { + case MEMCPY: + frag->data = mw_malloc(data_len); + if (frag->data == NULL) { + mws_error(log, UNIT_LOG_PREFIX "OOM while malloc @_optimized_add"); + return 1; + } + memcpy(frag->data, data, data_len); + break; + case EXTERNAL_FREE_AFTER_USE: + case CALLER_RESPONSIBLE: + frag->data = data; + break; + } + frag->free_fnc = data_free_fnc; + frag->len = data_len; + + frag->flags |= BUFFER_FRAG_DATA_EXTERNAL; + return 0; + } + +// this is fixed part of variable header for connect packet +// mqtt-v5.0-cs1, 3.1.2.1, 2.1.2.2 +static const char mqtt_protocol_name_frag[] = + { 0x00, 0x04, 'M', 'Q', 'T', 'T', MQTT_VERSION_5_0 }; + +#define MQTT_UTF8_STRING_SIZE(string) (2 + strlen(string)) + +// see 1.5.5 +#define MQTT_VARSIZE_INT_BYTES(value) ( value > 2097152 ? 4 : ( value > 16384 ? 3 : ( value > 128 ? 2 : 1 ) ) ) + +static size_t mqtt_ng_connect_size(struct mqtt_auth_properties *auth, + struct mqtt_lwt_properties *lwt) +{ + // First get the size of payload + variable header + size_t size = + + sizeof(mqtt_protocol_name_frag) /* Proto Name and Version */ + + 1 /* Connect Flags */ + + 2 /* Keep Alive */ + + 4 /* 3.1.2.11.1 Property Length - for now fixed to only Topic Alias Maximum, TODO TODO*/; + + // CONNECT payload. 3.1.3 + if (auth->client_id) + size += MQTT_UTF8_STRING_SIZE(auth->client_id); + + if (lwt) { + // 3.1.3.2 will properties TODO TODO + size += 1; + + // 3.1.3.3 + if (lwt->will_topic) + size += MQTT_UTF8_STRING_SIZE(lwt->will_topic); + + // 3.1.3.4 will payload + if (lwt->will_message) { + size += 2 + lwt->will_message_size; + } + } + + // 3.1.3.5 + if (auth->username) + size += MQTT_UTF8_STRING_SIZE(auth->username); + + // 3.1.3.6 + if (auth->password) + size += MQTT_UTF8_STRING_SIZE(auth->password); + + return size; +} + +#define BUFFER_TRANSACTION_NEW_FRAG(buf, flags, frag, on_fail) \ + { if(frag==NULL) { \ + frag = buffer_new_frag(buf, (flags)); } \ + if(frag==NULL) { on_fail; }} + +#define CHECK_BYTES_AVAILABLE(buf, needed, fail) \ + { if (BUFFER_BYTES_AVAILABLE(buf) < (size_t)needed) { \ + fail; } } + +#define DATA_ADVANCE(buf, bytes, frag) { size_t b = (bytes); (buf)->tail += b; (frag)->len += b; } + +// TODO maybe just user client->buf.tail? +#define WRITE_POS(frag) (&(frag->data[frag->len])) + +// [MQTT-1.5.2] Two Byte Integer +#define PACK_2B_INT(buffer, integer, frag) { *(uint16_t *)WRITE_POS(frag) = htobe16((integer)); \ + DATA_ADVANCE(buffer, sizeof(uint16_t), frag); } + +static int _optimized_add(struct header_buffer *buf, mqtt_wss_log_ctx_t log_ctx, void *data, size_t data_len, free_fnc_t data_free_fnc, struct buffer_fragment **frag) +{ + if (data_len > SMALL_STRING_DONT_FRAGMENT_LIMIT) { + buffer_frag_flag_t flags = BUFFER_FRAG_DATA_EXTERNAL; + if ((*frag)->flags & BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND) + flags |= BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND; + if( (*frag = buffer_new_frag(buf, flags)) == NULL ) { + mws_error(log_ctx, "Out of buffer space while generating the message"); + return 1; + } + if (frag_set_external_data(log_ctx, *frag, data, data_len, data_free_fnc)) { + mws_error(log_ctx, "Error adding external data to newly created fragment"); + return 1; + } + // we dont want to write to this fragment anymore + *frag = NULL; + } else if (data_len) { + // if the data are small dont bother creating new fragments + // store in buffer directly + CHECK_BYTES_AVAILABLE(buf, data_len, return 1); + memcpy(buf->tail, data, data_len); + DATA_ADVANCE(buf, data_len, *frag); + } + return 0; +} + +#define TRY_GENERATE_MESSAGE(generator_function, client, ...) \ + int rc = generator_function(&client->main_buffer, client->log, ##__VA_ARGS__); \ + if (rc == MQTT_NG_MSGGEN_BUFFER_OOM) { \ + LOCK_HDR_BUFFER(&client->main_buffer); \ + transaction_buffer_garbage_collect((&client->main_buffer), client->log); \ + UNLOCK_HDR_BUFFER(&client->main_buffer); \ + rc = generator_function(&client->main_buffer, client->log, ##__VA_ARGS__); \ + if (rc == MQTT_NG_MSGGEN_BUFFER_OOM && client->max_mem_bytes) { \ + LOCK_HDR_BUFFER(&client->main_buffer); \ + transaction_buffer_grow((&client->main_buffer), client->log, GROWTH_FACTOR, client->max_mem_bytes); \ + UNLOCK_HDR_BUFFER(&client->main_buffer); \ + rc = generator_function(&client->main_buffer, client->log, ##__VA_ARGS__); \ + } \ + if (rc == MQTT_NG_MSGGEN_BUFFER_OOM) \ + mws_error(client->log, "%s failed to generate message due to insufficient buffer space (line %d)", __FUNCTION__, __LINE__); \ + } \ + if (rc == MQTT_NG_MSGGEN_OK) { \ + pthread_mutex_lock(&client->stats_mutex); \ + client->stats.tx_messages_queued++; \ + pthread_mutex_unlock(&client->stats_mutex); \ + } \ + return rc; + +mqtt_msg_data mqtt_ng_generate_connect(struct transaction_buffer *trx_buf, + mqtt_wss_log_ctx_t log_ctx, + struct mqtt_auth_properties *auth, + struct mqtt_lwt_properties *lwt, + uint8_t clean_start, + uint16_t keep_alive) +{ + // Sanity Checks First (are given parameters correct and up to MQTT spec) + if (!auth->client_id) { + mws_error(log_ctx, "ClientID must be set. [MQTT-3.1.3-3]"); + return NULL; + } + + size_t len = strlen(auth->client_id); + if (!len) { + // [MQTT-3.1.3-6] server MAY allow empty client_id and treat it + // as specific client_id (not same as client_id not given) + // however server MUST allow ClientIDs between 1-23 bytes [MQTT-3.1.3-5] + // so we will warn client server might not like this and he is using it + // at his own risk! + mws_warn(log_ctx, "client_id provided is empty string. This might not be allowed by server [MQTT-3.1.3-6]"); + } + if(len > MQTT_MAX_CLIENT_ID) { + // [MQTT-3.1.3-5] server MUST allow client_id length 1-32 + // server MAY allow longer client_id, if user provides longer client_id + // warn them he is doing so at his own risk! + mws_warn(log_ctx, "client_id provided is longer than 23 bytes, server might not allow that [MQTT-3.1.3-5]"); + } + + if (lwt) { + if (lwt->will_message && lwt->will_message_size > 65535) { + mws_error(log_ctx, "Will message cannot be longer than 65535 bytes due to MQTT protocol limitations [MQTT-3.1.3-4] and [MQTT-1.5.6]"); + return NULL; + } + + if (!lwt->will_topic) { //TODO topic given with strlen==0 ? check specs + mws_error(log_ctx, "If will message is given will topic must also be given [MQTT-3.1.3.3]"); + return NULL; + } + + if (lwt->will_qos > MQTT_MAX_QOS) { + // refer to [MQTT-3-1.2-12] + mws_error(log_ctx, "QOS for LWT message is bigger than max"); + return NULL; + } + } + + // >> START THE RODEO << + transaction_buffer_transaction_start(trx_buf); + + // Calculate the resulting message size sans fixed MQTT header + size_t size = mqtt_ng_connect_size(auth, lwt); + + // Start generating the message + struct buffer_fragment *frag = NULL; + mqtt_msg_data ret = NULL; + + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback ); + ret = frag; + + // MQTT Fixed Header + size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + sizeof(mqtt_protocol_name_frag) + 1 /* CONNECT FLAGS */ + 2 /* keepalive */ + 1 /* Properties TODO now fixed 0*/; + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback); + + *WRITE_POS(frag) = MQTT_CPT_CONNECT << 4; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag); + + memcpy(WRITE_POS(frag), mqtt_protocol_name_frag, sizeof(mqtt_protocol_name_frag)); + DATA_ADVANCE(&trx_buf->hdr_buffer, sizeof(mqtt_protocol_name_frag), frag); + + // [MQTT-3.1.2.3] Connect flags + unsigned char *connect_flags = WRITE_POS(frag); + *connect_flags = 0; + if (auth->username) + *connect_flags |= MQTT_CONNECT_FLAG_USERNAME; + if (auth->password) + *connect_flags |= MQTT_CONNECT_FLAG_PASSWORD; + if (lwt) { + *connect_flags |= MQTT_CONNECT_FLAG_LWT; + *connect_flags |= lwt->will_qos << MQTT_CONNECT_FLAG_QOS_BITSHIFT; + if (lwt->will_retain) + *connect_flags |= MQTT_CONNECT_FLAG_LWT_RETAIN; + } + if (clean_start) + *connect_flags |= MQTT_CONNECT_FLAG_CLEAN_START; + + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + + PACK_2B_INT(&trx_buf->hdr_buffer, keep_alive, frag); + + // TODO Property Length [MQTT-3.1.3.2.1] temporary fixed to 3 (one property topic alias max) + DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(3, WRITE_POS(frag)), frag); + *WRITE_POS(frag) = MQTT_PROP_TOPIC_ALIAS_MAX; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + + PACK_2B_INT(&trx_buf->hdr_buffer, 65535, frag); + + // [MQTT-3.1.3.1] Client identifier + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback); + PACK_2B_INT(&trx_buf->hdr_buffer, strlen(auth->client_id), frag); + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, auth->client_id, strlen(auth->client_id), auth->client_id_free, &frag)) + goto fail_rollback; + + if (lwt != NULL) { + // Will Properties [MQTT-3.1.3.2] + // TODO for now fixed 0 + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 1, goto fail_rollback); + *WRITE_POS(frag) = 0; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + + // Will Topic [MQTT-3.1.3.3] + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback); + PACK_2B_INT(&trx_buf->hdr_buffer, strlen(lwt->will_topic), frag); + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, lwt->will_topic, strlen(lwt->will_topic), lwt->will_topic_free, &frag)) + goto fail_rollback; + + // Will Payload [MQTT-3.1.3.4] + if (lwt->will_message_size) { + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback); + PACK_2B_INT(&trx_buf->hdr_buffer, lwt->will_message_size, frag); + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, lwt->will_message, lwt->will_message_size, lwt->will_topic_free, &frag)) + goto fail_rollback; + } + } + + // [MQTT-3.1.3.5] + if (auth->username) { + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback); + PACK_2B_INT(&trx_buf->hdr_buffer, strlen(auth->username), frag); + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, auth->username, strlen(auth->username), auth->username_free, &frag)) + goto fail_rollback; + } + + // [MQTT-3.1.3.6] + if (auth->password) { + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, 2, goto fail_rollback); + PACK_2B_INT(&trx_buf->hdr_buffer, strlen(auth->password), frag); + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, auth->password, strlen(auth->password), auth->password_free, &frag)) + goto fail_rollback; + } + trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL; + transaction_buffer_transaction_commit(trx_buf); + return ret; +fail_rollback: + transaction_buffer_transaction_rollback(trx_buf, ret); + return NULL; +} + +int mqtt_ng_connect(struct mqtt_ng_client *client, + struct mqtt_auth_properties *auth, + struct mqtt_lwt_properties *lwt, + uint8_t clean_start, + uint16_t keep_alive) +{ + client->client_state = RAW; + client->parser.state = MQTT_PARSE_FIXED_HEADER_PACKET_TYPE; + + LOCK_HDR_BUFFER(&client->main_buffer); + client->main_buffer.sending_frag = NULL; + if (clean_start) + buffer_purge(&client->main_buffer.hdr_buffer); + UNLOCK_HDR_BUFFER(&client->main_buffer); + + pthread_rwlock_wrlock(&client->tx_topic_aliases.rwlock); + // according to MQTT spec topic aliases should not be persisted + // even if clean session is true + mqtt_ng_destroy_tx_alias_hash(client->tx_topic_aliases.stoi_dict); + client->tx_topic_aliases.stoi_dict = TX_ALIASES_INITIALIZE(); + if (client->tx_topic_aliases.stoi_dict == NULL) { + pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock); + return 1; + } + client->tx_topic_aliases.idx_assigned = 0; + pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock); + + mqtt_ng_destroy_rx_alias_hash(client->rx_aliases); + client->rx_aliases = RX_ALIASES_INITIALIZE(); + if (client->rx_aliases == NULL) + return 1; + + client->connect_msg = mqtt_ng_generate_connect(&client->main_buffer, client->log, auth, lwt, clean_start, keep_alive); + if (client->connect_msg == NULL) + return 1; + + pthread_mutex_lock(&client->stats_mutex); + if (clean_start) + client->stats.tx_messages_queued = 1; + else + client->stats.tx_messages_queued++; + + client->stats.tx_messages_sent = 0; + client->stats.rx_messages_rcvd = 0; + pthread_mutex_unlock(&client->stats_mutex); + + client->client_state = CONNECT_PENDING; + return 0; +} + +uint16_t get_unused_packet_id() { + static uint16_t packet_id = 0; + packet_id++; + return packet_id ? packet_id : ++packet_id; +} + +static inline size_t mqtt_ng_publish_size(const char *topic, + size_t msg_len, + uint16_t topic_id) +{ + size_t retval = 2 /* Topic Name Length */ + + (topic == NULL ? 0 : strlen(topic)) + + 2 /* Packet identifier */ + + 1 /* Properties Length TODO for now fixed to 1 property */ + + msg_len; + + if (topic_id) + retval += 3; + + return retval; +} + +int mqtt_ng_generate_publish(struct transaction_buffer *trx_buf, + mqtt_wss_log_ctx_t log_ctx, + char *topic, + free_fnc_t topic_free, + void *msg, + free_fnc_t msg_free, + size_t msg_len, + uint8_t publish_flags, + uint16_t *packet_id, + uint16_t topic_alias) +{ + // >> START THE RODEO << + transaction_buffer_transaction_start(trx_buf); + + // Calculate the resulting message size sans fixed MQTT header + size_t size = mqtt_ng_publish_size(topic, msg_len, topic_alias); + + // Start generating the message + struct buffer_fragment *frag = NULL; + mqtt_msg_data mqtt_msg = NULL; + + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback ); + // in case of QOS 0 we can garbage collect immediatelly after sending + uint8_t qos = (publish_flags >> 1) & 0x03; + if (!qos) + frag->flags |= BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND; + mqtt_msg = frag; + + // MQTT Fixed Header + size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + size - msg_len; + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback); + + *WRITE_POS(frag) = (MQTT_CPT_PUBLISH << 4) | (publish_flags & 0xF); + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag); + + // MQTT Variable Header + // [MQTT-3.3.2.1] + PACK_2B_INT(&trx_buf->hdr_buffer, topic == NULL ? 0 : strlen(topic), frag); + if (topic != NULL) { + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, topic, strlen(topic), topic_free, &frag)) + goto fail_rollback; + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + } + + // [MQTT-3.3.2.2] + mqtt_msg->packet_id = get_unused_packet_id(); + *packet_id = mqtt_msg->packet_id; + PACK_2B_INT(&trx_buf->hdr_buffer, mqtt_msg->packet_id, frag); + + // [MQTT-3.3.2.3.1] TODO Property Length for now fixed 0 + *WRITE_POS(frag) = topic_alias ? 3 : 0; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + + if(topic_alias) { + *WRITE_POS(frag) = MQTT_PROP_TOPIC_ALIAS; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + + PACK_2B_INT(&trx_buf->hdr_buffer, topic_alias, frag); + } + + if( (frag = buffer_new_frag(&trx_buf->hdr_buffer, BUFFER_FRAG_DATA_EXTERNAL)) == NULL ) + goto fail_rollback; + + if (frag_set_external_data(log_ctx, frag, msg, msg_len, msg_free)) + goto fail_rollback; + + trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL; + if (!qos) + trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND; + transaction_buffer_transaction_commit(trx_buf); + return MQTT_NG_MSGGEN_OK; +fail_rollback: + transaction_buffer_transaction_rollback(trx_buf, mqtt_msg); + return MQTT_NG_MSGGEN_BUFFER_OOM; +} + +#define PUBLISH_SP_SIZE 64 +int mqtt_ng_publish(struct mqtt_ng_client *client, + char *topic, + free_fnc_t topic_free, + void *msg, + free_fnc_t msg_free, + size_t msg_len, + uint8_t publish_flags, + uint16_t *packet_id) +{ + struct topic_alias_data *alias = NULL; + pthread_rwlock_rdlock(&client->tx_topic_aliases.rwlock); + c_rhash_get_ptr_by_str(client->tx_topic_aliases.stoi_dict, topic, (void**)&alias); + pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock); + + uint16_t topic_id = 0; + + if (alias != NULL) { + topic_id = alias->idx; + uint32_t cnt = __atomic_fetch_add(&alias->usage_count, 1, __ATOMIC_SEQ_CST); + if (cnt) { + topic = NULL; + topic_free = NULL; + } + } + + if (client->max_msg_size && PUBLISH_SP_SIZE + mqtt_ng_publish_size(topic, msg_len, topic_id) > client->max_msg_size) { + mws_error(client->log, "Message too big for server: %zu", msg_len); + return MQTT_NG_MSGGEN_MSG_TOO_BIG; + } + + TRY_GENERATE_MESSAGE(mqtt_ng_generate_publish, client, topic, topic_free, msg, msg_free, msg_len, publish_flags, packet_id, topic_id); +} + +static inline size_t mqtt_ng_subscribe_size(struct mqtt_sub *subs, size_t sub_count) +{ + size_t len = 2 /* Packet Identifier */ + 1 /* Properties Length TODO for now fixed 0 */; + len += sub_count * (2 /* topic filter string length */ + 1 /* [MQTT-3.8.3.1] Subscription Options Byte */); + + for (size_t i = 0; i < sub_count; i++) { + len += strlen(subs[i].topic); + } + return len; +} + +int mqtt_ng_generate_subscribe(struct transaction_buffer *trx_buf, mqtt_wss_log_ctx_t log_ctx, struct mqtt_sub *subs, size_t sub_count) +{ + // >> START THE RODEO << + transaction_buffer_transaction_start(trx_buf); + + // Calculate the resulting message size sans fixed MQTT header + size_t size = mqtt_ng_subscribe_size(subs, sub_count); + + // Start generating the message + struct buffer_fragment *frag = NULL; + mqtt_msg_data ret = NULL; + + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback); + ret = frag; + + // MQTT Fixed Header + size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + 3 /*Packet ID + Property Length*/; + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback); + + *WRITE_POS(frag) = (MQTT_CPT_SUBSCRIBE << 4) | 0x2 /* [MQTT-3.8.1-1] */; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag); + + // MQTT Variable Header + // [MQTT-3.8.2] PacketID + ret->packet_id = get_unused_packet_id(); + PACK_2B_INT(&trx_buf->hdr_buffer, ret->packet_id, frag); + + // [MQTT-3.8.2.1.1] Property Length // TODO for now fixed 0 + *WRITE_POS(frag) = 0; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + + for (size_t i = 0; i < sub_count; i++) { + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + PACK_2B_INT(&trx_buf->hdr_buffer, strlen(subs[i].topic), frag); + if (_optimized_add(&trx_buf->hdr_buffer, log_ctx, subs[i].topic, strlen(subs[i].topic), subs[i].topic_free, &frag)) + goto fail_rollback; + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, 0, frag, goto fail_rollback); + *WRITE_POS(frag) = subs[i].options; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + } + + trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL; + transaction_buffer_transaction_commit(trx_buf); + return MQTT_NG_MSGGEN_OK; +fail_rollback: + transaction_buffer_transaction_rollback(trx_buf, ret); + return MQTT_NG_MSGGEN_BUFFER_OOM; +} + +int mqtt_ng_subscribe(struct mqtt_ng_client *client, struct mqtt_sub *subs, size_t sub_count) +{ + TRY_GENERATE_MESSAGE(mqtt_ng_generate_subscribe, client, subs, sub_count); +} + +int mqtt_ng_generate_disconnect(struct transaction_buffer *trx_buf, mqtt_wss_log_ctx_t log_ctx, uint8_t reason_code) +{ + (void) log_ctx; + // >> START THE RODEO << + transaction_buffer_transaction_start(trx_buf); + + // Calculate the resulting message size sans fixed MQTT header + size_t size = reason_code ? 1 : 0; + + // Start generating the message + struct buffer_fragment *frag = NULL; + mqtt_msg_data ret = NULL; + + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD, frag, goto fail_rollback); + ret = frag; + + // MQTT Fixed Header + size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + (reason_code ? 1 : 0); + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback); + + *WRITE_POS(frag) = MQTT_CPT_DISCONNECT << 4; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag); + + if (reason_code) { + // MQTT Variable Header + // [MQTT-3.14.2.1] PacketID + *WRITE_POS(frag) = reason_code; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + } + + trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL; + transaction_buffer_transaction_commit(trx_buf); + return MQTT_NG_MSGGEN_OK; +fail_rollback: + transaction_buffer_transaction_rollback(trx_buf, ret); + return MQTT_NG_MSGGEN_BUFFER_OOM; +} + +int mqtt_ng_disconnect(struct mqtt_ng_client *client, uint8_t reason_code) +{ + TRY_GENERATE_MESSAGE(mqtt_ng_generate_disconnect, client, reason_code); +} + +static int mqtt_generate_puback(struct transaction_buffer *trx_buf, mqtt_wss_log_ctx_t log_ctx, uint16_t packet_id, uint8_t reason_code) +{ + (void) log_ctx; + // >> START THE RODEO << + transaction_buffer_transaction_start(trx_buf); + + // Calculate the resulting message size sans fixed MQTT header + size_t size = 2 /* Packet ID */ + (reason_code ? 1 : 0) /* reason code */; + + // Start generating the message + struct buffer_fragment *frag = NULL; + + BUFFER_TRANSACTION_NEW_FRAG(&trx_buf->hdr_buffer, BUFFER_FRAG_MQTT_PACKET_HEAD | BUFFER_FRAG_GARBAGE_COLLECT_ON_SEND, frag, goto fail_rollback); + + // MQTT Fixed Header + size_t needed_bytes = 1 /* Packet type */ + MQTT_VARSIZE_INT_BYTES(size) + size; + CHECK_BYTES_AVAILABLE(&trx_buf->hdr_buffer, needed_bytes, goto fail_rollback); + + *WRITE_POS(frag) = MQTT_CPT_PUBACK << 4; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + DATA_ADVANCE(&trx_buf->hdr_buffer, uint32_to_mqtt_vbi(size, WRITE_POS(frag)), frag); + + // MQTT Variable Header + PACK_2B_INT(&trx_buf->hdr_buffer, packet_id, frag); + + if (reason_code) { + // MQTT Variable Header + // [MQTT-3.14.2.1] PacketID + *WRITE_POS(frag) = reason_code; + DATA_ADVANCE(&trx_buf->hdr_buffer, 1, frag); + } + + trx_buf->hdr_buffer.tail_frag->flags |= BUFFER_FRAG_MQTT_PACKET_TAIL; + transaction_buffer_transaction_commit(trx_buf); + return MQTT_NG_MSGGEN_OK; +fail_rollback: + transaction_buffer_transaction_rollback(trx_buf, frag); + return MQTT_NG_MSGGEN_BUFFER_OOM; +} + +static int mqtt_ng_puback(struct mqtt_ng_client *client, uint16_t packet_id, uint8_t reason_code) +{ + TRY_GENERATE_MESSAGE(mqtt_generate_puback, client, packet_id, reason_code); +} + +int mqtt_ng_ping(struct mqtt_ng_client *client) +{ + client->ping_pending = 1; + return MQTT_NG_MSGGEN_OK; +} + +#define MQTT_NG_CLIENT_NEED_MORE_BYTES 0x10 +#define MQTT_NG_CLIENT_MQTT_PACKET_DONE 0x11 +#define MQTT_NG_CLIENT_PARSE_DONE 0x12 +#define MQTT_NG_CLIENT_WANT_WRITE 0x13 +#define MQTT_NG_CLIENT_OK_CALL_AGAIN 0 +#define MQTT_NG_CLIENT_PROTOCOL_ERROR -1 +#define MQTT_NG_CLIENT_SERVER_RETURNED_ERROR -2 +#define MQTT_NG_CLIENT_NOT_IMPL_YET -3 +#define MQTT_NG_CLIENT_OOM -4 +#define MQTT_NG_CLIENT_INTERNAL_ERROR -5 + +#define BUF_READ_CHECK_AT_LEAST(buf, x) \ + if (rbuf_bytes_available(buf) < (x)) \ + return MQTT_NG_CLIENT_NEED_MORE_BYTES; + +#define vbi_parser_reset_ctx(ctx) memset(ctx, 0, sizeof(struct mqtt_vbi_parser_ctx)) + +static int vbi_parser_parse(struct mqtt_vbi_parser_ctx *ctx, rbuf_t data, mqtt_wss_log_ctx_t log) +{ + if (ctx->bytes > MQTT_VBI_MAXBYTES - 1) { + mws_error(log, "MQTT Variable Byte Integer can't be longer than %d bytes", MQTT_VBI_MAXBYTES); + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + if (!ctx->bytes || ctx->data[ctx->bytes-1] & MQTT_VBI_CONTINUATION_FLAG) { + BUF_READ_CHECK_AT_LEAST(data, 1); + ctx->bytes++; + rbuf_pop(data, &ctx->data[ctx->bytes-1], 1); + if ( ctx->data[ctx->bytes-1] & MQTT_VBI_CONTINUATION_FLAG ) + return MQTT_NG_CLIENT_OK_CALL_AGAIN; + } + + if (mqtt_vbi_to_uint32(ctx->data, &ctx->result)) { + mws_error(log, "MQTT Variable Byte Integer failed to be parsed."); + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + + return MQTT_NG_CLIENT_PARSE_DONE; +} + +static void mqtt_properties_parser_ctx_reset(struct mqtt_properties_parser_ctx *ctx) +{ + ctx->state = PROPERTIES_LENGTH; + while (ctx->head) { + struct mqtt_property *f = ctx->head; + ctx->head = ctx->head->next; + if (f->type == MQTT_TYPE_STR || f->type == MQTT_TYPE_STR_PAIR) + mw_free(f->data.strings[0]); + if (f->type == MQTT_TYPE_STR_PAIR) + mw_free(f->data.strings[1]); + if (f->type == MQTT_TYPE_BIN) + mw_free(f->data.bindata); + mw_free(f); + } + ctx->tail = NULL; + ctx->properties_length = 0; + ctx->bytes_consumed = 0; + vbi_parser_reset_ctx(&ctx->vbi_parser_ctx); +} + +struct mqtt_property_type { + uint8_t id; + enum mqtt_datatype datatype; + const char* name; +}; + +const struct mqtt_property_type mqtt_property_types[] = { + { .id = MQTT_PROP_TOPIC_ALIAS, .name = MQTT_PROP_TOPIC_ALIAS_NAME, .datatype = MQTT_TYPE_UINT_16 }, + + { .id = MQTT_PROP_PAYLOAD_FMT_INDICATOR, .name = MQTT_PROP_PAYLOAD_FMT_INDICATOR_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_MSG_EXPIRY_INTERVAL, .name = MQTT_PROP_MSG_EXPIRY_INTERVAL_NAME, .datatype = MQTT_TYPE_UINT_32 }, + { .id = MQTT_PROP_CONTENT_TYPE, .name = MQTT_PROP_CONTENT_TYPE_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_RESPONSE_TOPIC, .name = MQTT_PROP_RESPONSE_TOPIC_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_CORRELATION_DATA, .name = MQTT_PROP_CORRELATION_DATA_NAME, .datatype = MQTT_TYPE_BIN }, + { .id = MQTT_PROP_SUB_IDENTIFIER, .name = MQTT_PROP_SUB_IDENTIFIER_NAME, .datatype = MQTT_TYPE_VBI }, + { .id = MQTT_PROP_SESSION_EXPIRY_INTERVAL, .name = MQTT_PROP_SESSION_EXPIRY_INTERVAL_NAME, .datatype = MQTT_TYPE_UINT_32 }, + { .id = MQTT_PROP_ASSIGNED_CLIENT_ID, .name = MQTT_PROP_ASSIGNED_CLIENT_ID_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_SERVER_KEEP_ALIVE, .name = MQTT_PROP_SERVER_KEEP_ALIVE_NAME, .datatype = MQTT_TYPE_UINT_16 }, + { .id = MQTT_PROP_AUTH_METHOD, .name = MQTT_PROP_AUTH_METHOD_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_AUTH_DATA, .name = MQTT_PROP_AUTH_DATA_NAME, .datatype = MQTT_TYPE_BIN }, + { .id = MQTT_PROP_REQ_PROBLEM_INFO, .name = MQTT_PROP_REQ_PROBLEM_INFO_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_WILL_DELAY_INTERVAL, .name = MQTT_PROP_WIIL_DELAY_INTERVAL_NAME, .datatype = MQTT_TYPE_UINT_32 }, + { .id = MQTT_PROP_REQ_RESP_INFORMATION, .name = MQTT_PROP_REQ_RESP_INFORMATION_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_RESP_INFORMATION, .name = MQTT_PROP_RESP_INFORMATION_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_SERVER_REF, .name = MQTT_PROP_SERVER_REF_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_REASON_STR, .name = MQTT_PROP_REASON_STR_NAME, .datatype = MQTT_TYPE_STR }, + { .id = MQTT_PROP_RECEIVE_MAX, .name = MQTT_PROP_RECEIVE_MAX_NAME, .datatype = MQTT_TYPE_UINT_16 }, + { .id = MQTT_PROP_TOPIC_ALIAS_MAX, .name = MQTT_PROP_TOPIC_ALIAS_MAX_NAME, .datatype = MQTT_TYPE_UINT_16 }, + // MQTT_PROP_TOPIC_ALIAS is first as it is most often used + { .id = MQTT_PROP_MAX_QOS, .name = MQTT_PROP_MAX_QOS_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_RETAIN_AVAIL, .name = MQTT_PROP_RETAIN_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_USR, .name = MQTT_PROP_USR_NAME, .datatype = MQTT_TYPE_STR_PAIR }, + { .id = MQTT_PROP_MAX_PKT_SIZE, .name = MQTT_PROP_MAX_PKT_SIZE_NAME, .datatype = MQTT_TYPE_UINT_32 }, + { .id = MQTT_PROP_WILDCARD_SUB_AVAIL, .name = MQTT_PROP_WILDCARD_SUB_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_SUB_ID_AVAIL, .name = MQTT_PROP_SUB_ID_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = MQTT_PROP_SHARED_SUB_AVAIL, .name = MQTT_PROP_SHARED_SUB_AVAIL_NAME, .datatype = MQTT_TYPE_UINT_8 }, + { .id = 0, .name = NULL, .datatype = MQTT_TYPE_UNKNOWN } +}; + +static int get_property_type_by_id(uint8_t property_id) { + for (int i = 0; mqtt_property_types[i].datatype != MQTT_TYPE_UNKNOWN; i++) { + if (mqtt_property_types[i].id == property_id) + return mqtt_property_types[i].datatype; + } + return MQTT_TYPE_UNKNOWN; +} + +struct mqtt_property *get_property_by_id(struct mqtt_property *props, uint8_t property_id) +{ + while (props) { + if (props->id == property_id) { + return props; + } + props = props->next; + } + return NULL; +} + +// Parses [MQTT-2.2.2] +static int parse_properties_array(struct mqtt_properties_parser_ctx *ctx, rbuf_t data, mqtt_wss_log_ctx_t log) +{ + int rc; + switch (ctx->state) { + case PROPERTIES_LENGTH: + rc = vbi_parser_parse(&ctx->vbi_parser_ctx, data, log); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + ctx->properties_length = ctx->vbi_parser_ctx.result; + ctx->bytes_consumed += ctx->vbi_parser_ctx.bytes; + ctx->vbi_length = ctx->vbi_parser_ctx.bytes; + if (!ctx->properties_length) + return MQTT_NG_CLIENT_PARSE_DONE; + ctx->state = PROPERTY_CREATE; + break; + } + return rc; + case PROPERTY_CREATE: + BUF_READ_CHECK_AT_LEAST(data, 1); + struct mqtt_property *prop = mw_calloc(1, sizeof(struct mqtt_property)); + if (ctx->head == NULL) { + ctx->head = prop; + ctx->tail = prop; + } else { + ctx->tail->next = prop; + ctx->tail = ctx->tail->next; + } + ctx->state = PROPERTY_ID; + /* FALLTHROUGH */ + case PROPERTY_ID: + rbuf_pop(data, (char*)&ctx->tail->id, 1); + ctx->bytes_consumed += 1; + ctx->tail->type = get_property_type_by_id(ctx->tail->id); + switch (ctx->tail->type) { + case MQTT_TYPE_UINT_16: + ctx->state = PROPERTY_TYPE_UINT16; + break; + case MQTT_TYPE_UINT_32: + ctx->state = PROPERTY_TYPE_UINT32; + break; + case MQTT_TYPE_UINT_8: + ctx->state = PROPERTY_TYPE_UINT8; + break; + case MQTT_TYPE_VBI: + ctx->state = PROPERTY_TYPE_VBI; + vbi_parser_reset_ctx(&ctx->vbi_parser_ctx); + break; + case MQTT_TYPE_STR: + case MQTT_TYPE_STR_PAIR: + ctx->str_idx = 0; + /* FALLTHROUGH */ + case MQTT_TYPE_BIN: + ctx->state = PROPERTY_TYPE_STR_BIN_LEN; + break; + default: + mws_error(log, "Unsupported property type %d for property id %d.", (int)ctx->tail->type, (int)ctx->tail->id); + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + break; + case PROPERTY_TYPE_STR_BIN_LEN: + BUF_READ_CHECK_AT_LEAST(data, sizeof(uint16_t)); + rbuf_pop(data, (char*)&ctx->tail->bindata_len, sizeof(uint16_t)); + ctx->tail->bindata_len = be16toh(ctx->tail->bindata_len); + ctx->bytes_consumed += 2; + switch (ctx->tail->type) { + case MQTT_TYPE_BIN: + ctx->state = PROPERTY_TYPE_BIN; + break; + case MQTT_TYPE_STR: + case MQTT_TYPE_STR_PAIR: + ctx->state = PROPERTY_TYPE_STR; + break; + default: + mws_error(log, "Unexpected datatype in PROPERTY_TYPE_STR_BIN_LEN %d", (int)ctx->tail->type); + return MQTT_NG_CLIENT_INTERNAL_ERROR; + } + break; + case PROPERTY_TYPE_STR: + BUF_READ_CHECK_AT_LEAST(data, ctx->tail->bindata_len); + ctx->tail->data.strings[ctx->str_idx] = mw_malloc(ctx->tail->bindata_len + 1); + rbuf_pop(data, ctx->tail->data.strings[ctx->str_idx], ctx->tail->bindata_len); + ctx->tail->data.strings[ctx->str_idx][ctx->tail->bindata_len] = 0; + ctx->str_idx++; + ctx->bytes_consumed += ctx->tail->bindata_len; + if (ctx->tail->type == MQTT_TYPE_STR_PAIR && ctx->str_idx < 2) { + ctx->state = PROPERTY_TYPE_STR_BIN_LEN; + break; + } + ctx->state = PROPERTY_NEXT; + break; + case PROPERTY_TYPE_BIN: + BUF_READ_CHECK_AT_LEAST(data, ctx->tail->bindata_len); + ctx->tail->data.bindata = mw_malloc(ctx->tail->bindata_len); + rbuf_pop(data, ctx->tail->data.bindata, ctx->tail->bindata_len); + ctx->bytes_consumed += ctx->tail->bindata_len; + ctx->state = PROPERTY_NEXT; + break; + case PROPERTY_TYPE_VBI: + rc = vbi_parser_parse(&ctx->vbi_parser_ctx, data, log); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + ctx->tail->data.uint32 = ctx->vbi_parser_ctx.result; + ctx->bytes_consumed += ctx->vbi_parser_ctx.bytes; + ctx->state = PROPERTY_NEXT; + break; + } + return rc; + case PROPERTY_TYPE_UINT8: + BUF_READ_CHECK_AT_LEAST(data, sizeof(uint8_t)); + rbuf_pop(data, (char*)&ctx->tail->data.uint8, sizeof(uint8_t)); + ctx->bytes_consumed += sizeof(uint8_t); + ctx->state = PROPERTY_NEXT; + break; + case PROPERTY_TYPE_UINT32: + BUF_READ_CHECK_AT_LEAST(data, sizeof(uint32_t)); + rbuf_pop(data, (char*)&ctx->tail->data.uint32, sizeof(uint32_t)); + ctx->tail->data.uint32 = be32toh(ctx->tail->data.uint32); + ctx->bytes_consumed += sizeof(uint32_t); + ctx->state = PROPERTY_NEXT; + break; + case PROPERTY_TYPE_UINT16: + BUF_READ_CHECK_AT_LEAST(data, sizeof(uint16_t)); + rbuf_pop(data, (char*)&ctx->tail->data.uint16, sizeof(uint16_t)); + ctx->tail->data.uint16 = be16toh(ctx->tail->data.uint16); + ctx->bytes_consumed += sizeof(uint16_t); + ctx->state = PROPERTY_NEXT; + /* FALLTHROUGH */ + case PROPERTY_NEXT: + if (ctx->properties_length > ctx->bytes_consumed - ctx->vbi_length) { + ctx->state = PROPERTY_CREATE; + break; + } else + return MQTT_NG_CLIENT_PARSE_DONE; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +static int parse_connack_varhdr(struct mqtt_ng_client *client) +{ + struct mqtt_ng_parser *parser = &client->parser; + switch (parser->varhdr_state) { + case MQTT_PARSE_VARHDR_INITIAL: + BUF_READ_CHECK_AT_LEAST(parser->received_data, 2); + rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.connack.flags, 1); + rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.connack.reason_code, 1); + parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS; + mqtt_properties_parser_ctx_reset(&parser->properties_parser); + break; + case MQTT_PARSE_VARHDR_PROPS: + return parse_properties_array(&parser->properties_parser, parser->received_data, client->log); + default: + ERROR("invalid state for connack varhdr parser"); + return MQTT_NG_CLIENT_INTERNAL_ERROR; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +static int parse_disconnect_varhdr(struct mqtt_ng_client *client) +{ + struct mqtt_ng_parser *parser = &client->parser; + switch (parser->varhdr_state) { + case MQTT_PARSE_VARHDR_INITIAL: + if (!parser->mqtt_fixed_hdr_remaining_length) { + // [MQTT-3.14.2.1] if reason code omitted act same as == 0 + parser->mqtt_packet.disconnect.reason_code = 0; + return MQTT_NG_CLIENT_PARSE_DONE; + } + BUF_READ_CHECK_AT_LEAST(parser->received_data, 1); + rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.connack.reason_code, 1); + if (parser->mqtt_fixed_hdr_remaining_length == 1) + return MQTT_NG_CLIENT_PARSE_DONE; + parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS; + mqtt_properties_parser_ctx_reset(&parser->properties_parser); + break; + case MQTT_PARSE_VARHDR_PROPS: + return parse_properties_array(&parser->properties_parser, parser->received_data, client->log); + default: + ERROR("invalid state for connack varhdr parser"); + return MQTT_NG_CLIENT_INTERNAL_ERROR; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +static int parse_puback_varhdr(struct mqtt_ng_client *client) +{ + struct mqtt_ng_parser *parser = &client->parser; + switch (parser->varhdr_state) { + case MQTT_PARSE_VARHDR_INITIAL: + BUF_READ_CHECK_AT_LEAST(parser->received_data, 2); + rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.puback.packet_id, 2); + parser->mqtt_packet.puback.packet_id = be16toh(parser->mqtt_packet.puback.packet_id); + if (parser->mqtt_fixed_hdr_remaining_length < 3) { + // [MQTT-3.4.2.1] if length is not big enough for reason code + // it is omitted and handled same as if it was present and == 0 + // initially missed this detail and was wondering WTF is going on (sigh) + parser->mqtt_packet.puback.reason_code = 0; + return MQTT_NG_CLIENT_PARSE_DONE; + } + parser->varhdr_state = MQTT_PARSE_VARHDR_OPTIONAL_REASON_CODE; + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_OPTIONAL_REASON_CODE: + BUF_READ_CHECK_AT_LEAST(parser->received_data, 1); + rbuf_pop(parser->received_data, (char*)&parser->mqtt_packet.puback.reason_code, 1); + // LOL so in CONNACK you have to have 0 byte to + // signify empty properties list + // but in PUBACK it can be omitted if remaining length doesn't allow it (sigh) + if (parser->mqtt_fixed_hdr_remaining_length < 4) + return MQTT_NG_CLIENT_PARSE_DONE; + + parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS; + mqtt_properties_parser_ctx_reset(&parser->properties_parser); + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_PROPS: + return parse_properties_array(&parser->properties_parser, parser->received_data, client->log); + default: + ERROR("invalid state for puback varhdr parser"); + return MQTT_NG_CLIENT_INTERNAL_ERROR; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +static int parse_suback_varhdr(struct mqtt_ng_client *client) +{ + int rc; + size_t avail; + struct mqtt_ng_parser *parser = &client->parser; + struct mqtt_suback *suback = &client->parser.mqtt_packet.suback; + switch (parser->varhdr_state) { + case MQTT_PARSE_VARHDR_INITIAL: + suback->reason_codes = NULL; + BUF_READ_CHECK_AT_LEAST(parser->received_data, 2); + rbuf_pop(parser->received_data, (char*)&suback->packet_id, 2); + suback->packet_id = be16toh(suback->packet_id); + parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS; + parser->mqtt_parsed_len = 2; + mqtt_properties_parser_ctx_reset(&parser->properties_parser); + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_PROPS: + rc = parse_properties_array(&parser->properties_parser, parser->received_data, client->log); + if (rc != MQTT_NG_CLIENT_PARSE_DONE) + return rc; + parser->mqtt_parsed_len += parser->properties_parser.bytes_consumed; + suback->reason_code_count = parser->mqtt_fixed_hdr_remaining_length - parser->mqtt_parsed_len; + suback->reason_codes = mw_calloc(suback->reason_code_count, sizeof(*suback->reason_codes)); + suback->reason_codes_pending = suback->reason_code_count; + parser->varhdr_state = MQTT_PARSE_REASONCODES; + /* FALLTHROUGH */ + case MQTT_PARSE_REASONCODES: + avail = rbuf_bytes_available(parser->received_data); + if (avail < 1) + return MQTT_NG_CLIENT_NEED_MORE_BYTES; + + suback->reason_codes_pending -= rbuf_pop(parser->received_data, (char*)suback->reason_codes, MIN(suback->reason_codes_pending, avail)); + + if (!suback->reason_codes_pending) + return MQTT_NG_CLIENT_PARSE_DONE; + + return MQTT_NG_CLIENT_NEED_MORE_BYTES; + default: + ERROR("invalid state for suback varhdr parser"); + return MQTT_NG_CLIENT_INTERNAL_ERROR; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +static int parse_publish_varhdr(struct mqtt_ng_client *client) +{ + int rc; + struct mqtt_ng_parser *parser = &client->parser; + struct mqtt_publish *publish = &client->parser.mqtt_packet.publish; + switch (parser->varhdr_state) { + case MQTT_PARSE_VARHDR_INITIAL: + BUF_READ_CHECK_AT_LEAST(parser->received_data, 2); + publish->topic = NULL; + publish->qos = ((parser->mqtt_control_packet_type >> 1) & 0x03); + rbuf_pop(parser->received_data, (char*)&publish->topic_len, 2); + publish->topic_len = be16toh(publish->topic_len); + parser->mqtt_parsed_len = 2; + if (!publish->topic_len) { + parser->varhdr_state = MQTT_PARSE_VARHDR_POST_TOPICNAME; + break; + } + publish->topic = mw_calloc(1, publish->topic_len + 1 /* add 0x00 */); + if (publish->topic == NULL) + return MQTT_NG_CLIENT_OOM; + parser->varhdr_state = MQTT_PARSE_VARHDR_TOPICNAME; + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_TOPICNAME: + // TODO check empty topic can be valid? In which case we have to skip this step + BUF_READ_CHECK_AT_LEAST(parser->received_data, publish->topic_len); + rbuf_pop(parser->received_data, publish->topic, publish->topic_len); + parser->mqtt_parsed_len += publish->topic_len; + parser->varhdr_state = MQTT_PARSE_VARHDR_POST_TOPICNAME; + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_POST_TOPICNAME: + mqtt_properties_parser_ctx_reset(&parser->properties_parser); + if (!publish->qos) { // PacketID present only for QOS > 0 [MQTT-3.3.2.2] + parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS; + break; + } + parser->varhdr_state = MQTT_PARSE_VARHDR_PACKET_ID; + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_PACKET_ID: + BUF_READ_CHECK_AT_LEAST(parser->received_data, 2); + rbuf_pop(parser->received_data, (char*)&publish->packet_id, 2); + publish->packet_id = be16toh(publish->packet_id); + parser->varhdr_state = MQTT_PARSE_VARHDR_PROPS; + parser->mqtt_parsed_len += 2; + /* FALLTHROUGH */ + case MQTT_PARSE_VARHDR_PROPS: + rc = parse_properties_array(&parser->properties_parser, parser->received_data, client->log); + if (rc != MQTT_NG_CLIENT_PARSE_DONE) + return rc; + parser->mqtt_parsed_len += parser->properties_parser.bytes_consumed; + parser->varhdr_state = MQTT_PARSE_PAYLOAD; + /* FALLTHROUGH */ + case MQTT_PARSE_PAYLOAD: + if (parser->mqtt_fixed_hdr_remaining_length < parser->mqtt_parsed_len) { + mw_free(publish->topic); + publish->topic = NULL; + ERROR("Error parsing PUBLISH message"); + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + publish->data_len = parser->mqtt_fixed_hdr_remaining_length - parser->mqtt_parsed_len; + if (!publish->data_len) { + publish->data = NULL; + return MQTT_NG_CLIENT_PARSE_DONE; // 0 length payload is OK [MQTT-3.3.3] + } + BUF_READ_CHECK_AT_LEAST(parser->received_data, publish->data_len); + + publish->data = mw_malloc(publish->data_len); + if (publish->data == NULL) { + mw_free(publish->topic); + publish->topic = NULL; + return MQTT_NG_CLIENT_OOM; + } + + rbuf_pop(parser->received_data, publish->data, publish->data_len); + parser->mqtt_parsed_len += publish->data_len; + + return MQTT_NG_CLIENT_PARSE_DONE; + default: + ERROR("invalid state for publish varhdr parser"); + return MQTT_NG_CLIENT_INTERNAL_ERROR; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +// TODO move to separate file, dont send whole client pointer just to be able +// to access LOG context send parser only which should include log +static int parse_data(struct mqtt_ng_client *client) +{ + int rc; + struct mqtt_ng_parser *parser = &client->parser; + switch(parser->state) { + case MQTT_PARSE_FIXED_HEADER_PACKET_TYPE: + BUF_READ_CHECK_AT_LEAST(parser->received_data, 1); + rbuf_pop(parser->received_data, (char*)&parser->mqtt_control_packet_type, 1); + vbi_parser_reset_ctx(&parser->vbi_parser); + parser->state = MQTT_PARSE_FIXED_HEADER_LEN; + break; + case MQTT_PARSE_FIXED_HEADER_LEN: + rc = vbi_parser_parse(&parser->vbi_parser, parser->received_data, client->log); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + parser->mqtt_fixed_hdr_remaining_length = parser->vbi_parser.result; + parser->state = MQTT_PARSE_VARIABLE_HEADER; + parser->varhdr_state = MQTT_PARSE_VARHDR_INITIAL; + break; + } + return rc; + case MQTT_PARSE_VARIABLE_HEADER: + switch (get_control_packet_type(parser->mqtt_control_packet_type)) { + case MQTT_CPT_CONNACK: + rc = parse_connack_varhdr(client); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + break; + } + return rc; + case MQTT_CPT_PUBACK: + rc = parse_puback_varhdr(client); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + break; + } + return rc; + case MQTT_CPT_SUBACK: + rc = parse_suback_varhdr(client); + if (rc != MQTT_NG_CLIENT_NEED_MORE_BYTES && rc != MQTT_NG_CLIENT_OK_CALL_AGAIN) { + mw_free(parser->mqtt_packet.suback.reason_codes); + } + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + break; + } + return rc; + case MQTT_CPT_PUBLISH: + rc = parse_publish_varhdr(client); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + break; + } + return rc; + case MQTT_CPT_PINGRESP: + if (parser->mqtt_fixed_hdr_remaining_length) { + ERROR ("PINGRESP has to be 0 Remaining Length."); // [MQTT-3.13.1] + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + break; + case MQTT_CPT_DISCONNECT: + rc = parse_disconnect_varhdr(client); + if (rc == MQTT_NG_CLIENT_PARSE_DONE) { + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + break; + } + return rc; + default: + ERROR("Parsing Control Packet Type %" PRIu8 " not implemented yet.", get_control_packet_type(parser->mqtt_control_packet_type)); + rbuf_bump_tail(parser->received_data, parser->mqtt_fixed_hdr_remaining_length); + parser->state = MQTT_PARSE_MQTT_PACKET_DONE; + return MQTT_NG_CLIENT_NOT_IMPL_YET; + } + // we could also return MQTT_NG_CLIENT_OK_CALL_AGAIN + // and be called again later + /* FALLTHROUGH */ + case MQTT_PARSE_MQTT_PACKET_DONE: + parser->state = MQTT_PARSE_FIXED_HEADER_PACKET_TYPE; + return MQTT_NG_CLIENT_MQTT_PACKET_DONE; + } + return MQTT_NG_CLIENT_OK_CALL_AGAIN; +} + +// set next MQTT fragment to send +// return 1 if nothing to send +// return -1 on error +// return 0 if there is fragment set +static int mqtt_ng_next_to_send(struct mqtt_ng_client *client) { + if (client->client_state == CONNECT_PENDING) { + client->main_buffer.sending_frag = client->connect_msg; + client->client_state = CONNECTING; + return 0; + } + if (client->client_state != CONNECTED) + return -1; + + struct buffer_fragment *frag = BUFFER_FIRST_FRAG(&client->main_buffer.hdr_buffer); + while (frag) { + if ( frag->sent != frag->len ) + break; + frag = frag->next; + } + + if ( client->ping_pending && (!frag || (frag->flags & BUFFER_FRAG_MQTT_PACKET_HEAD && frag->sent == 0)) ) { + client->ping_pending = 0; + ping_frag.sent = 0; + client->main_buffer.sending_frag = &ping_frag; + return 0; + } + + client->main_buffer.sending_frag = frag; + return frag == NULL ? 1 : 0; +} + +// send current fragment +// return 0 if whole remaining length could be sent as a whole +// return -1 if send buffer was filled and +// nothing could be written anymore +// return 1 if last fragment of a message was fully sent +static int send_fragment(struct mqtt_ng_client *client) { + struct buffer_fragment *frag = client->main_buffer.sending_frag; + + // for readability + unsigned char *ptr = frag->data + frag->sent; + size_t bytes = frag->len - frag->sent; + + size_t processed = 0; + + if (bytes) + processed = client->send_fnc_ptr(client->user_ctx, ptr, bytes); + else + WARN("This fragment was fully sent already. This should not happen!"); + + frag->sent += processed; + if (frag->sent != frag->len) + return -1; + + if (frag->flags & BUFFER_FRAG_MQTT_PACKET_TAIL) { + client->time_of_last_send = time(NULL); + pthread_mutex_lock(&client->stats_mutex); + if (client->main_buffer.sending_frag != &ping_frag) + client->stats.tx_messages_queued--; + client->stats.tx_messages_sent++; + pthread_mutex_unlock(&client->stats_mutex); + client->main_buffer.sending_frag = NULL; + return 1; + } + + client->main_buffer.sending_frag = frag->next; + + return 0; +} + +// attempt sending all fragments of current single MQTT packet +static int send_all_message_fragments(struct mqtt_ng_client *client) { + int rc; + while ( !(rc = send_fragment(client)) ); + return rc; +} + +static void try_send_all(struct mqtt_ng_client *client) { + do { + if (client->main_buffer.sending_frag == NULL && mqtt_ng_next_to_send(client)) + return; + } while(send_all_message_fragments(client) >= 0); +} + +static inline void mark_message_for_gc(struct buffer_fragment *frag) +{ + while (frag) { + frag->flags |= BUFFER_FRAG_GARBAGE_COLLECT; + buffer_frag_free_data(frag); + if (frag->flags & BUFFER_FRAG_MQTT_PACKET_TAIL) + return; + frag = frag->next; + } +} + +static int mark_packet_acked(struct mqtt_ng_client *client, uint16_t packet_id) +{ + LOCK_HDR_BUFFER(&client->main_buffer); + struct buffer_fragment *frag = BUFFER_FIRST_FRAG(&client->main_buffer.hdr_buffer); + while (frag) { + if ( (frag->flags & BUFFER_FRAG_MQTT_PACKET_HEAD) && frag->packet_id == packet_id) { + if (!frag->sent) { + ERROR("Received packet_id (%" PRIu16 ") belongs to MQTT packet which was not yet sent!", packet_id); + UNLOCK_HDR_BUFFER(&client->main_buffer); + return 1; + } + mark_message_for_gc(frag); + UNLOCK_HDR_BUFFER(&client->main_buffer); + return 0; + } + frag = frag->next; + } + ERROR("Received packet_id (%" PRIu16 ") is unknown!", packet_id); + UNLOCK_HDR_BUFFER(&client->main_buffer); + return 1; +} + +int handle_incoming_traffic(struct mqtt_ng_client *client) +{ + int rc; + struct mqtt_publish *pub; + while( (rc = parse_data(client)) == MQTT_NG_CLIENT_OK_CALL_AGAIN ); + if ( rc == MQTT_NG_CLIENT_MQTT_PACKET_DONE ) { + struct mqtt_property *prop; +#ifdef MQTT_DEBUG_VERBOSE + DEBUG("MQTT Packet Parsed Successfully!"); +#endif + pthread_mutex_lock(&client->stats_mutex); + client->stats.rx_messages_rcvd++; + pthread_mutex_unlock(&client->stats_mutex); + + switch (get_control_packet_type(client->parser.mqtt_control_packet_type)) { + case MQTT_CPT_CONNACK: +#ifdef MQTT_DEBUG_VERBOSE + DEBUG("Received CONNACK"); +#endif + LOCK_HDR_BUFFER(&client->main_buffer); + mark_message_for_gc(client->connect_msg); + UNLOCK_HDR_BUFFER(&client->main_buffer); + client->connect_msg = NULL; + if (client->client_state != CONNECTING) { + ERROR("Received unexpected CONNACK"); + client->client_state = ERROR; + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + if ((prop = get_property_by_id(client->parser.properties_parser.head, MQTT_PROP_MAX_PKT_SIZE)) != NULL) { + INFO("MQTT server limits message size to %" PRIu32, prop->data.uint32); + client->max_msg_size = prop->data.uint32; + } + if (client->connack_callback) + client->connack_callback(client->user_ctx, client->parser.mqtt_packet.connack.reason_code); + if (!client->parser.mqtt_packet.connack.reason_code) { + INFO("MQTT Connection Accepted By Server"); + client->client_state = CONNECTED; + break; + } + client->client_state = ERROR; + return MQTT_NG_CLIENT_SERVER_RETURNED_ERROR; + case MQTT_CPT_PUBACK: +#ifdef MQTT_DEBUG_VERBOSE + DEBUG("Received PUBACK %" PRIu16, client->parser.mqtt_packet.puback.packet_id); +#endif + if (mark_packet_acked(client, client->parser.mqtt_packet.puback.packet_id)) + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + if (client->puback_callback) + client->puback_callback(client->parser.mqtt_packet.puback.packet_id); + break; + case MQTT_CPT_PINGRESP: +#ifdef MQTT_DEBUG_VERBOSE + DEBUG("Received PINGRESP"); +#endif + break; + case MQTT_CPT_SUBACK: +#ifdef MQTT_DEBUG_VERBOSE + DEBUG("Received SUBACK %" PRIu16, client->parser.mqtt_packet.suback.packet_id); +#endif + if (mark_packet_acked(client, client->parser.mqtt_packet.suback.packet_id)) + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + break; + case MQTT_CPT_PUBLISH: +#ifdef MQTT_DEBUG_VERBOSE + DEBUG("Recevied PUBLISH"); +#endif + pub = &client->parser.mqtt_packet.publish; + if (pub->qos > 1) { + mw_free(pub->topic); + mw_free(pub->data); + return MQTT_NG_CLIENT_NOT_IMPL_YET; + } + if ( pub->qos == 1 && (rc = mqtt_ng_puback(client, pub->packet_id, 0)) ) { + client->client_state = ERROR; + ERROR("Error generating PUBACK reply for PUBLISH"); + return rc; + } + if ( (prop = get_property_by_id(client->parser.properties_parser.head, MQTT_PROP_TOPIC_ALIAS)) != NULL ) { + // Topic Alias property was sent from server + void *topic_ptr; + if (!c_rhash_get_ptr_by_uint64(client->rx_aliases, prop->data.uint8, &topic_ptr)) { + if (pub->topic != NULL) { + ERROR("We do not yet support topic alias reassignment"); + return MQTT_NG_CLIENT_NOT_IMPL_YET; + } + pub->topic = topic_ptr; + } else { + if (pub->topic == NULL) { + ERROR("Topic alias with id %d unknown and topic not set by server!", prop->data.uint8); + return MQTT_NG_CLIENT_PROTOCOL_ERROR; + } + c_rhash_insert_uint64_ptr(client->rx_aliases, prop->data.uint8, pub->topic); + } + } + if (client->msg_callback) + client->msg_callback(pub->topic, pub->data, pub->data_len, pub->qos); + // in case we have property topic alias and we have topic we take over the string + // and add pointer to it into topic alias list + if (prop == NULL) + mw_free(pub->topic); + mw_free(pub->data); + return MQTT_NG_CLIENT_WANT_WRITE; + case MQTT_CPT_DISCONNECT: + INFO ("Got MQTT DISCONNECT control packet from server. Reason code: %d", (int)client->parser.mqtt_packet.disconnect.reason_code); + client->client_state = DISCONNECTED; + break; + } + } + + return rc; +} + +int mqtt_ng_sync(struct mqtt_ng_client *client) +{ + if (client->client_state == RAW || client->client_state == DISCONNECTED) + return 0; + + if (client->client_state == ERROR) + return 1; + + LOCK_HDR_BUFFER(&client->main_buffer); + try_send_all(client); + UNLOCK_HDR_BUFFER(&client->main_buffer); + + int rc; + + while ((rc = handle_incoming_traffic(client)) != MQTT_NG_CLIENT_NEED_MORE_BYTES) { + if (rc < 0) + break; + if (rc == MQTT_NG_CLIENT_WANT_WRITE) { + LOCK_HDR_BUFFER(&client->main_buffer); + try_send_all(client); + UNLOCK_HDR_BUFFER(&client->main_buffer); + } + } + + if (rc < 0) + return rc; + + return 0; +} + +time_t mqtt_ng_last_send_time(struct mqtt_ng_client *client) +{ + return client->time_of_last_send; +} + +void mqtt_ng_set_max_mem(struct mqtt_ng_client *client, size_t bytes) +{ + client->max_mem_bytes = bytes; +} + +void mqtt_ng_get_stats(struct mqtt_ng_client *client, struct mqtt_ng_stats *stats) +{ + pthread_mutex_lock(&client->stats_mutex); + memcpy(stats, &client->stats, sizeof(struct mqtt_ng_stats)); + pthread_mutex_unlock(&client->stats_mutex); + + stats->tx_bytes_queued = 0; + stats->tx_buffer_reclaimable = 0; + + LOCK_HDR_BUFFER(&client->main_buffer); + stats->tx_buffer_used = BUFFER_BYTES_USED(&client->main_buffer.hdr_buffer); + stats->tx_buffer_free = BUFFER_BYTES_AVAILABLE(&client->main_buffer.hdr_buffer); + stats->tx_buffer_size = client->main_buffer.hdr_buffer.size; + struct buffer_fragment *frag = BUFFER_FIRST_FRAG(&client->main_buffer.hdr_buffer); + while (frag) { + stats->tx_bytes_queued += frag->len - frag->sent; + if (frag_is_marked_for_gc(frag)) + stats->tx_buffer_reclaimable += FRAG_SIZE_IN_BUFFER(frag); + + frag = frag->next; + } + UNLOCK_HDR_BUFFER(&client->main_buffer); +} + +int mqtt_ng_set_topic_alias(struct mqtt_ng_client *client, const char *topic) +{ + uint16_t idx; + pthread_rwlock_wrlock(&client->tx_topic_aliases.rwlock); + + if (client->tx_topic_aliases.idx_assigned >= client->tx_topic_aliases.idx_max) { + pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock); + mws_error(client->log, "Tx topic alias indexes were exhausted (current version of the library doesn't support reassigning yet. Feel free to contribute."); + return 0; //0 is not a valid topic alias + } + + struct topic_alias_data *alias; + if (!c_rhash_get_ptr_by_str(client->tx_topic_aliases.stoi_dict, topic, (void**)&alias)) { + // this is not a problem for library but might be helpful to warn user + // as it might indicate bug in their program (but also might be expected) + idx = alias->idx; + pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock); + mws_debug(client->log, "%s topic \"%s\" already has alias set. Ignoring.", __FUNCTION__, topic); + return idx; + } + + alias = mw_malloc(sizeof(struct topic_alias_data)); + idx = ++client->tx_topic_aliases.idx_assigned; + alias->idx = idx; + __atomic_store_n(&alias->usage_count, 0, __ATOMIC_SEQ_CST); + + c_rhash_insert_str_ptr(client->tx_topic_aliases.stoi_dict, topic, (void*)alias); + + pthread_rwlock_unlock(&client->tx_topic_aliases.rwlock); + return idx; +} diff --git a/src/aclk/mqtt_websockets/mqtt_ng.h b/src/aclk/mqtt_websockets/mqtt_ng.h new file mode 100644 index 000000000..4b0584d58 --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_ng.h @@ -0,0 +1,99 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include <stdint.h> +#include <sys/types.h> +#include <time.h> + +#include "c-rbuf/cringbuffer.h" +#include "common_public.h" + +#define MQTT_NG_MSGGEN_OK 0 +// MQTT_NG_MSGGEN_USER_ERROR means parameters given to this function +// do not make sense or are out of MQTT specs +#define MQTT_NG_MSGGEN_USER_ERROR 1 +#define MQTT_NG_MSGGEN_BUFFER_OOM 2 +#define MQTT_NG_MSGGEN_MSG_TOO_BIG 3 + +struct mqtt_ng_client; + +/* Converts integer to MQTT Variable Byte Integer as per 1.5.5 of MQTT 5 specs + * @param input value to be converted + * @param output pointer to memory where output will be written to. Must allow up to 4 bytes to be written. + * @return number of bytes written to output or <= 0 if error in which case contents of output are undefined + */ +int uint32_to_mqtt_vbi(uint32_t input, unsigned char *output); + +struct mqtt_lwt_properties { + char *will_topic; + free_fnc_t will_topic_free; + + void *will_message; + free_fnc_t will_message_free; + size_t will_message_size; + + int will_qos; + int will_retain; +}; + +struct mqtt_auth_properties { + char *client_id; + free_fnc_t client_id_free; + char *username; + free_fnc_t username_free; + char *password; + free_fnc_t password_free; +}; + +int mqtt_ng_connect(struct mqtt_ng_client *client, + struct mqtt_auth_properties *auth, + struct mqtt_lwt_properties *lwt, + uint8_t clean_start, + uint16_t keep_alive); + +int mqtt_ng_publish(struct mqtt_ng_client *client, + char *topic, + free_fnc_t topic_free, + void *msg, + free_fnc_t msg_free, + size_t msg_len, + uint8_t publish_flags, + uint16_t *packet_id); + +struct mqtt_sub { + char *topic; + free_fnc_t topic_free; + uint8_t options; +}; + +int mqtt_ng_subscribe(struct mqtt_ng_client *client, struct mqtt_sub *subscriptions, size_t subscription_count); + +int mqtt_ng_ping(struct mqtt_ng_client *client); + +typedef ssize_t (*mqtt_ng_send_fnc_t)(void *user_ctx, const void* buf, size_t len); + +struct mqtt_ng_init { + mqtt_wss_log_ctx_t log; + rbuf_t data_in; + mqtt_ng_send_fnc_t data_out_fnc; + void *user_ctx; + + void (*puback_callback)(uint16_t packet_id); + void (*connack_callback)(void* user_ctx, int connack_reply); + void (*msg_callback)(const char *topic, const void *msg, size_t msglen, int qos); +}; + +struct mqtt_ng_client *mqtt_ng_init(struct mqtt_ng_init *settings); + +void mqtt_ng_destroy(struct mqtt_ng_client *client); + +int mqtt_ng_disconnect(struct mqtt_ng_client *client, uint8_t reason_code); + +int mqtt_ng_sync(struct mqtt_ng_client *client); + +time_t mqtt_ng_last_send_time(struct mqtt_ng_client *client); + +void mqtt_ng_set_max_mem(struct mqtt_ng_client *client, size_t bytes); + +void mqtt_ng_get_stats(struct mqtt_ng_client *client, struct mqtt_ng_stats *stats); + +int mqtt_ng_set_topic_alias(struct mqtt_ng_client *client, const char *topic); diff --git a/src/aclk/mqtt_websockets/mqtt_wss_client.c b/src/aclk/mqtt_websockets/mqtt_wss_client.c new file mode 100644 index 000000000..a2aef80ce --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_wss_client.c @@ -0,0 +1,1132 @@ +// SPDX-License-Identifier: GPL-3.0-only +// Copyright (C) 2020 Timotej Šiškovič + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif + +#include "mqtt_wss_client.h" +#include "mqtt_ng.h" +#include "ws_client.h" +#include "common_internal.h" + +#include <stdlib.h> +#include <fcntl.h> +#include <unistd.h> +#include <poll.h> +#include <string.h> +#include <time.h> + +#include <sys/socket.h> +#include <netinet/in.h> +#include <arpa/inet.h> +#include <netinet/tcp.h> //TCP_NODELAY +#include <netdb.h> + +#include <openssl/err.h> +#include <openssl/ssl.h> + +#define PIPE_READ_END 0 +#define PIPE_WRITE_END 1 +#define POLLFD_SOCKET 0 +#define POLLFD_PIPE 1 + +#if (OPENSSL_VERSION_NUMBER < OPENSSL_VERSION_110) && (SSLEAY_VERSION_NUMBER >= OPENSSL_VERSION_097) +#include <openssl/conf.h> +#endif + +//TODO MQTT_PUBLISH_RETAIN should not be needed anymore +#define MQTT_PUBLISH_RETAIN 0x01 +#define MQTT_CONNECT_CLEAN_SESSION 0x02 +#define MQTT_CONNECT_WILL_RETAIN 0x20 + +char *util_openssl_ret_err(int err) +{ + switch(err){ + case SSL_ERROR_WANT_READ: + return "SSL_ERROR_WANT_READ"; + case SSL_ERROR_WANT_WRITE: + return "SSL_ERROR_WANT_WRITE"; + case SSL_ERROR_NONE: + return "SSL_ERROR_NONE"; + case SSL_ERROR_ZERO_RETURN: + return "SSL_ERROR_ZERO_RETURN"; + case SSL_ERROR_WANT_CONNECT: + return "SSL_ERROR_WANT_CONNECT"; + case SSL_ERROR_WANT_ACCEPT: + return "SSL_ERROR_WANT_ACCEPT"; + case SSL_ERROR_WANT_X509_LOOKUP: + return "SSL_ERROR_WANT_X509_LOOKUP"; +#ifdef SSL_ERROR_WANT_ASYNC + case SSL_ERROR_WANT_ASYNC: + return "SSL_ERROR_WANT_ASYNC"; +#endif +#ifdef SSL_ERROR_WANT_ASYNC_JOB + case SSL_ERROR_WANT_ASYNC_JOB: + return "SSL_ERROR_WANT_ASYNC_JOB"; +#endif +#ifdef SSL_ERROR_WANT_CLIENT_HELLO_CB + case SSL_ERROR_WANT_CLIENT_HELLO_CB: + return "SSL_ERROR_WANT_CLIENT_HELLO_CB"; +#endif + case SSL_ERROR_SYSCALL: + return "SSL_ERROR_SYSCALL"; + case SSL_ERROR_SSL: + return "SSL_ERROR_SSL"; + } + return "UNKNOWN"; +} + +struct mqtt_wss_client_struct { + ws_client *ws_client; + + mqtt_wss_log_ctx_t log; + +// immediate connection (e.g. proxy server) + char *host; + int port; + +// target of connection (e.g. where we want to connect to) + char *target_host; + int target_port; + + enum mqtt_wss_proxy_type proxy_type; + char *proxy_uname; + char *proxy_passwd; + +// nonblock IO related + int sockfd; + int write_notif_pipe[2]; + struct pollfd poll_fds[2]; + + SSL_CTX *ssl_ctx; + SSL *ssl; + int ssl_flags; + + struct mqtt_ng_client *mqtt; + + int mqtt_keepalive; + + pthread_mutex_t pub_lock; + +// signifies that we didn't write all MQTT wanted +// us to write during last cycle (e.g. due to buffer +// size) and thus we should arm POLLOUT + unsigned int mqtt_didnt_finish_write:1; + + unsigned int mqtt_connected:1; + unsigned int mqtt_disconnecting:1; + +// Application layer callback pointers + void (*msg_callback)(const char *, const void *, size_t, int); + void (*puback_callback)(uint16_t packet_id); + + pthread_mutex_t stat_lock; + struct mqtt_wss_stats stats; + +#ifdef MQTT_WSS_DEBUG + void (*ssl_ctx_keylog_cb)(const SSL *ssl, const char *line); +#endif +}; + +static void mws_connack_callback_ng(void *user_ctx, int code) +{ + mqtt_wss_client client = user_ctx; + switch(code) { + case 0: + client->mqtt_connected = 1; + return; +//TODO manual labor: all the CONNACK error codes with some nice error message + default: + mws_error(client->log, "MQTT CONNACK returned error %d", code); + return; + } +} + +static ssize_t mqtt_send_cb(void *user_ctx, const void* buf, size_t len) +{ + mqtt_wss_client client = user_ctx; +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "mqtt_pal_sendall(len=%d)", len); +#endif + int ret = ws_client_send(client->ws_client, WS_OP_BINARY_FRAME, buf, len); + if (ret >= 0 && (size_t)ret != len) { +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Not complete message sent (Msg=%d,Sent=%d). Need to arm POLLOUT!", len, ret); +#endif + client->mqtt_didnt_finish_write = 1; + } + return ret; +} + +mqtt_wss_client mqtt_wss_new(const char *log_prefix, + mqtt_wss_log_callback_t log_callback, + msg_callback_fnc_t msg_callback, + void (*puback_callback)(uint16_t packet_id)) +{ + mqtt_wss_log_ctx_t log; + + log = mqtt_wss_log_ctx_create(log_prefix, log_callback); + if(!log) + return NULL; + + SSL_library_init(); + SSL_load_error_strings(); + + mqtt_wss_client client = mw_calloc(1, sizeof(struct mqtt_wss_client_struct)); + if (!client) { + mws_error(log, "OOM alocating mqtt_wss_client"); + goto fail; + } + + pthread_mutex_init(&client->pub_lock, NULL); + pthread_mutex_init(&client->stat_lock, NULL); + + client->msg_callback = msg_callback; + client->puback_callback = puback_callback; + + client->ws_client = ws_client_new(0, &client->target_host, log); + if (!client->ws_client) { + mws_error(log, "Error creating ws_client"); + goto fail_1; + } + + client->log = log; + +#ifdef __APPLE__ + if (pipe(client->write_notif_pipe)) { +#else + if (pipe2(client->write_notif_pipe, O_CLOEXEC /*| O_DIRECT*/)) { +#endif + mws_error(log, "Couldn't create pipe"); + goto fail_2; + } + + client->poll_fds[POLLFD_PIPE].fd = client->write_notif_pipe[PIPE_READ_END]; + client->poll_fds[POLLFD_PIPE].events = POLLIN; + + client->poll_fds[POLLFD_SOCKET].events = POLLIN; + + struct mqtt_ng_init settings = { + .log = log, + .data_in = client->ws_client->buf_to_mqtt, + .data_out_fnc = &mqtt_send_cb, + .user_ctx = client, + .connack_callback = &mws_connack_callback_ng, + .puback_callback = puback_callback, + .msg_callback = msg_callback + }; + if ( (client->mqtt = mqtt_ng_init(&settings)) == NULL ) { + mws_error(log, "Error initializing internal MQTT client"); + goto fail_3; + } + + return client; + +fail_3: + close(client->write_notif_pipe[PIPE_WRITE_END]); + close(client->write_notif_pipe[PIPE_READ_END]); +fail_2: + ws_client_destroy(client->ws_client); +fail_1: + mw_free(client); +fail: + mqtt_wss_log_ctx_destroy(log); + return NULL; +} + +void mqtt_wss_set_max_buf_size(mqtt_wss_client client, size_t size) +{ + mqtt_ng_set_max_mem(client->mqtt, size); +} + +void mqtt_wss_destroy(mqtt_wss_client client) +{ + mqtt_ng_destroy(client->mqtt); + + close(client->write_notif_pipe[PIPE_WRITE_END]); + close(client->write_notif_pipe[PIPE_READ_END]); + + ws_client_destroy(client->ws_client); + + // deleted after client->ws_client + // as it "borrows" this pointer and might use it + if (client->target_host == client->host) + client->target_host = NULL; + if (client->target_host) + mw_free(client->target_host); + if (client->host) + mw_free(client->host); + mw_free(client->proxy_passwd); + mw_free(client->proxy_uname); + + if (client->ssl) + SSL_free(client->ssl); + + if (client->ssl_ctx) + SSL_CTX_free(client->ssl_ctx); + + if (client->sockfd > 0) + close(client->sockfd); + + pthread_mutex_destroy(&client->pub_lock); + pthread_mutex_destroy(&client->stat_lock); + + mqtt_wss_log_ctx_destroy(client->log); + mw_free(client); +} + +static int cert_verify_callback(int preverify_ok, X509_STORE_CTX *ctx) +{ + SSL *ssl; + X509 *err_cert; + mqtt_wss_client client; + int err = 0, depth; + char *err_str; + + ssl = X509_STORE_CTX_get_ex_data(ctx, SSL_get_ex_data_X509_STORE_CTX_idx()); + client = SSL_get_ex_data(ssl, 0); + + // TODO handle depth as per https://www.openssl.org/docs/man1.0.2/man3/SSL_CTX_set_verify.html + + if (!preverify_ok) { + err = X509_STORE_CTX_get_error(ctx); + depth = X509_STORE_CTX_get_error_depth(ctx); + err_cert = X509_STORE_CTX_get_current_cert(ctx); + err_str = X509_NAME_oneline(X509_get_subject_name(err_cert), NULL, 0); + + mws_error(client->log, "verify error:num=%d:%s:depth=%d:%s", err, + X509_verify_cert_error_string(err), depth, err_str); + + mw_free(err_str); + } + + if (!preverify_ok && err == X509_V_ERR_DEPTH_ZERO_SELF_SIGNED_CERT && + client->ssl_flags & MQTT_WSS_SSL_ALLOW_SELF_SIGNED) + { + preverify_ok = 1; + mws_error(client->log, "Self Signed Certificate Accepted as the connection was " + "requested with MQTT_WSS_SSL_ALLOW_SELF_SIGNED"); + } + + return preverify_ok; +} + +#define PROXY_CONNECT "CONNECT" +#define PROXY_HTTP "HTTP/1.1" +#define HTTP_ENDLINE "\x0D\x0A" +#define HTTP_HDR_TERMINATOR "\x0D\x0A\x0D\x0A" +#define HTTP_CODE_LEN 4 +#define HTTP_REASON_MAX_LEN 512 +static int http_parse_reply(mqtt_wss_client client, rbuf_t buf) +{ + char *ptr; + char http_code_s[4]; + int http_code; + int idx; + + if (rbuf_memcmp_n(buf, PROXY_HTTP, strlen(PROXY_HTTP))) { + mws_error(client->log, "http_proxy expected reply with \"" PROXY_HTTP "\""); + return 1; + } + + rbuf_bump_tail(buf, strlen(PROXY_HTTP)); + + if (!rbuf_pop(buf, http_code_s, 1) || http_code_s[0] != 0x20) { + mws_error(client->log, "http_proxy missing space after \"" PROXY_HTTP "\""); + return 2; + } + + if (!rbuf_pop(buf, http_code_s, HTTP_CODE_LEN)) { + mws_error(client->log, "http_proxy missing HTTP code"); + return 3; + } + + for (int i = 0; i < HTTP_CODE_LEN - 1; i++) + if (http_code_s[i] > 0x39 || http_code_s[i] < 0x30) { + mws_error(client->log, "http_proxy HTTP code non numeric"); + return 4; + } + + http_code_s[HTTP_CODE_LEN - 1] = 0; + http_code = atoi(http_code_s); + + // TODO check if we ever have more headers here + rbuf_find_bytes(buf, HTTP_ENDLINE, strlen(HTTP_ENDLINE), &idx); + if (idx >= HTTP_REASON_MAX_LEN) { + mws_error(client->log, "http_proxy returned reason that is too long"); + return 5; + } + + if (http_code != 200) { + ptr = mw_malloc(idx + 1); + if (!ptr) + return 6; + rbuf_pop(buf, ptr, idx); + ptr[idx] = 0; + + mws_error(client->log, "http_proxy returned error code %d \"%s\"", http_code, ptr); + mw_free(ptr); + return 7; + }/* else + rbuf_bump_tail(buf, idx);*/ + + rbuf_find_bytes(buf, HTTP_HDR_TERMINATOR, strlen(HTTP_HDR_TERMINATOR), &idx); + if (idx) + rbuf_bump_tail(buf, idx); + + rbuf_bump_tail(buf, strlen(HTTP_HDR_TERMINATOR)); + + if (rbuf_bytes_available(buf)) { + mws_error(client->log, "http_proxy unexpected trailing bytes after end of HTTP hdr"); + return 8; + } + + mws_debug(client->log, "http_proxy CONNECT succeeded"); + return 0; +} + +#if defined(OPENSSL_VERSION_NUMBER) && OPENSSL_VERSION_NUMBER < OPENSSL_VERSION_110 +static EVP_ENCODE_CTX *EVP_ENCODE_CTX_new(void) +{ + EVP_ENCODE_CTX *ctx = OPENSSL_malloc(sizeof(*ctx)); + + if (ctx != NULL) { + memset(ctx, 0, sizeof(*ctx)); + } + return ctx; +} +static void EVP_ENCODE_CTX_free(EVP_ENCODE_CTX *ctx) +{ + OPENSSL_free(ctx); + return; +} +#endif + +inline static int base64_encode_helper(unsigned char *out, int *outl, const unsigned char *in, int in_len) +{ + int len; + unsigned char *str = out; + EVP_ENCODE_CTX *ctx = EVP_ENCODE_CTX_new(); + EVP_EncodeInit(ctx); + EVP_EncodeUpdate(ctx, str, outl, in, in_len); + str += *outl; + EVP_EncodeFinal(ctx, str, &len); + *outl += len; + + str = out; + while(*str) { + if (*str != 0x0D && *str != 0x0A) + *out++ = *str++; + else + str++; + } + *out = 0; + + EVP_ENCODE_CTX_free(ctx); + return 0; +} + +static int http_proxy_connect(mqtt_wss_client client) +{ + int rc; + struct pollfd poll_fd; + rbuf_t r_buf = rbuf_create(4096); + if (!r_buf) + return 1; + char *r_buf_ptr; + size_t r_buf_linear_insert_capacity; + + poll_fd.fd = client->sockfd; + poll_fd.events = POLLIN; + + r_buf_ptr = rbuf_get_linear_insert_range(r_buf, &r_buf_linear_insert_capacity); + snprintf(r_buf_ptr, r_buf_linear_insert_capacity,"%s %s:%d %s" HTTP_ENDLINE, PROXY_CONNECT, client->target_host, client->target_port, PROXY_HTTP); + write(client->sockfd, r_buf_ptr, strlen(r_buf_ptr)); + + if (client->proxy_uname) { + size_t creds_plain_len = strlen(client->proxy_uname) + strlen(client->proxy_passwd) + 2; + char *creds_plain = mw_malloc(creds_plain_len); + if (!creds_plain) { + mws_error(client->log, "OOM creds_plain"); + rc = 6; + goto cleanup; + } + int creds_base64_len = (((4 * creds_plain_len / 3) + 3) & ~3); + // OpenSSL encoder puts newline every 64 output bytes + // we remove those but during encoding we need that space in the buffer + creds_base64_len += (1+(creds_base64_len/64)) * strlen("\n"); + char *creds_base64 = mw_malloc(creds_base64_len + 1); + if (!creds_base64) { + mw_free(creds_plain); + mws_error(client->log, "OOM creds_base64"); + rc = 6; + goto cleanup; + } + char *ptr = creds_plain; + strcpy(ptr, client->proxy_uname); + ptr += strlen(client->proxy_uname); + *ptr++ = ':'; + strcpy(ptr, client->proxy_passwd); + + int b64_len; + base64_encode_helper((unsigned char*)creds_base64, &b64_len, (unsigned char*)creds_plain, strlen(creds_plain)); + mw_free(creds_plain); + + r_buf_ptr = rbuf_get_linear_insert_range(r_buf, &r_buf_linear_insert_capacity); + snprintf(r_buf_ptr, r_buf_linear_insert_capacity,"Proxy-Authorization: Basic %s" HTTP_ENDLINE, creds_base64); + write(client->sockfd, r_buf_ptr, strlen(r_buf_ptr)); + mw_free(creds_base64); + } + write(client->sockfd, HTTP_ENDLINE, strlen(HTTP_ENDLINE)); + + // read until you find CRLF, CRLF (HTTP HDR end) + // or ring buffer is full + // or timeout + while ((rc = poll(&poll_fd, 1, 1000)) >= 0) { + if (!rc) { + mws_error(client->log, "http_proxy timeout waiting reply from proxy server"); + rc = 2; + goto cleanup; + } + r_buf_ptr = rbuf_get_linear_insert_range(r_buf, &r_buf_linear_insert_capacity); + if (!r_buf_ptr) { + mws_error(client->log, "http_proxy read ring buffer full"); + rc = 3; + goto cleanup; + } + if ((rc = read(client->sockfd, r_buf_ptr, r_buf_linear_insert_capacity)) < 0) { + if (errno == EWOULDBLOCK || errno == EAGAIN) { + continue; + } + mws_error(client->log, "http_proxy error reading from socket \"%s\"", strerror(errno)); + rc = 4; + goto cleanup; + } + rbuf_bump_head(r_buf, rc); + if (rbuf_find_bytes(r_buf, HTTP_HDR_TERMINATOR, strlen(HTTP_HDR_TERMINATOR), &rc)) { + rc = 0; + if (http_parse_reply(client, r_buf)) + rc = 5; + + goto cleanup; + } + } + mws_error(client->log, "proxy negotiation poll error \"%s\"", strerror(errno)); + rc = 5; +cleanup: + rbuf_free(r_buf); + return rc; +} + +int mqtt_wss_connect(mqtt_wss_client client, char *host, int port, struct mqtt_connect_params *mqtt_params, int ssl_flags, struct mqtt_wss_proxy *proxy) +{ + struct sockaddr_in addr; + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + + struct hostent *he; + struct in_addr **addr_list; + + if (!mqtt_params) { + mws_error(client->log, "mqtt_params can't be null!"); + return -1; + } + + // reset state in case this is reconnect + client->mqtt_didnt_finish_write = 0; + client->mqtt_connected = 0; + client->mqtt_disconnecting = 0; + ws_client_reset(client->ws_client); + + if (client->target_host == client->host) + client->target_host = NULL; + if (client->target_host) + mw_free(client->target_host); + if (client->host) + mw_free(client->host); + + if (proxy && proxy->type != MQTT_WSS_DIRECT) { + client->host = mw_strdup(proxy->host); + client->port = proxy->port; + client->target_host = mw_strdup(host); + client->target_port = port; + client->proxy_type = proxy->type; + if (proxy->username) + client->proxy_uname = mw_strdup(proxy->username); + if (proxy->password) + client->proxy_passwd = mw_strdup(proxy->password); + } else { + client->host = mw_strdup(host); + client->port = port; + client->target_host = client->host; + client->target_port = port; + } + + client->ssl_flags = ssl_flags; + + //TODO gethostbyname -> getaddinfo + // hstrerror -> gai_strerror + if ((he = gethostbyname(client->host)) == NULL) { + mws_error(client->log, "gethostbyname() error \"%s\"", hstrerror(h_errno)); + return -1; + } + + addr_list = (struct in_addr **)he->h_addr_list; + if(!addr_list[0]) { + mws_error(client->log, "No IP addr resolved"); + return -1; + } + mws_debug(client->log, "Resolved IP: %s", inet_ntoa(*addr_list[0])); + addr.sin_addr = *addr_list[0]; + addr.sin_port = htons(client->port); + + if (client->sockfd > 0) + close(client->sockfd); + client->sockfd = socket(AF_INET, SOCK_STREAM | DEFAULT_SOCKET_FLAGS, 0); + if (client->sockfd < 0) { + mws_error(client->log, "Couldn't create socket()"); + return -1; + } + +#ifndef SOCK_CLOEXEC + int flags = fcntl(client->sockfd, F_GETFD); + if (flags != -1) + (void) fcntl(client->sockfd, F_SETFD, flags| FD_CLOEXEC); +#endif + + int flag = 1; + int result = setsockopt(client->sockfd, + IPPROTO_TCP, + TCP_NODELAY, + &flag, + sizeof(int)); + if (result < 0) + mws_error(client->log, "Could not dissable NAGLE"); + + if (connect(client->sockfd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + mws_error(client->log, "Could not connect to remote endpoint \"%s\", port %d.\n", client->host, client->port); + return -3; + } + + client->poll_fds[POLLFD_SOCKET].fd = client->sockfd; + + if (fcntl(client->sockfd, F_SETFL, fcntl(client->sockfd, F_GETFL, 0) | O_NONBLOCK) == -1) { + mws_error(client->log, "Error setting O_NONBLOCK to TCP socket. \"%s\"", strerror(errno)); + return -8; + } + + if (client->proxy_type != MQTT_WSS_DIRECT) + if (http_proxy_connect(client)) + return -4; + +#if OPENSSL_VERSION_NUMBER < OPENSSL_VERSION_110 +#if (SSLEAY_VERSION_NUMBER >= OPENSSL_VERSION_097) + OPENSSL_config(NULL); +#endif + SSL_load_error_strings(); + SSL_library_init(); +#else + if (OPENSSL_init_ssl(OPENSSL_INIT_LOAD_CONFIG, NULL) != 1) { + mws_error(client->log, "Failed to initialize SSL"); + return -1; + }; +#endif + + // free SSL structs from possible previous connections + if (client->ssl) + SSL_free(client->ssl); + if (client->ssl_ctx) + SSL_CTX_free(client->ssl_ctx); + + client->ssl_ctx = SSL_CTX_new(SSLv23_client_method()); + if (!(client->ssl_flags & MQTT_WSS_SSL_DONT_CHECK_CERTS)) { + SSL_CTX_set_default_verify_paths(client->ssl_ctx); + SSL_CTX_set_verify(client->ssl_ctx, SSL_VERIFY_PEER | SSL_VERIFY_CLIENT_ONCE, cert_verify_callback); + } else + mws_error(client->log, "SSL Certificate checking completely disabled!!!"); + +#ifdef MQTT_WSS_DEBUG + if(client->ssl_ctx_keylog_cb) + SSL_CTX_set_keylog_callback(client->ssl_ctx, client->ssl_ctx_keylog_cb); +#endif + + client->ssl = SSL_new(client->ssl_ctx); + if (!(client->ssl_flags & MQTT_WSS_SSL_DONT_CHECK_CERTS)) { + if (!SSL_set_ex_data(client->ssl, 0, client)) { + mws_error(client->log, "Could not SSL_set_ex_data"); + return -4; + } + } + SSL_set_fd(client->ssl, client->sockfd); + SSL_set_connect_state(client->ssl); + + if (!SSL_set_tlsext_host_name(client->ssl, client->target_host)) { + mws_error(client->log, "Error setting TLS SNI host"); + return -7; + } + + result = SSL_connect(client->ssl); + if (result != -1 && result != 1) { + mws_error(client->log, "SSL could not connect"); + return -5; + } + if (result == -1) { + int ec = SSL_get_error(client->ssl, result); + if (ec != SSL_ERROR_WANT_READ && ec != SSL_ERROR_WANT_WRITE) { + mws_error(client->log, "Failed to start SSL connection"); + return -6; + } + } + + client->mqtt_keepalive = (mqtt_params->keep_alive ? mqtt_params->keep_alive : 400); + + mws_info(client->log, "Going to connect using internal MQTT 5 implementation"); + struct mqtt_auth_properties auth; + auth.client_id = (char*)mqtt_params->clientid; + auth.client_id_free = NULL; + auth.username = (char*)mqtt_params->username; + auth.username_free = NULL; + auth.password = (char*)mqtt_params->password; + auth.password_free = NULL; + struct mqtt_lwt_properties lwt; + lwt.will_topic = (char*)mqtt_params->will_topic; + lwt.will_topic_free = NULL; + lwt.will_message = (void*)mqtt_params->will_msg; + lwt.will_message_free = NULL; // TODO expose no copy version to API + lwt.will_message_size = mqtt_params->will_msg_len; + lwt.will_qos = (mqtt_params->will_flags & MQTT_WSS_PUB_QOSMASK); + lwt.will_retain = mqtt_params->will_flags & MQTT_WSS_PUB_RETAIN; + int ret = mqtt_ng_connect(client->mqtt, &auth, mqtt_params->will_msg ? &lwt : NULL, 1, client->mqtt_keepalive); + if (ret) { + mws_error(client->log, "Error generating MQTT connect"); + return 1; + } + + client->poll_fds[POLLFD_PIPE].events = POLLIN; + client->poll_fds[POLLFD_SOCKET].events = POLLIN; + // wait till MQTT connection is established + while (!client->mqtt_connected) { + if(mqtt_wss_service(client, -1)) { + mws_error(client->log, "Error connecting to MQTT WSS server \"%s\", port %d.", host, port); + return 2; + } + } + + return 0; +} + +#define NSEC_PER_USEC 1000ULL +#define USEC_PER_SEC 1000000ULL +#define NSEC_PER_MSEC 1000000ULL +#define NSEC_PER_SEC 1000000000ULL + +static inline uint64_t boottime_usec(mqtt_wss_client client) { + struct timespec ts; +#if defined(__APPLE__) || defined(__FreeBSD__) + if (clock_gettime(CLOCK_MONOTONIC, &ts) == -1) { +#else + if (clock_gettime(CLOCK_BOOTTIME, &ts) == -1) { +#endif + mws_error(client->log, "clock_gettimte failed"); + return 0; + } + return (uint64_t)ts.tv_sec * USEC_PER_SEC + (ts.tv_nsec % NSEC_PER_SEC) / NSEC_PER_USEC; +} + +#define MWS_TIMED_OUT 1 +#define MWS_ERROR 2 +#define MWS_OK 0 +static inline const char *mqtt_wss_error_tos(int ec) +{ + switch(ec) { + case MWS_TIMED_OUT: + return "Error: Operation was not able to finish in time"; + case MWS_ERROR: + return "Unspecified Error"; + default: + return "Unknown Error Code!"; + } + +} + +static inline int mqtt_wss_service_all(mqtt_wss_client client, int timeout_ms) +{ + uint64_t exit_by = boottime_usec(client) + (timeout_ms * NSEC_PER_MSEC); + uint64_t now; + client->poll_fds[POLLFD_SOCKET].events |= POLLOUT; // TODO when entering mwtt_wss_service use out buffer size to arm POLLOUT + while (rbuf_bytes_available(client->ws_client->buf_write)) { + now = boottime_usec(client); + if (now >= exit_by) + return MWS_TIMED_OUT; + if (mqtt_wss_service(client, exit_by - now)) + return MWS_ERROR; + } + return MWS_OK; +} + +void mqtt_wss_disconnect(mqtt_wss_client client, int timeout_ms) +{ + int ret; + + // block application from sending more MQTT messages + client->mqtt_disconnecting = 1; + + // send whatever was left at the time of calling this function + ret = mqtt_wss_service_all(client, timeout_ms / 4); + if(ret) + mws_error(client->log, + "Error while trying to send all remaining data in an attempt " + "to gracefully disconnect! EC=%d Desc:\"%s\"", + ret, + mqtt_wss_error_tos(ret)); + + // schedule and send MQTT disconnect + mqtt_ng_disconnect(client->mqtt, 0); + mqtt_ng_sync(client->mqtt); + + ret = mqtt_wss_service_all(client, timeout_ms / 4); + if(ret) + mws_error(client->log, + "Error while trying to send MQTT disconnect message in an attempt " + "to gracefully disconnect! EC=%d Desc:\"%s\"", + ret, + mqtt_wss_error_tos(ret)); + + // send WebSockets close message + uint16_t ws_rc = htobe16(1000); + ws_client_send(client->ws_client, WS_OP_CONNECTION_CLOSE, (const char*)&ws_rc, sizeof(ws_rc)); + ret = mqtt_wss_service_all(client, timeout_ms / 4); + if(ret) { + // Some MQTT/WSS servers will close socket on receipt of MQTT disconnect and + // do not wait for WebSocket to be closed properly + mws_warn(client->log, + "Error while trying to send WebSocket disconnect message in an attempt " + "to gracefully disconnect! EC=%d Desc:\"%s\".", + ret, + mqtt_wss_error_tos(ret)); + } + + // Service WSS connection until remote closes connection (usual) + // or timeout happens (unusual) in which case we close + mqtt_wss_service_all(client, timeout_ms / 4); + + close(client->sockfd); + client->sockfd = -1; +} + +static inline void mqtt_wss_wakeup(mqtt_wss_client client) +{ +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "mqtt_wss_wakup - forcing wake up of main loop"); +#endif + write(client->write_notif_pipe[PIPE_WRITE_END], " ", 1); +} + +#define THROWAWAY_BUF_SIZE 32 +char throwaway[THROWAWAY_BUF_SIZE]; +static inline void util_clear_pipe(int fd) +{ + (void)read(fd, throwaway, THROWAWAY_BUF_SIZE); +} + +static inline void set_socket_pollfds(mqtt_wss_client client, int ssl_ret) { + if (ssl_ret == SSL_ERROR_WANT_WRITE) + client->poll_fds[POLLFD_SOCKET].events |= POLLOUT; + if (ssl_ret == SSL_ERROR_WANT_READ) + client->poll_fds[POLLFD_SOCKET].events |= POLLIN; +} + +static int handle_mqtt_internal(mqtt_wss_client client) +{ + int rc = mqtt_ng_sync(client->mqtt); + if (rc) { + mws_error(client->log, "mqtt_ng_sync returned %d != 0", rc); + client->mqtt_connected = 0; + return 1; + } + return 0; +} + +#define SEC_TO_MSEC 1000 +static inline long long int t_till_next_keepalive_ms(mqtt_wss_client client) +{ + time_t last_send = mqtt_ng_last_send_time(client->mqtt); + long long int next_mqtt_keep_alive = (last_send * SEC_TO_MSEC) + + (client->mqtt_keepalive * (SEC_TO_MSEC * 0.75 /* SEND IN ADVANCE */)); + return(next_mqtt_keep_alive - (time(NULL) * SEC_TO_MSEC)); +} + +#ifdef MQTT_WSS_CPUSTATS +static inline uint64_t mqtt_wss_now_usec(mqtt_wss_client client) { + struct timespec ts; + if(clock_gettime(CLOCK_MONOTONIC, &ts) == -1) { + mws_error(client->log, "clock_gettime(CLOCK_MONOTONIC, ×pec) failed."); + return 0; + } + return (uint64_t)ts.tv_sec * USEC_PER_SEC + (ts.tv_nsec % NSEC_PER_SEC) / NSEC_PER_USEC; +} +#endif + +int mqtt_wss_service(mqtt_wss_client client, int timeout_ms) +{ + char *ptr; + size_t size; + int ret; + int send_keepalive = 0; + +#ifdef MQTT_WSS_CPUSTATS + uint64_t t1,t2; + t1 = mqtt_wss_now_usec(client); +#endif + +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, ">>>>> mqtt_wss_service <<<<<"); + mws_debug(client->log, "Waiting for events: %s%s%s", + (client->poll_fds[POLLFD_SOCKET].events & POLLIN) ? "SOCKET_POLLIN " : "", + (client->poll_fds[POLLFD_SOCKET].events & POLLOUT) ? "SOCKET_POLLOUT " : "", + (client->poll_fds[POLLFD_PIPE].events & POLLIN) ? "PIPE_POLLIN" : "" ); +#endif + + // Check user requested TO doesn't interfere with MQTT keep alives + long long int till_next_keep_alive = t_till_next_keepalive_ms(client); + if (client->mqtt_connected && (timeout_ms < 0 || timeout_ms >= till_next_keep_alive)) { + #ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Shortening Timeout requested %d to %lld to ensure keep-alive can be sent", timeout_ms, till_next_keep_alive); + #endif + timeout_ms = till_next_keep_alive; + send_keepalive = 1; + } + +#ifdef MQTT_WSS_CPUSTATS + t2 = mqtt_wss_now_usec(client); + client->stats.time_keepalive += t2 - t1; +#endif + + if ((ret = poll(client->poll_fds, 2, timeout_ms >= 0 ? timeout_ms : -1)) < 0) { + if (errno == EINTR) { + mws_warn(client->log, "poll interrupted by EINTR"); + return 0; + } + mws_error(client->log, "poll error \"%s\"", strerror(errno)); + return -2; + } + +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Poll events happened: %s%s%s%s", + (client->poll_fds[POLLFD_SOCKET].revents & POLLIN) ? "SOCKET_POLLIN " : "", + (client->poll_fds[POLLFD_SOCKET].revents & POLLOUT) ? "SOCKET_POLLOUT " : "", + (client->poll_fds[POLLFD_PIPE].revents & POLLIN) ? "PIPE_POLLIN " : "", + (!ret) ? "POLL_TIMEOUT" : ""); +#endif + +#ifdef MQTT_WSS_CPUSTATS + t1 = mqtt_wss_now_usec(client); +#endif + + if (ret == 0) { + if (send_keepalive) { + // otherwise we shortened the timeout ourselves to take care of + // MQTT keep alives +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Forcing MQTT Ping/keep-alive"); +#endif + mqtt_ng_ping(client->mqtt); + } else { + // if poll timed out and user requested timeout was being used + // return here let user do his work and he will call us back soon + return 0; + } + } + +#ifdef MQTT_WSS_CPUSTATS + t2 = mqtt_wss_now_usec(client); + client->stats.time_keepalive += t2 - t1; +#endif + + client->poll_fds[POLLFD_SOCKET].events = 0; + + if ((ptr = rbuf_get_linear_insert_range(client->ws_client->buf_read, &size))) { + if((ret = SSL_read(client->ssl, ptr, size)) > 0) { +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "SSL_Read: Read %d.", ret); +#endif + pthread_mutex_lock(&client->stat_lock); + client->stats.bytes_rx += ret; + pthread_mutex_unlock(&client->stat_lock); + rbuf_bump_head(client->ws_client->buf_read, ret); + } else { + int errnobkp = errno; + ret = SSL_get_error(client->ssl, ret); +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Read Err: %s", util_openssl_ret_err(ret)); +#endif + set_socket_pollfds(client, ret); + if (ret != SSL_ERROR_WANT_READ && + ret != SSL_ERROR_WANT_WRITE) { + mws_error(client->log, "SSL_read error: %d %s", ret, util_openssl_ret_err(ret)); + if (ret == SSL_ERROR_SYSCALL) + mws_error(client->log, "SSL_read SYSCALL errno: %d %s", errnobkp, strerror(errnobkp)); + return MQTT_WSS_ERR_CONN_DROP; + } + } + } + +#ifdef MQTT_WSS_CPUSTATS + t1 = mqtt_wss_now_usec(client); + client->stats.time_read_socket += t1 - t2; +#endif + + ret = ws_client_process(client->ws_client); + switch(ret) { + case WS_CLIENT_PROTOCOL_ERROR: + return MQTT_WSS_ERR_PROTO_WS; + case WS_CLIENT_NEED_MORE_BYTES: +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "WSCLIENT WANT READ"); +#endif + client->poll_fds[POLLFD_SOCKET].events |= POLLIN; + break; + case WS_CLIENT_CONNECTION_CLOSED: + return MQTT_WSS_ERR_CONN_DROP; + } + +#ifdef MQTT_WSS_CPUSTATS + t2 = mqtt_wss_now_usec(client); + client->stats.time_process_websocket += t2 - t1; +#endif + + // process MQTT stuff + if(client->ws_client->state == WS_ESTABLISHED) + if (handle_mqtt_internal(client)) + return MQTT_WSS_ERR_PROTO_MQTT; + + if (client->mqtt_didnt_finish_write) { + client->mqtt_didnt_finish_write = 0; + client->poll_fds[POLLFD_SOCKET].events |= POLLOUT; + } + +#ifdef MQTT_WSS_CPUSTATS + t1 = mqtt_wss_now_usec(client); + client->stats.time_process_mqtt += t1 - t2; +#endif + + if ((ptr = rbuf_get_linear_read_range(client->ws_client->buf_write, &size))) { +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Have data to write to SSL"); +#endif + if ((ret = SSL_write(client->ssl, ptr, size)) > 0) { +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "SSL_Write: Written %d of avail %d.", ret, size); +#endif + pthread_mutex_lock(&client->stat_lock); + client->stats.bytes_tx += ret; + pthread_mutex_unlock(&client->stat_lock); + rbuf_bump_tail(client->ws_client->buf_write, ret); + } else { + int errnobkp = errno; + ret = SSL_get_error(client->ssl, ret); +#ifdef DEBUG_ULTRA_VERBOSE + mws_debug(client->log, "Write Err: %s", util_openssl_ret_err(ret)); +#endif + set_socket_pollfds(client, ret); + if (ret != SSL_ERROR_WANT_READ && + ret != SSL_ERROR_WANT_WRITE) { + mws_error(client->log, "SSL_write error: %d %s", ret, util_openssl_ret_err(ret)); + if (ret == SSL_ERROR_SYSCALL) + mws_error(client->log, "SSL_write SYSCALL errno: %d %s", errnobkp, strerror(errnobkp)); + return MQTT_WSS_ERR_CONN_DROP; + } + } + } + + if(client->poll_fds[POLLFD_PIPE].revents & POLLIN) + util_clear_pipe(client->write_notif_pipe[PIPE_READ_END]); + +#ifdef MQTT_WSS_CPUSTATS + t2 = mqtt_wss_now_usec(client); + client->stats.time_write_socket += t2 - t1; +#endif + + return MQTT_WSS_OK; +} + +int mqtt_wss_publish5(mqtt_wss_client client, + char *topic, + free_fnc_t topic_free, + void *msg, + free_fnc_t msg_free, + size_t msg_len, + uint8_t publish_flags, + uint16_t *packet_id) +{ + if (client->mqtt_disconnecting) { + mws_error(client->log, "mqtt_wss is disconnecting can't publish"); + return 1; + } + + if (!client->mqtt_connected) { + mws_error(client->log, "MQTT is offline. Can't send message."); + return 1; + } + uint8_t mqtt_flags = 0; + + mqtt_flags = (publish_flags & MQTT_WSS_PUB_QOSMASK) << 1; + if (publish_flags & MQTT_WSS_PUB_RETAIN) + mqtt_flags |= MQTT_PUBLISH_RETAIN; + + int rc = mqtt_ng_publish(client->mqtt, topic, topic_free, msg, msg_free, msg_len, mqtt_flags, packet_id); + if (rc == MQTT_NG_MSGGEN_MSG_TOO_BIG) + return MQTT_WSS_ERR_TOO_BIG_FOR_SERVER; + + mqtt_wss_wakeup(client); + + return rc; +} + +int mqtt_wss_subscribe(mqtt_wss_client client, char *topic, int max_qos_level) +{ + (void)max_qos_level; //TODO now hardcoded + if (!client->mqtt_connected) { + mws_error(client->log, "MQTT is offline. Can't subscribe."); + return 1; + } + + if (client->mqtt_disconnecting) { + mws_error(client->log, "mqtt_wss is disconnecting can't subscribe"); + return 1; + } + + struct mqtt_sub sub = { + .topic = topic, + .topic_free = NULL, + .options = /* max_qos_level & 0x3 TODO when QOS > 1 implemented */ 0x01 | (0x01 << 3) + }; + mqtt_ng_subscribe(client->mqtt, &sub, 1); + + mqtt_wss_wakeup(client); + return 0; +} + +struct mqtt_wss_stats mqtt_wss_get_stats(mqtt_wss_client client) +{ + struct mqtt_wss_stats current; + pthread_mutex_lock(&client->stat_lock); + current = client->stats; + memset(&client->stats, 0, sizeof(client->stats)); + pthread_mutex_unlock(&client->stat_lock); + mqtt_ng_get_stats(client->mqtt, ¤t.mqtt); + return current; +} + +int mqtt_wss_set_topic_alias(mqtt_wss_client client, const char *topic) +{ + return mqtt_ng_set_topic_alias(client->mqtt, topic); +} + +#ifdef MQTT_WSS_DEBUG +void mqtt_wss_set_SSL_CTX_keylog_cb(mqtt_wss_client client, void (*ssl_ctx_keylog_cb)(const SSL *ssl, const char *line)) +{ + client->ssl_ctx_keylog_cb = ssl_ctx_keylog_cb; +} +#endif diff --git a/src/aclk/mqtt_websockets/mqtt_wss_client.h b/src/aclk/mqtt_websockets/mqtt_wss_client.h new file mode 100644 index 000000000..4bdea4db9 --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_wss_client.h @@ -0,0 +1,162 @@ +// SPDX-License-Identifier: GPL-3.0-only +// Copyright (C) 2020 Timotej Šiškovič + +#ifndef MQTT_WSS_CLIENT_H +#define MQTT_WSS_CLIENT_H + +#include <stdint.h> +#include <stddef.h> //size_t + +#include "mqtt_wss_log.h" +#include "common_public.h" + +// All OK call me at your earliest convinience +#define MQTT_WSS_OK 0 +/* All OK, poll timeout you requested when calling mqtt_wss_service expired - you might want to know if timeout + * happened or we got some data or handle same as MQTT_WSS_OK + */ +#define MQTT_WSS_OK_TO 1 +// Connection was closed by remote +#define MQTT_WSS_ERR_CONN_DROP -1 +// Error in MQTT protocol (e.g. malformed packet) +#define MQTT_WSS_ERR_PROTO_MQTT -2 +// Error in WebSocket protocol (e.g. malformed packet) +#define MQTT_WSS_ERR_PROTO_WS -3 + +#define MQTT_WSS_ERR_TX_BUF_TOO_SMALL -4 +#define MQTT_WSS_ERR_RX_BUF_TOO_SMALL -5 + +#define MQTT_WSS_ERR_TOO_BIG_FOR_SERVER -6 +// if client was initialized with MQTT 3 but MQTT 5 feature +// was requested by user of library +#define MQTT_WSS_ERR_CANT_DO -8 + +typedef struct mqtt_wss_client_struct *mqtt_wss_client; + +typedef void (*msg_callback_fnc_t)(const char *topic, const void *msg, size_t msglen, int qos); +/* Creates new instance of MQTT over WSS. Doesn't start connection. + * @param log_prefix this is prefix to be used when logging to discern between multiple + * mqtt_wss instances. Can be NULL. + * @param log_callback is function pointer to fnc to be called when mqtt_wss wants + * to log. This allows plugging this library into your own logging system/solution. + * If NULL STDOUT/STDERR will be used. + * @param msg_callback is function pointer to function which will be called + * when application level message arrives from broker (for subscribed topics). + * Can be NULL if you are not interested about incoming messages. + * @param puback_callback is function pointer to function to be called when QOS1 Publish + * is acknowledged by server + */ +mqtt_wss_client mqtt_wss_new(const char *log_prefix, + mqtt_wss_log_callback_t log_callback, + msg_callback_fnc_t msg_callback, + void (*puback_callback)(uint16_t packet_id)); + +void mqtt_wss_set_max_buf_size(mqtt_wss_client client, size_t size); + +void mqtt_wss_destroy(mqtt_wss_client client); + +struct mqtt_connect_params; +struct mqtt_wss_proxy; + +#define MQTT_WSS_SSL_CERT_CHECK_FULL 0x00 +#define MQTT_WSS_SSL_ALLOW_SELF_SIGNED 0x01 +#define MQTT_WSS_SSL_DONT_CHECK_CERTS 0x08 + +/* Will block until the MQTT over WSS connection is established or return error + * @param client mqtt_wss_client which should connect + * @param host to connect to (where MQTT over WSS server is listening) + * @param port to connect to (where MQTT over WSS server is listening) + * @param mqtt_params pointer to mqtt_connect_params structure which contains MQTT credentials and settings + * @param ssl_flags parameters for OpenSSL, 0=MQTT_WSS_SSL_CERT_CHECK_FULL + */ +int mqtt_wss_connect(mqtt_wss_client client, char *host, int port, struct mqtt_connect_params *mqtt_params, int ssl_flags, struct mqtt_wss_proxy *proxy); +int mqtt_wss_service(mqtt_wss_client client, int timeout_ms); +void mqtt_wss_disconnect(mqtt_wss_client client, int timeout_ms); + +// we redefine this instead of using MQTT-C flags as in future +// we want to support different MQTT implementations if needed +enum mqtt_wss_publish_flags { + MQTT_WSS_PUB_QOS0 = 0x0, + MQTT_WSS_PUB_QOS1 = 0x1, + MQTT_WSS_PUB_QOS2 = 0x2, + MQTT_WSS_PUB_QOSMASK = 0x3, + MQTT_WSS_PUB_RETAIN = 0x4 +}; + +struct mqtt_connect_params { + const char *clientid; + const char *username; + const char *password; + const char *will_topic; + const void *will_msg; + enum mqtt_wss_publish_flags will_flags; + size_t will_msg_len; + int keep_alive; + int drop_on_publish_fail; +}; + +enum mqtt_wss_proxy_type { + MQTT_WSS_DIRECT = 0, + MQTT_WSS_PROXY_HTTP +}; + +struct mqtt_wss_proxy { + enum mqtt_wss_proxy_type type; + const char *host; + int port; + const char *username; + const char *password; +}; + +/* TODO!!! update the description + * Publishes MQTT message and gets message id + * @param client mqtt_wss_client which should transfer the message + * @param topic MQTT topic to publish message to (0 terminated C string) + * @param msg Message to be published (no need for 0 termination) + * @param msg_len Length of the message to be published + * @param publish_flags see enum mqtt_wss_publish_flags e.g. (MQTT_WSS_PUB_QOS1 | MQTT_WSS_PUB_RETAIN) + * @param packet_id is 16 bit unsigned int representing ID that can be used to pair with PUBACK callback + * for usages where application layer wants to know which messages are delivered when + * @return Returns 0 on success + */ +int mqtt_wss_publish5(mqtt_wss_client client, + char *topic, + free_fnc_t topic_free, + void *msg, + free_fnc_t msg_free, + size_t msg_len, + uint8_t publish_flags, + uint16_t *packet_id); + +int mqtt_wss_set_topic_alias(mqtt_wss_client client, const char *topic); + +/* Subscribes to MQTT topic + * @param client mqtt_wss_client which should do the subscription + * @param topic MQTT topic to subscribe to + * @param max_qos_level maximum QOS level that broker can send to us on this subscription + * @return Returns 0 on success + */ +int mqtt_wss_subscribe(mqtt_wss_client client, char *topic, int max_qos_level); + + +struct mqtt_wss_stats { + uint64_t bytes_tx; + uint64_t bytes_rx; +#ifdef MQTT_WSS_CPUSTATS + uint64_t time_keepalive; + uint64_t time_read_socket; + uint64_t time_write_socket; + uint64_t time_process_websocket; + uint64_t time_process_mqtt; +#endif + struct mqtt_ng_stats mqtt; +}; + +struct mqtt_wss_stats mqtt_wss_get_stats(mqtt_wss_client client); + +#ifdef MQTT_WSS_DEBUG +#include <openssl/ssl.h> +void mqtt_wss_set_SSL_CTX_keylog_cb(mqtt_wss_client client, void (*ssl_ctx_keylog_cb)(const SSL *ssl, const char *line)); +#endif + +#endif /* MQTT_WSS_CLIENT_H */ diff --git a/src/aclk/mqtt_websockets/mqtt_wss_log.c b/src/aclk/mqtt_websockets/mqtt_wss_log.c new file mode 100644 index 000000000..5e606c12b --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_wss_log.c @@ -0,0 +1,130 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#include <stdlib.h> +#include <stdarg.h> +#include <string.h> +#include <stdio.h> + +#include "mqtt_wss_log.h" +#include "common_internal.h" + +struct mqtt_wss_log_ctx { + mqtt_wss_log_callback_t extern_log_fnc; + char *ctx_prefix; + char *buffer; + char *buffer_w_ptr; + size_t buffer_bytes_avail; +}; + +#define LOG_BUFFER_SIZE 1024 * 4 +#define LOG_CTX_PREFIX_SEV_STR " : " +#define LOG_CTX_PREFIX_LIMIT 15 +#define LOG_CTX_PREFIX_LIMIT_STR (LOG_CTX_PREFIX_LIMIT - (2 + strlen(LOG_CTX_PREFIX_SEV_STR))) // with [] characters and affixed ' ' it is total 15 chars +#if (LOG_CTX_PREFIX_LIMIT * 10) > LOG_BUFFER_SIZE +#error "LOG_BUFFER_SIZE too small" +#endif +mqtt_wss_log_ctx_t mqtt_wss_log_ctx_create(const char *ctx_prefix, mqtt_wss_log_callback_t log_callback) +{ + mqtt_wss_log_ctx_t ctx = mw_calloc(1, sizeof(struct mqtt_wss_log_ctx)); + if(!ctx) + return NULL; + + if(log_callback) { + ctx->extern_log_fnc = log_callback; + ctx->buffer = mw_calloc(1, LOG_BUFFER_SIZE); + if(!ctx->buffer) + goto cleanup; + + ctx->buffer_w_ptr = ctx->buffer; + if(ctx_prefix) { + *(ctx->buffer_w_ptr++) = '['; + strncpy(ctx->buffer_w_ptr, ctx_prefix, LOG_CTX_PREFIX_LIMIT_STR); + ctx->buffer_w_ptr += strnlen(ctx_prefix, LOG_CTX_PREFIX_LIMIT_STR); + *(ctx->buffer_w_ptr++) = ']'; + } + strcpy(ctx->buffer_w_ptr, LOG_CTX_PREFIX_SEV_STR); + ctx->buffer_w_ptr += strlen(LOG_CTX_PREFIX_SEV_STR); + // no term '\0' -> calloc is used + + ctx->buffer_bytes_avail = LOG_BUFFER_SIZE - strlen(ctx->buffer); + + return ctx; + } + + if(ctx_prefix) { + ctx->ctx_prefix = strndup(ctx_prefix, LOG_CTX_PREFIX_LIMIT_STR); + if(!ctx->ctx_prefix) + goto cleanup; + } + + return ctx; + +cleanup: + mw_free(ctx); + return NULL; +} + +void mqtt_wss_log_ctx_destroy(mqtt_wss_log_ctx_t ctx) +{ + mw_free(ctx->ctx_prefix); + mw_free(ctx->buffer); + mw_free(ctx); +} + +static inline char severity_to_c(int severity) +{ + switch (severity) { + case MQTT_WSS_LOG_FATAL: + return 'F'; + case MQTT_WSS_LOG_ERROR: + return 'E'; + case MQTT_WSS_LOG_WARN: + return 'W'; + case MQTT_WSS_LOG_INFO: + return 'I'; + case MQTT_WSS_LOG_DEBUG: + return 'D'; + default: + return '?'; + } +} + +void mws_log(int severity, mqtt_wss_log_ctx_t ctx, const char *fmt, va_list args) +{ + size_t size; + + if(ctx->extern_log_fnc) { + size = vsnprintf(ctx->buffer_w_ptr, ctx->buffer_bytes_avail, fmt, args); + *(ctx->buffer_w_ptr - 3) = severity_to_c(severity); + + ctx->extern_log_fnc(severity, ctx->buffer); + + if(size >= ctx->buffer_bytes_avail) + mws_error(ctx, "Last message of this type was truncated! Consider what you log or increase LOG_BUFFER_SIZE if really needed."); + + return; + } + + if(ctx->ctx_prefix) + printf("[%s] ", ctx->ctx_prefix); + + printf("%c: ", severity_to_c(severity)); + + vprintf(fmt, args); + putchar('\n'); +} + +#define DEFINE_MWS_SEV_FNC(severity_fncname, severity) \ +void mws_ ## severity_fncname(mqtt_wss_log_ctx_t ctx, const char *fmt, ...) \ +{ \ + va_list args; \ + va_start(args, fmt); \ + mws_log(severity, ctx, fmt, args); \ + va_end(args); \ +} + +DEFINE_MWS_SEV_FNC(fatal, MQTT_WSS_LOG_FATAL) +DEFINE_MWS_SEV_FNC(error, MQTT_WSS_LOG_ERROR) +DEFINE_MWS_SEV_FNC(warn, MQTT_WSS_LOG_WARN ) +DEFINE_MWS_SEV_FNC(info, MQTT_WSS_LOG_INFO ) +DEFINE_MWS_SEV_FNC(debug, MQTT_WSS_LOG_DEBUG) diff --git a/src/aclk/mqtt_websockets/mqtt_wss_log.h b/src/aclk/mqtt_websockets/mqtt_wss_log.h new file mode 100644 index 000000000..6ae60d870 --- /dev/null +++ b/src/aclk/mqtt_websockets/mqtt_wss_log.h @@ -0,0 +1,39 @@ +// Copyright: SPDX-License-Identifier: GPL-3.0-only + +#ifndef MQTT_WSS_LOG_H +#define MQTT_WSS_LOG_H + +typedef enum mqtt_wss_log_type { + MQTT_WSS_LOG_DEBUG = 0x01, + MQTT_WSS_LOG_INFO = 0x02, + MQTT_WSS_LOG_WARN = 0x03, + MQTT_WSS_LOG_ERROR = 0x81, + MQTT_WSS_LOG_FATAL = 0x88 +} mqtt_wss_log_type_t; + +typedef void (*mqtt_wss_log_callback_t)(mqtt_wss_log_type_t, const char*); + +typedef struct mqtt_wss_log_ctx *mqtt_wss_log_ctx_t; + +/** Creates logging context with optional prefix and optional callback + * @param ctx_prefix String to be prefixed to every log message. + * This is useful if multiple clients are instantiated to be able to + * know which one this message belongs to. Can be `NULL` for no prefix. + * @param log_callback Callback to be called instead of logging to + * `STDOUT` or `STDERR` (if debug enabled otherwise silent). Callback has to be + * pointer to function of `void function(mqtt_wss_log_type_t, const char*)` type. + * If `NULL` default will be used (silent or STDERR/STDOUT). + * @return mqtt_wss_log_ctx_t or `NULL` on error */ +mqtt_wss_log_ctx_t mqtt_wss_log_ctx_create(const char *ctx_prefix, mqtt_wss_log_callback_t log_callback); + +/** Destroys logging context and cleans up the memory + * @param ctx Context to destroy */ +void mqtt_wss_log_ctx_destroy(mqtt_wss_log_ctx_t ctx); + +void mws_fatal(mqtt_wss_log_ctx_t ctx, const char *fmt, ...); +void mws_error(mqtt_wss_log_ctx_t ctx, const char *fmt, ...); +void mws_warn (mqtt_wss_log_ctx_t ctx, const char *fmt, ...); +void mws_info (mqtt_wss_log_ctx_t ctx, const char *fmt, ...); +void mws_debug(mqtt_wss_log_ctx_t ctx, const char *fmt, ...); + +#endif /* MQTT_WSS_LOG_H */ diff --git a/src/aclk/mqtt_websockets/test.c b/src/aclk/mqtt_websockets/test.c new file mode 100644 index 000000000..59a9f3474 --- /dev/null +++ b/src/aclk/mqtt_websockets/test.c @@ -0,0 +1,90 @@ +// SPDX-License-Identifier: GPL-3.0-only +// Copyright (C) 2020 Timotej Šiškovič + +#include <unistd.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> + +#include "mqtt_wss_client.h" + +int test_exit = 0; +int port = 0; + +void mqtt_wss_log_cb(mqtt_wss_log_type_t log_type, const char* str) +{ + (void)log_type; + puts(str); +} + +#define TEST_MSGLEN_MAX 512 +void msg_callback(const char *topic, const void *msg, size_t msglen, int qos) +{ + char cmsg[TEST_MSGLEN_MAX]; + size_t len = (msglen < TEST_MSGLEN_MAX - 1) ? msglen : (TEST_MSGLEN_MAX - 1); + memcpy(cmsg, + msg, + len); + cmsg[len] = 0; + + if (!strcmp(cmsg, "shutdown")) + test_exit = 1; + + printf("Got Message From Broker Topic \"%s\" QOS %d MSG: \"%s\"\n", topic, qos, cmsg); +} + +#define TESTMSG "Hello World!" +int client_handle(mqtt_wss_client client) +{ + struct mqtt_connect_params params = { + .clientid = "test", + .username = "anon", + .password = "anon", + .keep_alive = 10 + }; + +/* struct mqtt_wss_proxy proxy = { + .host = "127.0.0.1", + .port = 3128, + .type = MQTT_WSS_PROXY_HTTP + };*/ + + while (mqtt_wss_connect(client, "127.0.0.1", port, ¶ms, MQTT_WSS_SSL_ALLOW_SELF_SIGNED, NULL /*&proxy*/)) { + printf("Connect failed\n"); + sleep(1); + printf("Attempting Reconnect\n"); + } + printf("Connection succeeded\n"); + + mqtt_wss_subscribe(client, "test", 1); + + while (!test_exit) { + if(mqtt_wss_service(client, -1) < 0) + return 1; + } + return 0; +} + +int main(int argc, char **argv) +{ + if (argc >= 2) + port = atoi(argv[1]); + if (!port) + port = 9002; + printf("Using port %d\n", port); + + mqtt_wss_client client = mqtt_wss_new("main", mqtt_wss_log_cb, msg_callback, NULL); + if (!client) { + printf("Couldn't initialize mqtt_wss\n"); + return 1; + } + while (!test_exit) { + printf("client_handle = %d\n", client_handle(client)); + } + if (test_exit) { + mqtt_wss_disconnect(client, 2000); + } + + mqtt_wss_destroy(client); + return 0; +} diff --git a/src/aclk/mqtt_websockets/ws_client.c b/src/aclk/mqtt_websockets/ws_client.c new file mode 100644 index 000000000..240e889ca --- /dev/null +++ b/src/aclk/mqtt_websockets/ws_client.c @@ -0,0 +1,744 @@ +// Copyright (C) 2020 Timotej Šiškovič +// SPDX-License-Identifier: GPL-3.0-only +// +// This program 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, version 3. +// +// 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 <fcntl.h> +#include <unistd.h> +#include <string.h> +#include <errno.h> +#include <ctype.h> + +#include <openssl/evp.h> + +#include "ws_client.h" +#include "common_internal.h" + +#ifdef MQTT_WEBSOCKETS_DEBUG +#include "../c-rbuf/src/ringbuffer_internal.h" +#endif + +#define UNIT_LOG_PREFIX "ws_client: " +#define FATAL(fmt, ...) mws_fatal(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define ERROR(fmt, ...) mws_error(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define WARN(fmt, ...) mws_warn (client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define INFO(fmt, ...) mws_info (client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) +#define DEBUG(fmt, ...) mws_debug(client->log, UNIT_LOG_PREFIX fmt, ##__VA_ARGS__) + +const char *websocket_upgrage_hdr = "GET /mqtt HTTP/1.1\x0D\x0A" + "Host: %s\x0D\x0A" + "Upgrade: websocket\x0D\x0A" + "Connection: Upgrade\x0D\x0A" + "Sec-WebSocket-Key: %s\x0D\x0A" + "Origin: http://example.com\x0D\x0A" + "Sec-WebSocket-Protocol: mqtt\x0D\x0A" + "Sec-WebSocket-Version: 13\x0D\x0A\x0D\x0A"; + +const char *mqtt_protoid = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"; + +#define DEFAULT_RINGBUFFER_SIZE (1024*128) +#define ENTROPY_SOURCE "/dev/urandom" +ws_client *ws_client_new(size_t buf_size, char **host, mqtt_wss_log_ctx_t log) +{ + ws_client *client; + + if(!host) + return NULL; + + client = mw_calloc(1, sizeof(ws_client)); + if (!client) + return NULL; + + client->host = host; + client->log = log; + + client->buf_read = rbuf_create(buf_size ? buf_size : DEFAULT_RINGBUFFER_SIZE); + if (!client->buf_read) + goto cleanup; + + client->buf_write = rbuf_create(buf_size ? buf_size : DEFAULT_RINGBUFFER_SIZE); + if (!client->buf_write) + goto cleanup_1; + + client->buf_to_mqtt = rbuf_create(buf_size ? buf_size : DEFAULT_RINGBUFFER_SIZE); + if (!client->buf_to_mqtt) + goto cleanup_2; + + client->entropy_fd = open(ENTROPY_SOURCE, O_RDONLY | O_CLOEXEC); + if (client->entropy_fd < 1) { + ERROR("Error opening entropy source \"" ENTROPY_SOURCE "\". Reason: \"%s\"", strerror(errno)); + goto cleanup_3; + } + + return client; + +cleanup_3: + rbuf_free(client->buf_to_mqtt); +cleanup_2: + rbuf_free(client->buf_write); +cleanup_1: + rbuf_free(client->buf_read); +cleanup: + mw_free(client); + return NULL; +} + +void ws_client_free_headers(ws_client *client) +{ + struct http_header *ptr = client->hs.headers; + struct http_header *tmp; + + while (ptr) { + tmp = ptr; + ptr = ptr->next; + mw_free(tmp); + } + + client->hs.headers = NULL; + client->hs.headers_tail = NULL; + client->hs.hdr_count = 0; +} + +void ws_client_destroy(ws_client *client) +{ + ws_client_free_headers(client); + mw_free(client->hs.nonce_reply); + mw_free(client->hs.http_reply_msg); + close(client->entropy_fd); + rbuf_free(client->buf_read); + rbuf_free(client->buf_write); + rbuf_free(client->buf_to_mqtt); + mw_free(client); +} + +void ws_client_reset(ws_client *client) +{ + ws_client_free_headers(client); + mw_free(client->hs.nonce_reply); + client->hs.nonce_reply = NULL; + mw_free(client->hs.http_reply_msg); + client->hs.http_reply_msg = NULL; + rbuf_flush(client->buf_read); + rbuf_flush(client->buf_write); + rbuf_flush(client->buf_to_mqtt); + client->state = WS_RAW; + client->hs.hdr_state = WS_HDR_HTTP; + client->rx.parse_state = WS_FIRST_2BYTES; +} + +#define MAX_HTTP_HDR_COUNT 128 +int ws_client_add_http_header(ws_client *client, struct http_header *hdr) +{ + if (client->hs.hdr_count > MAX_HTTP_HDR_COUNT) { + ERROR("Too many HTTP response header fields"); + return -1; + } + + if (client->hs.headers) + client->hs.headers_tail->next = hdr; + else + client->hs.headers = hdr; + + client->hs.headers_tail = hdr; + client->hs.hdr_count++; + + return 0; +} + +int ws_client_want_write(ws_client *client) +{ + return rbuf_bytes_available(client->buf_write); +} + +#define RAND_SRC "/dev/urandom" +static int ws_client_get_nonce(ws_client *client, char *dest, unsigned int size) +{ + // we do not need crypto secure random here + // it's just used for protocol negotiation + int rd; + int f = open(RAND_SRC, O_RDONLY | O_CLOEXEC); + if (f < 0) { + ERROR("Error opening \"%s\". Err: \"%s\"", RAND_SRC, strerror(errno)); + return -2; + } + + if ((rd = read(f, dest, size)) > 0) { + close(f); + return rd; + } + close(f); + return -1; +} + +#define WEBSOCKET_NONCE_SIZE 16 +#define TEMP_BUF_SIZE 4096 +int ws_client_start_handshake(ws_client *client) +{ + char nonce[WEBSOCKET_NONCE_SIZE]; + char nonce_b64[256]; + char second[TEMP_BUF_SIZE]; + unsigned int md_len; + unsigned char *digest; + EVP_MD_CTX *md_ctx; + const EVP_MD *md; + + if(!*client->host) { + ERROR("Hostname has not been set. We should not be able to come here!"); + return 1; + } + + ws_client_get_nonce(client, nonce, WEBSOCKET_NONCE_SIZE); + EVP_EncodeBlock((unsigned char *)nonce_b64, (const unsigned char *)nonce, WEBSOCKET_NONCE_SIZE); + snprintf(second, TEMP_BUF_SIZE, websocket_upgrage_hdr, + *client->host, + nonce_b64); + if(rbuf_bytes_free(client->buf_write) < strlen(second)) { + ERROR("Write buffer capacity too low."); + return 1; + } + + rbuf_push(client->buf_write, second, strlen(second)); + client->state = WS_HANDSHAKE; + + //Calculating expected Sec-WebSocket-Accept reply + snprintf(second, TEMP_BUF_SIZE, "%s%s", nonce_b64, mqtt_protoid); + +#if (OPENSSL_VERSION_NUMBER < OPENSSL_VERSION_110) + md_ctx = EVP_MD_CTX_create(); +#else + md_ctx = EVP_MD_CTX_new(); +#endif + if (md_ctx == NULL) { + ERROR("Cant create EVP_MD Context"); + return 1; + } + + md = EVP_get_digestbyname("sha1"); + if (!md) { + ERROR("Unknown message digest"); + return 1; + } + + if ((digest = (unsigned char *)OPENSSL_malloc(EVP_MD_size(EVP_sha256()))) == NULL) { + ERROR("Cant alloc digest"); + return 1; + } + + EVP_DigestInit_ex(md_ctx, md, NULL); + EVP_DigestUpdate(md_ctx, second, strlen(second)); + EVP_DigestFinal_ex(md_ctx, digest, &md_len); + + EVP_EncodeBlock((unsigned char *)nonce_b64, digest, md_len); + + mw_free(client->hs.nonce_reply); + client->hs.nonce_reply = mw_strdup(nonce_b64); + + OPENSSL_free(digest); + +#if (OPENSSL_VERSION_NUMBER < OPENSSL_VERSION_110) + EVP_MD_CTX_destroy(md_ctx); +#else + EVP_MD_CTX_free(md_ctx); +#endif + + return 0; +} + +#define BUF_READ_MEMCMP_CONST(const, err) \ + if (rbuf_memcmp_n(client->buf_read, const, strlen(const))) { \ + ERROR(err); \ + rbuf_flush(client->buf_read); \ + return WS_CLIENT_PROTOCOL_ERROR; \ + } + +#define BUF_READ_CHECK_AT_LEAST(x) \ + if (rbuf_bytes_available(client->buf_read) < x) \ + return WS_CLIENT_NEED_MORE_BYTES; + +#define MAX_HTTP_LINE_LENGTH 1024*4 +#define HTTP_SC_LENGTH 4 // "XXX " http status code as C string +#define WS_CLIENT_HTTP_HDR "HTTP/1.1 " +#define WS_CONN_ACCEPT "sec-websocket-accept" +#define HTTP_HDR_SEPARATOR ": " +#define WS_NONCE_STRLEN_B64 28 +#define WS_HTTP_NEWLINE "\r\n" +#define HTTP_HEADER_NAME_MAX_LEN 256 +#if HTTP_HEADER_NAME_MAX_LEN > MAX_HTTP_LINE_LENGTH +#error "Buffer too small" +#endif +#if WS_NONCE_STRLEN_B64 > MAX_HTTP_LINE_LENGTH +#error "Buffer too small" +#endif + +#define HTTP_HDR_LINE_CHECK_LIMIT(x) if ((x) >= MAX_HTTP_LINE_LENGTH) \ +{ \ + ERROR("HTTP line received is too long. Maximum is %d", MAX_HTTP_LINE_LENGTH); \ + return WS_CLIENT_PROTOCOL_ERROR; \ +} + +int ws_client_parse_handshake_resp(ws_client *client) +{ + char buf[HTTP_SC_LENGTH]; + int idx_crlf, idx_sep; + char *ptr; + size_t bytes; + switch (client->hs.hdr_state) { + case WS_HDR_HTTP: + BUF_READ_CHECK_AT_LEAST(strlen(WS_CLIENT_HTTP_HDR)) + BUF_READ_MEMCMP_CONST(WS_CLIENT_HTTP_HDR, "Expected \"HTTP1.1\" header"); + rbuf_bump_tail(client->buf_read, strlen(WS_CLIENT_HTTP_HDR)); + client->hs.hdr_state = WS_HDR_RC; + break; + case WS_HDR_RC: + BUF_READ_CHECK_AT_LEAST(HTTP_SC_LENGTH); // "XXX " http return code + rbuf_pop(client->buf_read, buf, HTTP_SC_LENGTH); + if (buf[HTTP_SC_LENGTH - 1] != 0x20) { + ERROR("HTTP status code received is not terminated by space (0x20)"); + return WS_CLIENT_PROTOCOL_ERROR; + } + buf[HTTP_SC_LENGTH - 1] = 0; + client->hs.http_code = atoi(buf); + if (client->hs.http_code < 100 || client->hs.http_code >= 600) { + ERROR("HTTP status code received not in valid range 100-600"); + return WS_CLIENT_PROTOCOL_ERROR; + } + client->hs.hdr_state = WS_HDR_ENDLINE; + break; + case WS_HDR_ENDLINE: + ptr = rbuf_find_bytes(client->buf_read, WS_HTTP_NEWLINE, strlen(WS_HTTP_NEWLINE), &idx_crlf); + if (!ptr) { + bytes = rbuf_bytes_available(client->buf_read); + HTTP_HDR_LINE_CHECK_LIMIT(bytes); + return WS_CLIENT_NEED_MORE_BYTES; + } + HTTP_HDR_LINE_CHECK_LIMIT(idx_crlf); + + client->hs.http_reply_msg = mw_malloc(idx_crlf+1); + rbuf_pop(client->buf_read, client->hs.http_reply_msg, idx_crlf); + client->hs.http_reply_msg[idx_crlf] = 0; + rbuf_bump_tail(client->buf_read, strlen(WS_HTTP_NEWLINE)); + client->hs.hdr_state = WS_HDR_PARSE_HEADERS; + break; + case WS_HDR_PARSE_HEADERS: + ptr = rbuf_find_bytes(client->buf_read, WS_HTTP_NEWLINE, strlen(WS_HTTP_NEWLINE), &idx_crlf); + if (!ptr) { + bytes = rbuf_bytes_available(client->buf_read); + HTTP_HDR_LINE_CHECK_LIMIT(bytes); + return WS_CLIENT_NEED_MORE_BYTES; + } + HTTP_HDR_LINE_CHECK_LIMIT(idx_crlf); + + if (!idx_crlf) { // empty line, header end + rbuf_bump_tail(client->buf_read, strlen(WS_HTTP_NEWLINE)); + client->hs.hdr_state = WS_HDR_PARSE_DONE; + return 0; + } + + ptr = rbuf_find_bytes(client->buf_read, HTTP_HDR_SEPARATOR, strlen(HTTP_HDR_SEPARATOR), &idx_sep); + if (!ptr || idx_sep > idx_crlf) { + ERROR("Expected HTTP hdr field key/value separator \": \" before endline in non empty HTTP header line"); + return WS_CLIENT_PROTOCOL_ERROR; + } + if (idx_crlf == idx_sep + (int)strlen(HTTP_HDR_SEPARATOR)) { + ERROR("HTTP Header value cannot be empty"); + return WS_CLIENT_PROTOCOL_ERROR; + } + + if (idx_sep > HTTP_HEADER_NAME_MAX_LEN) { + ERROR("HTTP header too long (%d)", idx_sep); + return WS_CLIENT_PROTOCOL_ERROR; + } + + struct http_header *hdr = mw_calloc(1, sizeof(struct http_header) + idx_crlf); //idx_crlf includes ": " that will be used as 2 \0 bytes + hdr->key = ((char*)hdr) + sizeof(struct http_header); + hdr->value = hdr->key + idx_sep + 1; + + bytes = rbuf_pop(client->buf_read, hdr->key, idx_sep); + rbuf_bump_tail(client->buf_read, strlen(HTTP_HDR_SEPARATOR)); + + bytes = rbuf_pop(client->buf_read, hdr->value, idx_crlf - idx_sep - strlen(HTTP_HDR_SEPARATOR)); + rbuf_bump_tail(client->buf_read, strlen(WS_HTTP_NEWLINE)); + + for (int i = 0; hdr->key[i]; i++) + hdr->key[i] = tolower(hdr->key[i]); + +// DEBUG("HTTP header \"%s\" received. Value \"%s\"", hdr->key, hdr->value); + + if (ws_client_add_http_header(client, hdr)) + return WS_CLIENT_PROTOCOL_ERROR; + + if (!strcmp(hdr->key, WS_CONN_ACCEPT)) { + if (strcmp(client->hs.nonce_reply, hdr->value)) { + ERROR("Received NONCE \"%s\" does not match expected nonce of \"%s\"", hdr->value, client->hs.nonce_reply); + return WS_CLIENT_PROTOCOL_ERROR; + } + client->hs.nonce_matched = 1; + } + + break; + case WS_HDR_PARSE_DONE: + if (!client->hs.nonce_matched) { + ERROR("Missing " WS_CONN_ACCEPT " header"); + return WS_CLIENT_PROTOCOL_ERROR; + } + if (client->hs.http_code != 101) { + ERROR("HTTP return code not 101. Received %d with msg \"%s\".", client->hs.http_code, client->hs.http_reply_msg); + return WS_CLIENT_PROTOCOL_ERROR; + } + + client->state = WS_ESTABLISHED; + client->hs.hdr_state = WS_HDR_ALL_DONE; + INFO("Websocket Connection Accepted By Server"); + return WS_CLIENT_PARSING_DONE; + case WS_HDR_ALL_DONE: + FATAL("This is error we should never come here!"); + return WS_CLIENT_PROTOCOL_ERROR; + } + return 0; +} + +#define BYTE_MSB 0x80 +#define WS_FINAL_FRAG BYTE_MSB +#define WS_PAYLOAD_MASKED BYTE_MSB + +static inline size_t get_ws_hdr_size(size_t payload_size) +{ + size_t hdr_len = 2 + 4 /*mask*/; + if(payload_size > 125) + hdr_len += 2; + if(payload_size > 65535) + hdr_len += 6; + return hdr_len; +} + +#define MAX_POSSIBLE_HDR_LEN 14 +int ws_client_send(ws_client *client, enum websocket_opcode frame_type, const char *data, size_t size) +{ + // TODO maybe? implement fragmenting, it is not necessary though + // as both tested MQTT brokers have no reuirement of one MQTT envelope + // be equal to one WebSockets envelope. Therefore there is no need to send + // one big MQTT message as single fragmented WebSocket envelope + char hdr[MAX_POSSIBLE_HDR_LEN]; + char *ptr = hdr; + char *mask; + int size_written = 0; + size_t j = 0; + + size_t w_buff_free = rbuf_bytes_free(client->buf_write); + size_t hdr_len = get_ws_hdr_size(size); + + if (w_buff_free < hdr_len * 2) { +#ifdef DEBUG_ULTRA_VERBOSE + DEBUG("Write buffer full. Can't write requested %d size.", size); +#endif + return 0; + } + + if (w_buff_free < (hdr_len + size)) { +#ifdef DEBUG_ULTRA_VERBOSE + DEBUG("Can't write whole MQTT packet of %d bytes into the buffer. Will do partial send of %d.", size, w_buff_free - hdr_len); +#endif + size = w_buff_free - hdr_len; + hdr_len = get_ws_hdr_size(size); + // the actual needed header size might decrease if we cut number of bytes + // if decrease of size crosses 65535 or 125 boundary + // but I can live with that at least for now + // worst case is we have 6 more bytes we could have written + // no bigus dealus + } + + *ptr++ = frame_type | WS_FINAL_FRAG; + + //generate length + *ptr = WS_PAYLOAD_MASKED; + if (size > 65535) { + *ptr++ |= 0x7f; + uint64_t be = htobe64(size); + memcpy(ptr, (void *)&be, sizeof(be)); + ptr += sizeof(be); + } else if (size > 125) { + *ptr++ |= 0x7e; + uint16_t be = htobe16(size); + memcpy(ptr, (void *)&be, sizeof(be)); + ptr += sizeof(be); + } else + *ptr++ |= size; + + mask = ptr; + if (read(client->entropy_fd, mask, sizeof(uint32_t)) < (ssize_t)sizeof(uint32_t)) { + ERROR("Unable to get mask from \"" ENTROPY_SOURCE "\""); + return -2; + } + + rbuf_push(client->buf_write, hdr, hdr_len); + + if (!size) + return 0; + + // copy and mask data in the write ringbuffer + while (size - size_written) { + size_t writable_bytes; + char *w_ptr = rbuf_get_linear_insert_range(client->buf_write, &writable_bytes); + if(!writable_bytes) + break; + + writable_bytes = (writable_bytes > size) ? (size - size_written) : writable_bytes; + + memcpy(w_ptr, &data[size_written], writable_bytes); + rbuf_bump_head(client->buf_write, writable_bytes); + + for (size_t i = 0; i < writable_bytes; i++, j++) + w_ptr[i] ^= mask[j % 4]; + size_written += writable_bytes; + } + return size_written; +} + +static int check_opcode(ws_client *client,enum websocket_opcode oc) +{ + switch(oc) { + case WS_OP_BINARY_FRAME: + case WS_OP_CONNECTION_CLOSE: + case WS_OP_PING: + return 0; + case WS_OP_CONTINUATION_FRAME: + FATAL("WS_OP_CONTINUATION_FRAME NOT IMPLEMENTED YET!!!!"); + return 0; + case WS_OP_TEXT_FRAME: + FATAL("WS_OP_TEXT_FRAME NOT IMPLEMENTED YET!!!!"); + return 0; + case WS_OP_PONG: + FATAL("WS_OP_PONG NOT IMPLEMENTED YET!!!!"); + return 0; + default: + return WS_CLIENT_PROTOCOL_ERROR; + } +} + +static inline void ws_client_rx_post_hdr_state(ws_client *client) +{ + switch(client->rx.opcode) { + case WS_OP_BINARY_FRAME: + client->rx.parse_state = WS_PAYLOAD_DATA; + return; + case WS_OP_CONNECTION_CLOSE: + client->rx.parse_state = WS_PAYLOAD_CONNECTION_CLOSE; + return; + case WS_OP_PING: + client->rx.parse_state = WS_PAYLOAD_PING_REQ_PAYLOAD; + return; + default: + client->rx.parse_state = WS_PAYLOAD_SKIP_UNKNOWN_PAYLOAD; + return; + } +} + +#define LONGEST_POSSIBLE_HDR_PART 8 +int ws_client_process_rx_ws(ws_client *client) +{ + char buf[LONGEST_POSSIBLE_HDR_PART]; + size_t size; + switch (client->rx.parse_state) { + case WS_FIRST_2BYTES: + BUF_READ_CHECK_AT_LEAST(2); + rbuf_pop(client->buf_read, buf, 2); + client->rx.opcode = buf[0] & (char)~BYTE_MSB; + + if (!(buf[0] & (char)~WS_FINAL_FRAG)) { + ERROR("Not supporting fragmented messages yet!"); + return WS_CLIENT_PROTOCOL_ERROR; + } + + if (check_opcode(client, client->rx.opcode) == WS_CLIENT_PROTOCOL_ERROR) + return WS_CLIENT_PROTOCOL_ERROR; + + if (buf[1] & (char)WS_PAYLOAD_MASKED) { + ERROR("Mask is not allowed in Server->Client Websocket direction."); + return WS_CLIENT_PROTOCOL_ERROR; + } + + switch (buf[1]) { + case 127: + client->rx.parse_state = WS_PAYLOAD_EXTENDED_64; + break; + case 126: + client->rx.parse_state = WS_PAYLOAD_EXTENDED_16; + break; + default: + client->rx.payload_length = buf[1]; + ws_client_rx_post_hdr_state(client); + } + break; + case WS_PAYLOAD_EXTENDED_16: + BUF_READ_CHECK_AT_LEAST(2); + rbuf_pop(client->buf_read, buf, 2); + client->rx.payload_length = be16toh(*((uint16_t *)buf)); + ws_client_rx_post_hdr_state(client); + break; + case WS_PAYLOAD_EXTENDED_64: + BUF_READ_CHECK_AT_LEAST(LONGEST_POSSIBLE_HDR_PART); + rbuf_pop(client->buf_read, buf, LONGEST_POSSIBLE_HDR_PART); + client->rx.payload_length = be64toh(*((uint64_t *)buf)); + ws_client_rx_post_hdr_state(client); + break; + case WS_PAYLOAD_DATA: + // TODO not pretty? + while (client->rx.payload_processed < client->rx.payload_length) { + size_t remaining = client->rx.payload_length - client->rx.payload_processed; + if (!rbuf_bytes_available(client->buf_read)) + return WS_CLIENT_NEED_MORE_BYTES; + char *insert = rbuf_get_linear_insert_range(client->buf_to_mqtt, &size); + if (!insert) { +#ifdef DEBUG_ULTRA_VERBOSE + DEBUG("BUFFER TOO FULL. Avail %d req %d", (int)size, (int)remaining); +#endif + return WS_CLIENT_BUFFER_FULL; + } + size = (size > remaining) ? remaining : size; + size = rbuf_pop(client->buf_read, insert, size); + rbuf_bump_head(client->buf_to_mqtt, size); + client->rx.payload_processed += size; + } + client->rx.parse_state = WS_PACKET_DONE; + break; + case WS_PAYLOAD_CONNECTION_CLOSE: + // for WS_OP_CONNECTION_CLOSE allowed is + // a) empty payload + // b) 2byte reason code + // c) 2byte reason code followed by message + if (client->rx.payload_length == 1) { + ERROR("WebScoket CONNECTION_CLOSE can't have payload of size 1"); + return WS_CLIENT_PROTOCOL_ERROR; + } + if (!client->rx.payload_length) { + INFO("WebSocket server closed the connection without giving reason."); + client->rx.parse_state = WS_PACKET_DONE; + break; + } + client->rx.parse_state = WS_PAYLOAD_CONNECTION_CLOSE_EC; + break; + case WS_PAYLOAD_CONNECTION_CLOSE_EC: + BUF_READ_CHECK_AT_LEAST(sizeof(uint16_t)); + + rbuf_pop(client->buf_read, buf, sizeof(uint16_t)); + client->rx.specific_data.op_close.ec = be16toh(*((uint16_t *)buf)); + client->rx.payload_processed += sizeof(uint16_t); + + if(client->rx.payload_processed == client->rx.payload_length) { + INFO("WebSocket server closed the connection with EC=%d. Without message.", + client->rx.specific_data.op_close.ec); + client->rx.parse_state = WS_PACKET_DONE; + break; + } + client->rx.parse_state = WS_PAYLOAD_CONNECTION_CLOSE_MSG; + break; + case WS_PAYLOAD_CONNECTION_CLOSE_MSG: + if (!client->rx.specific_data.op_close.reason) + client->rx.specific_data.op_close.reason = mw_malloc(client->rx.payload_length + 1); + + while (client->rx.payload_processed < client->rx.payload_length) { + if (!rbuf_bytes_available(client->buf_read)) + return WS_CLIENT_NEED_MORE_BYTES; + client->rx.payload_processed += rbuf_pop(client->buf_read, + &client->rx.specific_data.op_close.reason[client->rx.payload_processed - sizeof(uint16_t)], + client->rx.payload_length - client->rx.payload_processed); + } + client->rx.specific_data.op_close.reason[client->rx.payload_length] = 0; + INFO("WebSocket server closed the connection with EC=%d and reason \"%s\"", + client->rx.specific_data.op_close.ec, + client->rx.specific_data.op_close.reason); + mw_free(client->rx.specific_data.op_close.reason); + client->rx.specific_data.op_close.reason = NULL; + client->rx.parse_state = WS_PACKET_DONE; + break; + case WS_PAYLOAD_SKIP_UNKNOWN_PAYLOAD: + BUF_READ_CHECK_AT_LEAST(client->rx.payload_length); + WARN("Skipping Websocket Packet of unsupported/unknown type"); + if (client->rx.payload_length) + rbuf_bump_tail(client->buf_read, client->rx.payload_length); + client->rx.parse_state = WS_PACKET_DONE; + return WS_CLIENT_PARSING_DONE; + case WS_PAYLOAD_PING_REQ_PAYLOAD: + if (client->rx.payload_length > rbuf_get_capacity(client->buf_read) / 2) { + ERROR("Ping arrived with payload which is too big!"); + return WS_CLIENT_INTERNAL_ERROR; + } + BUF_READ_CHECK_AT_LEAST(client->rx.payload_length); + client->rx.specific_data.ping_msg = mw_malloc(client->rx.payload_length); + rbuf_pop(client->buf_read, client->rx.specific_data.ping_msg, client->rx.payload_length); + // TODO schedule this instead of sending right away + // then attempt to send as soon as buffer space clears up + size = ws_client_send(client, WS_OP_PONG, client->rx.specific_data.ping_msg, client->rx.payload_length); + if (size != client->rx.payload_length) { + ERROR("Unable to send the PONG as one packet back. Closing connection."); + return WS_CLIENT_PROTOCOL_ERROR; + } + client->rx.parse_state = WS_PACKET_DONE; + return WS_CLIENT_PARSING_DONE; + case WS_PACKET_DONE: + client->rx.parse_state = WS_FIRST_2BYTES; + client->rx.payload_processed = 0; + if (client->rx.opcode == WS_OP_CONNECTION_CLOSE) + return WS_CLIENT_CONNECTION_CLOSED; + return WS_CLIENT_PARSING_DONE; + default: + FATAL("Unknown parse state"); + return WS_CLIENT_INTERNAL_ERROR; + } + return 0; +} + +int ws_client_process(ws_client *client) +{ + int ret; + switch(client->state) { + case WS_RAW: + if (ws_client_start_handshake(client)) + return WS_CLIENT_INTERNAL_ERROR; + return WS_CLIENT_NEED_MORE_BYTES; + case WS_HANDSHAKE: + do { + ret = ws_client_parse_handshake_resp(client); + if (ret == WS_CLIENT_PROTOCOL_ERROR) + client->state = WS_ERROR; + if (ret == WS_CLIENT_PARSING_DONE && client->state == WS_ESTABLISHED) + ret = WS_CLIENT_NEED_MORE_BYTES; + } while (!ret); + break; + case WS_ESTABLISHED: + do { + ret = ws_client_process_rx_ws(client); + switch(ret) { + case WS_CLIENT_PROTOCOL_ERROR: + client->state = WS_ERROR; + break; + case WS_CLIENT_CONNECTION_CLOSED: + client->state = WS_CONN_CLOSED_GRACEFUL; + break; + } + // if ret == 0 we can continue parsing + // if ret == WS_CLIENT_PARSING_DONE we processed + // one websocket packet and attempt processing + // next one if data available in the buffer + } while (!ret || ret == WS_CLIENT_PARSING_DONE); + break; + case WS_ERROR: + ERROR("ws_client is in error state. Restart the connection!"); + return WS_CLIENT_PROTOCOL_ERROR; + case WS_CONN_CLOSED_GRACEFUL: + ERROR("Connection has been gracefully closed. Calling this is useless (and probably bug) until you reconnect again."); + return WS_CLIENT_CONNECTION_CLOSED; + default: + FATAL("Unknown connection state! Probably memory corruption."); + return WS_CLIENT_INTERNAL_ERROR; + } + return ret; +} diff --git a/src/aclk/mqtt_websockets/ws_client.h b/src/aclk/mqtt_websockets/ws_client.h new file mode 100644 index 000000000..0ccbd29a8 --- /dev/null +++ b/src/aclk/mqtt_websockets/ws_client.h @@ -0,0 +1,120 @@ +// SPDX-License-Identifier: GPL-3.0-only +// Copyright (C) 2020 Timotej Šiškovič + +#ifndef WS_CLIENT_H +#define WS_CLIENT_H + +#include "c-rbuf/cringbuffer.h" +#include "mqtt_wss_log.h" + +#include <stdint.h> + +#define WS_CLIENT_NEED_MORE_BYTES 0x10 +#define WS_CLIENT_PARSING_DONE 0x11 +#define WS_CLIENT_CONNECTION_CLOSED 0x12 +#define WS_CLIENT_PROTOCOL_ERROR -0x10 +#define WS_CLIENT_BUFFER_FULL -0x11 +#define WS_CLIENT_INTERNAL_ERROR -0x12 + +enum websocket_client_conn_state { + WS_RAW = 0, + WS_HANDSHAKE, + WS_ESTABLISHED, + WS_ERROR, // connection has to be restarted if this is reached + WS_CONN_CLOSED_GRACEFUL +}; + +enum websocket_client_hdr_parse_state { + WS_HDR_HTTP = 0, // need to check HTTP/1.1 + WS_HDR_RC, // need to read HTTP code + WS_HDR_ENDLINE, // need to read rest of the first line + WS_HDR_PARSE_HEADERS, // rest of the header until CRLF CRLF + WS_HDR_PARSE_DONE, + WS_HDR_ALL_DONE +}; + +enum websocket_client_rx_ws_parse_state { + WS_FIRST_2BYTES = 0, + WS_PAYLOAD_EXTENDED_16, + WS_PAYLOAD_EXTENDED_64, + WS_PAYLOAD_DATA, // BINARY payload to be passed to MQTT + WS_PAYLOAD_CONNECTION_CLOSE, + WS_PAYLOAD_CONNECTION_CLOSE_EC, + WS_PAYLOAD_CONNECTION_CLOSE_MSG, + WS_PAYLOAD_SKIP_UNKNOWN_PAYLOAD, + WS_PAYLOAD_PING_REQ_PAYLOAD, // PING payload to be sent back as PONG + WS_PACKET_DONE +}; + +enum websocket_opcode { + WS_OP_CONTINUATION_FRAME = 0x0, + WS_OP_TEXT_FRAME = 0x1, + WS_OP_BINARY_FRAME = 0x2, + WS_OP_CONNECTION_CLOSE = 0x8, + WS_OP_PING = 0x9, + WS_OP_PONG = 0xA +}; + +struct ws_op_close_payload { + uint16_t ec; + char *reason; +}; + +struct http_header { + char *key; + char *value; + struct http_header *next; +}; + +typedef struct websocket_client { + enum websocket_client_conn_state state; + + struct ws_handshake { + enum websocket_client_hdr_parse_state hdr_state; + char *nonce_reply; + int nonce_matched; + int http_code; + char *http_reply_msg; + struct http_header *headers; + struct http_header *headers_tail; + int hdr_count; + } hs; + + struct ws_rx { + enum websocket_client_rx_ws_parse_state parse_state; + enum websocket_opcode opcode; + uint64_t payload_length; + uint64_t payload_processed; + union { + struct ws_op_close_payload op_close; + char *ping_msg; + } specific_data; + } rx; + + rbuf_t buf_read; // from SSL + rbuf_t buf_write; // to SSL and then to socket + // TODO if ringbuffer gets multiple tail support + // we can work without buf_to_mqtt and thus reduce + // memory usage and remove one more memcpy buf_read->buf_to_mqtt + rbuf_t buf_to_mqtt; // RAW data for MQTT lib + + int entropy_fd; + + // careful host is borrowed, don't free + char **host; + mqtt_wss_log_ctx_t log; +} ws_client; + +ws_client *ws_client_new(size_t buf_size, char **host, mqtt_wss_log_ctx_t log); +void ws_client_destroy(ws_client *client); +void ws_client_reset(ws_client *client); + +int ws_client_start_handshake(ws_client *client); + +int ws_client_want_write(ws_client *client); + +int ws_client_process(ws_client *client); + +int ws_client_send(ws_client *client, enum websocket_opcode frame_type, const char *data, size_t size); + +#endif /* WS_CLIENT_H */ |