aqhomecontrol/aqhome/msg/mqtt/m_mqtt.c

/****************************************************************************
 * This file is part of the project AqHome.
 * AqHome (c) by 2025 Martin Preuss, all rights reserved.
 *
 * The license for this file can be found in the file COPYING which you
 * should have received along with this file.
 ****************************************************************************/

#ifdef HAVE_CONFIG_H
# include <config.h>
#endif

#include "aqhome/msg/mqtt/m_mqtt.h"

#include <gwenhywfar/text.h>
#include <gwenhywfar/debug.h>



/* ------------------------------------------------------------------------------------------------
 * forward declarations
 * ------------------------------------------------------------------------------------------------
 */




/* ------------------------------------------------------------------------------------------------
 * implementation
 * ------------------------------------------------------------------------------------------------
 */

AQH_MESSAGE *AQH_MqttMessage_new(uint8_t typeAndFlags, uint32_t payloadLen, const uint8_t *payload)
{
  if (payloadLen>0xfffffffu) {
    DBG_ERROR(AQH_LOGDOMAIN, "Too many bytes in payload, can't encode into MQTT message");
    return NULL;
  }
  else {
    AQH_MESSAGE *msg;
    uint8_t *ptr;
    uint32_t msgBufLen;
    uint32_t len;
    uint32_t bytesUsed=0;
    int i=0;

    msgBufLen=16+payloadLen;
    msg=AQH_Message_new();
    AQH_Message_SetData(msg, NULL, msgBufLen);                /* auto-malloc len bytes */

    ptr=AQH_Message_GetMsgPointer(msg);
    *(ptr++)=typeAndFlags;
    bytesUsed++;

    /* store address length */
    len=payloadLen;
    do {
      uint8_t b;

      b=len & 0x7f;
      len>>=7;
      if (len)
	b|=0x80;
      *(ptr++)=b;
      bytesUsed++;
    } while(len && i<4);
    if (payload && payloadLen) {
      memmove(ptr, payload, payloadLen);
      bytesUsed+=payloadLen;
    }
    AQH_Message_SetUsedSize(msg, bytesUsed);
    return msg;
  }
}



void AQH_MqttMessage_AppendStringWithLenToBuffer(GWEN_BUFFER *buf, const char *s)
{
  unsigned int len;

  len=strlen(s);
  GWEN_Buffer_AppendByte(buf, (len>>8) & 0xff);
  GWEN_Buffer_AppendByte(buf, len & 0xff);
  if (s && *s)
    GWEN_Buffer_AppendString(buf, s);
}



char *AQH_MqttMessage_ExtractStringAt(const uint8_t *ptr, uint32_t len)
{
  if (len>1) {
    uint32_t slen;

    slen=(ptr[0]<<8)+ptr[1];
    if (slen) {
      char *result;

      if (slen>(len-2)) {
        DBG_ERROR(AQH_LOGDOMAIN, "Invalid string length (%lu, remaining %lu)",
                  (unsigned long int) slen, (unsigned long int) len);
        return NULL;
      }

      result=(char*) malloc(slen+1);
      if (result==NULL) {
        DBG_ERROR(AQH_LOGDOMAIN, "Error on malloc");
        return NULL;
      }
      memmove(result, ptr+2, slen);
      result[slen]=0;
      return result;
    }
  }
  return NULL;
}



int AQH_MqttMessage_GetOffsetOfPayload(const AQH_MESSAGE *msg)
{
  const uint8_t *ptr;
  int usedBytes;
  int idx;

  ptr=AQH_Message_GetMsgPointer(msg);
  usedBytes=AQH_Message_GetUsedSize(msg);
  if (usedBytes>5)
    usedBytes=5;
  for(idx=1; idx<usedBytes; idx++) {
    uint8_t b;

    b=ptr[idx];
    if (!(b & 0x80))
      break;
  }
  if (idx>=usedBytes) {
    DBG_ERROR(AQH_LOGDOMAIN, "Could not determine length of size field");
    return GWEN_ERROR_BAD_DATA;
  }
  return idx+1;
}



int AQH_MqttMessage_GetTypeAndFlags(const AQH_MESSAGE *msg)
{
  const uint8_t *ptr;
  uint32_t usedBytes;

  ptr=AQH_Message_GetMsgPointer(msg);
  usedBytes=AQH_Message_GetUsedSize(msg);
  if (usedBytes)
    return *ptr;
  else {
    DBG_ERROR(AQH_LOGDOMAIN, "No data in message");
    return 0;
  }
}



int AQH_MqttMessage_SkipStringAt(const uint8_t *ptr, uint32_t len)
{
  if (len>1) {
    uint32_t slen;

    slen=(ptr[0]<<8)+ptr[1];
    if (slen) {
      if (slen>(len-2)) {
        DBG_ERROR(AQH_LOGDOMAIN, "Invalid string length (%lu, remaining %lu)",
                  (unsigned long int) slen, (unsigned long int) len);
        return GWEN_ERROR_BAD_DATA;
      }
    }
    return slen+2;
  }
  return GWEN_ERROR_BAD_DATA;
}



const char *AQH_MqttMessage_MsgTypeToString(uint8_t t)
{
  switch(t & 0xf0) {
  case (AQH_MQTTMSG_MSGTYPE_CONNECT & 0xf0):     return "CONNECT";
  case (AQH_MQTTMSG_MSGTYPE_CONNACK & 0xf0):     return "CONACK";
  case (AQH_MQTTMSG_MSGTYPE_PUBLISH & 0xf0):     return "PUBLISH";
  case (AQH_MQTTMSG_MSGTYPE_PUBACK & 0xf0):      return "PUBACK";
  case (AQH_MQTTMSG_MSGTYPE_PUBREC & 0xf0):      return "PUBREC";
  case (AQH_MQTTMSG_MSGTYPE_PUBREL & 0xf0):      return "PUBREL";
  case (AQH_MQTTMSG_MSGTYPE_PUBCOMP & 0xf0):     return "PUBCOMP";
  case (AQH_MQTTMSG_MSGTYPE_SUBSCRIBE & 0xf0):   return "SUBSCRIBE";
  case (AQH_MQTTMSG_MSGTYPE_SUBACK & 0xf0):      return "SUBACK";
  case (AQH_MQTTMSG_MSGTYPE_UNSUBSCRIBE & 0xf0): return "UNSUBSCRIBE";
  case (AQH_MQTTMSG_MSGTYPE_UNSUBACK & 0xf0):    return "UNSUBACK";
  case (AQH_MQTTMSG_MSGTYPE_PINGREQ & 0xf0):     return "PINGREQ";
  case (AQH_MQTTMSG_MSGTYPE_PINGRESP & 0xf0):    return "PINGRESP";
  case (AQH_MQTTMSG_MSGTYPE_DISCONNECT & 0xf0):  return "DISCONNECT";
  default:                                       return "(unknown)";
  }
}