72 lines
1.5 KiB
C
72 lines
1.5 KiB
C
|
/*
|
||
|
|
* mqtt.c
|
||
|
|
*
|
||
|
|
* Created on: 08/04/2026
|
||
|
|
* Author: carec
|
||
|
|
*/
|
||
|
|
#include "mqtt.h"
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
static uint16_t put_str(uint8_t *p, const char *s)
|
||
|
|
{
|
||
|
|
uint16_t len = strlen(s);
|
||
|
|
p[0] = len >> 8;
|
||
|
|
p[1] = len & 0xFF;
|
||
|
|
memcpy(&p[2], s, len);
|
||
|
|
return 2 + len;
|
||
|
|
}
|
||
|
|
|
||
|
|
static uint16_t enc_len(uint8_t *out, uint32_t len)
|
||
|
|
{
|
||
|
|
uint16_t i = 0;
|
||
|
|
do {
|
||
|
|
uint8_t b = len % 128;
|
||
|
|
len /= 128;
|
||
|
|
if (len > 0) b |= 0x80;
|
||
|
|
out[i++] = b;
|
||
|
|
} while (len);
|
||
|
|
return i;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t mqtt_build_connect(uint8_t *out,
|
||
|
|
const char *client_id,
|
||
|
|
const char *username,
|
||
|
|
const char *password,
|
||
|
|
uint16_t keepalive)
|
||
|
|
{
|
||
|
|
uint8_t tmp[256];
|
||
|
|
uint16_t i = 0;
|
||
|
|
|
||
|
|
i += put_str(&tmp[i], "MQTT");
|
||
|
|
tmp[i++] = 0x04;
|
||
|
|
|
||
|
|
uint8_t flags = 0x02;
|
||
|
|
if (username) flags |= 0x80;
|
||
|
|
if (password) flags |= 0x40;
|
||
|
|
|
||
|
|
tmp[i++] = flags;
|
||
|
|
tmp[i++] = keepalive >> 8;
|
||
|
|
tmp[i++] = keepalive & 0xFF;
|
||
|
|
|
||
|
|
i += put_str(&tmp[i], client_id);
|
||
|
|
if (username) i += put_str(&tmp[i], username);
|
||
|
|
if (password) i += put_str(&tmp[i], password);
|
||
|
|
|
||
|
|
uint16_t o = 0;
|
||
|
|
out[o++] = 0x10;
|
||
|
|
o += enc_len(&out[o], i);
|
||
|
|
memcpy(&out[o], tmp, i);
|
||
|
|
o += i;
|
||
|
|
|
||
|
|
return o;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool mqtt_parse_connack(const uint8_t *buf, uint16_t len)
|
||
|
|
{
|
||
|
|
if (len < 4) return false;
|
||
|
|
if (buf[0] != 0x20) return false;
|
||
|
|
if (buf[3] != 0x00) return false;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|