diff --git a/atsha204a/CMakeLists.txt b/atsha204a/CMakeLists.txt
index 3c8247a..a74f3ae 100644
--- a/atsha204a/CMakeLists.txt
+++ b/atsha204a/CMakeLists.txt
@@ -11,14 +11,13 @@ MESSAGE(STATUS "CMAKE_C_COMPILER: " ${CMAKE_C_COMPILER})
 add_compile_options(-Wall -Werror)
 include_directories(include)
 
-set(SRC src/cmd.c
-	src/crc.c
-	src/debug.c
-	src/io.c
-	src/main.c
-	src/i2c_linux.c
-	src/packet.c
-	src/personalize.c)
+set(SRC ${CMAKE_SOURCE_DIR}/src/cmd.c
+	${CMAKE_SOURCE_DIR}/src/crc.c
+	${CMAKE_SOURCE_DIR}/src/debug.c
+	${CMAKE_SOURCE_DIR}/src/io.c
+	${CMAKE_SOURCE_DIR}/src/i2c_linux.c
+	${CMAKE_SOURCE_DIR}/src/packet.c
+	${CMAKE_SOURCE_DIR}/src/personalize.c)
 
 #add_definitions(-DPERSONALIZE)
 set(I2C_DEVICE "/dev/i2c-0")
@@ -26,7 +25,7 @@ set(I2C_DEVICE "/dev/i2c-0")
 #add_definitions(-DEXT_DEBUG_INFO)
 add_definitions(-DCMAKE_BUILD_TYPE=Debug)
 
