diff --git a/cantcoap.cpp b/cantcoap.cpp index 7a217fc..a1aa383 100644 --- a/cantcoap.cpp +++ b/cantcoap.cpp @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "cantcoap.h" #include "arpa/inet.h" +#include "sysdep.h" /// Memory-managed constructor. Buffer for PDU is dynamically sized and allocated by the object. /** @@ -886,23 +887,17 @@ CoapPDU::Code CoapPDU::httpStatusToCode(int httpStatus) { */ int CoapPDU::setMessageID(uint16_t messageID) { // message ID is stored in network byte order - uint16_t networkOrder = htons(messageID); - // bytes 2 and 3 hold the ID - _pdu[2] &= 0x00; - _pdu[2] |= (networkOrder >> 8); // MSB - _pdu[3] &= 0x00; - _pdu[3] |= (networkOrder & 0x00FF); // LSB + 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 - uint16_t networkOrder = 0x0000; - networkOrder |= _pdu[2]; - networkOrder <<= 8; - networkOrder |= _pdu[3]; - return ntohs(networkOrder); + uint8_t *from = &_pdu[2]; + uint16_t messageID = endian_load16(uint16_t, from); + return messageID; } /// Returns the length of the PDU. @@ -1276,11 +1271,8 @@ int CoapPDU::setContentFormat(CoapPDU::ContentFormat format) { return 0; } - uint16_t networkOrder = htons(format); - c[0] &= 0x00; - c[0] |= (networkOrder >> 8); // MSB - c[1] &= 0x00; - c[1] |= (networkOrder & 0x00FF); // LSB + uint8_t *to = c; + endian_store16(to, format); if(addOption(CoapPDU::COAP_OPTION_CONTENT_FORMAT,2,c)!=0) { DBG("Error setting content format"); return 1; @@ -1353,13 +1345,9 @@ uint16_t CoapPDU::getOptionValueLength(uint8_t *option) { if(length==13) { return (option[offset]+13); } else { - // need to convert to host order - uint16_t networkOrder = 0x0000; - networkOrder |= option[offset++]; - networkOrder <<= 8; - networkOrder |= option[offset]; - uint16_t hostOrder = ntohs(networkOrder); - return hostOrder+269; + uint8_t *from = &option[offset]; + uint16_t value = endian_load16(uint16_t, from); + return value+269; } } @@ -1377,14 +1365,9 @@ uint16_t CoapPDU::getOptionDelta(uint8_t *option) { // single byte option delta return (option[1]+13); } else if(delta==14) { - // double byte option delta - // need to convert to host order - uint16_t networkOrder = 0x0000; - networkOrder |= option[1]; - networkOrder <<= 8; - networkOrder |= option[2]; - uint16_t hostOrder = ntohs(networkOrder); - return hostOrder+269; + 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; @@ -1471,11 +1454,10 @@ void CoapPDU::setOptionDelta(int optionPosition, uint16_t optionDelta) { } else { // 2 extra bytes, network byte order uint16_t _pdu[headerStart] |= 0xE0; // 14 in first nibble - optionDelta = htons(optionDelta-269); - _pdu[++optionPosition] &= 0x00; - _pdu[optionPosition] |= (optionDelta >> 8); // MSB - _pdu[++optionPosition] &= 0x00; - _pdu[optionPosition] |= (optionDelta & 0x00FF); // LSB + optionDelta -= 269; + uint8_t *to = &_pdu[optionPosition]; + optionPosition += 2; + endian_store16(to, optionDelta); } } @@ -1510,11 +1492,10 @@ int CoapPDU::insertOption( } else { // 2 extra bytes, network byte order uint16_t _pdu[headerStart] |= 0xE0; // 14 in first nibble - optionDelta = htons(optionDelta-269); - _pdu[++insertionPosition] &= 0x00; - _pdu[insertionPosition] |= (optionDelta >> 8); // MSB - _pdu[++insertionPosition] &= 0x00; - _pdu[insertionPosition] |= (optionDelta & 0x00FF); // LSB + optionDelta -= 269; + uint8_t *to = &_pdu[insertionPosition]; + insertionPosition += 2; + endian_store16(to, optionDelta); } // set the option value length bytes @@ -1528,11 +1509,10 @@ int CoapPDU::insertOption( _pdu[headerStart] |= 0x0E; // 14 in second nibble // this is in network byte order DBG("optionValueLength: %u",optionValueLength); - uint16_t networkOrder = htons(optionValueLength-269); - _pdu[++insertionPosition] &= 0x00; - _pdu[insertionPosition] |= (networkOrder >> 8); // MSB - _pdu[++insertionPosition] &= 0x00; - _pdu[insertionPosition] |= (networkOrder & 0x00FF); // LSB + uint8_t *to = &_pdu[insertionPosition]; + optionValueLength -= 269; + endian_store16(to, optionValueLength); + insertionPosition += 2; } // and finally copy the option value itself diff --git a/sysdep.h b/sysdep.h new file mode 100644 index 0000000..6654493 --- /dev/null +++ b/sysdep.h @@ -0,0 +1,143 @@ +/* + * Modified header taken from MessagePack implmenetation, original license below: + * MessagePack system dependencies + * + * Copyright (C) 2008-2010 FURUHASHI Sadayuki + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef SYSDEP_H +#define SYSDEP_H + +#include + +#ifndef _WIN32 +#include /* __BYTE_ORDER */ +#endif + +#if !defined(__LITTLE_ENDIAN__) && !defined(__BIG_ENDIAN__) +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define __LITTLE_ENDIAN__ +#elif __BYTE_ORDER == __BIG_ENDIAN +#define __BIG_ENDIAN__ +#elif _WIN32 +#define __LITTLE_ENDIAN__ +#endif +#endif + +#ifdef __LITTLE_ENDIAN__ + +#ifdef _WIN32 +# if defined(ntohs) +# define endian_be16(x) ntohs(x) +# elif defined(_byteswap_ushort) || (defined(_MSC_VER) && _MSC_VER >= 1400) +# define endian_be16(x) ((uint16_t)_byteswap_ushort((unsigned short)x)) +# else +# define endian_be16(x) ( \ + ((((uint16_t)x) << 8) ) | \ + ((((uint16_t)x) >> 8) ) ) +# endif +#else +# define endian_be16(x) ntohs(x) +#endif + +#ifdef _WIN32 +# if defined(ntohl) +# define endian_be32(x) ntohl(x) +# elif defined(_byteswap_ulong) || (defined(_MSC_VER) && _MSC_VER >= 1400) +# define endian_be32(x) ((uint32_t)_byteswap_ulong((unsigned long)x)) +# else +# define endian_be32(x) \ + ( ((((uint32_t)x) << 24) ) | \ + ((((uint32_t)x) << 8) & 0x00ff0000U ) | \ + ((((uint32_t)x) >> 8) & 0x0000ff00U ) | \ + ((((uint32_t)x) >> 24) ) ) +# endif +#else +# define endian_be32(x) ntohl(x) +#endif + +#if defined(_byteswap_uint64) || (defined(_MSC_VER) && _MSC_VER >= 1400) +# define endian_be64(x) (_byteswap_uint64(x)) +#elif defined(bswap_64) +# define endian_be64(x) bswap_64(x) +#elif defined(__DARWIN_OSSwapInt64) +# define endian_be64(x) __DARWIN_OSSwapInt64(x) +#else +#define endian_be64(x) \ + ( ((((uint64_t)x) << 56) ) | \ + ((((uint64_t)x) << 40) & 0x00ff000000000000ULL ) | \ + ((((uint64_t)x) << 24) & 0x0000ff0000000000ULL ) | \ + ((((uint64_t)x) << 8) & 0x000000ff00000000ULL ) | \ + ((((uint64_t)x) >> 8) & 0x00000000ff000000ULL ) | \ + ((((uint64_t)x) >> 24) & 0x0000000000ff0000ULL ) | \ + ((((uint64_t)x) >> 40) & 0x000000000000ff00ULL ) | \ + ((((uint64_t)x) >> 56) ) ) +#endif + +#define endian_load16(cast, from) ((cast)( \ + (((uint16_t)((uint8_t*)(from))[0]) << 8) | \ + (((uint16_t)((uint8_t*)(from))[1]) ) )) + +#define endian_load32(cast, from) ((cast)( \ + (((uint32_t)((uint8_t*)(from))[0]) << 24) | \ + (((uint32_t)((uint8_t*)(from))[1]) << 16) | \ + (((uint32_t)((uint8_t*)(from))[2]) << 8) | \ + (((uint32_t)((uint8_t*)(from))[3]) ) )) + +#define endian_load64(cast, from) ((cast)( \ + (((uint64_t)((uint8_t*)(from))[0]) << 56) | \ + (((uint64_t)((uint8_t*)(from))[1]) << 48) | \ + (((uint64_t)((uint8_t*)(from))[2]) << 40) | \ + (((uint64_t)((uint8_t*)(from))[3]) << 32) | \ + (((uint64_t)((uint8_t*)(from))[4]) << 24) | \ + (((uint64_t)((uint8_t*)(from))[5]) << 16) | \ + (((uint64_t)((uint8_t*)(from))[6]) << 8) | \ + (((uint64_t)((uint8_t*)(from))[7]) ) )) + +#else // __LITTLE_ENDIAN__ + +#define endian_be16(x) (x) +#define endian_be32(x) (x) +#define endian_be64(x) (x) + +#define endian_load16(cast, from) ((cast)( \ + (((uint16_t)((uint8_t*)(from))[0]) << 8) | \ + (((uint16_t)((uint8_t*)(from))[1]) ) )) + +#define endian_load32(cast, from) ((cast)( \ + (((uint32_t)((uint8_t*)(from))[0]) << 24) | \ + (((uint32_t)((uint8_t*)(from))[1]) << 16) | \ + (((uint32_t)((uint8_t*)(from))[2]) << 8) | \ + (((uint32_t)((uint8_t*)(from))[3]) ) )) + +#define endian_load64(cast, from) ((cast)( \ + (((uint64_t)((uint8_t*)(from))[0]) << 56) | \ + (((uint64_t)((uint8_t*)(from))[1]) << 48) | \ + (((uint64_t)((uint8_t*)(from))[2]) << 40) | \ + (((uint64_t)((uint8_t*)(from))[3]) << 32) | \ + (((uint64_t)((uint8_t*)(from))[4]) << 24) | \ + (((uint64_t)((uint8_t*)(from))[5]) << 16) | \ + (((uint64_t)((uint8_t*)(from))[6]) << 8) | \ + (((uint64_t)((uint8_t*)(from))[7]) ) )) +#endif + +#define endian_store16(to, num) \ + do { uint16_t val = endian_be16(num); memcpy(to, &val, 2); } while(0) +#define endian_store32(to, num) \ + do { uint32_t val = endian_be32(num); memcpy(to, &val, 4); } while(0) +#define endian_store64(to, num) \ + do { uint64_t val = endian_be64(num); memcpy(to, &val, 8); } while(0) + +#endif // SYSDEP_H +