2 files changed, 61 insertions, 24 deletions

diff --git a/src/core/base16.c b/src/core/base16.c
index 1f0e536c5..7fa4b2009 100644
--- a/src/core/base16.c
+++ b/src/core/base16.c
@@ -61,6 +61,48 @@ void base16_encode ( const uint8_t *raw, size_t len, char *encoded ) {
 }
 
 /**
+ * Decode hexadecimal string
+ *
+ * @v encoded		Encoded string
+ * @v separator		Byte separator character, or 0 for no separator
+ * @v data		Buffer
+ * @v len		Length of buffer
+ * @ret len		Length of data, or negative error
+ */
+int hex_decode ( const char *encoded, char separator, void *data, size_t len ) {
+	uint8_t *out = data;
+	unsigned int count = 0;
+	unsigned int sixteens;
+	unsigned int units;
+
+	while ( *encoded ) {
+
+		/* Check separator, if applicable */
+		if ( count && separator && ( ( *(encoded++) != separator ) ) )
+			return -EINVAL;
+
+		/* Extract digits.  Note that either digit may be NUL,
+		 * which would be interpreted as an invalid value by
+		 * strtoul_charval(); there is therefore no need for an
+		 * explicit end-of-string check.
+		 */
+		sixteens = strtoul_charval ( *(encoded++) );
+		if ( sixteens >= 16 )
+			return -EINVAL;
+		units = strtoul_charval ( *(encoded++) );
+		if ( units >= 16 )
+			return -EINVAL;
+
+		/* Store result */
+		if ( count < len )
+			out[count] = ( ( sixteens << 4 ) | units );
+		count++;
+
+	}
+	return count;
+}
+
+/**
  * Base16-decode data
  *
  * @v encoded		Encoded string
@@ -75,33 +117,15 @@ void base16_encode ( const uint8_t *raw, size_t len, char *encoded ) {
  * to provide a buffer of the correct size.
  */
 int base16_decode ( const char *encoded, uint8_t *raw ) {
-	const char *encoded_bytes = encoded;
-	uint8_t *raw_bytes = raw;
-	char buf[3];
-	char *endp;
-	size_t len;
-
-	while ( encoded_bytes[0] ) {
-		if ( ! encoded_bytes[1] ) {
-			DBG ( "Base16-encoded string \"%s\" has invalid "
-			      "length\n", encoded );
-			return -EINVAL;
-		}
-		memcpy ( buf, encoded_bytes, 2 );
-		buf[2] = '\0';
-		*(raw_bytes++) = strtoul ( buf, &endp, 16 );
-		if ( *endp != '\0' ) {
-			DBG ( "Base16-encoded string \"%s\" has invalid "
-			      "byte \"%s\"\n", encoded, buf );
-			return -EINVAL;
-		}
-		encoded_bytes += 2;
-	}
-	len = ( raw_bytes - raw );
+	int len;
+
+	len = hex_decode ( encoded, 0, raw, -1UL );
+	if ( len < 0 )
+		return len;
 
 	DBG ( "Base16-decoded \"%s\" to:\n", encoded );
 	DBG_HDA ( 0, raw, len );
 	assert ( len <= base16_decoded_max_len ( encoded ) );
 
-	return ( len );
+	return len;
 }
diff --git a/src/core/misc.c b/src/core/misc.c
index 11342481a..eaceddfea 100644
--- a/src/core/misc.c
+++ b/src/core/misc.c
@@ -33,6 +33,19 @@ int inet_aton ( const char *cp, struct in_addr *inp ) {
 	return 0;
 }
 
+unsigned int strtoul_charval ( unsigned int charval ) {
+
+	if ( charval >= 'a' ) {
+		charval = ( charval - 'a' + 10 );
+	} else if ( charval >= 'A' ) {
+		charval = ( charval - 'A' + 10 );
+	} else if ( charval <= '9' ) {
+		charval = ( charval - '0' );
+	}
+
+	return charval;
+}
+
 unsigned long strtoul ( const char *p, char **endp, int base ) {
 	unsigned long ret = 0;
 	int negative = 0;