Add toolchain and mbtk source
Change-Id: Ie12546301367ea59240bf23d5e184ad7e36e40b3
diff --git a/mbtk/mbtk_lib/src/mbtk_coap_pdu.cpp b/mbtk/mbtk_lib/src/mbtk_coap_pdu.cpp
new file mode 100755
index 0000000..c9f046d
--- /dev/null
+++ b/mbtk/mbtk_lib/src/mbtk_coap_pdu.cpp
@@ -0,0 +1,2065 @@
+/*
+Copyright (c) 2013, Ashley Mills.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// version, 2 bits
+// type, 2 bits
+// 00 Confirmable
+// 01 Non-confirmable
+// 10 Acknowledgement
+// 11 Reset
+
+// token length, 4 bits
+// length of token in bytes (only 0 to 8 bytes allowed)
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "mbtk_coap_pdu.h"
+
+//#include "mbtk_log.h"
+//#ifdef FEATURE_MBTK_ECOAP
+/// Memory-managed constructor. Buffer for PDU is dynamically sized and allocated by the object.
+/**
+ * When using this constructor, the CoapPDU class will allocate space for the PDU.
+ * Contrast this with the parameterized constructors, which allow the use of an external buffer.
+ *
+ * Note, the PDU container and space can be reused by issuing a CoapPDU::reset(). If the new PDU exceeds the
+ * space of the previously allocated memory, then further memory will be dynamically allocated.
+ *
+ * Deleting the object will free the Object container and all dynamically allocated memory.
+ *
+ * \note It would have been nice to use something like UDP_CORK or MSG_MORE, to allow separate buffers
+ * for token, options, and payload but these FLAGS aren't implemented for UDP in LwIP so stuck with one buffer for now.
+ *
+ * CoAP version defaults to 1.
+ *
+ * \sa CoapPDU::CoapPDU(uint8_t *pdu, int pduLength), CoapPDU::CoapPDU::(uint8_t *buffer, int bufferLength, int pduLength),
+ * CoapPDU:CoapPDU()~
+ *
+ */
+
+//#define printf(fmtstr) LOGD(fmtstr)
+//#define printf(fmtstr ,arg1) LOGD(fmtstr, arg1)
+//#define printf(fmtstr, arg1, arg2) LOGD(fmtstr , arg1, arg2)
+//#define printf(fmtstr, arg1, arg2, arg3) LOGD(fmtstr , arg1, arg2, arg3)
+
+/*
+void* operator new(unsigned int size, void* alloc, ds_appsrv_mem_e_type mem_type) throw()
+{
+// Do nothing
+ return alloc;
+}
+
+void operator delete(void* obj, void* alloc, ds_appsrv_mem_e_type mem_type) throw()
+{
+// Do nothing
+}
+
+*/
+
+CoapPDU::CoapPDU()
+{
+ // pdu
+ _pdu = (uint8_t*)calloc(4,sizeof(uint8_t));
+ _pduLength = 4;
+ _bufferLength = _pduLength;
+
+ //options
+ _numOptions = 0;
+ _maxAddedOptionNumber = 0;
+
+ // payload
+ _payloadPointer = NULL;
+ _payloadLength = 0;
+
+ _constructedFromBuffer = 0;
+
+ setVersion(1);
+}
+
+/// Construct object from external buffer that may be larger than actual PDU.
+/**
+ * This differs from CoapPDU::CoapPDU(uint8_t *pdu, int pduLength) in that the buffer may be larger
+ * than the actual CoAP PDU contained int the buffer. This is typically used when a large buffer is reused
+ * multiple times. Note that \b pduLength can be 0.
+ *
+ * If an actual CoAP PDU is passed in the buffer, \b pduLength should match its length. CoapPDU::validate() must
+ * be called to initiate the object before member functions can be used.
+ *
+ * A PDU constructed in this manner must be validated with CoapPDU::validate() before the member variables will be accessible.
+ *
+ * \warning The validation call parses the PDU structure to set some internal parameters. If you do
+ * not validate the PDU, then the behaviour of member access functions will be undefined.
+ *
+ * The buffer can be reused by issuing a CoapPDU::reset() but the class will not change the size of the buffer. If the
+ * newly constructed PDU exceeds the size of the buffer, the function called (for example CoapPDU::addOption) will fail.
+ *
+ * Deleting this object will only delete the Object container and will not delete the PDU buffer.
+ *
+ * \param buffer A buffer which either contains a CoAP PDU or is intended to be used to construct one.
+ * \param bufferLength The length of the buffer
+ * \param pduLength If the buffer contains a CoAP PDU, this specifies the length of the PDU within the buffer.
+ *
+ * \sa CoapPDU::CoapPDU(), CoapPDU::CoapPDU(uint8_t *pdu, int pduLength)
+ */
+CoapPDU::CoapPDU(uint8_t *buffer, int bufferLength, int pduLength)
+{
+ printf("CoapPDU(),bufferLength:%d, pduLength；%d\n",bufferLength,pduLength);
+ // sanity
+ if(pduLength<4&&pduLength!=0)
+ {
+ printf("PDU cannot have a length less than 4");
+ }
+
+ // pdu
+ _pdu = buffer;
+ _bufferLength = bufferLength;
+ if(pduLength==0)
+ {
+ // this is actually a fresh pdu, header always exists
+ _pduLength = 4;
+ // make sure header is zeroed
+ _pdu[0] = 0x00;
+ _pdu[1] = 0x00;
+ _pdu[2] = 0x00;
+ _pdu[3] = 0x00;
+ setVersion(1);
+ }
+ else
+ {
+ _pduLength = pduLength;
+ }
+
+ _constructedFromBuffer = 1;
+
+ // options
+ _numOptions = 0;
+ _maxAddedOptionNumber = 0;
+
+ // payload
+ _payloadPointer = NULL;
+ _payloadLength = 0;
+}
+
+/// Reset CoapPDU container so it can be reused to build a new PDU.
+/**
+ * This resets the CoapPDU container, setting the pdu length, option count, etc back to zero. The
+ * PDU can then be populated as if it were newly constructed.
+ *
+ * Note that the space available will depend on how the CoapPDU was originally constructed:
+ * -# CoapPDU::CoapPDU()
+ *
+ * Available space initially be \b _pduLength. But further space will be allocated as needed on demand,
+ * limited only by the OS/environment.
+ *
+ * -# CoapPDU::CoapPDU(uint8_t *pdu, int pduLength)
+ *
+ * Space is limited by the variable \b pduLength. The PDU cannot exceed \b pduLength bytes.
+ *
+ * -# CoapPDU::CoapPDU(uint8_t *buffer, int bufferLength, int pduLength)
+ *
+ * Space is limited by the variable \b bufferLength. The PDU cannot exceed \b bufferLength bytes.
+ *
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::reset()
+{
+ // pdu
+ memset(_pdu,0x00,_bufferLength);
+ // packet always has at least a header
+ _pduLength = 4;
+
+ // options
+ _numOptions = 0;
+ _maxAddedOptionNumber = 0;
+ // payload
+ _payloadPointer = NULL;
+ _payloadLength = 0;
+ content.clear();
+ return 0;
+}
+
+/// Validates a PDU constructed using an external buffer.
+/**
+ * When a CoapPDU is constructed using an external buffer, the programmer must call this function to
+ * check that the received PDU is a valid CoAP PDU.
+ *
+ * \warning The validation call parses the PDU structure to set some internal parameters. If you do
+ * not validate the PDU, then the behaviour of member access functions will be undefined.
+ *
+ * \return 1 if the PDU validates correctly, 0 if not. XXX maybe add some error codes
+ */
+int CoapPDU::validate()
+{
+ if(_pduLength<4)
+ {
+ printf("PDU has to be a minimum of 4 bytes. This: %d bytes",_pduLength);
+ return 0;
+ }
+
+ // check header
+ // 0 1 2 3
+ // 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ // |Ver| T | TKL | Code | Message ID |
+ // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ // | Token (if any, TKL bytes) ...
+ // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ // | Options (if any) ...
+ // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ // |1 1 1 1 1 1 1 1| Payload (if any) ...
+ // +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+ // version must be 1
+ int version = getVersion();
+ if (version != 1)
+ {
+ printf("Invalid version: %d", version);
+ return 0;
+ }
+ printf("Version: %d", version);
+ printf("Type: %d", getType());
+
+ // token length must be between 0 and 8
+ int tokenLength = getTokenLength();
+ if(tokenLength<0||tokenLength>8)
+ {
+ printf("Invalid token length: %d",tokenLength);
+ return 0;
+ }
+ printf("Token length: %d",tokenLength);
+ // check total length
+ if((COAP_HDR_SIZE+tokenLength)>_pduLength)
+ {
+ printf("Token length would make pdu longer than actual length.");
+ return 0;
+ }
+
+ // check that code is valid
+ int code = getCode();
+ if(code<COAP_EMPTY ||
+ (code>COAP_LASTMETHOD&&code<COAP_CREATED) ||
+ (code>COAP_CONTENT&&code<COAP_CONTINUE) ||
+ (code>COAP_CONTINUE&&code<COAP_BAD_REQUEST) ||
+ (code>COAP_NOT_ACCEPTABLE&&code<COAP_PRECONDITION_FAILED) ||
+ (code==0x8E) ||
+ (code>COAP_UNSUPPORTED_CONTENT_FORMAT&&code<COAP_INTERNAL_SERVER_ERROR) ||
+ (code>COAP_PROXYING_NOT_SUPPORTED) )
+ {
+ printf("Invalid CoAP code: %d",code);
+ return 0;
+ }
+ printf("CoAP code: %d",code);
+
+ // token can be anything so nothing to check
+
+ // check that options all make sense
+ uint16_t optionDelta =0, optionNumber = 0, optionValueLength = 0;
+ int totalLength = 0;
+
+ // first option occurs after token
+ int optionPos = COAP_HDR_SIZE + getTokenLength();
+
+ // may be 0 options
+ if(optionPos==_pduLength)
+ {
+ printf("No options. No payload.");
+ _numOptions = 0;
+ _payloadLength = 0;
+ return 1;
+ }
+
+ int bytesRemaining = _pduLength-optionPos;
+ int numOptions = 0;
+ uint8_t upperNibble = 0x00, lowerNibble = 0x00;
+
+ // walk over options and record information
+ while(1)
+ {
+ // check for payload marker
+ if(bytesRemaining>0)
+ {
+ uint8_t optionHeader = _pdu[optionPos];
+ if(optionHeader==0xFF)
+ {
+ // payload
+ if(bytesRemaining>1)
+ {
+ _payloadPointer = &_pdu[optionPos+1];
+ _payloadLength = (bytesRemaining-1);
+ _numOptions = numOptions;
+ printf("Payload found, length: %d",_payloadLength);
+ return 1;
+ }
+ // payload marker but no payload
+ _payloadPointer = NULL;
+ _payloadLength = 0;
+ printf("Payload marker but no payload.");
+ return 0;
+ }
+
+ // check that option delta and option length are valid values
+ upperNibble = (optionHeader & 0xF0) >> 4;
+ lowerNibble = (optionHeader & 0x0F);
+ if(upperNibble==0x0F||lowerNibble==0x0F)
+ {
+ printf("Expected option header or payload marker, got: 0x%x%x",upperNibble,lowerNibble);
+ return 0;
+ }
+ printf("Option header byte appears sane: 0x%x%x",upperNibble,lowerNibble);
+ }
+ else
+ {
+ printf("No more data. No payload.");
+ _payloadPointer = NULL;
+ _payloadLength = 0;
+ _numOptions = numOptions;
+ return 1;
+ }
+
+ // skip over option header byte
+ bytesRemaining--;
+
+ // check that there is enough space for the extended delta and length bytes (if any)
+ int headerBytesNeeded = computeExtraBytes(upperNibble);
+ printf("%d extra bytes needed for extended delta",headerBytesNeeded);
+ if(headerBytesNeeded>bytesRemaining)
+ {
+ printf("Not enough space for extended option delta, needed %d, have %d.",headerBytesNeeded,bytesRemaining);
+ return 0;
+ }
+ headerBytesNeeded += computeExtraBytes(lowerNibble);
+ if(headerBytesNeeded>bytesRemaining)
+ {
+ printf("Not enough space for extended option length, needed %d, have %d.",
+ (headerBytesNeeded-computeExtraBytes(upperNibble)),bytesRemaining);
+ return 0;
+ }
+ printf("Enough space for extended delta and length: %d, continuing.",headerBytesNeeded);
+
+ // extract option details
+ optionDelta = getOptionDelta(&_pdu[optionPos]);
+ optionNumber += optionDelta;
+ optionValueLength = getOptionValueLength(&_pdu[optionPos]);
+ printf("Got option: %d with length %d",optionNumber,optionValueLength);
+ // compute total length
+ totalLength = 1; // mandatory header
+ totalLength += computeExtraBytes(optionDelta);
+ totalLength += computeExtraBytes(optionValueLength);
+ totalLength += optionValueLength;
+ // check there is enough space
+ if(optionPos+totalLength>_pduLength)
+ {
+ printf("Not enough space for option payload, needed %d, have %d.",(totalLength-headerBytesNeeded-1),_pduLength-optionPos);
+ return 0;
+ }
+ printf("Enough space for option payload: %d %d",optionValueLength,(totalLength-headerBytesNeeded-1));
+
+ // recompute bytesRemaining
+ bytesRemaining -= totalLength;
+ bytesRemaining++; // correct for previous --
+
+ // move to next option
+ optionPos += totalLength;
+
+ // inc number of options XXX
+ numOptions++;
+ }
+
+ return 1;
+}
+
+/// Destructor. Does not free buffer if constructor passed an external buffer.
+/**
+ * The destructor acts differently, depending on how the object was initially constructed (from buffer or not):
+ *
+ * -# CoapPDU::CoapPDU()
+ *
+ * Complete object is destroyed.
+ *
+ * -# CoapPDU::CoapPDU(uint8_t *pdu, int pduLength)
+ *
+ * Only object container is destroyed. \b pdu is left intact.
+ *
+ * -# CoapPDU::CoapPDU(uint8_t *buffer, int bufferLength, int pduLength)
+ *
+ * Only object container is destroyed. \b pdu is left intact.
+ *
+ */
+CoapPDU::~CoapPDU()
+{
+ if(!_constructedFromBuffer)
+ {
+ free(_pdu);
+ }
+}
+
+/// Returns a pointer to the internal buffer.
+uint8_t* CoapPDU::getPDUPointer()
+{
+ return _pdu;
+}
+
+/// Set the PDU length to the length specified.
+/**
+ * This is used when re-using a PDU container before calling CoapPDU::validate() as it
+ * is not possible to deduce the length of a PDU since the payload has no length marker.
+ * \param len The length of the PDU
+ */
+void CoapPDU::setPDULength(int len)
+{
+ _pduLength = len;
+}
+
+/// Shorthand function for setting a resource URI.
+/**
+ * Calls CoapPDU::setURI(uri,strlen(uri).
+ */
+int CoapPDU::setURI(char *uri)
+{
+ return setURI(uri,strlen(uri));
+}
+
+/// Shorthand function for setting a resource URI.
+/**
+ * This will parse the supplied \b uri and construct enough URI_PATH and URI_QUERY options to encode it.
+ * The options are added to the PDU.
+ *
+ * At present only simple URI formatting is handled, only '/','?', and '&' separators, and no port or protocol specificaiton.
+ *
+ * The function will split on '/' and create URI_PATH elements until it either reaches the end of the string
+ * in which case it will stop or if it reaches '?' it will start splitting on '&' and create URI_QUERY elements
+ * until it reaches the end of the string.
+ *
+ * Here is an example:
+ *
+ * /a/b/c/d?x=1&y=2&z=3
+ *
+ * Will be broken into four URI_PATH elements "a", "b", "c", "d", and three URI_QUERY elements "x=1", "y=2", "z=3"
+ *
+ * TODO: Add protocol extraction, port extraction, and some malformity checking.
+ *
+ * \param uri The uri to parse.
+ * \param urilen The length of the uri to parse.
+ *
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setURI(char *uri, int urilen)
+{
+ // only '/', '?', '&' and ascii chars allowed
+
+ // sanitation
+ if(urilen<=0||uri==NULL)
+ {
+ printf("Null or zero-length uri passed.");
+ return 1;
+ }
+
+ // single character URI path (including '/' case)
+ if(urilen==1)
+ {
+ addOption(COAP_OPTION_URI_PATH,1,(uint8_t*)uri);
+ return 0;
+ }
+
+ // TODO, queries
+ // extract ? to mark where to stop processing path components
+ // and then process the query params
+
+ // local vars
+ char *startP=uri,*endP=NULL;
+ int oLen = 0;
+ char splitChar = '/';
+ int queryStageTriggered = 0;
+ uint16_t optionType = COAP_OPTION_URI_PATH;
+ while(1)
+ {
+ // stop at end of string or query
+ if(*startP==0x00||*(startP+1)==0x00)
+ {
+ break;
+ }
+
+ // ignore leading slash
+ if(*startP==splitChar)
+ {
+ printf("Skipping leading slash");
+ startP++;
+ }
+
+ // find next split point
+ endP = strchr(startP,splitChar);
+
+ // might not be another slash
+ if(endP==NULL)
+ {
+ printf("Ending out of slash");
+ // check if there is a ?
+ endP = strchr(startP,'?');
+ // done if no queries
+ if(endP==NULL)
+ {
+ endP = uri+urilen;
+ }
+ else
+ {
+ queryStageTriggered = 1;
+ }
+ }
+
+ // get length of segment
+ oLen = endP-startP;
+
+#ifdef DEBUG
+ char *b = (char*)malloc(oLen+1);
+ memcpy(b,startP,oLen);
+ b[oLen] = 0x00;
+ printf("Adding URI_PATH %s",b);
+ free(b);
+#endif
+
+ // add option
+ if(addOption(optionType,oLen,(uint8_t*)startP)!=0)
+ {
+ printf("Error adding option");
+ return 1;
+ }
+ startP = endP;
+
+ if(queryStageTriggered)
+ {
+ splitChar = '&';
+ optionType = COAP_OPTION_URI_QUERY;
+ startP++;
+ queryStageTriggered = false;
+ }
+ }
+
+ return 0;
+}
+
+/// Shorthand for adding a URI QUERY to the option list.
+/**
+ * Adds a new option to the CoAP PDU that encodes a URI_QUERY.
+ *
+ * \param query The uri query to encode.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::addURIQuery(char *query)
+{
+ return addOption(COAP_OPTION_URI_QUERY,strlen(query),(uint8_t*)query);
+}
+
+/// Concatenates any URI_PATH elements and URI_QUERY elements into a single string.
+/**
+ * Parses the PDU options and extracts all URI_PATH and URI_QUERY elements,
+ * concatenating them into a single string with slash and amphersand separators accordingly.
+ *
+ * The produced string will be NULL terminated.
+ *
+ * \param dst Buffer into which to copy the concatenated path elements.
+ * \param dstlen Length of buffer.
+ * \param outLen Pointer to integer, into which URI length will be placed.
+ *
+ * \return 0 on success, 1 on failure. \b outLen will contain the length of the concatenated elements.
+ */
+int CoapPDU::getURI(char *dst, int dstlen, int *outLen)
+{
+ if(outLen==NULL)
+ {
+ printf("Output length pointer is NULL");
+ return 1;
+ }
+
+ if(dst==NULL)
+ {
+ printf("NULL destination buffer");
+ *outLen = 0;
+ return 1;
+ }
+
+ // check destination space
+ if(dstlen<=0)
+ {
+ *dst = 0x00;
+ *outLen = 0;
+ printf("Destination buffer too small (0)!");
+ return 1;
+ }
+ // check option count
+ if(_numOptions==0)
+ {
+ *dst = 0x00;
+ *outLen = 0;
+ return 0;
+ }
+ // get options
+ CoapPDU::CoapOption *options = getOptions();
+ if(options==NULL)
+ {
+ *dst = 0x00;
+ *outLen = 0;
+ return 0;
+ }
+ // iterate over options to construct URI
+ CoapOption *o = NULL;
+ int bytesLeft = dstlen-1; // space for 0x00
+ int oLen = 0;
+ // add slash at beggining
+ if(bytesLeft>=1)
+ {
+ *dst = '/';
+ dst++;
+ bytesLeft--;
+ }
+ else
+ {
+ printf("No space for initial slash needed 1, got %d",bytesLeft);
+ free(options);
+ return 1;
+ }
+
+ char separator = '/';
+ int firstQuery = 1;
+
+ for(int i=0; i<_numOptions; i++)
+ {
+ o = &options[i];
+ oLen = o->optionValueLength;
+ if(o->optionNumber==COAP_OPTION_URI_PATH||o->optionNumber==COAP_OPTION_URI_QUERY)
+ {
+ // if the option is a query, change the separator to &
+ if(o->optionNumber==COAP_OPTION_URI_QUERY)
+ {
+ if(firstQuery)
+ {
+ // change previous '/' to a '?'
+ *(dst-1) = '?';
+ firstQuery = 0;
+ }
+ separator = '&';
+ }
+
+ // check space
+ if(oLen>bytesLeft)
+ {
+ printf("Destination buffer too small, needed %d, got %d",oLen,bytesLeft);
+ free(options);
+ return 1;
+ }
+
+ // case where single '/' exists
+ if(oLen==1&&o->optionValuePointer[0]=='/')
+ {
+ *dst = 0x00;
+ *outLen = 1;
+ free(options);
+ return 0;
+ }
+
+ // copy URI path or query component
+ memcpy(dst,o->optionValuePointer,oLen);
+
+ // adjust counters
+ dst += oLen;
+ bytesLeft -= oLen;
+
+ // add separator following (don't know at this point if another option is coming)
+ if(bytesLeft>=1)
+ {
+ *dst = separator;
+ dst++;
+ bytesLeft--;
+ }
+ else
+ {
+ printf("Ran out of space after processing option");
+ free(options);
+ return 1;
+ }
+ }
+ }
+
+ // remove terminating separator
+ dst--;
+ bytesLeft++;
+ // add null terminating byte (always space since reserved)
+ *dst = 0x00;
+ *outLen = (dstlen-1)-bytesLeft;
+ free(options);
+ return 0;
+}
+
+/// Sets the CoAP version.
+/**
+ * \param version CoAP version between 0 and 3.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setVersion(uint8_t version)
+{
+ if(version>3)
+ {
+ return 0;
+ }
+
+ _pdu[0] &= 0x3F;
+ _pdu[0] |= (version << 6);
+ return 1;
+}
+
+/**
+ * Gets the CoAP Version.
+ * @return The CoAP version between 0 and 3.
+ */
+uint8_t CoapPDU::getVersion()
+{
+ return (_pdu[0]&0xC0)>>6;
+}
+
+/**
+ * Sets the type of this CoAP PDU.
+ * \param mt The type, one of:
+ * - COAP_CONFIRMABLE
+ * - COAP_NON_CONFIRMABLE
+ * - COAP_ACKNOWLEDGEMENT
+ * - COAP_RESET.
+ */
+void CoapPDU::setType(int mt)
+{
+ _pdu[0] &= 0xCF;
+ _pdu[0] |= mt;
+}
+
+/// Returns the type of the PDU.
+int CoapPDU::getType()
+{
+ return (int)(_pdu[0]&0x30);
+}
+
+
+/// Set the token length.
+/**
+ * \param tokenLength The length of the token in bytes, between 0 and 8.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setTokenLength(uint8_t tokenLength)
+{
+ if(tokenLength>8)
+ return 1;
+
+ _pdu[0] &= 0xF0;
+ _pdu[0] |= tokenLength;
+ return 0;
+}
+
+/// Returns the token length.
+int CoapPDU::getTokenLength()
+{
+ return _pdu[0] & 0x0F;
+}
+
+/// Returns a pointer to the PDU token.
+uint8_t* CoapPDU::getTokenPointer()
+{
+ if(getTokenLength()==0)
+ {
+ return NULL;
+ }
+ return &_pdu[4];
+}
+
+/// Set the PDU token to the supplied byte sequence.
+/**
+ * This sets the PDU token to \b token and sets the token length to \b tokenLength.
+ * \param token A sequence of bytes representing the token.
+ * \param tokenLength The length of the byte sequence.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setToken(uint8_t *token, uint8_t tokenLength)
+{
+ printf("Setting token");
+ if(token==NULL)
+ {
+ printf("NULL pointer passed as token reference");
+ return 1;
+ }
+
+ if(tokenLength==0)
+ {
+ printf("Token has zero length");
+ return 1;
+ }
+
+ // if tokenLength has not changed, just copy the new value
+ uint8_t oldTokenLength = getTokenLength();
+ if(tokenLength==oldTokenLength)
+ {
+ memcpy((void*)&_pdu[4],token,tokenLength);
+ return 0;
+ }
+
+ // otherwise compute new length of PDU
+ uint8_t oldPDULength = _pduLength;
+ _pduLength -= oldTokenLength;
+ _pduLength += tokenLength;
+
+ // now, have to shift old memory around, but shift direction depends
+ // whether pdu is now bigger or smaller
+ if(_pduLength>oldPDULength)
+ {
+ // new PDU is bigger, need to allocate space for new PDU
+ if(!_constructedFromBuffer)
+ {
+ uint8_t *newMemory = (uint8_t*)realloc(_pdu,_pduLength);
+ if(newMemory==NULL)
+ {
+ // malloc failed
+ printf("Failed to allocate memory for token");
+ _pduLength = oldPDULength;
+ return 1;
+ }
+ _pdu = newMemory;
+ _bufferLength = _pduLength;
+ }
+ else
+ {
+ // constructed from buffer, check space
+ if(_pduLength>_bufferLength)
+ {
+ printf("Buffer too small to contain token, needed %d, got %d.",_pduLength-oldPDULength,_bufferLength-oldPDULength);
+ _pduLength = oldPDULength;
+ return 1;
+ }
+ }
+
+ // and then shift everything after token up to end of new PDU
+ // memory overlaps so do this manually so to avoid additional mallocs
+ int shiftOffset = _pduLength-oldPDULength;
+ int shiftAmount = _pduLength-tokenLength-COAP_HDR_SIZE; // everything after token
+ shiftPDUUp(shiftOffset,shiftAmount);
+
+ // now copy the token into the new space and set official token length
+ memcpy((void*)&_pdu[4],token,tokenLength);
+ setTokenLength(tokenLength);
+
+ // and return success
+ return 0;
+ }
+
+ // new PDU is smaller, copy the new token value over the old one
+ memcpy((void*)&_pdu[4],token,tokenLength);
+ // and shift everything after the new token down
+ int startLocation = COAP_HDR_SIZE+tokenLength;
+ int shiftOffset = oldPDULength-_pduLength;
+ int shiftAmount = oldPDULength-oldTokenLength-COAP_HDR_SIZE;
+ shiftPDUDown(startLocation,shiftOffset,shiftAmount);
+ // then reduce size of buffer
+ if(!_constructedFromBuffer)
+ {
+ uint8_t *newMemory = (uint8_t*)realloc(_pdu,_pduLength);
+ if(newMemory==NULL)
+ {
+ // malloc failed, PDU in inconsistent state
+ printf("Failed to shrink PDU for new token. PDU probably broken");
+ return 1;
+ }
+ _pdu = newMemory;
+ _bufferLength = _pduLength;
+ }
+
+ // and officially set the new tokenLength
+ setTokenLength(tokenLength);
+ return 0;
+}
+
+/// Sets the CoAP response code
+void CoapPDU::setCode(int code)
+{
+ _pdu[1] = code;
+ // there is a limited set of response codes
+}
+
+/// Gets the CoAP response code
+int CoapPDU::getCode()
+{
+ return (int)_pdu[1];
+}
+
+
+/// Converts a http status code as an integer, to a CoAP code.
+/**
+ * \param httpStatus the HTTP status code as an integer (e.g 200)
+ * \return The correct corresponding int on success,
+ * CoapPDU::COAP_UNDEFINED_CODE on failure.
+ */
+int CoapPDU::httpStatusToCode(int httpStatus)
+{
+ switch(httpStatus)
+ {
+ case 1:
+ return CoapPDU::COAP_GET;
+ case 2:
+ return CoapPDU::COAP_POST;
+ case 3:
+ return CoapPDU::COAP_PUT;
+ case 4:
+ return CoapPDU::COAP_DELETE;
+ case 201:
+ return CoapPDU::COAP_CREATED;
+ case 202:
+ return CoapPDU::COAP_DELETED;
+ case 203:
+ return CoapPDU::COAP_VALID;
+ case 204:
+ return CoapPDU::COAP_CHANGED;
+ case 205:
+ return CoapPDU::COAP_CONTENT;
+ case 400:
+ return CoapPDU::COAP_BAD_REQUEST;
+ case 401:
+ return CoapPDU::COAP_UNAUTHORIZED;
+ case 402:
+ return CoapPDU::COAP_BAD_OPTION;
+ case 403:
+ return CoapPDU::COAP_FORBIDDEN;
+ case 404:
+ return CoapPDU::COAP_NOT_FOUND;
+ case 405:
+ return CoapPDU::COAP_METHOD_NOT_ALLOWED;
+ case 406:
+ return CoapPDU::COAP_NOT_ACCEPTABLE;
+ case 412:
+ return CoapPDU::COAP_PRECONDITION_FAILED;
+ case 413:
+ return CoapPDU::COAP_REQUEST_ENTITY_TOO_LARGE;
+ case 415:
+ return CoapPDU::COAP_UNSUPPORTED_CONTENT_FORMAT;
+ case 500:
+ return CoapPDU::COAP_INTERNAL_SERVER_ERROR;
+ case 501:
+ return CoapPDU::COAP_NOT_IMPLEMENTED;
+ case 502:
+ return CoapPDU::COAP_BAD_GATEWAY;
+ case 503:
+ return CoapPDU::COAP_SERVICE_UNAVAILABLE;
+ case 504:
+ return CoapPDU::COAP_GATEWAY_TIMEOUT;
+ case 505:
+ return CoapPDU::COAP_PROXYING_NOT_SUPPORTED;
+ default:
+ return CoapPDU::COAP_UNDEFINED_CODE;
+ }
+}
+
+/// Set messageID to the supplied value.
+/**
+ * \param messageID A 16bit message id.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setMessageID(uint16_t messageID)
+{
+ // message ID is stored in network byte order
+ uint8_t *to = &_pdu[2];
+ endian_store16(to, messageID);
+ return 0;
+}
+
+/// Returns the 16 bit message ID of the PDU.
+uint16_t CoapPDU::getMessageID()
+{
+ // mesasge ID is stored in network byteorder
+ uint8_t *from = &_pdu[2];
+ uint16_t messageID = endian_load16(uint16_t, from);
+ return messageID;
+}
+
+/// Returns the length of the PDU.
+int CoapPDU::getPDULength()
+{
+ return _pduLength;
+}
+
+/// Return the number of options that the PDU has.
+int CoapPDU::getNumOptions()
+{
+ return _numOptions;
+}
+
+
+/**
+ * This returns the options as a sequence of structs.
+ */
+CoapPDU::CoapOption* CoapPDU::getOptions()
+{
+ printf("getOptions() called, %d options.",_numOptions);
+
+ uint16_t optionDelta =0, optionNumber = 0, optionValueLength = 0;
+ int totalLength = 0;
+
+ if(_numOptions==0)
+ {
+ return NULL;
+ }
+
+ // malloc space for options
+ CoapOption *options = (CoapOption*)malloc(_numOptions*sizeof(CoapOption));
+ if(options==NULL)
+ {
+ printf("Failed to allocate memory for options.");
+ return NULL;
+ }
+
+ // first option occurs after token
+ int optionPos = COAP_HDR_SIZE + getTokenLength();
+
+ // walk over options and record information
+ for(int i=0; i<_numOptions; i++)
+ {
+ // extract option details
+ optionDelta = getOptionDelta(&_pdu[optionPos]);
+ optionNumber += optionDelta;
+ optionValueLength = getOptionValueLength(&_pdu[optionPos]);
+ // compute total length
+ totalLength = 1; // mandatory header
+ totalLength += computeExtraBytes(optionDelta);
+ totalLength += computeExtraBytes(optionValueLength);
+ totalLength += optionValueLength;
+ // record option details
+ options[i].optionNumber = optionNumber;
+ options[i].optionDelta = optionDelta;
+ options[i].optionValueLength = optionValueLength;
+ options[i].totalLength = totalLength;
+ options[i].optionPointer = &_pdu[optionPos];
+ options[i].optionValuePointer = &_pdu[optionPos+totalLength-optionValueLength];
+ // move to next option
+ optionPos += totalLength;
+ }
+
+ return options;
+}
+
+/// Add an option to the PDU.
+/**
+ * Unlike other implementations, options can be added in any order, and in-memory manipulation will be
+ * performed to ensure the correct ordering of options (they use a delta encoding of option numbers).
+ * Re-ordering memory like this incurs a small performance cost, so if you care about this, then you
+ * might want to add options in ascending order of option number.
+ * \param optionNumber The number of the option, see the enum CoapPDU::Option for shorthand notations.
+ * \param optionLength The length of the option payload in bytes.
+ * \param optionValue A pointer to the byte sequence that is the option payload (bytes will be copied).
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::addOption(uint16_t insertedOptionNumber, uint16_t optionValueLength, uint8_t *optionValue)
+{
+ // this inserts the option in memory, and re-computes the deltas accordingly
+ // prevOption <-- insertionPosition
+ // nextOption
+
+ // find insertion location and previous option number
+ uint16_t prevOptionNumber = 0; // option number of option before insertion point
+ int insertionPosition = findInsertionPosition(insertedOptionNumber,&prevOptionNumber);
+ printf("inserting option at position %d, after option with number: %hu",insertionPosition,prevOptionNumber);
+
+ // compute option delta length
+ uint16_t optionDelta = insertedOptionNumber-prevOptionNumber;
+ uint8_t extraDeltaBytes = computeExtraBytes(optionDelta);
+
+ // compute option length length
+ uint16_t extraLengthBytes = computeExtraBytes(optionValueLength);
+
+ // compute total length of option
+ uint16_t optionLength = COAP_OPTION_HDR_BYTE + extraDeltaBytes + extraLengthBytes + optionValueLength;
+
+ // if this is at the end of the PDU, job is done, just malloc and insert
+ if(insertionPosition==_pduLength)
+ {
+ printf("Inserting at end of PDU");
+ // optionNumber must be biggest added
+ _maxAddedOptionNumber = insertedOptionNumber;
+
+ // set new PDU length and allocate space for extra option
+ int oldPDULength = _pduLength;
+ _pduLength += optionLength;
+ if(!_constructedFromBuffer)
+ {
+ uint8_t *newMemory = (uint8_t*)realloc(_pdu,_pduLength);
+ if(newMemory==NULL)
+ {
+ printf("Failed to allocate memory for option.");
+ _pduLength = oldPDULength;
+ // malloc failed
+ return 1;
+ }
+ _pdu = newMemory;
+ _bufferLength = _pduLength;
+ }
+ else
+ {
+ // constructed from buffer, check space
+ if(_pduLength>_bufferLength)
+ {
+ printf("Buffer too small for new option: needed %d, got %d.",_pduLength-oldPDULength,_bufferLength-oldPDULength);
+ _pduLength = oldPDULength;
+ return 1;
+ }
+ }
+
+ // insert option at position
+ insertOption(insertionPosition,optionDelta,optionValueLength,optionValue);
+ _numOptions++;
+ return 0;
+ }
+ // XXX could do 0xFF pdu payload case for changing of dynamically allocated application space SDUs < yeah, if you're insane
+
+ // the next option might (probably) needs it's delta changing
+ // I want to take this into account when allocating space for the new
+ // option, to avoid having to do two mallocs, first get info about this option
+ int nextOptionDelta = getOptionDelta(&_pdu[insertionPosition]);
+ int nextOptionNumber = prevOptionNumber + nextOptionDelta;
+ int nextOptionDeltaBytes = computeExtraBytes(nextOptionDelta);
+ printf("nextOptionDeltaBytes: %d",nextOptionDeltaBytes);
+ // recompute option delta, relative to inserted option
+ int newNextOptionDelta = nextOptionNumber-insertedOptionNumber;
+ int newNextOptionDeltaBytes = computeExtraBytes(newNextOptionDelta);
+ printf("newNextOptionDeltaBytes: %d",newNextOptionDeltaBytes);
+ // determine adjustment
+ int optionDeltaAdjustment = newNextOptionDeltaBytes-nextOptionDeltaBytes;
+
+ // create space for new option, including adjustment space for option delta
+ printf("Creating space");
+ int mallocLength = optionLength+optionDeltaAdjustment;
+ int oldPDULength = _pduLength;
+ _pduLength += mallocLength;
+
+ if(!_constructedFromBuffer)
+ {
+ uint8_t *newMemory = (uint8_t*)realloc(_pdu,_pduLength);
+ if(newMemory==NULL)
+ {
+ printf("Failed to allocate memory for option");
+ _pduLength = oldPDULength;
+ return 1;
+ }
+ _pdu = newMemory;
+ _bufferLength = _pduLength;
+ }
+ else
+ {
+ // constructed from buffer, check space
+ if(_pduLength>_bufferLength)
+ {
+ printf("Buffer too small to contain option, needed %d, got %d.",_pduLength-oldPDULength,_bufferLength-oldPDULength);
+ _pduLength = oldPDULength;
+ return 1;
+ }
+ }
+
+ // move remainder of PDU data up to create hole for new option
+ printf("Shifting PDU.");
+ shiftPDUUp(mallocLength,_pduLength-(insertionPosition+mallocLength));
+
+ // adjust option delta bytes of following option
+ // move the option header to the correct position
+ int nextHeaderPos = insertionPosition+mallocLength;
+ _pdu[nextHeaderPos-optionDeltaAdjustment] = _pdu[nextHeaderPos];
+ nextHeaderPos -= optionDeltaAdjustment;
+ // and set the new value
+ setOptionDelta(nextHeaderPos, newNextOptionDelta);
+
+ // new option shorter
+ // p p n n x x x x x
+ // p p n n x x x x x -
+ // p p - n n x x x x x
+ // p p - - n x x x x x
+ // p p o o n x x x x x
+
+ // new option longer
+ // p p n n x x x x x
+ // p p n n x x x x x - - -
+ // p p - - - n n x x x x x
+ // p p - - n n n x x x x x
+ // p p o o n n n x x x x x
+
+ // note, it can only ever be shorter or the same since if an option was inserted the delta got smaller
+ // but I'll leave that little comment in, just to show that it would work even if the delta got bigger
+
+ // now insert the new option into the gap
+ printf("Inserting new option...");
+ insertOption(insertionPosition,optionDelta,optionValueLength,optionValue);
+ printf("done\r\n");
+
+ // done, mark it with B!
+ _numOptions++;
+ return 0;
+}
+
+/// Allocate space for a payload.
+/**
+ * For dynamically constructed PDUs, this will allocate space for a payload in the object
+ * and return a pointer to it. If the PDU was constructed from a buffer, this doesn't
+ * malloc anything, it just changes the _pduLength and returns the payload pointer.
+ *
+ * \note The pointer returned points into the PDU buffer.
+ * \param len The length of the payload buffer to allocate.
+ * \return Either a pointer to the payload buffer, or NULL if there wasn't enough space / allocation failed.
+ */
+uint8_t* CoapPDU::mallocPayload(int len)
+{
+ printf("Entering mallocPayload");
+ // sanity checks
+ if(len==0)
+ {
+ printf("Cannot allocate a zero length payload");
+ return NULL;
+ }
+
+ // further sanity
+ if(len==_payloadLength)
+ {
+ printf("Space for payload of specified length already exists");
+ if(_payloadPointer==NULL)
+ {
+ printf("Garbage PDU. Payload length is %d, but existing _payloadPointer NULL",_payloadLength);
+ return NULL;
+ }
+ return _payloadPointer;
+ }
+
+ printf("_bufferLength: %d, _pduLength: %d, _payloadLength: %d",_bufferLength,_pduLength,_payloadLength);
+
+ // might be making payload bigger (including bigger than 0) or smaller
+ int markerSpace = 1;
+ int payloadSpace = len;
+ // is this a resizing?
+ if(_payloadLength!=0)
+ {
+ // marker already exists
+ markerSpace = 0;
+ // compute new payload length (can be negative if shrinking payload)
+ payloadSpace = len-_payloadLength;
+ }
+
+ // make space for payload (and payload marker if necessary)
+ int newLen = _pduLength+payloadSpace+markerSpace;
+ if(!_constructedFromBuffer)
+ {
+ uint8_t* newPDU = (uint8_t*)realloc(_pdu,newLen);
+ if(newPDU==NULL)
+ {
+ printf("Cannot allocate (or shrink) space for payload");
+ return NULL;
+ }
+ _pdu = newPDU;
+ _bufferLength = newLen;
+ }
+ else
+ {
+ // constructed from buffer, check space
+ printf("newLen: %d, _bufferLength: %d",newLen,_bufferLength);
+ if(newLen>_bufferLength)
+ {
+ printf("Buffer too small to contain desired payload, needed %d, got %d.",newLen-_pduLength,_bufferLength-_pduLength);
+ return NULL;
+ }
+ }
+
+ // deal with fresh allocation case separately
+ if(_payloadPointer==NULL)
+ {
+ // set payload marker
+ _pdu[_pduLength] = 0xFF;
+ // payload at end of old PDU
+ _payloadPointer = &_pdu[_pduLength+1];
+ _pduLength = newLen;
+ _payloadLength = len;
+ return _payloadPointer;
+ }
+
+ // otherwise, just adjust length of PDU
+ _pduLength = newLen;
+ _payloadLength = len;
+ printf("Leaving mallocPayload");
+ return _payloadPointer;
+}
+
+/// Set the payload to the byte sequence specified. Allocates memory in dynamic PDU if necessary.
+/**
+ * This will set the payload to \b payload. It will allocate memory in the case where the PDU was
+ * constructed without an external buffer.
+ *
+ * This will fail either if the fixed buffer isn't big enough, or if memory could not be allocated
+ * in the non-external-buffer case.
+ *
+ * \param payload Pointer to payload byte sequence.
+ * \param len Length of payload byte sequence.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setPayload(uint8_t *payload, int len)
+{
+ if(payload==NULL)
+ {
+ printf("NULL payload pointer.");
+ return 1;
+ }
+
+ uint8_t *payloadPointer = mallocPayload(len);
+ if(payloadPointer==NULL)
+ {
+ printf("Allocation of payload failed");
+ return 1;
+ }
+
+ // copy payload contents
+ memcpy(payloadPointer,payload,len);
+
+ return 0;
+}
+
+/// Returns a pointer to the payload buffer.
+uint8_t* CoapPDU::getPayloadPointer()
+{
+ return _payloadPointer;
+}
+
+/// Gets the length of the payload buffer.
+int CoapPDU::getPayloadLength()
+{
+ return _payloadLength;
+}
+
+/// Returns a pointer to a buffer which is a copy of the payload buffer (dynamically allocated).
+uint8_t* CoapPDU::getPayloadCopy()
+{
+ if(_payloadLength==0)
+ {
+ return NULL;
+ }
+
+ // malloc space for copy
+ uint8_t *payload = (uint8_t*)malloc(_payloadLength);
+ if(payload==NULL)
+ {
+ printf("Unable to allocate memory for payload");
+ return NULL;
+ }
+
+ // copy and return
+ memcpy(payload,_payloadPointer,_payloadLength);
+ return payload;
+}
+
+/// Shorthand for setting the content-format option.
+/**
+ * Sets the content-format to the specified value (adds an option).
+ * \param format The content format, one of:
+ *
+ * - COAP_CONTENT_FORMAT_TEXT_PLAIN
+ * - COAP_CONTENT_FORMAT_APP_LINK
+ * - COAP_CONTENT_FORMAT_APP_XML
+ * - COAP_CONTENT_FORMAT_APP_OCTET
+ * - COAP_CONTENT_FORMAT_APP_EXI
+ * - COAP_CONTENT_FORMAT_APP_JSON
+ *
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::setContentFormat(int format)
+{
+ if(format==0)
+ {
+ // minimal representation means null option value
+ if(addOption(CoapPDU::COAP_OPTION_CONTENT_FORMAT,0,NULL)!=0)
+ {
+ printf("Error setting content format");
+ return 1;
+ }
+ return 0;
+ }
+
+ uint8_t c[2];
+
+ // just use 1 byte if can do it
+ //if((uint16_t)format <= 0xffu) {
+ if(1)
+ {
+ c[0] = format;
+ if(addOption(CoapPDU::COAP_OPTION_CONTENT_FORMAT,1,c)!=0)
+ {
+ printf("Error setting content format");
+ return 1;
+ }
+ return 0;
+ }
+
+ uint8_t *to = c;
+ endian_store16(to, format);
+ if(addOption(CoapPDU::COAP_OPTION_CONTENT_FORMAT,2,c)!=0)
+ {
+ printf("Error setting content format");
+ return 1;
+ }
+ return 0;
+}
+
+// PRIVATE PRIVATE PRIVATE PRIVATE PRIVATE PRIVATE PRIVATE
+// PRIVATE PRIVATE PRIVATE PRIVATE PRIVATE PRIVATE PRIVATE
+
+/// Moves a block of bytes to end of PDU from given offset.
+/**
+ * This moves the block of bytes _pdu[_pduLength-1-shiftOffset-shiftAmount] ... _pdu[_pduLength-1-shiftOffset]
+ * to the end of the PDU.
+ * \param shiftOffset End of block to move, relative to end of PDU (-1).
+ * \param shiftAmount Length of block to move.
+ */
+void CoapPDU::shiftPDUUp(int shiftOffset, int shiftAmount)
+{
+ printf("shiftOffset: %d, shiftAmount: %d",shiftOffset,shiftAmount);
+ int destPointer = _pduLength-1;
+ int srcPointer = destPointer-shiftOffset;
+ while(shiftAmount--)
+ {
+ _pdu[destPointer] = _pdu[srcPointer];
+ destPointer--;
+ srcPointer--;
+ }
+}
+
+/// Moves a block of bytes down a specified number of steps.
+/**
+ * Moves the block of bytes _pdu[startLocation+shiftOffset] ... _pdu[startLocation+shiftOffset+shiftAmount]
+ * down to \b startLocation.
+ * \param startLocation Index where to shift the block to.
+ * \param shiftOffset Where the block starts, relative to start index.
+ * \param shiftAmount Length of block to shift.
+ */
+void CoapPDU::shiftPDUDown(int startLocation, int shiftOffset, int shiftAmount)
+{
+ printf("startLocation: %d, shiftOffset: %d, shiftAmount: %d",startLocation,shiftOffset,shiftAmount);
+ int srcPointer = startLocation+shiftOffset;
+ while(shiftAmount--)
+ {
+ _pdu[startLocation] = _pdu[srcPointer];
+ startLocation++;
+ srcPointer++;
+ }
+}
+
+/// Gets the payload length of an option.
+/**
+ * \param option Pointer to location of option in PDU.
+ * \return The 16 bit option-payload length.
+ */
+uint16_t CoapPDU::getOptionValueLength(uint8_t *option)
+{
+ uint16_t delta = (option[0] & 0xF0) >> 4;
+ uint16_t length = (option[0] & 0x0F);
+ // no extra bytes
+ if(length<13)
+ {
+ return length;
+ }
+
+ // extra bytes skip header
+ int offset = 1;
+ // skip extra option delta bytes
+ if(delta==13)
+ {
+ offset++;
+ }
+ else if(delta==14)
+ {
+ offset+=2;
+ }
+
+ // process length
+ if(length==13)
+ {
+ return (option[offset]+13);
+ }
+ else
+ {
+ uint8_t *from = &option[offset];
+ uint16_t value = endian_load16(uint16_t, from);
+ return value+269;
+ }
+
+}
+
+/// Gets the delta of an option.
+/**
+ * \param option Pointer to location of option in PDU.
+ * \return The 16 bit delta.
+ */
+uint16_t CoapPDU::getOptionDelta(uint8_t *option)
+{
+ uint16_t delta = (option[0] & 0xF0) >> 4;
+ if(delta<13)
+ {
+ return delta;
+ }
+ else if(delta==13)
+ {
+ // single byte option delta
+ return (option[1]+13);
+ }
+ else if(delta==14)
+ {
+ uint8_t *from = &option[1];
+ uint16_t value = endian_load16(uint16_t, from);
+ return value+269;
+ }
+ else
+ {
+ // should only ever occur in payload marker
+ return delta;
+ }
+}
+
+/// Finds the insertion position in the current list of options for the specified option.
+/**
+ * \param optionNumber The option's number.
+ * \param prevOptionNumber A pointer to a uint16_t which will store the option number of the option previous
+ * to the insertion point.
+ * \return 0 on success, 1 on failure. \b prevOptionNumber will contain the option number of the option
+ * before the insertion position (for example 0 if no options have been inserted).
+ */
+int CoapPDU::findInsertionPosition(uint16_t optionNumber, uint16_t *prevOptionNumber)
+{
+ // zero this for safety
+ *prevOptionNumber = 0x00;
+
+ printf("_pduLength: %d",_pduLength);
+
+ // if option is bigger than any currently stored, it goes at the end
+ // this includes the case that no option has yet been added
+ if( (optionNumber >= _maxAddedOptionNumber) || (_pduLength == (COAP_HDR_SIZE+getTokenLength())) )
+ {
+ *prevOptionNumber = _maxAddedOptionNumber;
+ return _pduLength;
+ }
+
+ // otherwise walk over the options
+ int optionPos = COAP_HDR_SIZE + getTokenLength();
+ uint16_t optionDelta = 0, optionValueLength = 0;
+ uint16_t currentOptionNumber = 0;
+ while(optionPos<_pduLength && _pdu[optionPos]!=0xFF)
+ {
+ optionDelta = getOptionDelta(&_pdu[optionPos]);
+ currentOptionNumber += optionDelta;
+ optionValueLength = getOptionValueLength(&_pdu[optionPos]);
+ // test if this is insertion position
+ if(currentOptionNumber>optionNumber)
+ {
+ return optionPos;
+ }
+ // keep track of the last valid option number
+ *prevOptionNumber = currentOptionNumber;
+ // move onto next option
+ optionPos += computeExtraBytes(optionDelta);
+ optionPos += computeExtraBytes(optionValueLength);
+ optionPos += optionValueLength;
+ optionPos++; // (for mandatory option header byte)
+ }
+ return optionPos;
+
+}
+
+/// CoAP uses a minimal-byte representation for length fields. This returns the number of bytes needed to represent a given length.
+int CoapPDU::computeExtraBytes(uint16_t n)
+{
+ if(n<13)
+ {
+ return 0;
+ }
+
+ if(n<269)
+ {
+ return 1;
+ }
+
+ return 2;
+}
+
+/// Set the option delta to the specified value.
+/**
+ * This assumes space has been made for the option delta.
+ * \param optionPosition The index of the option in the PDU.
+ * \param optionDelta The option delta value to set.
+ */
+void CoapPDU::setOptionDelta(int optionPosition, uint16_t optionDelta)
+{
+ int headerStart = optionPosition;
+ // clear the old option delta bytes
+ _pdu[headerStart] &= 0x0F;
+
+ // set the option delta bytes
+ if(optionDelta<13)
+ {
+ _pdu[headerStart] |= (optionDelta << 4);
+ }
+ else if(optionDelta<269)
+ {
+ // 1 extra byte
+ _pdu[headerStart] |= 0xD0; // 13 in first nibble
+ _pdu[++optionPosition] &= 0x00;
+ _pdu[optionPosition] |= (optionDelta-13);
+ }
+ else
+ {
+ // 2 extra bytes, network byte order uint16_t
+ _pdu[headerStart] |= 0xE0; // 14 in first nibble
+ optionDelta -= 269;
+ uint8_t *to = &_pdu[++optionPosition];
+ endian_store16(to, optionDelta);
+ }
+}
+
+/// Insert an option in-memory at the specified location.
+/**
+ * This assumes that there is enough space at the location specified.
+ * \param insertionPosition Position in the PDU where the option should be placed.
+ * \param optionDelta The delta value for the option.
+ * \param optionValueLength The length of the option value.
+ * \param optionValue A pointer to the sequence of bytes representing the option value.
+ * \return 0 on success, 1 on failure.
+ */
+int CoapPDU::insertOption(
+ int insertionPosition,
+ uint16_t optionDelta,
+ uint16_t optionValueLength,
+ uint8_t *optionValue)
+{
+
+ int headerStart = insertionPosition;
+
+ // clear old option header start
+ _pdu[headerStart] &= 0x00;
+
+ // set the option delta bytes
+ if(optionDelta<13)
+ {
+ _pdu[headerStart] |= (optionDelta << 4);
+ }
+ else if(optionDelta<269)
+ {
+ // 1 extra byte
+ _pdu[headerStart] |= 0xD0; // 13 in first nibble
+ _pdu[++insertionPosition] &= 0x00;
+ _pdu[insertionPosition] |= (optionDelta-13);
+ }
+ else
+ {
+ // 2 extra bytes, network byte order uint16_t
+ _pdu[headerStart] |= 0xE0; // 14 in first nibble
+ optionDelta -= 269;
+ uint8_t *to = &_pdu[++insertionPosition];
+ endian_store16(to, optionDelta);
+ insertionPosition += 1;
+ }
+
+ // set the option value length bytes
+ if(optionValueLength<13)
+ {
+ _pdu[headerStart] |= (optionValueLength & 0x000F);
+ }
+ else if(optionValueLength<269)
+ {
+ _pdu[headerStart] |= 0x0D; // 13 in second nibble
+ _pdu[++insertionPosition] &= 0x00;
+ _pdu[insertionPosition] |= (optionValueLength-13);
+ }
+ else
+ {
+ _pdu[headerStart] |= 0x0E; // 14 in second nibble
+ // this is in network byte order
+ printf("optionValueLength: %u",optionValueLength);
+ uint8_t *to = &_pdu[++insertionPosition];
+ optionValueLength -= 269;
+ endian_store16(to, optionValueLength);
+ insertionPosition += 1;
+ }
+
+ // and finally copy the option value itself
+ memcpy(&_pdu[++insertionPosition],optionValue,optionValueLength);
+
+ return 0;
+}
+
+void CoapPDU::getOptionValueById
+(
+ uint16_t optionNumber,
+ uint16_t *optionValueLength,
+ uint8_t *optionValuePointer
+)
+{
+ if(_numOptions == 0)
+ return;
+ CoapOption* options = getOptions();
+ int i = 0;
+ for(; i < _numOptions; i++)
+ {
+ if(options[i].optionNumber == optionNumber)
+ {
+ *optionValueLength = options[i].optionValueLength;
+ memcpy(optionValuePointer, options[i].optionValuePointer, options[i].optionValueLength);
+ }
+ }
+ free(options);
+}
+
+int CoapPDU::hasOption
+(
+ uint16_t optionNumber
+)
+{
+ int res = 0;
+ if(_numOptions == 0)
+ return res;
+ CoapOption* options = getOptions();
+ int i = 0;
+ for(; i < _numOptions; i++)
+ {
+ if(options[i].optionNumber == optionNumber)
+ {
+ res = 1;
+ break;
+ }
+ }
+ free(options);
+ return res;
+}
+
+const char *CoapPDU::printHuman()
+{
+ content.clear();
+ char temp1[128];
+
+ sprintf(temp1,"PDU is %d bytes long\r\n",_pduLength);
+ content.append(temp1);
+
+ sprintf(temp1,"CoAP Version: %d\r\n",getVersion());
+ content.append("Message Type: ");
+ switch(getType())
+ {
+ case COAP_CONFIRMABLE:
+ content.append("Confirmable");
+ break;
+
+ case COAP_NON_CONFIRMABLE:
+ content.append("Non-Confirmable");
+ break;
+
+ case COAP_ACKNOWLEDGEMENT:
+ content.append("Acknowledgement");
+ break;
+
+ case COAP_RESET:
+ content.append("Reset");
+ break;
+ }
+
+ sprintf(temp1,"\r\nToken length: %d\r\n",getTokenLength());
+ content.append(temp1);
+ content.append("Code: ");
+ switch(getCode())
+ {
+ case COAP_EMPTY:
+ content.append("0.00 Empty");
+ break;
+ case COAP_GET:
+ content.append("0.01 GET");
+ break;
+ case COAP_POST:
+ content.append("0.02 POST");
+ break;
+ case COAP_PUT:
+ content.append("0.03 PUT");
+ break;
+ case COAP_DELETE:
+ content.append("0.04 DELETE");
+ break;
+ case COAP_CREATED:
+ content.append("2.01 Created");
+ break;
+ case COAP_DELETED:
+ content.append("2.02 Deleted");
+ break;
+ case COAP_VALID:
+ content.append("2.03 Valid");
+ break;
+ case COAP_CHANGED:
+ content.append("2.04 Changed");
+ break;
+ case COAP_CONTENT:
+ content.append("2.05 Content");
+ break;
+ case COAP_CONTINUE:
+ content.append("2.31 Continue");
+ break;
+ case COAP_BAD_REQUEST:
+ content.append("4.00 Bad Request");
+ break;
+ case COAP_UNAUTHORIZED:
+ content.append("4.01 Unauthorized");
+ break;
+ case COAP_BAD_OPTION:
+ content.append("4.02 Bad Option");
+ break;
+ case COAP_FORBIDDEN:
+ content.append("4.03 Forbidden");
+ break;
+ case COAP_NOT_FOUND:
+ content.append("4.04 Not Found");
+ break;
+ case COAP_METHOD_NOT_ALLOWED:
+ content.append("4.05 Method Not Allowed");
+ break;
+ case COAP_NOT_ACCEPTABLE:
+ content.append("4.06 Not Acceptable");
+ break;
+ case COAP_PRECONDITION_FAILED:
+ content.append("4.12 Precondition Failed");
+ break;
+ case COAP_REQUEST_ENTITY_TOO_LARGE:
+ content.append("4.13 Request Entity Too Large");
+ break;
+ case COAP_UNSUPPORTED_CONTENT_FORMAT:
+ content.append("4.15 Unsupported Content-Format");
+ break;
+ case COAP_INTERNAL_SERVER_ERROR:
+ content.append("5.00 Internal Server Error");
+ break;
+ case COAP_NOT_IMPLEMENTED:
+ content.append("5.01 Not Implemented");
+ break;
+ case COAP_BAD_GATEWAY:
+ content.append("5.02 Bad Gateway");
+ break;
+ case COAP_SERVICE_UNAVAILABLE:
+ content.append("5.03 Service Unavailable");
+ break;
+ case COAP_GATEWAY_TIMEOUT:
+ content.append("5.04 Gateway Timeout");
+ break;
+ case COAP_PROXYING_NOT_SUPPORTED:
+ content.append("5.05 Proxying Not Supported");
+ break;
+ default:
+ sprintf(temp1, "Undefined Code %u",(unsigned)(getCode()));
+ content.append(temp1);
+
+ }
+
+ // print message ID
+ sprintf(temp1,"\r\nMessage ID: %u\r\n",getMessageID());
+ content.append(temp1);
+
+ // print token value
+ int tokenLength = getTokenLength();
+ uint8_t *tokenPointer = getPDUPointer()+COAP_HDR_SIZE;
+ if(tokenLength==0)
+ {
+ content.append("No token.\r\n");
+ }
+ else
+ {
+ //memset(temp1,0,50);
+ sprintf(temp1,"Token of %d bytes.\r\n",tokenLength);
+ content.append(temp1);
+ content.append("Value: 0x");
+ char temp2[5];
+ for(int j=0; j<tokenLength; j++)
+ {
+ sprintf(temp2,"%.2X",tokenPointer[j]);
+ content.append(temp2);
+ }
+ }
+
+ // print options
+ CoapPDU::CoapOption* options = getOptions();
+ if(options==NULL)
+ {
+ content.append("\r\nNO options");
+ }
+ else
+ {
+ //memset(temp1,0,50);
+ sprintf(temp1,"\r\n%d options:",_numOptions);
+ content.append(temp1);
+ }
+
+ for(int i=0; i<_numOptions; i++)
+ {
+ char optionTemp[128];
+ sprintf(optionTemp,"\r\nOPTION (%d/%d)\r\n"
+ "Option number (delta): %hu (%hu)\r\n"
+ "Name: "
+ ,i + 1,_numOptions,
+ options[i].optionNumber,options[i].optionDelta);
+ content.append(optionTemp);
+ boolean asString = FALSE;
+ switch(options[i].optionNumber)
+ {
+ case COAP_OPTION_IF_MATCH:
+ content.append("IF_MATCH");
+ break;
+ case COAP_OPTION_URI_HOST:
+ content.append("URI_HOST");
+ asString = TRUE;
+ break;
+ case COAP_OPTION_ETAG:
+ content.append("ETAG");
+ break;
+ case COAP_OPTION_IF_NONE_MATCH:
+ content.append("IF_NONE_MATCH");
+ break;
+ case COAP_OPTION_OBSERVE:
+ content.append("OBSERVE");
+ break;
+ case COAP_OPTION_URI_PORT:
+ content.append("URI_PORT");
+ break;
+ case COAP_OPTION_LOCATION_PATH:
+ content.append("LOCATION_PATH");
+ break;
+ case COAP_OPTION_URI_PATH:
+ content.append("URI_PATH");
+ asString = TRUE;
+ break;
+ case COAP_OPTION_CONTENT_FORMAT:
+ content.append("CONTENT_FORMAT");
+ break;
+ case COAP_OPTION_MAX_AGE:
+ content.append("MAX_AGE");
+ break;
+ case COAP_OPTION_URI_QUERY:
+ content.append("URI_QUERY");
+ asString = TRUE;
+ break;
+ case COAP_OPTION_ACCEPT:
+ content.append("ACCEPT");
+ break;
+ case COAP_OPTION_LOCATION_QUERY:
+ content.append("LOCATION_QUERY");
+ asString = TRUE;
+ break;
+ case COAP_OPTION_PROXY_URI:
+ content.append("PROXY_URI");
+ asString = TRUE;
+ break;
+ case COAP_OPTION_PROXY_SCHEME:
+ content.append("PROXY_SCHEME");
+ asString = TRUE;
+ break;
+ case COAP_OPTION_BLOCK1:
+ content.append("BLOCK1");
+ break;
+ case COAP_OPTION_BLOCK2:
+ content.append("BLOCK2");
+ break;
+ case COAP_OPTION_SIZE1:
+ content.append("SIZE1");
+ break;
+ case COAP_OPTION_SIZE2:
+ content.append("SIZE2");
+ break;
+ default:
+ //memset(temp1,0,50);
+ sprintf(temp1,"Unknown option %u",(unsigned)options[i].optionNumber);
+ content.append(temp1);
+ break;
+ }
+ //memset(temp1,0,50);
+ sprintf(temp1,"\r\nValue length: %u\r\n",options[i].optionValueLength);
+ content.append(temp1);
+ if(asString)
+ {
+ content.append("Value: ");
+ }
+ else
+ {
+ content.append("Value: 0x");
+ }
+ char temp3[5];
+ for(int j=0; j<options[i].optionValueLength; j++)
+ {
+ if(asString)
+ {
+ char c = options[i].optionValuePointer[j];
+ if((c>='!'&&c<='~')||c==' ')
+ {
+ sprintf(temp3,"%c", c);
+ }
+ else
+ {
+ sprintf(temp3,"\\%.2d",c);
+ }
+ content.append(temp3);
+ }
+ else
+ {
+ sprintf(temp3,"%.2X",options[i].optionValuePointer[j]);
+ content.append(temp3);
+ }
+ }
+ }
+
+ // print payload
+ if(_payloadLength==0)
+ {
+ content.append("\r\nNo payload.\r\n");
+ }
+ else
+ {
+ //memset(temp1,0,50);
+ sprintf(temp1,"\r\nPayload of %d bytes\r\nValue: 0x",_payloadLength);
+ content.append(temp1);
+ char temp4[5];
+ for(int j=0; j<_payloadLength; j++)
+ {
+ sprintf(temp4,"%.2X",_payloadPointer[j]);
+ content.append(temp4);
+ }
+ content.append("\r\n");
+ }
+ if(options)
+ free(options);
+ return content.c_str();
+}
+
+const char * CoapPDU::printHex()
+{
+ content.clear();
+ char temp[5];
+ for(int i=0; i<_pduLength; i++)
+ {
+ sprintf(temp,"%.2X",_pdu[i]);
+ content.append(temp);
+ }
+ content.append("\r\n");
+ return content.c_str();
+}
+//#endif