-add_executable(${PROJECT_NAME} ${SRC})
+add_executable(${PROJECT_NAME} ${SRC} ${CMAKE_SOURCE_DIR}/src/main.c)
 
 target_compile_definitions(${PROJECT_NAME}
 	PRIVATE -DI2C_DEVICE="${I2C_DEVICE}"
diff --git a/atsha204a/include/cmd.h b/atsha204a/include/cmd.h
index 6f4fa19..4e22b0e 100644
--- a/atsha204a/include/cmd.h
+++ b/atsha204a/include/cmd.h
@@ -105,6 +105,8 @@ enum {
  */
 uint8_t SLOT_CONFIG_ADDR(uint8_t slotnbr);
 
+#define SLOT_DATA_SIZE         32
+
 #define SLOT_ADDR(id) (8 * id)
 #define SLOT_CONFIG_OFFSET(slotnbr) (slotnbr % 2 ? 2 : 0)
 #define SLOT_CONFIG_SIZE 0x2
diff --git a/atsha204a/scripts/atsha204a_test_mac.py b/atsha204a/scripts/atsha204a_test_mac.py
deleted file mode 100755
index d649744..0000000
--- a/atsha204a/scripts/atsha204a_test_mac.py
+++ /dev/null
@@ -1,45 +0,0 @@
-#!/usr/bin/env python3
-import hashlib
-
-# Using mode = 0x06, ie:
-# Bit 7 = 0: MBZ
-# Bit 6 = 0: SN[2:3], SN[4:7] set to zero
-# Bit 5 = 0: OTP[0:7] set to zero
-# Bit 4 = 0: OTP[0:10] set to zero
-# Bit 3 = 0: MBZ
-# Bit 2 = 1: Value of TempKey.SourceFlag = Input
-# Bit 1 = 1: First 32 bytes filled with value in TempKey
-# Bit 0 = 0: Second 32 bytes filled with value in challenge
-
-mac_in = [
-# 32 bytes: TempKey[0:31]
-0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
-0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
-0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
-0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
-# 32 bytes: Challenge[0:31]
-0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
-0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
-0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
-0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
-# 1 byte: Opcode
-0x08,
-# 1 byte: Mode
-0x06,
-# 2 bytes: Param2[LSB], Param2[MSB]
-0x00, 0x00,
-# 8 bytes: OTP[0:7] or zeros
-0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-# 3 bytes OTP[8:10] or zeros
-0x00, 0x00, 0x00,
-# 1 byte SN[8]
-0xee,
-# 4 bytes: SN[4:7] or zeros
-0x00, 0x00, 0x00, 0x00,
-# 2 bytes: SN[0:1]
-0x01, 0x23,
-# 2 bytes SN[2:3] or zeros
-0x00, 0x00
-]
-
-print(hashlib.sha256(bytes(mac_in)).hexdigest())
diff --git a/atsha204a/scripts/atsha204a_test_sha.py b/atsha204a/scripts/atsha204a_test_sha.py
deleted file mode 100755
index 8b17149..0000000
--- a/atsha204a/scripts/atsha204a_test_sha.py
+++ /dev/null
@@ -1,11 +0,0 @@
-#!/usr/bin/env python3
-import hashlib
-
-msg = [
-0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
-0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
-0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
-0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
-]
-
-print(hashlib.sha256(bytes(msg)).hexdigest())
diff --git a/atsha204a/tests/CMakeLists.txt b/atsha204a/tests/CMakeLists.txt
index 605bade..7782c6c 100644
--- a/atsha204a/tests/CMakeLists.txt
+++ b/atsha204a/tests/CMakeLists.txt
@@ -2,8 +2,8 @@ project(s96-204_tests C)
 
 cmake_minimum_required(VERSION 3.0.2)
 
-file(GLOB SRC ${CMAKE_SOURCE_DIR}/src/*.c)
-list(REMOVE_ITEM SRC ${CMAKE_SOURCE_DIR}/src/main.c)
+find_package(OpenSSL REQUIRED)
+include_directories(${OPENSSL_INCLUDE_DIR})
 
 add_executable(${PROJECT_NAME} EXCLUDE_FROM_ALL ${SRC} tests.c)
 
@@ -11,3 +11,4 @@ target_compile_definitions(${PROJECT_NAME}
 	PRIVATE -DI2C_DEVICE="${I2C_DEVICE}"
 	PRIVATE -DDEBUG
 )
+target_link_libraries(${PROJECT_NAME} ${OPENSSL_LIBRARIES})
diff --git a/atsha204a/tests/tests.c b/atsha204a/tests/tests.c
index d057ccb..753c665 100644
--- a/atsha204a/tests/tests.c
+++ b/atsha204a/tests/tests.c
@@ -2,6 +2,8 @@
  * Copyright 2017, Linaro Ltd and contributors
  * SPDX-License-Identifier: Apache-2.0
  */
+#include <openssl/hmac.h>
+#include <openssl/sha.h>
 #include <stdlib.h>
 #include <string.h>
 
@@ -24,50 +26,57 @@ struct atsha204a_testcase {
 };
 
 static struct io_interface *ioif;
-uint8_t buf[32] = { 0 };
 
-static int test_random(void)
+static void sha256(uint8_t *msg, size_t len, uint8_t hash[SHA_LEN])
 {
-	int ret = STATUS_EXEC_ERROR;
+	SHA256_CTX ctx;
+	SHA256_Init(&ctx);
+	SHA256_Update(&ctx, msg, len);
+	SHA256_Final(hash, &ctx);
+}
 
-	printf("\n - Random -\n");
+static unsigned char *hmac_sha256(uint8_t *msg, size_t msg_len, uint8_t *key,
+				  size_t key_len, uint8_t *hmac, unsigned int *hmac_len)
+{
+	return HMAC(EVP_sha256(), key, key_len, msg, msg_len, hmac, hmac_len);
+}
 
-	ret = cmd_get_random(ioif, buf, RANDOM_LEN);
+static int test_random(void)
+{
+	int ret;
+	uint8_t buf[RANDOM_LEN];
+	logd("\n - Random -\n");
+	ret = cmd_get_random(ioif, buf, sizeof(buf));
 	CHECK_RES("random", ret, buf, RANDOM_LEN);
-
 	return ret;
 }
 
 static int test_devrev(void)
 {
-	int ret = STATUS_EXEC_ERROR;
-
-	printf("\n - Devrev -\n");
-
-	ret = cmd_get_devrev(ioif, buf, DEVREV_LEN);
+	int ret;
+	uint8_t buf[DEVREV_LEN];
+	logd("\n - DevRev -\n");
+	ret = cmd_get_devrev(ioif, buf, sizeof(buf));
 	CHECK_RES("devrev", ret, buf, DEVREV_LEN);
-
 	return ret;
 }
 
 static int test_nonce(void)
 {
-	int ret = STATUS_EXEC_ERROR;
-
+	int ret;
+	uint8_t buf[NONCE_LONG_LEN];
 	uint8_t in_short[NONCE_SHORT_NUMIN] = {
 		  0x00, 0x01, 0x02, 0x03,
 		  0x04, 0x05, 0x06, 0x07,
 		  0x08, 0x09, 0x0a, 0x0b,
 		  0x0c, 0x0d, 0x0e, 0x0f,
 		  0x10, 0x11, 0x12, 0x13 };
-
 	uint8_t in_long[NONCE_LONG_NUMIN] = {
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF };
-
-	printf("\n - Nonce short -\n");
+	logd("\n - Nonce (short) -\n");
 	ret = cmd_get_nonce(ioif, in_short, sizeof(in_short), NONCE_MODE_UPDATE_SEED, buf, NONCE_LONG_LEN);
 	CHECK_RES("nonce", ret, buf, NONCE_LONG_LEN);
 
@@ -76,62 +85,256 @@ static int test_nonce(void)
 	 * packet, since there is no random number returned back to the
 	 * caller.
 	 */
-	printf("\n - Nonce long -\n");
+	logd("\n - Nonce (long) -\n");
 	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, buf, 1);
 	CHECK_RES("nonce (long) response code", ret, buf, 1);
 
 	return ret;
 }
 
+/*
+ * GenDig
+ *
+ * Since we cannot access the generated digest directly, we verify
+ * GenDig implicitly as follows:
+ *
+ * 1. Run Nonce to populate TempKey (use pass-through for simplicity)
+ * 2. Run GenDigest and compute the expected value
+ * 3. Generate MACs both in hardware and software
+ * 4. Compare values
+ */
 static int test_gendig(void)
 {
-	int ret = STATUS_EXEC_ERROR;
+	int ret;
+	uint8_t nonce_res;
+
+	uint8_t digest_e[SHA_LEN] = { 0 };
+
+	uint8_t buf_a[MAC_LEN] = { 0 }; /* actual (atsha204a) */
+	uint8_t buf_e[MAC_LEN] = { 0 }; /* expected (openssl) */
 
-	printf("\n - Gendig -\n");
-        /*
-	 * Use slot 3: This is very much configuration dependent,
-         * and it works with the default (factory) settings.
-         */
-	ret = cmd_gen_dig(ioif, NULL, 0, ZONE_DATA, 3);
+	uint8_t nonce_in[NONCE_LONG_NUMIN] = {
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF
+	};
+
+	uint8_t mac_challenge[32] = {
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF
+	};
+
+	uint8_t digest_in[] = {
+		/* 32 bytes: Data[SlotID] */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 1 byte: Opcode */
+		0x15,
+		/* 1 byte: Param1 */
+		0x02,
+		/* 2 bytes: Param2[LSB], Param2[MSB] */
+		0x00, 0x00,
+		/* 1 byte SN[8] */
+		0xee,
+		/* 2 bytes: SN[0:1] */
+		0x01, 0x23,
+		/* 25 bytes: MBZ */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00,
+		/* 32 bytes: TempKey value */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+	};
+
+	uint8_t mac_in[] = {
+		/* 32 bytes: TempKey[0:31]; populated with digest */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 32 bytes: Challenge[0:31] */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		/* 1 byte: Opcode */
+		0x08,
+		/* 1 byte: Mode */
+		0x06,
+		/* 2 bytes: Param2[LSB], Param2[MSB] */
+		0x00, 0x00,
+		/* 8 bytes: OTP[0:7] or zeros */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 3 bytes OTP[8:10] or zeros */
+		0x00, 0x00, 0x00,
+		/* 1 byte SN[8] */
+		0xee,
+		/* 4 bytes: SN[4:7] or zeros */
+		0x00, 0x00, 0x00, 0x00,
+		/* 2 bytes: SN[0:1] */
+		0x01, 0x23,
+		/* 2 bytes SN[2:3] or zeros */
+		0x00, 0x00
+	};
+
+	logd("\n - Gendig -\n");
+
+	ret = cmd_get_nonce(ioif, nonce_in, sizeof(nonce_in),
+			    NONCE_MODE_PASSTHROUGH, &nonce_res, sizeof(nonce_res));
+	CHECK_RES("nonce (long) response code", ret, &nonce_res,
+		  sizeof(nonce_res));
+
+	ret = cmd_gen_dig(ioif, NULL, 0, ZONE_DATA, 0);
 	if (ret != STATUS_OK) {
-		logd("Could not generate digest\n");
+		loge("Could not generate digest\n");
 	}
-	return ret;
+
+	ret = cmd_get_mac(ioif, mac_challenge, sizeof(mac_challenge), 0x06,
+			  0, buf_a, sizeof(buf_a));
+	CHECK_RES("actual mac", ret, buf_a, MAC_LEN);
+
+	sha256(digest_in, sizeof(digest_in), digest_e);
+	hexdump("expected digest", digest_e, MAC_LEN);
+
+	memcpy(mac_in, digest_e, 32);
+	sha256(mac_in, sizeof(mac_in), buf_e);
+	hexdump("expected mac", buf_e, MAC_LEN);
+
+	return memcmp(buf_a, buf_e, MAC_LEN);
 }
 
 static int test_hmac(void)
 {
-	int ret = STATUS_EXEC_ERROR;
+	int ret;
+	uint8_t nonce_res;
+	unsigned int hmac_len = 0;
+
+	uint8_t buf_a[HMAC_LEN] = { 0 }; /* actual (atsha204a) */
+	uint8_t buf_e[HMAC_LEN] = { 0 }; /* expected (openssl) */
+
+	uint8_t in_long[NONCE_LONG_NUMIN] = {
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF };
+
+	uint8_t msg[] = {
+		/* 32 bytes: MBZ */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 32 bytes: TempKey */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		/* 1 byte: Opcode */
+		0x11,
+		/* 1 byte: Mode */
+		0x04,
+		/* 2 bytes: SlotID */
+		0x00, 0x00,
+		/* 8 bytes: OTP[0:7] or zeros */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 8 bytes: OTP[8:10] or zeros */
+		0x00, 0x00, 0x00,
+		/* 1 byte SN[8] */
+		0xee,
+		/* 4 bytes: SN[4:7] or zeros */
+		0x00, 0x00, 0x00, 0x00,
+		/* 2 bytes: SN[0:1] */
+		0x01, 0x23,
+		/* 2 bytes SN[2:3] or zeros */
+		0x00, 0x00
+	};
 
-	printf("\n - HMAC -\n");
+	uint8_t key[] = {
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+
+	logd("\n - HMAC -\n");
+	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH,
+			    &nonce_res, sizeof(nonce_res));
+	CHECK_RES("nonce (long) response code", ret, &nonce_res, sizeof(nonce_res));
 
 	/* 1 << 2 is to set the TempKey.SourceFlag, since we just above did a
 	 * passthrough nonce and therefore we used no internal randomness. */
-	ret = cmd_get_hmac(ioif, 1 << 2, 0, buf);
-	CHECK_RES("hmac", ret, buf, HMAC_LEN);
+	ret = cmd_get_hmac(ioif, 1 << 2, 0, buf_a);
+	CHECK_RES("hmac", ret, buf_a, HMAC_LEN);
 
-	return ret;
+	hmac_sha256(msg, sizeof(msg), key, sizeof(key), buf_e, &hmac_len);
+
+	return memcmp(buf_a, buf_e, HMAC_LEN);
 }
 
 static int test_mac(void)
 {
-	int ret = STATUS_EXEC_ERROR;
+	int ret;
+	uint8_t nonce_buf[NONCE_SHORT_LEN] = { 0 };
+
+	uint8_t buf_a[MAC_LEN] = { 0 }; /* actual (atsha204a) */
+	uint8_t buf_e[MAC_LEN] = { 0 }; /* expected (openssl) */
 
-	uint8_t mac_buf[MAC_LEN] = { 0 };
 	uint8_t in_long[NONCE_LONG_NUMIN] = {
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF };
+
 	uint8_t mac_challenge[32] = {
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF };
 
-	printf("\n - MAC -\n");
-	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, buf, 1);
-	CHECK_RES("nonce for mac", ret, buf, 1);
+	uint8_t computed_in[] = {
+		/* 32 bytes: TempKey[0:31] */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		/* 32 bytes: Challenge[0:31] */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		/* 1 byte: Opcode */
+		0x08,
+		/* 1 byte: Mode */
+		0x06,
+		/* 2 bytes: Param2[LSB], Param2[MSB] */
+		0x00, 0x00,
+		/* 8 bytes: OTP[0:7] or zeros */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 3 bytes OTP[8:10] or zeros */
+		0x00, 0x00, 0x00,
+		/* 1 byte SN[8] */
+		0xee,
+		/* 4 bytes: SN[4:7] or zeros */
+		0x00, 0x00, 0x00, 0x00,
+		/* 2 bytes: SN[0:1] */
+		0x01, 0x23,
+		/* 2 bytes SN[2:3] or zeros */
+		0x00, 0x00
+	};
+
+	logd("\n - MAC -\n");
+	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, nonce_buf, sizeof(nonce_buf));
+	CHECK_RES("Nonce for MAC", ret, nonce_buf, sizeof(nonce_buf));
+
 	/*
 	 * Mode = 0x06
 	 * Bit 0: The 2nd 32 bytes are taken from the input challenge
@@ -143,17 +346,22 @@ static int test_mac(void)
 	 * Bit 6: Don't include SN[2:3] and SN[4:7]; fill with zeros
 	 * Bit 7: MBZ
 	 */
-	ret = cmd_get_mac(ioif, mac_challenge, sizeof(mac_challenge), 0x06, 0, mac_buf, sizeof(mac_buf));
-	CHECK_RES("mac", ret, mac_buf, MAC_LEN);
+	ret = cmd_get_mac(ioif, mac_challenge, sizeof(mac_challenge), 0x06, 0, buf_a, sizeof(buf_a));
+	CHECK_RES("MAC", ret, buf_a, MAC_LEN);
 
-	return ret;
+	/* Now compute the expected value */
+	sha256(computed_in, sizeof(computed_in), buf_e);
+	hexdump("expected mac", buf_e, MAC_LEN);
+
+	return memcmp(buf_a, buf_e, MAC_LEN);
 }
 
 static int test_checkmac(void)
 {
-	int ret = STATUS_EXEC_ERROR;
+	int ret;
 
-	uint8_t mac_resp;
+	uint8_t resp;
+	uint8_t nonce_buf[NONCE_LONG_LEN] = { 0 };;
 	uint8_t mac_buf[MAC_LEN] = { 0 };
 	uint8_t in_long[NONCE_LONG_NUMIN] = {
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
@@ -167,9 +375,9 @@ static int test_checkmac(void)
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF };
 	uint8_t check_mac_data[77] = { 0 };
 
-	printf("\n - MAC -\n");
-	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, buf, 1);
-	CHECK_RES("nonce for mac", ret, buf, 1);
+	logd("\n - MAC -\n");
+	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, nonce_buf, 1);
+	CHECK_RES("nonce for mac", ret, nonce_buf, 1);
 	/*
 	 * Mode = 0x06
 	 * Bit 0: The 2nd 32 bytes are taken from the input challenge
@@ -184,7 +392,7 @@ static int test_checkmac(void)
 	ret = cmd_get_mac(ioif, mac_challenge, sizeof(mac_challenge), 0x06, 0, mac_buf, sizeof(mac_buf));
 	CHECK_RES("mac", ret, mac_buf, MAC_LEN);
 
-	printf("\n - CheckMAC -\n");
+	logd("\n - CheckMAC -\n");
 	/* Data 1 (32 bytes): ClientChal
 	 * Data 2 (32 bytes): ClientResp
 	 * Data 3 (13 bytes): OtherData
@@ -206,8 +414,8 @@ static int test_checkmac(void)
 	check_mac_data[75] = 0x00; /* SN[2] or zero */
 	check_mac_data[76] = 0x00; /* SN[3] or zero */
 
