64 lines
1.3 KiB
C++
64 lines
1.3 KiB
C++
#include <string.h>
|
|
#include "utils.h"
|
|
|
|
static inline uint8_t _char2byte(char c)
|
|
{
|
|
if('0' <= c && c <= '9') return (uint8_t)(c - '0');
|
|
if('A' <= c && c <= 'F') return (uint8_t)(c - 'A' + 10);
|
|
if('a' <= c && c <= 'f') return (uint8_t)(c - 'a' + 10);
|
|
return 0xFF;
|
|
}
|
|
|
|
int hex2bytes(const char *str, uint8_t *bytes, int32_t length)
|
|
{
|
|
int result;
|
|
if(!str || !bytes || length <= 0)
|
|
return -1;
|
|
|
|
for(result = 0; *str; result++)
|
|
{
|
|
uint8_t msn = _char2byte(*str++);
|
|
if(msn == 0xFF) return -1;
|
|
|
|
uint8_t lsn = _char2byte(*str++);
|
|
if(lsn == 0xFF) return -1;
|
|
|
|
uint8_t bin = (msn << 4) + lsn;
|
|
|
|
if(length-- <= 0)
|
|
return -1;
|
|
|
|
*bytes++ = bin;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void bytes2hex(const uint8_t *bytes, int32_t length, char *str, int32_t strLength)
|
|
{
|
|
const char binHex[] = "0123456789ABCDEF";
|
|
|
|
if(!str || strLength < 3)
|
|
return;
|
|
*str = 0;
|
|
|
|
if(!bytes || length <= 0 || strLength <= 2 * length)
|
|
{
|
|
strncpy(str, "ERR", strLength);
|
|
return;
|
|
}
|
|
|
|
for(; length > 0; length--, strLength -= 2)
|
|
{
|
|
uint8_t byte = *bytes++;
|
|
|
|
*str++ = binHex[(byte >> 4) & 0x0F];
|
|
*str++ = binHex[byte & 0x0F];
|
|
}
|
|
|
|
if(strLength-- <= 0)
|
|
return;
|
|
|
|
*str++ = 0;
|
|
}
|