aqhomecontrol/aqhome/mqtt/msg_mqtt_publish.c

/****************************************************************************
 * This file is part of the project AqHome.
 * AqHome (c) by 2023 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/mqtt/msg_mqtt_publish.h"

#include <gwenhywfar/debug.h>



static int _dumpPayload(uint8_t flags, const uint8_t *payloadPtr, uint32_t payloadLen, GWEN_BUFFER *dbuf);
static void _appendStringWithLen(GWEN_BUFFER *buf, const char *s);
static int _dumpString(const uint8_t *ptr, uint32_t len, GWEN_BUFFER *buf);





GWEN_MSG *GWEN_PublishMqttMsg_new(uint8_t flags, uint16_t packetId, const char *sTopic, const uint8_t *messagePtr, uint32_t messageLen)
{
  if (sTopic && *sTopic && messagePtr && messageLen) {
    GWEN_MSG *msg;
    GWEN_BUFFER *buf;

    buf=GWEN_Buffer_new(0, 64, 0, 1);
    _appendStringWithLen(buf, sTopic);
    if (flags & (AQH_MQTTMSG_FLAGS_QOS2 | AQH_MQTTMSG_FLAGS_QOS1)) {
      GWEN_Buffer_AppendByte(buf, (packetId>>8) & 0xff);
      GWEN_Buffer_AppendByte(buf, packetId & 0xff);
    }
    GWEN_Buffer_AppendByte(buf, (messageLen>>8) & 0xff);
    GWEN_Buffer_AppendByte(buf, messageLen & 0xff);
    GWEN_Buffer_AppendBytes(buf, (const char*) messagePtr, messageLen);

    msg=GWEN_MqttMsg_new(AQH_MQTTMSG_MSGTYPE_PUBLISH | flags, GWEN_Buffer_GetUsedBytes(buf), (const uint8_t*) GWEN_Buffer_GetStart(buf));
    GWEN_Buffer_free(buf);
    if (msg==NULL) {
      DBG_INFO(AQH_LOGDOMAIN, "here");
      return NULL;
    }
    return msg;
  }
  else {
    DBG_ERROR(AQH_LOGDOMAIN, "Missing topic or message");
    return NULL;
  }
}



void AQH_PublishMqttMsg_DumpToBuffer(const GWEN_MSG *msg, GWEN_BUFFER *dbuf, const char *sText)
{
  const uint8_t *msgPtr;
  uint32_t msgLen;

  msgPtr=GWEN_Msg_GetConstBuffer(msg);
  msgLen=GWEN_Msg_GetBytesInBuffer(msg);

  if (msgLen>1) {
    uint32_t payloadLen;
    const uint8_t *payloadPtr;
    int rv;

    payloadLen=GWEN_Msg_GetParsedPayloadSize(msg);
    payloadPtr=msgPtr+GWEN_Msg_GetParsedPayloadOffset(msg);

    GWEN_Buffer_AppendArgs(dbuf, "%s %s", AQH_MqttMsg_MsgTypeToString(msgPtr[0] & 0xf0), sText);

    GWEN_Buffer_AppendString(dbuf, "(");
    rv=_dumpPayload((msgPtr[0] & 0x0f), payloadPtr, payloadLen, dbuf);
    if (rv<0) {
      DBG_INFO(AQH_LOGDOMAIN, "here (%d)", rv);
    }
    else {
      GWEN_Buffer_AppendString(dbuf, ")");
    }
  }
}



int _dumpPayload(uint8_t flags, const uint8_t *payloadPtr, uint32_t payloadLen, GWEN_BUFFER *dbuf)
{
  int rv;

  GWEN_Buffer_AppendString(dbuf, "flags:");
  if (flags & AQH_MQTTMSG_FLAGS_DUP)
    GWEN_Buffer_AppendString(dbuf, " DUP");
  if (flags & AQH_MQTTMSG_FLAGS_RETAIN)
    GWEN_Buffer_AppendString(dbuf, " RETAIN");
  GWEN_Buffer_AppendArgs(dbuf, " QOS%d", (flags>>1) & 0x03);

  GWEN_Buffer_AppendString(dbuf, " topic: ");
  rv=_dumpString(payloadPtr, payloadLen, dbuf);
  if (rv<0) {
    DBG_INFO(AQH_LOGDOMAIN, "here (%d)", rv);
    return rv;
  }
  payloadLen-=rv;
  payloadPtr+=rv;

  if (flags & (AQH_MQTTMSG_FLAGS_QOS2 | AQH_MQTTMSG_FLAGS_QOS1)) {
    if (payloadLen<2) {
      DBG_ERROR(AQH_LOGDOMAIN, "Msg too small");
      return GWEN_ERROR_BAD_DATA;
    }
    else {
      GWEN_Buffer_AppendArgs(dbuf, " packet id: %d", (payloadPtr[0]<<8)+payloadPtr[1]);
      payloadLen-=2;
      payloadPtr+=2;
    }
  }

  GWEN_Buffer_AppendString(dbuf, " message: ");
  rv=_dumpString(payloadPtr, payloadLen, dbuf);
  if (rv<0) {
    DBG_INFO(AQH_LOGDOMAIN, "here (%d)", rv);
    return rv;
  }
  payloadLen-=rv;
  payloadPtr+=rv;

  return 0;
}



void _appendStringWithLen(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);
}



int _dumpString(const uint8_t *ptr, uint32_t len, GWEN_BUFFER *buf)
{
  if (len>1) {
    int 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;
      }
      GWEN_Buffer_AppendBytes(buf, (const char*) ptr+2, slen);
    }
    return slen+2;
  }
  return GWEN_ERROR_BAD_DATA;
}