-	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, buf, 1);
-	CHECK_RES("nonce for mac", ret, buf, 1);
+	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, nonce_buf, 1);
+	CHECK_RES("nonce for mac", ret, nonce_buf, 1);
 
 	/* Mode = 0x05
 	 * Bit 0: The 2nd 32 bytes are taken from ClientChal
@@ -219,15 +427,15 @@ static int test_checkmac(void)
 	 * Bit 6: MBZ
 	 * Bit 7: MBZ
 	 */
-	ret = cmd_check_mac(ioif, check_mac_data, sizeof(check_mac_data), 0x06, 0, &mac_resp, 1);
-	CHECK_RES("checkmac", ret, &mac_resp, 1);
+	ret = cmd_check_mac(ioif, check_mac_data, sizeof(check_mac_data), 0x06, 0, &resp, 1);
+	CHECK_RES("checkmac", ret, &resp, 1);
 
 	return ret;
 }
 
 static int test_sha(void)
 {
-	int ret = STATUS_EXEC_ERROR;
+	int ret;
 
 	/* The caller is required to pass the padding and length
 	 * bytes of the message (Sect. 13.1)
@@ -245,47 +453,155 @@ static int test_sha(void)
 		/* Length */
 		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00
 	};
+	uint8_t buf_a[SHA_LEN] = { 0 }; /* actual (atsha204a) */
+	uint8_t buf_e[SHA_LEN] = { 0 }; /* expected (openssl) */
 
