diff --git a/src/common/inc/string_utilities.h b/src/common/inc/string_utilities.h index 7ee0a25a..fbcd9b2a 100644 --- a/src/common/inc/string_utilities.h +++ b/src/common/inc/string_utilities.h @@ -114,6 +114,17 @@ StringUtils_compareStrings(const char* a, const char* b); void StringUtils_sortList(LinkedList list); +/** + * \brief Convert IPv6 address string to byte array (uint8_t array) + * + * \param addressString the IPv6 address string in standard notation + * \param ipV6Addr buffer to store the IPv6 address bytes (has to be at least 16 bytes) + * + * \return true when parsing has been successful, false otherwise + */ +bool +StringUtils_convertIPv6AdddressStringToByteArray(const char* addressString, uint8_t ipV6Addr[]); + #ifdef __cplusplus } #endif diff --git a/src/common/string_utilities.c b/src/common/string_utilities.c index 33e8eac0..0029673a 100644 --- a/src/common/string_utilities.c +++ b/src/common/string_utilities.c @@ -378,3 +378,127 @@ StringUtils_sortList(LinkedList list) list->next = sortedList->next; } +static bool +convertHexStrToUint16(char* hexStr, uint16_t* result) +{ + int strSize = strlen(hexStr); + + if (strSize > 4) + return false; + + int val = 0; + int i; + int nibble; + + for (i = 0; i < strSize; i++) { + char nibbleChar = hexStr[i]; + + if ((nibbleChar > 47) && (nibbleChar < 58)) { + nibble = nibbleChar - 48; + } + else if ((nibbleChar > 96) && (nibbleChar < 103)) { + nibble = nibbleChar - 87; + } + else if ((nibbleChar > 64) && (nibbleChar < 71)) { + nibble = nibbleChar - 55; + } + else { + return false; + } + + val <<= 4; + val += nibble; + } + + *result = val; + + return true; +} + +bool +StringUtils_convertIPv6AdddressStringToByteArray(const char* addressString, uint8_t ipV6Addr[]) +{ + char tokenBuf[100]; + + uint16_t addrBlocks[8]; + + int blockCount = 0; + int emptyBlockIndex = 0; + bool hasEmptyBlock = false; + bool end = false; + + char* savePtr = (char*) addressString; + char* sepPos = strchr(savePtr, ':'); + + while (sepPos) { + + memcpy(tokenBuf, savePtr, sepPos - savePtr); + tokenBuf[sepPos - savePtr] = 0; + savePtr = sepPos + 1; + + if (strlen(tokenBuf) == 0) { + + if (hasEmptyBlock) { + return false; + } + + hasEmptyBlock = true; + emptyBlockIndex = blockCount; + } + else { + uint16_t blockVal; + + if (convertHexStrToUint16(tokenBuf, &blockVal) == false) + return false; + + addrBlocks[blockCount] = blockVal; + + blockCount ++; + } + + if (blockCount == 8) + break; + + if (end) + break; + + sepPos = strchr(savePtr, ':'); + + if (sepPos == NULL) { + if (*savePtr != 0) + sepPos = strchr(savePtr, 0); + end = true; + } + } + + if (hasEmptyBlock) { + /* shift blocks */ + int shiftBlocks = blockCount - emptyBlockIndex; + int shift = 8 - blockCount; + + int s; + + /* fill empty blocks with zero */ + for (s = blockCount; s < 8; s++) { + addrBlocks[s] = 0; + blockCount++; + } + + for (s = 0; s < shiftBlocks; s++) { + addrBlocks[s + emptyBlockIndex + shift] = addrBlocks[s + emptyBlockIndex]; + addrBlocks[s + emptyBlockIndex] = 0; + } + } + + if (blockCount != 8) + return false; + + int i = 0; + + for (i = 0; i < 8; i++) { + ipV6Addr[(i * 2)] = addrBlocks[i] / 0x100; + ipV6Addr[(i * 2) + 1] = addrBlocks[i] & 0xff; + } + + return true; +}