aqhomecontrol/aqhome/mqtt/msg_mqtt_connect.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_connect.h"

#include <gwenhywfar/debug.h>



static int _dumpPayload(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_ConnectMqttMsg_new(const char *protoName,
                                  uint8_t protoLevel,
                                  uint8_t connectFlags,
                                  uint16_t keepAliveTime,
                                  const char *clientId,
                                  const char *userName,
                                  const char *password)
{
  GWEN_MSG *msg;
  GWEN_BUFFER *buf;

  buf=GWEN_Buffer_new(0, 64, 0, 1);
  _appendStringWithLen(buf, protoName?protoName:"MQTT");
  GWEN_Buffer_AppendByte(buf, protoLevel?protoLevel:4);
  GWEN_Buffer_AppendByte(buf, connectFlags);
  GWEN_Buffer_AppendByte(buf, (keepAliveTime>>8) & 0xff);
  GWEN_Buffer_AppendByte(buf, keepAliveTime & 0xff);

  _appendStringWithLen(buf, clientId);
  /* here could be inserted: will topic, will message */
  if (connectFlags & AQH_MQTTMSG_CONNECT_FLAGS_USERNAME)
    _appendStringWithLen(buf, userName);
  if (connectFlags & AQH_MQTTMSG_CONNECT_FLAGS_PASSWD)
    _appendStringWithLen(buf, password);

  msg=GWEN_MqttMsg_new(AQH_MQTTMSG_MSGTYPE_CONNECT, 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;
}



void AQH_ConnectMqttMsg_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(payloadPtr, payloadLen, dbuf);
    if (rv<0) {
      DBG_INFO(AQH_LOGDOMAIN, "here (%d)", rv);
    }
    else {
      GWEN_Buffer_AppendString(dbuf, ")");
    }
  }
}



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

  GWEN_Buffer_AppendString(dbuf, "proto: ");
  rv=_dumpString(payloadPtr, payloadLen, dbuf);
  if (rv<0) {
    DBG_INFO(AQH_LOGDOMAIN, "here (%d)", rv);
    return rv;
  }
  payloadLen-=rv;
  payloadPtr+=rv;
  if (payloadLen<4) { /* expect at least proto ver(1), flags(1) and keepAlive(2) bytes  */
    DBG_ERROR(AQH_LOGDOMAIN, "Msg too small");
    return GWEN_ERROR_BAD_DATA;
  }
  else {
    GWEN_Buffer_AppendArgs(dbuf, " proto ver: %d", payloadPtr[0]);
    payloadLen--;
    payloadPtr++;
  
    connectFlags=payloadPtr[0];
    GWEN_Buffer_AppendArgs(dbuf, " flags: %02x", connectFlags);
    payloadLen--;
    payloadPtr++;
  
    keepAliveTime=(payloadPtr[0]<<8)+payloadPtr[1];
    GWEN_Buffer_AppendArgs(dbuf, " keep alive: %d", keepAliveTime);
    payloadLen-=2;
    payloadPtr+=2;
  }

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

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

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

  if (connectFlags & AQH_MQTTMSG_CONNECT_FLAGS_USERNAME) {
    GWEN_Buffer_AppendString(dbuf, " username: ");
    rv=_dumpString(payloadPtr, payloadLen, dbuf);
    if (rv<0) {
      DBG_INFO(AQH_LOGDOMAIN, "here (%d)", rv);
      return rv;
    }
    payloadLen-=rv;
    payloadPtr+=rv;
  }
  if (connectFlags & AQH_MQTTMSG_CONNECT_FLAGS_PASSWD) {
    GWEN_Buffer_AppendString(dbuf, " password: ");
    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;
}