Ian Craggs / MQTTSNPacket

Low level MQTTSN packet library, part of the Eclipse Paho project: http://eclipse.org/paho

Dependents:   MQTTSN sara-n200-hello-mqtt-sn MQTTSN_2

The master source for this project is held at: https://github.com/eclipse/paho.mqtt-sn.embedded-c

MQTTSNPacket.c@0:c524a894b5e8, 2015-02-26 (annotated)

Committer:
icraggs
Date:
Thu Feb 26 15:59:36 2015 +0000
Revision:
0:c524a894b5e8
Child:
1:7fa362fa563f
First version that works nicely :-)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
icraggs 0:c524a894b5e8 1 /*******************************************************************************
icraggs 0:c524a894b5e8 2 * Copyright (c) 2014 IBM Corp.
icraggs 0:c524a894b5e8 3 *
icraggs 0:c524a894b5e8 4 * All rights reserved. This program and the accompanying materials
icraggs 0:c524a894b5e8 5 * are made available under the terms of the Eclipse Public License v1.0
icraggs 0:c524a894b5e8 6 * and Eclipse Distribution License v1.0 which accompany this distribution.
icraggs 0:c524a894b5e8 7 *
icraggs 0:c524a894b5e8 8 * The Eclipse Public License is available at
icraggs 0:c524a894b5e8 9 * http://www.eclipse.org/legal/epl-v10.html
icraggs 0:c524a894b5e8 10 * and the Eclipse Distribution License is available at
icraggs 0:c524a894b5e8 11 * http://www.eclipse.org/org/documents/edl-v10.php.
icraggs 0:c524a894b5e8 12 *
icraggs 0:c524a894b5e8 13 * Contributors:
icraggs 0:c524a894b5e8 14 * Ian Craggs - initial API and implementation and/or initial documentation
icraggs 0:c524a894b5e8 15 *******************************************************************************/
icraggs 0:c524a894b5e8 16
icraggs 0:c524a894b5e8 17 #include "StackTrace.h"
icraggs 0:c524a894b5e8 18 #include "MQTTSNPacket.h"
icraggs 0:c524a894b5e8 19
icraggs 0:c524a894b5e8 20 #include <string.h>
icraggs 0:c524a894b5e8 21
icraggs 0:c524a894b5e8 22 static char* packet_names[] =
icraggs 0:c524a894b5e8 23 {
icraggs 0:c524a894b5e8 24 "ADVERTISE", "SEARCHGW", "GWINFO", "RESERVED", "CONNECT", "CONNACK",
icraggs 0:c524a894b5e8 25 "WILLTOPICREQ", "WILLTOPIC", "WILLMSGREQ", "WILLMSG", "REGISTER", "REGACK",
icraggs 0:c524a894b5e8 26 "PUBLISH", "PUBACK", "PUBCOMP", "PUBREC", "PUBREL", "RESERVED",
icraggs 0:c524a894b5e8 27 "SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK", "PINGREQ", "PINGRESP",
icraggs 0:c524a894b5e8 28 "DISCONNECT", "RESERVED", "WILLTOPICUPD", "WILLTOPICRESP", "WILLMSGUPD",
icraggs 0:c524a894b5e8 29 "WILLMSGRESP"
icraggs 0:c524a894b5e8 30 };
icraggs 0:c524a894b5e8 31
icraggs 0:c524a894b5e8 32
icraggs 0:c524a894b5e8 33 /**
icraggs 0:c524a894b5e8 34 * Returns a character string representing the packet name given a MsgType code
icraggs 0:c524a894b5e8 35 * @param code MsgType code
icraggs 0:c524a894b5e8 36 * @return the corresponding packet name
icraggs 0:c524a894b5e8 37 */
icraggs 0:c524a894b5e8 38 char* MQTTSNPacket_name(int code)
icraggs 0:c524a894b5e8 39 {
icraggs 0:c524a894b5e8 40 return (code >= 0 && code <= MQTTSN_WILLMSGRESP) ? packet_names[code] : "UNKNOWN";
icraggs 0:c524a894b5e8 41 }
icraggs 0:c524a894b5e8 42
icraggs 0:c524a894b5e8 43
icraggs 0:c524a894b5e8 44 /**
icraggs 0:c524a894b5e8 45 * Calculates the full packet length including length field
icraggs 0:c524a894b5e8 46 * @param length the length of the MQTT-SN packet without the length field
icraggs 0:c524a894b5e8 47 * @return the total length of the MQTT-SN packet including the length field
icraggs 0:c524a894b5e8 48 */
icraggs 0:c524a894b5e8 49 int MQTTSNPacket_len(int length)
icraggs 0:c524a894b5e8 50 {
icraggs 0:c524a894b5e8 51 return (length > 255) ? length + 3 : length + 1;
icraggs 0:c524a894b5e8 52 }
icraggs 0:c524a894b5e8 53
icraggs 0:c524a894b5e8 54
icraggs 0:c524a894b5e8 55 /**
icraggs 0:c524a894b5e8 56 * Encodes the MQTT-SN message length
icraggs 0:c524a894b5e8 57 * @param buf the buffer into which the encoded data is written
icraggs 0:c524a894b5e8 58 * @param length the length to be encoded
icraggs 0:c524a894b5e8 59 * @return the number of bytes written to the buffer
icraggs 0:c524a894b5e8 60 */
icraggs 0:c524a894b5e8 61 int MQTTSNPacket_encode(unsigned char* buf, int length)
icraggs 0:c524a894b5e8 62 {
icraggs 0:c524a894b5e8 63 int rc = 0;
icraggs 0:c524a894b5e8 64
icraggs 0:c524a894b5e8 65 FUNC_ENTRY;
icraggs 0:c524a894b5e8 66 if (length > 255)
icraggs 0:c524a894b5e8 67 {
icraggs 0:c524a894b5e8 68 buf[rc++] = 0x01;
icraggs 0:c524a894b5e8 69 writeInt(&buf, length);
icraggs 0:c524a894b5e8 70 rc += 2;
icraggs 0:c524a894b5e8 71 }
icraggs 0:c524a894b5e8 72 else
icraggs 0:c524a894b5e8 73 buf[rc++] = length;
icraggs 0:c524a894b5e8 74
icraggs 0:c524a894b5e8 75 FUNC_EXIT_RC(rc);
icraggs 0:c524a894b5e8 76 return rc;
icraggs 0:c524a894b5e8 77 }
icraggs 0:c524a894b5e8 78
icraggs 0:c524a894b5e8 79
icraggs 0:c524a894b5e8 80 /**
icraggs 0:c524a894b5e8 81 * Obtains the MQTT-SN packet length from received data
icraggs 0:c524a894b5e8 82 * @param getcharfn pointer to function to read the next character from the data source
icraggs 0:c524a894b5e8 83 * @param value the decoded length returned
icraggs 0:c524a894b5e8 84 * @return the number of bytes read from the socket
icraggs 0:c524a894b5e8 85 */
icraggs 0:c524a894b5e8 86 int MQTTSNPacket_decode(unsigned char* buf, int buflen, int* value)
icraggs 0:c524a894b5e8 87 {
icraggs 0:c524a894b5e8 88 int len = MQTTSNPACKET_READ_ERROR;
icraggs 0:c524a894b5e8 89 #define MAX_NO_OF_LENGTH_BYTES 3
icraggs 0:c524a894b5e8 90
icraggs 0:c524a894b5e8 91 FUNC_ENTRY;
icraggs 0:c524a894b5e8 92 if (buflen <= 0)
icraggs 0:c524a894b5e8 93 goto exit;
icraggs 0:c524a894b5e8 94
icraggs 0:c524a894b5e8 95 if (buf[0] == 1)
icraggs 0:c524a894b5e8 96 {
icraggs 0:c524a894b5e8 97 unsigned char* bufptr = &buf[1];
icraggs 0:c524a894b5e8 98 if (buflen < 3)
icraggs 0:c524a894b5e8 99 goto exit;
icraggs 0:c524a894b5e8 100 *value = readInt(&bufptr);
icraggs 0:c524a894b5e8 101 len = 3;
icraggs 0:c524a894b5e8 102 }
icraggs 0:c524a894b5e8 103 else
icraggs 0:c524a894b5e8 104 {
icraggs 0:c524a894b5e8 105 *value = buf[0];
icraggs 0:c524a894b5e8 106 len = 1;
icraggs 0:c524a894b5e8 107 }
icraggs 0:c524a894b5e8 108 exit:
icraggs 0:c524a894b5e8 109 FUNC_EXIT_RC(len);
icraggs 0:c524a894b5e8 110 return len;
icraggs 0:c524a894b5e8 111 }
icraggs 0:c524a894b5e8 112
icraggs 0:c524a894b5e8 113
icraggs 0:c524a894b5e8 114 /**
icraggs 0:c524a894b5e8 115 * Calculates an integer from two bytes read from the input buffer
icraggs 0:c524a894b5e8 116 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
icraggs 0:c524a894b5e8 117 * @return the integer value calculated
icraggs 0:c524a894b5e8 118 */
icraggs 0:c524a894b5e8 119 int readInt(unsigned char** pptr)
icraggs 0:c524a894b5e8 120 {
icraggs 0:c524a894b5e8 121 unsigned char* ptr = *pptr;
icraggs 0:c524a894b5e8 122 int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1));
icraggs 0:c524a894b5e8 123 *pptr += 2;
icraggs 0:c524a894b5e8 124 return len;
icraggs 0:c524a894b5e8 125 }
icraggs 0:c524a894b5e8 126
icraggs 0:c524a894b5e8 127
icraggs 0:c524a894b5e8 128 /**
icraggs 0:c524a894b5e8 129 * Reads one character from the input buffer.
icraggs 0:c524a894b5e8 130 * @param pptr pointer to the input buffer - incremented by the number of bytes used & returned
icraggs 0:c524a894b5e8 131 * @return the character read
icraggs 0:c524a894b5e8 132 */
icraggs 0:c524a894b5e8 133 char readChar(unsigned char** pptr)
icraggs 0:c524a894b5e8 134 {
icraggs 0:c524a894b5e8 135 char c = **pptr;
icraggs 0:c524a894b5e8 136 (*pptr)++;
icraggs 0:c524a894b5e8 137 return c;
icraggs 0:c524a894b5e8 138 }
icraggs 0:c524a894b5e8 139
icraggs 0:c524a894b5e8 140
icraggs 0:c524a894b5e8 141 /**
icraggs 0:c524a894b5e8 142 * Writes one character to an output buffer.
icraggs 0:c524a894b5e8 143 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
icraggs 0:c524a894b5e8 144 * @param c the character to write
icraggs 0:c524a894b5e8 145 */
icraggs 0:c524a894b5e8 146 void writeChar(unsigned char** pptr, char c)
icraggs 0:c524a894b5e8 147 {
icraggs 0:c524a894b5e8 148 **pptr = (unsigned char)c;
icraggs 0:c524a894b5e8 149 (*pptr)++;
icraggs 0:c524a894b5e8 150 }
icraggs 0:c524a894b5e8 151
icraggs 0:c524a894b5e8 152
icraggs 0:c524a894b5e8 153 /**
icraggs 0:c524a894b5e8 154 * Writes an integer as 2 bytes to an output buffer.
icraggs 0:c524a894b5e8 155 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
icraggs 0:c524a894b5e8 156 * @param anInt the integer to write: 0 to 65535
icraggs 0:c524a894b5e8 157 */
icraggs 0:c524a894b5e8 158 void writeInt(unsigned char** pptr, int anInt)
icraggs 0:c524a894b5e8 159 {
icraggs 0:c524a894b5e8 160 **pptr = (unsigned char)(anInt / 256);
icraggs 0:c524a894b5e8 161 (*pptr)++;
icraggs 0:c524a894b5e8 162 **pptr = (unsigned char)(anInt % 256);
icraggs 0:c524a894b5e8 163 (*pptr)++;
icraggs 0:c524a894b5e8 164 }
icraggs 0:c524a894b5e8 165
icraggs 0:c524a894b5e8 166
icraggs 0:c524a894b5e8 167 /**
icraggs 0:c524a894b5e8 168 * Writes a "UTF" string to an output buffer. Converts C string to length-delimited.
icraggs 0:c524a894b5e8 169 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
icraggs 0:c524a894b5e8 170 * @param string the C string to write
icraggs 0:c524a894b5e8 171 */
icraggs 0:c524a894b5e8 172 void writeCString(unsigned char** pptr, char* string)
icraggs 0:c524a894b5e8 173 {
icraggs 0:c524a894b5e8 174 int len = strlen(string);
icraggs 0:c524a894b5e8 175 memcpy(*pptr, string, len);
icraggs 0:c524a894b5e8 176 *pptr += len;
icraggs 0:c524a894b5e8 177 }
icraggs 0:c524a894b5e8 178
icraggs 0:c524a894b5e8 179
icraggs 0:c524a894b5e8 180 int getLenStringLen(char* ptr)
icraggs 0:c524a894b5e8 181 {
icraggs 0:c524a894b5e8 182 int len = 256*((unsigned char)(*ptr)) + (unsigned char)(*(ptr+1));
icraggs 0:c524a894b5e8 183 return len;
icraggs 0:c524a894b5e8 184 }
icraggs 0:c524a894b5e8 185
icraggs 0:c524a894b5e8 186
icraggs 0:c524a894b5e8 187 void writeMQTTSNString(unsigned char** pptr, MQTTSNString MQTTSNString)
icraggs 0:c524a894b5e8 188 {
icraggs 0:c524a894b5e8 189 if (MQTTSNString.lenstring.len > 0)
icraggs 0:c524a894b5e8 190 {
icraggs 0:c524a894b5e8 191 memcpy(*pptr, MQTTSNString.lenstring.data, MQTTSNString.lenstring.len);
icraggs 0:c524a894b5e8 192 *pptr += MQTTSNString.lenstring.len;
icraggs 0:c524a894b5e8 193 }
icraggs 0:c524a894b5e8 194 else if (MQTTSNString.cstring)
icraggs 0:c524a894b5e8 195 writeCString(pptr, MQTTSNString.cstring);
icraggs 0:c524a894b5e8 196 }
icraggs 0:c524a894b5e8 197
icraggs 0:c524a894b5e8 198
icraggs 0:c524a894b5e8 199 /**
icraggs 0:c524a894b5e8 200 * @param MQTTSNString the MQTTSNString structure into which the data is to be read
icraggs 0:c524a894b5e8 201 * @param pptr pointer to the output buffer - incremented by the number of bytes used & returned
icraggs 0:c524a894b5e8 202 * @param enddata pointer to the end of the data: do not read beyond
icraggs 0:c524a894b5e8 203 * @return 1 if successful, 0 if not
icraggs 0:c524a894b5e8 204 */
icraggs 0:c524a894b5e8 205 int readMQTTSNString(MQTTSNString* MQTTSNString, unsigned char** pptr, unsigned char* enddata)
icraggs 0:c524a894b5e8 206 {
icraggs 0:c524a894b5e8 207 int rc = 0;
icraggs 0:c524a894b5e8 208
icraggs 0:c524a894b5e8 209 FUNC_ENTRY;
icraggs 0:c524a894b5e8 210 MQTTSNString->lenstring.len = enddata - *pptr;
icraggs 0:c524a894b5e8 211 if (MQTTSNString->lenstring.len > 0)
icraggs 0:c524a894b5e8 212 {
icraggs 0:c524a894b5e8 213 MQTTSNString->lenstring.data = (char*)*pptr;
icraggs 0:c524a894b5e8 214 *pptr += MQTTSNString->lenstring.len;
icraggs 0:c524a894b5e8 215 }
icraggs 0:c524a894b5e8 216 else
icraggs 0:c524a894b5e8 217 MQTTSNString->lenstring.data = NULL;
icraggs 0:c524a894b5e8 218 MQTTSNString->cstring = NULL;
icraggs 0:c524a894b5e8 219 rc = 1;
icraggs 0:c524a894b5e8 220 FUNC_EXIT_RC(rc);
icraggs 0:c524a894b5e8 221 return rc;
icraggs 0:c524a894b5e8 222 }
icraggs 0:c524a894b5e8 223
icraggs 0:c524a894b5e8 224
icraggs 0:c524a894b5e8 225 /**
icraggs 0:c524a894b5e8 226 * Return the length of the MQTTSNString - C string if there is one, otherwise the length delimited string
icraggs 0:c524a894b5e8 227 * @param MQTTSNString the string to return the length of
icraggs 0:c524a894b5e8 228 * @return the length of the string
icraggs 0:c524a894b5e8 229 */
icraggs 0:c524a894b5e8 230 int MQTTSNstrlen(MQTTSNString MQTTSNString)
icraggs 0:c524a894b5e8 231 {
icraggs 0:c524a894b5e8 232 int rc = 0;
icraggs 0:c524a894b5e8 233
icraggs 0:c524a894b5e8 234 if (MQTTSNString.cstring)
icraggs 0:c524a894b5e8 235 rc = strlen(MQTTSNString.cstring);
icraggs 0:c524a894b5e8 236 else
icraggs 0:c524a894b5e8 237 rc = MQTTSNString.lenstring.len;
icraggs 0:c524a894b5e8 238 return rc;
icraggs 0:c524a894b5e8 239 }
icraggs 0:c524a894b5e8 240
icraggs 0:c524a894b5e8 241
icraggs 0:c524a894b5e8 242 /**
icraggs 0:c524a894b5e8 243 * Helper function to read packet data from some source into a buffer
icraggs 0:c524a894b5e8 244 * @param buf the buffer into which the packet will be serialized
icraggs 0:c524a894b5e8 245 * @param buflen the length in bytes of the supplied buffer
icraggs 0:c524a894b5e8 246 * @param getfn pointer to a function which will read any number of bytes from the needed source
icraggs 0:c524a894b5e8 247 * @return integer MQTT packet type, or MQTTSNPACKET_READ_ERROR on error
icraggs 0:c524a894b5e8 248 */
icraggs 0:c524a894b5e8 249 int MQTTSNPacket_read(unsigned char* buf, int buflen, int (*getfn)(unsigned char*, int))
icraggs 0:c524a894b5e8 250 {
icraggs 0:c524a894b5e8 251 int rc = MQTTSNPACKET_READ_ERROR;
icraggs 0:c524a894b5e8 252 const int MQTTSN_MIN_PACKET_LENGTH = 3;
icraggs 0:c524a894b5e8 253 int len = 0; /* the length of the whole packet including length field */
icraggs 0:c524a894b5e8 254 int lenlen = 0;
icraggs 0:c524a894b5e8 255 int datalen = 0;
icraggs 0:c524a894b5e8 256
icraggs 0:c524a894b5e8 257 /* 1. read a packet - UDP style */
icraggs 0:c524a894b5e8 258 if ((len = (*getfn)(buf, buflen)) < MQTTSN_MIN_PACKET_LENGTH)
icraggs 0:c524a894b5e8 259 goto exit;
icraggs 0:c524a894b5e8 260
icraggs 0:c524a894b5e8 261 /* 2. read the length. This is variable in itself */
icraggs 0:c524a894b5e8 262 lenlen = MQTTSNPacket_decode(buf, len, &datalen);
icraggs 0:c524a894b5e8 263 if (datalen != len)
icraggs 0:c524a894b5e8 264 goto exit; /* there was an error */
icraggs 0:c524a894b5e8 265
icraggs 0:c524a894b5e8 266 rc = buf[lenlen]; /* return the packet type */
icraggs 0:c524a894b5e8 267 exit:
icraggs 0:c524a894b5e8 268 return rc;
icraggs 0:c524a894b5e8 269 }
icraggs 0:c524a894b5e8 270
icraggs 0:c524a894b5e8 271
icraggs 0:c524a894b5e8 272