1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702 |
- #include "mqtt.h"
- enum MQTTErrors mqtt_sync(struct mqtt_client *client) {
-
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- if (client->error != MQTT_OK && client->reconnect_callback != NULL) {
- client->reconnect_callback(client, &client->reconnect_state);
-
- } else {
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- }
-
- enum MQTTErrors err;
- if (client->inspector_callback != NULL) {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- err = client->inspector_callback(client);
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- if (err != MQTT_OK) return err;
- }
-
- err = (enum MQTTErrors)__mqtt_recv(client);
- if (err != MQTT_OK) return err;
-
- err = (enum MQTTErrors)__mqtt_send(client);
- return err;
- }
- uint16_t __mqtt_next_pid(struct mqtt_client *client) {
- if (client->pid_lfsr == 0) {
- client->pid_lfsr = 163u;
- }
-
- int pid_exists = 0;
- do {
- unsigned lsb = client->pid_lfsr & 1;
- (client->pid_lfsr) >>= 1;
- if (lsb) {
- client->pid_lfsr ^= 0xB400u;
- }
-
- pid_exists = 0;
- struct mqtt_queued_message *curr;
- for(curr = mqtt_mq_get(&(client->mq), 0); curr >= client->mq.queue_tail; --curr) {
- if (curr->packet_id == client->pid_lfsr) {
- pid_exists = 1;
- break;
- }
- }
- } while(pid_exists);
- return client->pid_lfsr;
- }
- enum MQTTErrors mqtt_init(struct mqtt_client *client,
- mqtt_pal_socket_handle sockfd,
- uint8_t *sendbuf, size_t sendbufsz,
- uint8_t *recvbuf, size_t recvbufsz,
- void (*publish_response_callback)(void** state,struct mqtt_response_publish *publish))
- {
- if (client == NULL || sendbuf == NULL || recvbuf == NULL) {
- return MQTT_ERROR_NULLPTR;
- }
-
- MQTT_PAL_MUTEX_INIT(&client->mutex);
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- client->socketfd = sockfd;
- mqtt_mq_init(&client->mq, sendbuf, sendbufsz);
- client->recv_buffer.mem_start = recvbuf;
- client->recv_buffer.mem_size = recvbufsz;
- client->recv_buffer.curr = client->recv_buffer.mem_start;
- client->recv_buffer.curr_sz = client->recv_buffer.mem_size;
- client->error = MQTT_ERROR_CONNECT_NOT_CALLED;
- client->response_timeout = 30;
- client->number_of_timeouts = 0;
- client->number_of_keep_alives = 0;
- client->typical_response_time = -1.0;
- client->publish_response_callback = publish_response_callback;
- client->inspector_callback = NULL;
- client->reconnect_callback = NULL;
- client->reconnect_state = NULL;
- return MQTT_OK;
- }
- void mqtt_init_reconnect(struct mqtt_client *client,
- void (*reconnect)(struct mqtt_client *, void**),
- void *reconnect_state,
- void (*publish_response_callback)(void** state, struct mqtt_response_publish *publish))
- {
-
- MQTT_PAL_MUTEX_INIT(&client->mutex);
- client->socketfd = (mqtt_pal_socket_handle) -1;
- mqtt_mq_init(&client->mq, NULL, 0);
- client->recv_buffer.mem_start = NULL;
- client->recv_buffer.mem_size = 0;
- client->recv_buffer.curr = NULL;
- client->recv_buffer.curr_sz = 0;
- client->error = MQTT_ERROR_INITIAL_RECONNECT;
- client->response_timeout = 30;
- client->number_of_timeouts = 0;
- client->number_of_keep_alives = 0;
- client->typical_response_time = -1.0;
- client->publish_response_callback = publish_response_callback;
- client->inspector_callback = NULL;
- client->reconnect_callback = reconnect;
- client->reconnect_state = reconnect_state;
- }
- void mqtt_reinit(struct mqtt_client* client,
- mqtt_pal_socket_handle socketfd,
- uint8_t *sendbuf, size_t sendbufsz,
- uint8_t *recvbuf, size_t recvbufsz)
- {
- client->error = MQTT_ERROR_CONNECT_NOT_CALLED;
- client->socketfd = socketfd;
- mqtt_mq_init(&client->mq, sendbuf, sendbufsz);
- client->recv_buffer.mem_start = recvbuf;
- client->recv_buffer.mem_size = recvbufsz;
- client->recv_buffer.curr = client->recv_buffer.mem_start;
- client->recv_buffer.curr_sz = client->recv_buffer.mem_size;
- }
- #define MQTT_CLIENT_TRY_PACK(tmp, msg, client, pack_call, release) \
- if (client->error < 0) { \
- if (release) MQTT_PAL_MUTEX_UNLOCK(&client->mutex); \
- return client->error; \
- } \
- tmp = pack_call; \
- if (tmp < 0) { \
- client->error = (enum MQTTErrors)tmp; \
- if (release) MQTT_PAL_MUTEX_UNLOCK(&client->mutex); \
- return (enum MQTTErrors)tmp; \
- } else if (tmp == 0) { \
- mqtt_mq_clean(&client->mq); \
- tmp = pack_call; \
- if (tmp < 0) { \
- client->error = (enum MQTTErrors)tmp; \
- if (release) MQTT_PAL_MUTEX_UNLOCK(&client->mutex); \
- return (enum MQTTErrors)tmp; \
- } else if(tmp == 0) { \
- client->error = MQTT_ERROR_SEND_BUFFER_IS_FULL; \
- if (release) MQTT_PAL_MUTEX_UNLOCK(&client->mutex); \
- return (enum MQTTErrors)MQTT_ERROR_SEND_BUFFER_IS_FULL; \
- } \
- } \
- msg = mqtt_mq_register(&client->mq, (size_t)tmp); \
- enum MQTTErrors mqtt_connect(struct mqtt_client *client,
- const char* client_id,
- const char* will_topic,
- const void* will_message,
- size_t will_message_size,
- const char* user_name,
- const char* password,
- uint8_t connect_flags,
- uint16_t keep_alive)
- {
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
-
- client->keep_alive = keep_alive;
- if (client->error == MQTT_ERROR_CONNECT_NOT_CALLED) {
- client->error = MQTT_OK;
- }
-
-
- MQTT_CLIENT_TRY_PACK(rv, msg, client,
- mqtt_pack_connection_request(
- client->mq.curr, client->mq.curr_sz,
- client_id, will_topic, will_message,
- will_message_size,user_name, password,
- connect_flags, keep_alive
- ),
- 1
- );
-
- msg->control_type = MQTT_CONTROL_CONNECT;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- enum MQTTErrors mqtt_publish(struct mqtt_client *client,
- const char* topic_name,
- void* application_message,
- size_t application_message_size,
- uint8_t publish_flags)
- {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- uint16_t packet_id = __mqtt_next_pid(client);
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_publish_request(
- client->mq.curr, client->mq.curr_sz,
- topic_name,
- packet_id,
- application_message,
- application_message_size,
- publish_flags
- ),
- 1
- );
-
- msg->control_type = MQTT_CONTROL_PUBLISH;
- msg->packet_id = packet_id;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- ssize_t __mqtt_puback(struct mqtt_client *client, uint16_t packet_id) {
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_pubxxx_request(
- client->mq.curr, client->mq.curr_sz,
- MQTT_CONTROL_PUBACK,
- packet_id
- ),
- 0
- );
-
- msg->control_type = MQTT_CONTROL_PUBACK;
- msg->packet_id = packet_id;
- return MQTT_OK;
- }
- ssize_t __mqtt_pubrec(struct mqtt_client *client, uint16_t packet_id) {
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_pubxxx_request(
- client->mq.curr, client->mq.curr_sz,
- MQTT_CONTROL_PUBREC,
- packet_id
- ),
- 0
- );
-
- msg->control_type = MQTT_CONTROL_PUBREC;
- msg->packet_id = packet_id;
- return MQTT_OK;
- }
- ssize_t __mqtt_pubrel(struct mqtt_client *client, uint16_t packet_id) {
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_pubxxx_request(
- client->mq.curr, client->mq.curr_sz,
- MQTT_CONTROL_PUBREL,
- packet_id
- ),
- 0
- );
-
- msg->control_type = MQTT_CONTROL_PUBREL;
- msg->packet_id = packet_id;
- return MQTT_OK;
- }
- ssize_t __mqtt_pubcomp(struct mqtt_client *client, uint16_t packet_id) {
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_pubxxx_request(
- client->mq.curr, client->mq.curr_sz,
- MQTT_CONTROL_PUBCOMP,
- packet_id
- ),
- 0
- );
-
- msg->control_type = MQTT_CONTROL_PUBCOMP;
- msg->packet_id = packet_id;
- return MQTT_OK;
- }
- enum MQTTErrors mqtt_subscribe(struct mqtt_client *client,
- const char* topic_name,
- int max_qos_level)
- {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- uint16_t packet_id = __mqtt_next_pid(client);
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_subscribe_request(
- client->mq.curr, client->mq.curr_sz,
- packet_id,
- topic_name,
- max_qos_level,
- NULL
- ),
- 1
- );
-
- msg->control_type = MQTT_CONTROL_SUBSCRIBE;
- msg->packet_id = packet_id;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- enum MQTTErrors mqtt_unsubscribe(struct mqtt_client *client,
- const char* topic_name)
- {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- uint16_t packet_id = __mqtt_next_pid(client);
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_unsubscribe_request(
- client->mq.curr, client->mq.curr_sz,
- packet_id,
- topic_name,
- NULL
- ),
- 1
- );
-
- msg->control_type = MQTT_CONTROL_UNSUBSCRIBE;
- msg->packet_id = packet_id;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- enum MQTTErrors mqtt_ping(struct mqtt_client *client) {
- enum MQTTErrors rv;
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- rv = __mqtt_ping(client);
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- }
- enum MQTTErrors __mqtt_ping(struct mqtt_client *client)
- {
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_ping_request(
- client->mq.curr, client->mq.curr_sz
- ),
- 0
- );
-
- msg->control_type = MQTT_CONTROL_PINGREQ;
-
- return MQTT_OK;
- }
- enum MQTTErrors mqtt_disconnect(struct mqtt_client *client)
- {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- ssize_t rv;
- struct mqtt_queued_message *msg;
-
- MQTT_CLIENT_TRY_PACK(
- rv, msg, client,
- mqtt_pack_disconnect(
- client->mq.curr, client->mq.curr_sz
- ),
- 1
- );
-
- msg->control_type = MQTT_CONTROL_DISCONNECT;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- ssize_t __mqtt_send(struct mqtt_client *client)
- {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- uint8_t inspected;
- if (client->error < 0 && client->error != MQTT_ERROR_SEND_BUFFER_IS_FULL) {
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return client->error;
- }
-
- int len = (int)(mqtt_mq_length(&client->mq));
- int inflight_qos2 = 0;
- for(int i = 0; i < len; ++i) {
- struct mqtt_queued_message *msg = mqtt_mq_get(&client->mq, i);
- int resend = 0;
- if (msg->state == MQTT_QUEUED_UNSENT) {
-
- resend = 1;
- } else if (msg->state == MQTT_QUEUED_AWAITING_ACK) {
-
- if (MQTT_PAL_TIME() > msg->time_sent + client->response_timeout) {
- resend = 1;
- client->number_of_timeouts += 1;
- }
- }
-
- if (msg->control_type == MQTT_CONTROL_PUBLISH
- && (msg->state == MQTT_QUEUED_UNSENT || msg->state == MQTT_QUEUED_AWAITING_ACK))
- {
- inspected = 0x03 & ((msg->start[0]) >> 1);
- if (inspected == 2) {
- if (inflight_qos2) {
- resend = 0;
- }
- inflight_qos2 = 1;
- }
- }
-
- if (!resend) {
- continue;
- }
-
- ssize_t tmp = mqtt_pal_sendall(client->socketfd, msg->start, msg->size, 0);
- if (tmp < 0) {
- client->error = (enum MQTTErrors)tmp;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return tmp;
- }
-
- client->time_of_last_send = MQTT_PAL_TIME();
- msg->time_sent = client->time_of_last_send;
-
- switch (msg->control_type) {
- case MQTT_CONTROL_PUBACK:
- case MQTT_CONTROL_PUBCOMP:
- case MQTT_CONTROL_DISCONNECT:
- msg->state = MQTT_QUEUED_COMPLETE;
- break;
- case MQTT_CONTROL_PUBLISH:
- inspected = 0x03 & ((msg->start[0]) >> 1);
- if (inspected == 0) {
- msg->state = MQTT_QUEUED_COMPLETE;
- } else if (inspected == 1) {
- msg->state = MQTT_QUEUED_AWAITING_ACK;
-
- msg->start[1] |= MQTT_PUBLISH_DUP;
- } else {
- msg->state = MQTT_QUEUED_AWAITING_ACK;
- }
- break;
- case MQTT_CONTROL_CONNECT:
- case MQTT_CONTROL_PUBREC:
- case MQTT_CONTROL_PUBREL:
- case MQTT_CONTROL_SUBSCRIBE:
- case MQTT_CONTROL_UNSUBSCRIBE:
- case MQTT_CONTROL_PINGREQ:
- msg->state = MQTT_QUEUED_AWAITING_ACK;
- break;
- default:
- client->error = MQTT_ERROR_MALFORMED_REQUEST;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_MALFORMED_REQUEST;
- }
- }
-
- mqtt_pal_time_t keep_alive_timeout = client->time_of_last_send + (mqtt_pal_time_t) ((float) (client->keep_alive) * 0.75);
- if (MQTT_PAL_TIME() > keep_alive_timeout) {
- ssize_t rv = __mqtt_ping(client);
- if (rv != MQTT_OK) {
- client->error = (enum MQTTErrors)rv;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- }
- }
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- ssize_t __mqtt_recv(struct mqtt_client *client)
- {
- MQTT_PAL_MUTEX_LOCK(&client->mutex);
- struct mqtt_response response;
-
- while(1) {
-
- ssize_t rv, consumed;
- rv = mqtt_pal_recvall(client->socketfd, client->recv_buffer.curr, client->recv_buffer.curr_sz, 0);
- if (rv < 0) {
-
- client->error = (enum MQTTErrors)rv;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- } else {
- client->recv_buffer.curr += rv;
- client->recv_buffer.curr_sz -= (size_t)rv;
- }
-
- consumed = mqtt_unpack_response(&response, client->recv_buffer.mem_start, (size_t)(client->recv_buffer.curr - client->recv_buffer.mem_start));
- if (consumed < 0) {
- client->error = (enum MQTTErrors)consumed;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return consumed;
- } else if (consumed == 0) {
-
- if (client->recv_buffer.curr_sz == 0) {
- client->error = MQTT_ERROR_RECV_BUFFER_TOO_SMALL;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_RECV_BUFFER_TOO_SMALL;
- }
-
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
-
- struct mqtt_queued_message *msg = NULL;
-
- switch (response.fixed_header.control_type) {
- case MQTT_CONTROL_CONNACK:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_CONNECT, NULL);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = (double) (MQTT_PAL_TIME() - msg->time_sent);
-
- if (response.decoded.connack.return_code != MQTT_CONNACK_ACCEPTED) {
- client->error = MQTT_ERROR_CONNECTION_REFUSED;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_CONNECTION_REFUSED;
- }
- break;
- case MQTT_CONTROL_PUBLISH:
-
- if (response.decoded.publish.qos_level == 1) {
- rv = __mqtt_puback(client, response.decoded.publish.packet_id);
- if (rv != MQTT_OK) {
- client->error = (enum MQTTErrors)rv;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- }
- } else if (response.decoded.publish.qos_level == 2) {
-
- if (mqtt_mq_find(&client->mq, MQTT_CONTROL_PUBREC, &response.decoded.publish.packet_id) != NULL) {
- break;
- }
- rv = __mqtt_pubrec(client, response.decoded.publish.packet_id);
- if (rv != MQTT_OK) {
- client->error = (enum MQTTErrors)rv;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- }
- }
-
- client->publish_response_callback(&client->publish_response_callback_state, &response.decoded.publish);
- break;
- case MQTT_CONTROL_PUBACK:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_PUBLISH, &response.decoded.puback.packet_id);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
- break;
- case MQTT_CONTROL_PUBREC:
-
- if (mqtt_mq_find(&client->mq, MQTT_CONTROL_PUBREL, &response.decoded.pubrec.packet_id) != NULL) {
- break;
- }
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_PUBLISH, &response.decoded.pubrec.packet_id);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
-
- rv = __mqtt_pubrel(client, response.decoded.pubrec.packet_id);
- if (rv != MQTT_OK) {
- client->error = (enum MQTTErrors)rv;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- }
- break;
- case MQTT_CONTROL_PUBREL:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_PUBREC, &response.decoded.pubrel.packet_id);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
-
- rv = __mqtt_pubcomp(client, response.decoded.pubrec.packet_id);
- if (rv != MQTT_OK) {
- client->error = (enum MQTTErrors)rv;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return rv;
- }
- break;
- case MQTT_CONTROL_PUBCOMP:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_PUBREL, &response.decoded.pubcomp.packet_id);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
- break;
- case MQTT_CONTROL_SUBACK:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_SUBSCRIBE, &response.decoded.suback.packet_id);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
-
- if (response.decoded.suback.return_codes[0] == MQTT_SUBACK_FAILURE) {
- client->error = MQTT_ERROR_SUBSCRIBE_FAILED;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_SUBSCRIBE_FAILED;
- }
- break;
- case MQTT_CONTROL_UNSUBACK:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_UNSUBSCRIBE, &response.decoded.unsuback.packet_id);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
- break;
- case MQTT_CONTROL_PINGRESP:
-
- msg = mqtt_mq_find(&client->mq, MQTT_CONTROL_PINGREQ, NULL);
- if (msg == NULL) {
- client->error = MQTT_ERROR_ACK_OF_UNKNOWN;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_ACK_OF_UNKNOWN;
- }
- msg->state = MQTT_QUEUED_COMPLETE;
-
- client->typical_response_time = 0.875 * (client->typical_response_time) + 0.125 * (double) (MQTT_PAL_TIME() - msg->time_sent);
- break;
- default:
- client->error = MQTT_ERROR_MALFORMED_RESPONSE;
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_ERROR_MALFORMED_RESPONSE;
- }
-
- void *dest = client->recv_buffer.mem_start;
- void *src = client->recv_buffer.mem_start + consumed;
- size_t n = (size_t)(client->recv_buffer.curr - client->recv_buffer.mem_start - consumed);
- memmove(dest, src, n);
- client->recv_buffer.curr -= consumed;
- client->recv_buffer.curr_sz += (size_t)consumed;
- }
-
- MQTT_PAL_MUTEX_UNLOCK(&client->mutex);
- return MQTT_OK;
- }
- #define MQTT_BITFIELD_RULE_VIOLOATION(bitfield, rule_value, rule_mask) ((bitfield ^ rule_value) & rule_mask)
- struct mqtt_fixed_header_rules_s{
- const uint8_t control_type_is_valid[16];
- const uint8_t required_flags[16];
- const uint8_t mask_required_flags[16];
- } ;
- struct mqtt_fixed_header_rules_s mqtt_fixed_header_rules =
- {
- {
- 0x00,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x01,
- 0x00
- },
- {
- 0x00,
- 0x00,
- 0x00,
- 0x00,
- 0x00,
- 0x00,
- 0x02,
- 0x00,
- 0x02,
- 0x00,
- 0x02,
- 0x00,
- 0x00,
- 0x00,
- 0x00,
- 0x00
- },
- {
- 0x00,
- 0x0F,
- 0x0F,
- 0x00,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x0F,
- 0x00
- }
- };
- static ssize_t mqtt_fixed_header_rule_violation(const struct mqtt_fixed_header *fixed_header) {
- uint8_t control_type;
- uint8_t control_flags;
- uint8_t required_flags;
- uint8_t mask_required_flags;
-
- control_type = (uint8_t)fixed_header->control_type;
- control_flags = fixed_header->control_flags;
- required_flags = mqtt_fixed_header_rules.required_flags[control_type];
- mask_required_flags = mqtt_fixed_header_rules.mask_required_flags[control_type];
-
- if (!mqtt_fixed_header_rules.control_type_is_valid[control_type]) {
- return MQTT_ERROR_CONTROL_FORBIDDEN_TYPE;
- }
-
-
- if(MQTT_BITFIELD_RULE_VIOLOATION(control_flags, required_flags, mask_required_flags)) {
- return MQTT_ERROR_CONTROL_INVALID_FLAGS;
- }
- return 0;
- }
- ssize_t mqtt_unpack_fixed_header(struct mqtt_response *response, const uint8_t *buf, size_t bufsz) {
- struct mqtt_fixed_header *fixed_header;
- const uint8_t *start = buf;
- int lshift;
- ssize_t errcode;
-
-
- if (response == NULL || buf == NULL) {
- return MQTT_ERROR_NULLPTR;
- }
- fixed_header = &(response->fixed_header);
-
- if (bufsz == 0) return 0;
-
- fixed_header->control_type = (enum MQTTControlPacketType)(*buf >> 4);
- fixed_header->control_flags = (uint8_t)(*buf & 0x0F);
-
- fixed_header->remaining_length = 0;
- lshift = 0;
- do {
-
- --bufsz;
- ++buf;
- if (bufsz == 0) return 0;
-
- fixed_header->remaining_length += (uint32_t)((*buf & 0x7F) << lshift);
- lshift += 7;
- } while(*buf & 0x80);
-
- --bufsz;
- ++buf;
-
- errcode = mqtt_fixed_header_rule_violation(fixed_header);
- if (errcode) {
- return errcode;
- }
-
- if (bufsz < fixed_header->remaining_length) {
- return 0;
- }
-
- return buf - start;
- }
- ssize_t mqtt_pack_fixed_header(uint8_t *buf, size_t bufsz, const struct mqtt_fixed_header *fixed_header) {
- const uint8_t *start = buf;
- ssize_t errcode;
- uint32_t remaining_length;
-
-
- if (fixed_header == NULL || buf == NULL) {
- return MQTT_ERROR_NULLPTR;
- }
-
- errcode = mqtt_fixed_header_rule_violation(fixed_header);
- if (errcode) {
- return errcode;
- }
-
- if (bufsz == 0) return 0;
-
- *buf = (uint8_t)((((uint8_t) fixed_header->control_type) << 4) & 0xF0);
- *buf = (uint8_t)(*buf | (((uint8_t) fixed_header->control_flags) & 0x0F));
- remaining_length = fixed_header->remaining_length;
- do {
-
- --bufsz;
- ++buf;
- if (bufsz == 0) return 0;
-
-
- *buf = remaining_length & 0x7F;
- if(remaining_length > 127) *buf |= 0x80;
- remaining_length = remaining_length >> 7;
- } while(*buf & 0x80);
-
-
- --bufsz;
- ++buf;
-
- if (bufsz < fixed_header->remaining_length) {
- return 0;
- }
-
- return buf - start;
- }
- ssize_t mqtt_pack_connection_request(uint8_t* buf, size_t bufsz,
- const char* client_id,
- const char* will_topic,
- const void* will_message,
- size_t will_message_size,
- const char* user_name,
- const char* password,
- uint8_t connect_flags,
- uint16_t keep_alive)
- {
- struct mqtt_fixed_header fixed_header;
- uint32_t remaining_length;
-
- const uint8_t * start = buf;
- ssize_t rv;
- uint8_t temp;
-
- fixed_header.control_type = MQTT_CONTROL_CONNECT;
- fixed_header.control_flags = 0x00;
-
- connect_flags = (uint8_t)(connect_flags & ~MQTT_CONNECT_RESERVED);
- remaining_length = 10;
- if (client_id == NULL) {
-
- return MQTT_ERROR_CONNECT_NULL_CLIENT_ID;
- } else {
-
- remaining_length += (uint32_t)__mqtt_packed_cstrlen(client_id);
- }
-
- if (will_topic != NULL) {
-
- connect_flags |= MQTT_CONNECT_WILL_FLAG;
- remaining_length += (uint32_t)__mqtt_packed_cstrlen(will_topic);
-
- if (will_message == NULL) {
-
- return MQTT_ERROR_CONNECT_NULL_WILL_MESSAGE;
- }
- remaining_length += (uint32_t)(2 + will_message_size);
-
- temp = connect_flags & 0x18;
- if (temp == 0x18) {
-
- return MQTT_ERROR_CONNECT_FORBIDDEN_WILL_QOS;
- }
- } else {
-
- connect_flags &= (uint8_t)~MQTT_CONNECT_WILL_FLAG;
- connect_flags &= (uint8_t)~0x18;
- connect_flags &= (uint8_t)~MQTT_CONNECT_WILL_RETAIN;
- }
- if (user_name != NULL) {
-
- connect_flags |= MQTT_CONNECT_USER_NAME;
- remaining_length += (uint32_t)__mqtt_packed_cstrlen(user_name);
- } else {
- connect_flags &= (uint8_t)~MQTT_CONNECT_USER_NAME;
- }
- if (password != NULL) {
-
- connect_flags |= MQTT_CONNECT_PASSWORD;
- remaining_length += (uint32_t)__mqtt_packed_cstrlen(password);
- } else {
- connect_flags &= (uint8_t)~MQTT_CONNECT_PASSWORD;
- }
-
- fixed_header.remaining_length = remaining_length;
-
- rv = mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- if (rv <= 0) {
-
- return rv;
- }
- buf += rv;
- bufsz -= (size_t)rv;
-
- if (bufsz < fixed_header.remaining_length) {
- return 0;
- }
-
- *buf++ = 0x00;
- *buf++ = 0x04;
- *buf++ = (uint8_t) 'M';
- *buf++ = (uint8_t) 'Q';
- *buf++ = (uint8_t) 'T';
- *buf++ = (uint8_t) 'T';
- *buf++ = MQTT_PROTOCOL_LEVEL;
- *buf++ = connect_flags;
- *(uint16_t*) buf = (uint16_t) MQTT_PAL_HTONS(keep_alive);
- buf += 2;
-
- buf += __mqtt_pack_str(buf, client_id);
- if (connect_flags & MQTT_CONNECT_WILL_FLAG) {
- buf += __mqtt_pack_str(buf, will_topic);
- memcpy(buf, will_message, will_message_size);
- buf += will_message_size;
- }
- if (connect_flags & MQTT_CONNECT_USER_NAME) {
- buf += __mqtt_pack_str(buf, user_name);
- }
- if (connect_flags & MQTT_CONNECT_PASSWORD) {
- buf += __mqtt_pack_str(buf, password);
- }
-
- return buf - start;
- }
- ssize_t mqtt_unpack_connack_response(struct mqtt_response *mqtt_response, const uint8_t *buf) {
- const uint8_t *const start = buf;
- struct mqtt_response_connack *response;
-
- if (mqtt_response->fixed_header.remaining_length != 2) {
- return MQTT_ERROR_MALFORMED_RESPONSE;
- }
-
- response = &(mqtt_response->decoded.connack);
-
- if (*buf & 0xFE) {
-
- return MQTT_ERROR_CONNACK_FORBIDDEN_FLAGS;
- } else {
- response->session_present_flag = *buf++;
- }
- if (*buf > 5u) {
-
- return MQTT_ERROR_CONNACK_FORBIDDEN_CODE;
- } else {
- response->return_code = (enum MQTTConnackReturnCode) *buf++;
- }
- return buf - start;
- }
- ssize_t mqtt_pack_disconnect(uint8_t *buf, size_t bufsz) {
- struct mqtt_fixed_header fixed_header;
- fixed_header.control_type = MQTT_CONTROL_DISCONNECT;
- fixed_header.control_flags = 0;
- fixed_header.remaining_length = 0;
- return mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- }
- ssize_t mqtt_pack_ping_request(uint8_t *buf, size_t bufsz) {
- struct mqtt_fixed_header fixed_header;
- fixed_header.control_type = MQTT_CONTROL_PINGREQ;
- fixed_header.control_flags = 0;
- fixed_header.remaining_length = 0;
- return mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- }
- ssize_t mqtt_pack_publish_request(uint8_t *buf, size_t bufsz,
- const char* topic_name,
- uint16_t packet_id,
- void* application_message,
- size_t application_message_size,
- uint8_t publish_flags)
- {
- const uint8_t *const start = buf;
- ssize_t rv;
- struct mqtt_fixed_header fixed_header;
- uint16_t remaining_length;
- uint8_t inspected_qos;
-
- if(buf == NULL || topic_name == NULL) {
- return MQTT_ERROR_NULLPTR;
- }
-
- inspected_qos = (uint8_t)((publish_flags & 0x06) >> 1);
-
- fixed_header.control_type = MQTT_CONTROL_PUBLISH;
-
- remaining_length = (uint16_t)__mqtt_packed_cstrlen(topic_name);
- if (inspected_qos > 0) {
- remaining_length = (uint16_t)(remaining_length + 2);
- }
- remaining_length = (uint16_t) (remaining_length + application_message_size);
- fixed_header.remaining_length = remaining_length;
-
- if (inspected_qos == 0) {
- publish_flags &= (uint8_t)~MQTT_PUBLISH_DUP;
- }
-
- if (inspected_qos == 3) {
- return MQTT_ERROR_PUBLISH_FORBIDDEN_QOS;
- }
- fixed_header.control_flags = publish_flags & 0x7;
-
- rv = mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- if (rv <= 0) {
-
- return rv;
- }
- buf += rv;
- bufsz -= (size_t)rv;
-
- if (bufsz < remaining_length) {
- return 0;
- }
-
- buf += __mqtt_pack_str(buf, topic_name);
- if (inspected_qos > 0) {
- *(uint16_t*) buf = (uint16_t) MQTT_PAL_HTONS(packet_id);
- buf += 2;
- }
-
- memcpy(buf, application_message, application_message_size);
- buf += application_message_size;
- return buf - start;
- }
- ssize_t mqtt_unpack_publish_response(struct mqtt_response *mqtt_response, const uint8_t *buf)
- {
-
- const uint8_t *start = buf;
- struct mqtt_fixed_header *fixed_header;
- struct mqtt_response_publish *response;
-
- fixed_header = &(mqtt_response->fixed_header);
- response = &(mqtt_response->decoded.publish);
-
- response->dup_flag = (uint8_t)((fixed_header->control_flags & MQTT_PUBLISH_DUP) >> 3);
- response->qos_level = (uint8_t)((fixed_header->control_flags & 0x06) >> 1);
- response->retain_flag = fixed_header->control_flags & MQTT_PUBLISH_RETAIN;
-
- if (mqtt_response->fixed_header.remaining_length < 4) {
- return MQTT_ERROR_MALFORMED_RESPONSE;
- }
-
- response->topic_name_size = (uint16_t) MQTT_PAL_NTOHS(*(const uint16_t*) buf);
- buf += 2;
- response->topic_name = buf;
- buf += response->topic_name_size;
- if (response->qos_level > 0) {
- response->packet_id = (uint16_t) MQTT_PAL_NTOHS(*(const uint16_t*) buf);
- buf += 2;
- }
-
- response->application_message = buf;
- if (response->qos_level == 0) {
- response->application_message_size = fixed_header->remaining_length - response->topic_name_size - 2;
- } else {
- response->application_message_size = fixed_header->remaining_length - response->topic_name_size - 4;
- }
- buf += response->application_message_size;
-
-
- return buf - start;
- }
- ssize_t mqtt_pack_pubxxx_request(uint8_t *buf, size_t bufsz,
- enum MQTTControlPacketType control_type,
- uint16_t packet_id)
- {
- const uint8_t *const start = buf;
- struct mqtt_fixed_header fixed_header;
- ssize_t rv;
- if (buf == NULL) {
- return MQTT_ERROR_NULLPTR;
- }
-
- fixed_header.control_type = control_type;
- if (control_type == MQTT_CONTROL_PUBREL) {
- fixed_header.control_flags = 0x02;
- } else {
- fixed_header.control_flags = 0;
- }
- fixed_header.remaining_length = 2;
- rv = mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- if (rv <= 0) {
- return rv;
- }
- buf += rv;
- bufsz -= (size_t)rv;
- if (bufsz < fixed_header.remaining_length) {
- return 0;
- }
-
- *(uint16_t*) buf = (uint16_t) MQTT_PAL_HTONS(packet_id);
- buf += 2;
- return buf - start;
- }
- ssize_t mqtt_unpack_pubxxx_response(struct mqtt_response *mqtt_response, const uint8_t *buf)
- {
- const uint8_t *const start = buf;
- uint16_t packet_id;
-
- if (mqtt_response->fixed_header.remaining_length != 2) {
- return MQTT_ERROR_MALFORMED_RESPONSE;
- }
-
- packet_id = (uint16_t) MQTT_PAL_NTOHS(*(const uint16_t*) buf);
- buf += 2;
- if (mqtt_response->fixed_header.control_type == MQTT_CONTROL_PUBACK) {
- mqtt_response->decoded.puback.packet_id = packet_id;
- } else if (mqtt_response->fixed_header.control_type == MQTT_CONTROL_PUBREC) {
- mqtt_response->decoded.pubrec.packet_id = packet_id;
- } else if (mqtt_response->fixed_header.control_type == MQTT_CONTROL_PUBREL) {
- mqtt_response->decoded.pubrel.packet_id = packet_id;
- } else {
- mqtt_response->decoded.pubcomp.packet_id = packet_id;
- }
- return buf - start;
- }
- ssize_t mqtt_unpack_suback_response (struct mqtt_response *mqtt_response, const uint8_t *buf) {
- const uint8_t *const start = buf;
- uint32_t remaining_length = mqtt_response->fixed_header.remaining_length;
-
-
- if (remaining_length < 3) {
- return MQTT_ERROR_MALFORMED_RESPONSE;
- }
-
- mqtt_response->decoded.suback.packet_id = (uint16_t) MQTT_PAL_NTOHS(*(const uint16_t*) buf);
- buf += 2;
- remaining_length -= 2;
-
- mqtt_response->decoded.suback.num_return_codes = (size_t) remaining_length;
- mqtt_response->decoded.suback.return_codes = buf;
- buf += remaining_length;
- return buf - start;
- }
- #pragma GCC diagnostic push
- #pragma GCC diagnostic ignored "-Wvarargs"
- ssize_t mqtt_pack_subscribe_request(uint8_t *buf, size_t bufsz, uint16_t packet_id, ...) {
- va_list args;
- const uint8_t *const start = buf;
- ssize_t rv;
- struct mqtt_fixed_header fixed_header;
- unsigned int num_subs = 0;
- unsigned int i;
- const char *topic[MQTT_SUBSCRIBE_REQUEST_MAX_NUM_TOPICS];
- uint8_t max_qos[MQTT_SUBSCRIBE_REQUEST_MAX_NUM_TOPICS];
-
- va_start(args, (uint16_t)packet_id);
- while(1) {
- topic[num_subs] = va_arg(args, const char*);
- if (topic[num_subs] == NULL) {
-
- break;
- }
- max_qos[num_subs] = (uint8_t) va_arg(args, unsigned int);
- ++num_subs;
- if (num_subs >= MQTT_SUBSCRIBE_REQUEST_MAX_NUM_TOPICS) {
- return MQTT_ERROR_SUBSCRIBE_TOO_MANY_TOPICS;
- }
- }
- va_end(args);
-
- fixed_header.control_type = MQTT_CONTROL_SUBSCRIBE;
- fixed_header.control_flags = 2u;
- fixed_header.remaining_length = 2u;
- for(i = 0; i < num_subs; ++i) {
-
- fixed_header.remaining_length += (uint32_t )(__mqtt_packed_cstrlen(topic[i]) + 1);
- }
-
- rv = mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- if (rv <= 0) {
- return rv;
- }
- buf += rv;
- bufsz -= (size_t)rv;
-
- if (bufsz < fixed_header.remaining_length) {
- return 0;
- }
-
- *(uint16_t*) buf = (uint16_t) MQTT_PAL_HTONS(packet_id);
- buf += 2;
-
- for(i = 0; i < num_subs; ++i) {
- buf += __mqtt_pack_str(buf, topic[i]);
- *buf++ = max_qos[i];
- }
- return buf - start;
- }
- ssize_t mqtt_unpack_unsuback_response(struct mqtt_response *mqtt_response, const uint8_t *buf)
- {
- const uint8_t *const start = buf;
- if (mqtt_response->fixed_header.remaining_length != 2) {
- return MQTT_ERROR_MALFORMED_RESPONSE;
- }
-
- mqtt_response->decoded.unsuback.packet_id = (uint16_t) MQTT_PAL_NTOHS(*(const uint16_t*) buf);
- buf += 2;
- return buf - start;
- }
- ssize_t mqtt_pack_unsubscribe_request(uint8_t *buf, size_t bufsz, uint16_t packet_id, ...) {
- va_list args;
- const uint8_t *const start = buf;
- ssize_t rv;
- struct mqtt_fixed_header fixed_header;
- unsigned int num_subs = 0;
- unsigned int i;
- const char *topic[MQTT_UNSUBSCRIBE_REQUEST_MAX_NUM_TOPICS];
-
- va_start(args, (uint16_t)packet_id);
- while(1) {
- topic[num_subs] = va_arg(args, const char*);
- if (topic[num_subs] == NULL) {
-
- break;
- }
- ++num_subs;
- if (num_subs >= MQTT_UNSUBSCRIBE_REQUEST_MAX_NUM_TOPICS) {
- return MQTT_ERROR_UNSUBSCRIBE_TOO_MANY_TOPICS;
- }
- }
- va_end(args);
-
- fixed_header.control_type = MQTT_CONTROL_UNSUBSCRIBE;
- fixed_header.control_flags = 2u;
- fixed_header.remaining_length = 2u;
- for(i = 0; i < num_subs; ++i) {
-
- fixed_header.remaining_length += (uint32_t)(__mqtt_packed_cstrlen(topic[i]));
- }
-
- rv = mqtt_pack_fixed_header(buf, bufsz, &fixed_header);
- if (rv <= 0) {
- return rv;
- }
- buf += rv;
- bufsz -= (size_t)rv;
-
- if (bufsz < fixed_header.remaining_length) {
- return 0;
- }
-
- *(uint16_t*) buf = (uint16_t) MQTT_PAL_HTONS(packet_id);
- buf += 2;
-
- for(i = 0; i < num_subs; ++i) {
- buf += __mqtt_pack_str(buf, topic[i]);
- }
- return buf - start;
- }
- #pragma GCC diagnostic pop
- void mqtt_mq_init(struct mqtt_message_queue *mq, void *buf, size_t bufsz)
- {
- mq->mem_start = buf;
- mq->mem_end = (uint8_t *)buf + bufsz;
- mq->curr = (uint8_t *)buf;
- mq->queue_tail = (struct mqtt_queued_message *)mq->mem_end;
- mq->curr_sz = (size_t)(mqtt_mq_currsz(mq));
- }
- struct mqtt_queued_message* mqtt_mq_register(struct mqtt_message_queue *mq, size_t nbytes)
- {
-
- --(mq->queue_tail);
- mq->queue_tail->start = mq->curr;
- mq->queue_tail->size = nbytes;
- mq->queue_tail->state = MQTT_QUEUED_UNSENT;
-
- mq->curr += nbytes;
- mq->curr_sz = (size_t)(mqtt_mq_currsz(mq));
- return mq->queue_tail;
- }
- void mqtt_mq_clean(struct mqtt_message_queue *mq) {
- struct mqtt_queued_message *new_head;
- for(new_head = mqtt_mq_get(mq, 0); new_head >= mq->queue_tail; --new_head) {
- if (new_head->state != MQTT_QUEUED_COMPLETE) break;
- }
-
-
- if (new_head < mq->queue_tail) {
- mq->curr = (uint8_t *)mq->mem_start;
- mq->queue_tail = (struct mqtt_queued_message *)mq->mem_end;
- mq->curr_sz = (size_t)(mqtt_mq_currsz(mq));
- return;
- } else if (new_head == mqtt_mq_get(mq, 0)) {
-
- return;
- }
-
- size_t n = (size_t)(mq->curr - new_head->start);
- size_t removing = (size_t)(new_head->start - (uint8_t*) mq->mem_start);
- memmove(mq->mem_start, new_head->start, n);
- mq->curr = (uint8_t *)((uint8_t *)mq->mem_start + n);
-
- ssize_t new_tail_idx = new_head - mq->queue_tail;
- memmove(mqtt_mq_get(mq, new_tail_idx), mq->queue_tail, (sizeof(struct mqtt_queued_message) * (size_t)(new_tail_idx + 1)));
- mq->queue_tail = mqtt_mq_get(mq, new_tail_idx);
-
- for(ssize_t i = 0; i < new_tail_idx + 1; ++i) {
- mqtt_mq_get(mq, i)->start -= removing;
- }
-
- mq->curr_sz = (size_t)(mqtt_mq_currsz(mq));
- }
- struct mqtt_queued_message* mqtt_mq_find(struct mqtt_message_queue *mq, enum MQTTControlPacketType control_type, uint16_t *packet_id)
- {
- struct mqtt_queued_message *curr;
- for(curr = mqtt_mq_get(mq, 0); curr >= mq->queue_tail; --curr) {
- if (curr->control_type == control_type) {
- if ((packet_id == NULL && curr->state != MQTT_QUEUED_COMPLETE) ||
- (packet_id != NULL && *packet_id == curr->packet_id)) {
- return curr;
- }
- }
- }
- return NULL;
- }
- ssize_t mqtt_unpack_response(struct mqtt_response* response, const uint8_t *buf, size_t bufsz) {
- const uint8_t *const start = buf;
- ssize_t rv = mqtt_unpack_fixed_header(response, buf, bufsz);
- if (rv <= 0) return rv;
- else buf += rv;
- switch(response->fixed_header.control_type) {
- case MQTT_CONTROL_CONNACK:
- rv = mqtt_unpack_connack_response(response, buf);
- break;
- case MQTT_CONTROL_PUBLISH:
- rv = mqtt_unpack_publish_response(response, buf);
- break;
- case MQTT_CONTROL_PUBACK:
- rv = mqtt_unpack_pubxxx_response(response, buf);
- break;
- case MQTT_CONTROL_PUBREC:
- rv = mqtt_unpack_pubxxx_response(response, buf);
- break;
- case MQTT_CONTROL_PUBREL:
- rv = mqtt_unpack_pubxxx_response(response, buf);
- break;
- case MQTT_CONTROL_PUBCOMP:
- rv = mqtt_unpack_pubxxx_response(response, buf);
- break;
- case MQTT_CONTROL_SUBACK:
- rv = mqtt_unpack_suback_response(response, buf);
- break;
- case MQTT_CONTROL_UNSUBACK:
- rv = mqtt_unpack_unsuback_response(response, buf);
- break;
- case MQTT_CONTROL_PINGRESP:
- return rv;
- default:
- return MQTT_ERROR_RESPONSE_INVALID_CONTROL_TYPE;
- }
- if (rv < 0) return rv;
- buf += rv;
- return buf - start;
- }
- ssize_t __mqtt_pack_str(uint8_t *buf, const char* str) {
- uint16_t length = (uint16_t)strlen(str);
-
- *(uint16_t*) buf = (uint16_t) MQTT_PAL_HTONS(length);
- buf += 2;
-
- for(int i = 0; i < length; ++i) {
- *(buf++) = (uint8_t)str[i];
- }
-
-
- return length + 2;
- }
- static const char *MQTT_ERRORS_STR[] = {
- "MQTT_UNKNOWN_ERROR",
- __ALL_MQTT_ERRORS(GENERATE_STRING)
- };
- const char* mqtt_error_str(enum MQTTErrors error) {
- int offset = error - MQTT_ERROR_UNKNOWN;
- if (offset >= 0) {
- return MQTT_ERRORS_STR[offset];
- } else if (error == 0) {
- return "MQTT_ERROR: Buffer too small.";
- } else if (error > 0) {
- return "MQTT_OK";
- } else {
- return MQTT_ERRORS_STR[0];
- }
- }
|