#define _GNU_SOURCE #include #include #include #include #include #include "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); 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; char *data; 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; }; 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, 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 #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 = (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 - (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 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 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; }