-	printf("\n - SHA \n");
+	logd("\n - SHA \n");
+	ret = cmd_sha(ioif, NULL, 0, buf_a, 1);
+	CHECK_RES("sha init", ret, buf_a, 1);
 
-	ret = cmd_sha(ioif, NULL, 0, buf, 1);
-	CHECK_RES("sha init", ret, buf, 1);
+	ret = cmd_sha(ioif, sha_in, sizeof(sha_in), buf_a, SHA_LEN);
+	CHECK_RES("sha compute", ret, buf_a, SHA_LEN);
 
-	ret = cmd_sha(ioif, sha_in, sizeof(sha_in), buf, SHA_LEN);
-	CHECK_RES("sha compute", ret, buf, SHA_LEN);
+	/* Now compute the expected value. We only pass the message
+	 * part as the padding is computed by openssl.
+	 */
+	sha256(sha_in, 32, buf_e);
+	hexdump("expected hash", buf_e, SHA_LEN);
 
-	return ret;
+	return memcmp(buf_a, buf_e, SHA_LEN);
 }
-
+/* DeriveKey
+ *
+ * Since we cannot read the generated key directly, we
+ * validate DeriveKey implicitly as follows:
+ *
+ * 1. Compute the key
+ * 2. Generate the MAC of a known challenge using both actual
+ *    and computed keys
+ * 3. Compare MACs
+ */
 static int test_derivekey(void)
 {
-	int ret = STATUS_EXEC_ERROR;
-
+	int ret;
 	uint8_t in_long[NONCE_LONG_NUMIN] = {
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
 		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
 		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF
 	};
+	uint8_t buf[NONCE_SHORT_LEN] = { 0 };
+	uint8_t key_e[SLOT_DATA_SIZE] = { 0 };
 
-	printf("\n - Derive Key -\n");
+	uint8_t mac_a[MAC_LEN] = { 0 }; /* actual (atsha204a) */
+	uint8_t mac_e[MAC_LEN] = { 0 }; /* expected (openssl) */
 
+	uint8_t sha_in[] = {
+		/* 32 bytes: Parent key (ie Key 0) */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 1 byte: Opcode */
+		0x1c,
+		/* 1 byte: Param1 */
+		0x04,
+		/* 2 bytes: Param2[LSB], Param2[MSB] */
+		0x06, 0x00,
+		/* 1 byte: SN[8] */
+		0xee,
+		/* 2 bytes: SN[0:1] */
+		0x01, 0x23,
+		/* 25 bytes: Zero */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00,
+		/* 32 bytes: TempKey[0:31] */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff
+	};
+
+	uint8_t mac_in[] = {
+		/* 32 bytes: Slot6 - Populate with the key */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 32 bytes: Challenge[0:31] */
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+		0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF,
+		/* 1 byte: Opcode */
+		0x08,
+		/* 1 byte: Mode */
+		0x00,
+		/* 2 bytes: Param2[LSB], Param2[MSB] */
+		0x06, 0x00,
+		/* 8 bytes: OTP[0:7] or zeros */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* 3 bytes OTP[8:10] or zeros */
+		0x00, 0x00, 0x00,
+		/* 1 byte SN[8] */
+		0xee,
+		/* 4 bytes: SN[4:7] or zeros */
+		0x00, 0x00, 0x00, 0x00,
+		/* 2 bytes: SN[0:1] */
+		0x01, 0x23,
+		/* 2 bytes SN[2:3] or zeros */
+		0x00, 0x00
+	};
+
+	logd("\n - Derive Key -\n");
 	ret = cmd_get_nonce(ioif, in_long, sizeof(in_long), NONCE_MODE_PASSTHROUGH, buf, 1);
 	CHECK_RES("nonce for derive key", ret, buf, 1);
 
-	ret = cmd_derive_key(ioif, 1 << 2, 4, NULL, 0);
+	/* Only test the case where the new key is derived from the parent key
+	 * (aka Create). Testing a rolling key not as straightforward, as we can't
+	 * verify the expected value unless we know the current key. It would
+	 * still be possible if we used UpdateCount to perform as many rolling
+	 * operations during the computation of the expected value.
+	 */
+	ret = cmd_derive_key(ioif, 1 << 2, 0x06, NULL, 0);
 	if (ret != STATUS_OK) {
 		loge("Derive Key failed\n");
+		goto out;
 	}
 
-	return ret;
+	/*
+	 * Mode = 0x00 0000 0000
+	 * Bit 0: The 2nd 32 bytes are taken from the input challenge
+	 * Bit 1: The 1st 32 bytes are filled from one of the data slots
+	 * Bit 2: Value of TempKey.SourceFlag
+	 * Bit 3: MBZ
+	 * Bit 4: Don't include OTP[0:10]; fill with zeros
+	 * Bit 5: Don't include OTP[0:7]; fill with zeros
+	 * Bit 6: Don't include SN[2:3] and SN[4:7]; fill with zeros
+	 * Bit 7: MBZ
+	 */
+	ret = cmd_get_mac(ioif, in_long, sizeof(in_long), 0x00, 0x06, mac_a, sizeof(mac_a));
+	CHECK_RES("actual mac", ret, mac_a, sizeof(mac_a));
+
+	sha256(sha_in, sizeof(sha_in), key_e);
+	hexdump("key", key_e, sizeof(key_e));
+
+	/* Populate the top 32 bytes with the key */
+	memcpy(mac_in, key_e, sizeof(key_e));
+
+	sha256(mac_in, sizeof(mac_in), mac_e);
+	hexdump("expected mac", mac_e, MAC_LEN);
+
+out:
+	return memcmp(mac_a, mac_e, MAC_LEN);
 }
 
 static int test_pause(void)
 {
-	int ret = STATUS_EXEC_ERROR;
-
-	printf("\n - Pause -\n");
+	int ret;
+	logd("\n - Pause -\n");
 	ret = cmd_pause(ioif, 0xf00);
 	if (ret != STATUS_OK) {
 		logd("Device paused\n");