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@1:7fa362fa563f, 2016-01-06 (annotated)

Committer:
icraggs
Date:
Wed Jan 06 14:19:27 2016 +0000
Revision:
1:7fa362fa563f
Parent:
0:c524a894b5e8 
Internal function name changes to avoid name clashes

Who changed what in which revision?

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