Vboot Reference: Add the "real" reference firmware verification function (VerifyFirmware).

The old VerifyFirmware function (now called VerifyFirmwareImage) works on the FirmwareImage structure. This CL adds a verification function which can be used directly on packed binary verified boot firmware blobs. This function can be used as the reference implementation for verified boot in firmware. In addition, all functions that work on FirmwareImage structure have been renames to distinguish them from those which work on binary firmware blobs.

In addition, this adds some new crypto utility functions and refactors old ones.
BUG=670
TEST=Added tests for the new function and they pass.

Review URL: http://codereview.chromium.org/650105
diff --git a/common/utility_stub.c b/common/utility_stub.c
index ca12fa6..1012112 100644
--- a/common/utility_stub.c
+++ b/common/utility_stub.c
@@ -30,19 +30,17 @@
 
 void* Memset(void* dest, const uint8_t c, size_t n) {
   while (n--) {
-    *((uint8_t*)dest) = c;
+    *((uint8_t*)dest++) = c;
   }
   return dest;
 }
 
 int SafeMemcmp(const void* s1, const void* s2, size_t n) {
-  int match = 1;
+  int match = 0;
   const unsigned char* us1 = s1;
   const unsigned char* us2 = s2;
   while (n--) {
     if (*us1++ != *us2++)
-      match = 0;
-    else
       match = 1;
   }
 
diff --git a/crypto/rsa_utility.c b/crypto/rsa_utility.c
index 2215b7c..a64b062 100644
--- a/crypto/rsa_utility.c
+++ b/crypto/rsa_utility.c
@@ -7,6 +7,7 @@
 
 #include "padding.h"
 #include "rsa_utility.h"
+#include "sha_utility.h"
 #include "utility.h"
 
 int RSAProcessedKeySize(int algorithm) {
@@ -19,12 +20,20 @@
   return (2 * key_len + sizeof(int) + sizeof(uint32_t));
 }
 
-RSAPublicKey* RSAPublicKeyFromBuf(uint8_t* buf, int len) {
+void RSAPublicKeyFree(RSAPublicKey* key) {
+  if (key) {
+    Free(key->n);
+    Free(key->rr);
+    Free(key);
+  }
+}
+
+RSAPublicKey* RSAPublicKeyFromBuf(const uint8_t* buf, int len) {
   RSAPublicKey* key = (RSAPublicKey*) Malloc(sizeof(RSAPublicKey));
   MemcpyState st;
   int key_len;
 
-  st.remaining_buf = buf;
+  st.remaining_buf = (uint8_t*) buf;
   st.remaining_len = len;
 
   StatefulMemcpy(&st, &key->len, sizeof(key->len));
@@ -44,3 +53,36 @@
 
   return key;
 }
+
+int RSAVerifyBinary_f(const uint8_t* key_blob,
+                      const RSAPublicKey* key,
+                      const uint8_t* buf,
+                      int len,
+                      const uint8_t* sig,
+                      int algorithm) {
+  RSAPublicKey* verification_key = NULL;
+  uint8_t* digest = NULL;
+  int key_size;
+  int sig_size;
+  int success;
+
+  if (algorithm >= kNumAlgorithms)
+    return 0;  /* Invalid algorithm. */
+  key_size = RSAProcessedKeySize(algorithm);
+  sig_size = siglen_map[algorithm] * sizeof(uint32_t);
+
+  if (key_blob && !key)
+    verification_key = RSAPublicKeyFromBuf(key_blob, key_size);
+  else if (!key_blob && key)
+    verification_key = (RSAPublicKey*) key;  /* Supress const warning. */
+  else
+    return 0; /* Both can't be NULL or non-NULL. */
+
+  digest = DigestBuf(buf, len, algorithm);
+  success = RSA_verify(verification_key, sig, sig_size, algorithm, digest);
+
+  Free(digest);
+  if (!key)
+    RSAPublicKeyFree(verification_key);  /* Only free if we allocated it. */
+  return success;
+}
diff --git a/crypto/sha_utility.c b/crypto/sha_utility.c
index 5e3f7e0..e4c5e37 100644
--- a/crypto/sha_utility.c
+++ b/crypto/sha_utility.c
@@ -108,7 +108,7 @@
   return digest;
 }
 
-uint8_t* DigestBuf(uint8_t* buf, int len, int sig_algorithm) {
+uint8_t* DigestBuf(const uint8_t* buf, int len, int sig_algorithm) {
   uint8_t* digest = (uint8_t*) Malloc(SHA512_DIGEST_SIZE); /* Use the max. */
   /* Define an array mapping [sig_algorithm] to function pointers to the
    * SHA{1|256|512} functions.
diff --git a/include/file_keys.h b/include/file_keys.h
index 9236172..46735ec 100644
--- a/include/file_keys.h
+++ b/include/file_keys.h
@@ -25,4 +25,12 @@
  */
 RSAPublicKey* RSAPublicKeyFromFile(char* input_file);
 
+/* Helper function to invoke external program to calculate signature on
+ * [input_file] using private key [key_file] and signature algorithm
+ * [algorithm].
+ *
+ * Returns the signature. Caller owns the buffer and must Free() it.
+ */
+uint8_t* SignatureFile(char* input_fie, char* key_file, int algorithm);
+
 #endif  /* VBOOT_REFERENCE_FILE_KEYS_H_ */
diff --git a/include/firmware_image.h b/include/firmware_image.h
index e2cbb5c..1a9dc83 100644
--- a/include/firmware_image.h
+++ b/include/firmware_image.h
@@ -17,7 +17,8 @@
 #define FIRMWARE_MAGIC_SIZE 8
 #define FIRMWARE_PREAMBLE_SIZE 8
 
-#define ROOT_SIGNATURE_ALGORITHM 11 /* RSA 8192 and SHA-512. */
+/* RSA 8192 and SHA-512. */
+#define ROOT_SIGNATURE_ALGORITHM 11
 #define ROOT_SIGNATURE_ALGORITHM_STRING "11"
 
 typedef struct FirmwareImage {
@@ -27,7 +28,7 @@
   uint16_t sign_algorithm;  /* Signature algorithm used by the signing key. */
   uint8_t* sign_key;  /* Pre-processed public half of signing key. */
   uint16_t key_version;  /* Key Version# for preventing rollbacks. */
-  uint8_t header_hash[SHA512_DIGEST_SIZE];  /* SHA-512 hash of the header.*/
+  uint8_t header_checksum[SHA512_DIGEST_SIZE];  /* SHA-512 hash of the header.*/
 
   uint8_t key_signature[RSA8192NUMBYTES];   /* Signature of the header above. */
 
@@ -56,8 +57,8 @@
  *
  * Returns a filled up FirmwareImage on success, NULL on error.
  */
-FirmwareImage* ReadFirmware(const char* input_file,
-                             FirmwareImage* image);
+FirmwareImage* ReadFirmwareImage(const char* input_file,
+                                 FirmwareImage* image);
 
 /* Write firmware header from [image] to an open file pointed by the
  * file descriptor [fd].
@@ -74,13 +75,79 @@
  *
  * Return [image] on success, NULL on error.
  */
-FirmwareImage* WriteFirmware(const char* input_file,
-                             FirmwareImage* image);
+FirmwareImage* WriteFirmwareImage(const char* input_file,
+                                  FirmwareImage* image);
 
 /* Pretty print the contents of [image]. Only headers and metadata information
  * is printed.
  */
-void PrintFirmware(const FirmwareImage* image);
+void PrintFirmwareImage(const FirmwareImage* image);
+
+/* Error Codes for VerifyFirmware* family of functions. */
+#define VERIFY_FIRMWARE_SUCCESS 0
+#define VERIFY_FIRMWARE_INVALID_IMAGE 1
+#define VERIFY_FIRMWARE_ROOT_SIGNATURE_FAILED 2
+#define VERIFY_FIRMWARE_INVALID_ALGORITHM 3
+#define VERIFY_FIRMWARE_PREAMBLE_SIGNATURE_FAILED 4
+#define VERIFY_FIRMWARE_SIGNATURE_FAILED 5
+#define VERIFY_FIRMWARE_WRONG_MAGIC 6
+#define VERIFY_FIRMWARE_MAX 7  /* Generic catch-all. */
+
+char* kVerifyFirmwareErrors[VERIFY_FIRMWARE_MAX];
+
+/* Checks for the sanity of the firmware header pointed by [header_blob].
+ * If [dev_mode] is enabled, also checks the root key signature using the
+ * pre-processed public root key [root_key_blob].
+ *
+ * On success, put signature algorithm in [algorithm], header length
+ * in [header_len], and return 0.
+ * Else, return error code on failure.
+ */
+int VerifyFirmwareHeader(const uint8_t* root_key_blob,
+                         const uint8_t* header_blob,
+                         const int dev_mode,
+                         int* algorithm,
+                         int* header_len);
+
+/* Checks the preamble signature on firmware preamble pointed by
+ * [preamble_blob] using the signing key [sign_key].
+ *
+ * On success, put firmware length into [firmware_len], and return 0.
+ * Else, return error code on failure.
+ */
+int VerifyFirmwarePreamble(RSAPublicKey* sign_key,
+                           const uint8_t* preamble_blob,
+                           int algorithm,
+                           int* firmware_len);
+
+/* Checks the signature on the firmware data at location [firmware_data_start].
+ * The length of the actual firmware data is firmware_len and it is assumed to
+ * be prepended with the signature whose size depends on the signature_algorithm
+ * [algorithm].
+ *
+ * Return 0 on success, error code on failure.
+ */
+int VerifyFirmwareData(RSAPublicKey* sign_key,
+                       const uint8_t* firmware_data_start,
+                       int firmware_len,
+                       int algorithm);
+
+/* Performs a chained verify of the firmware blob [firmware_blob]. If
+ * [dev_mode] is 0 [inactive], then the pre-processed public root key
+ * [root_key_blob] is used the verify the signature of the signing key,
+ * else the check is skipped.
+ *
+ * Returns 0 on success, error code on failure.
+ *
+ * NOTE: The length of the firmware blob is derived from reading the fields
+ * in the first few bytes of the buffer. This might look risky but in firmware
+ * land, the start address of the firmware_blob will always be fixed depending
+ * on the memory map on the particular platform. In addition, the signature on
+ * length itself is checked early in the verification process for extra safety.
+ */
+int VerifyFirmware(const uint8_t* root_key_blob,
+                   const uint8_t* firmware_blob,
+                   const int dev_mode);
 
 /* Performs a chained verify of the firmware [image]. If [dev_mode] is
  * 0 (inactive), then the [root_key] is used to verify the signature of
@@ -88,33 +155,13 @@
  *
  * Returns 0 on success, error code on failure.
  */
-int VerifyFirmware(const RSAPublicKey* root_key,
-                   const FirmwareImage* image,
-                   const int dev_mode);
-
-/* Error Codes for VerifyFirmware. */
-#define VERIFY_SUCCESS 0
-#define VERIFY_INVALID_IMAGE 1
-#define VERIFY_ROOT_SIGNATURE_FAILED 2
-#define VERIFY_INVALID_ALGORITHM 3
-#define VERIFY_PREAMBLE_SIGNATURE_FAILED 4
-#define VERIFY_FIRMWARE_SIGNATURE_FAILED 5
-#define VERIFY_MAX 6  /* Generic catch-all. */
-
-char* kVerifyFirmwareErrors[VERIFY_MAX];
+int VerifyFirmwareImage(const RSAPublicKey* root_key,
+                        const FirmwareImage* image,
+                        const int dev_mode);
 
 /* Maps error codes from VerifyFirmware() to error description. */
 char* VerifyErrorString(int error);
 
-
-/* Helper function to invoke external program to calculate signature on
- * [input_file] using private key [key_file] and signature algorithm
- * [algorithm].
- *
- * Returns the signature. Caller owns the buffer and must Free() it.
- */
-uint8_t* SignatureFile(char* input_fie, char* key_file, int algorithm);
-
 /* Add a root key signature to the key header to a firmware image [image]
  * using the private root key in file [root_key_file].
  *
diff --git a/include/rsa_utility.h b/include/rsa_utility.h
index 808d17a..71492ae 100644
--- a/include/rsa_utility.h
+++ b/include/rsa_utility.h
@@ -14,8 +14,34 @@
  * [algorithm]. */
 int RSAProcessedKeySize(int algorithm);
 
+/* Deep free the contents of [key]. */
+void RSAPublicKeyFree(RSAPublicKey* key);
+
 /* Create a RSAPublic key structure from binary blob [buf] of length
- * [len]. */
-RSAPublicKey* RSAPublicKeyFromBuf(uint8_t* buf, int len);
+ * [len].
+ *
+ * Caller owns the returned key and must free it.
+ */
+RSAPublicKey* RSAPublicKeyFromBuf(const uint8_t* buf, int len);
+
+/* Perform RSA signature verification on [buf] of length [len] against expected
+ * signature [sig] using signature algorithm [algorithm]. The public key used
+ * for verification can either be in the form of a pre-process key blob
+ * [key_blob] or RSAPublicKey structure [key]. One of [key_blob] or [key] must
+ * be non-NULL, and the other NULL or the function will fail.
+ *
+ * Returns 1 on verification success, 0 on verification failure or invalid
+ * arguments.
+ *
+ * Note: This function is for use in the firmware and assumes all pointers point
+ * to areas in the memory of the right size.
+ *
+ */
+int RSAVerifyBinary_f(const uint8_t* key_blob,
+                      const RSAPublicKey* key,
+                      const uint8_t* buf,
+                      int len,
+                      const uint8_t* sig,
+                      int algorithm);
 
 #endif  /* VBOOT_REFERENCE_RSA_UTILITY_H_ */
diff --git a/include/sha_utility.h b/include/sha_utility.h
index d786b08..c5e2331 100644
--- a/include/sha_utility.h
+++ b/include/sha_utility.h
@@ -48,6 +48,6 @@
  * [len] based on the signature [algorithm].
  * Caller owns the returned digest and must free it.
  */
-uint8_t* DigestBuf(uint8_t* buf, int len, int sig_algorithm);
+uint8_t* DigestBuf(const uint8_t* buf, int len, int sig_algorithm);
 
 #endif  /* VBOOT_REFERENCE_SHA_UTILITY_H_ */
diff --git a/include/utility.h b/include/utility.h
index 87d17ea..3618263 100644
--- a/include/utility.h
+++ b/include/utility.h
@@ -27,8 +27,8 @@
 /* Set [n] bytes starting at [s] to [c]. */
 void* Memset(void *dest, const uint8_t c, size_t n);
 
-/* Compare [n] bytes starting at [s1] with [s2] and return 1 if they match,
- * 0 if they don't. Time taken to perform the comparison is only dependent on
+/* Compare [n] bytes starting at [s1] with [s2] and return 0 if they match,
+ * 1 if they don't. Time taken to perform the comparison is only dependent on
  * [n] and not on the relationship of the match between [s1] and [s2].
  */
 int SafeMemcmp(const void* s1, const void* s2, size_t n);
diff --git a/tests/firmware_image_tests.c b/tests/firmware_image_tests.c
index 42b44ab..40018d7 100644
--- a/tests/firmware_image_tests.c
+++ b/tests/firmware_image_tests.c
@@ -31,7 +31,7 @@
                                          int firmware_version,
                                          int firmware_len) {
   FirmwareImage* image = FirmwareImageNew();
-  uint8_t* header_hash;
+  uint8_t* header_checksum;
   DigestContext ctx;
 
   Memcpy(image->magic, FIRMWARE_MAGIC, FIRMWARE_MAGIC_SIZE);
@@ -41,7 +41,14 @@
   Memcpy(image->sign_key, sign_key, RSAProcessedKeySize(image->sign_algorithm));
   image->key_version = key_version;
 
-  /* Calculate SHA-512 digest on header and populate header_hash. */
+  /* Update correct header length. */
+  image->header_len = (sizeof(image->header_len) +
+                       sizeof(image->sign_algorithm) +
+                       RSAProcessedKeySize(image->sign_algorithm) +
+                       sizeof(image->key_version) +
+                       sizeof(image->header_checksum));
+
+  /* Calculate SHA-512 digest on header and populate header_checksum. */
   DigestInit(&ctx, ROOT_SIGNATURE_ALGORITHM);
   DigestUpdate(&ctx, (uint8_t*) &image->header_len,
                sizeof(image->header_len));
@@ -51,16 +58,10 @@
                RSAProcessedKeySize(image->sign_algorithm));
   DigestUpdate(&ctx, (uint8_t*) &image->key_version,
                sizeof(image->key_version));
-  header_hash = DigestFinal(&ctx);
-  Memcpy(image->header_hash, header_hash, SHA512_DIGEST_SIZE);
-  Free(header_hash);
+  header_checksum = DigestFinal(&ctx);
+  Memcpy(image->header_checksum, header_checksum, SHA512_DIGEST_SIZE);
+  Free(header_checksum);
 
-  /* Update correct header length. */
-  image->header_len = (sizeof(image->header_len) +
-                       sizeof(image->sign_algorithm) +
-                       RSAProcessedKeySize(image->sign_algorithm) +
-                       sizeof(image->key_version) +
-                       sizeof(image->header_hash));
 
   /* Populate firmware and preamble with dummy data. */
   image->firmware_version = firmware_version;
@@ -76,59 +77,77 @@
 #define DEV_MODE_ENABLED 1
 #define DEV_MODE_DISABLED 0
 
-/* Normal Firmware Verification Tests. */
-int VerifyFirmwareTest(FirmwareImage* image, RSAPublicKey* root_key) {
+/* Normal Firmware Blob Verification Tests. */
+int VerifyFirmwareTest(uint8_t* firmware_blob, uint8_t* root_key_blob) {
   int success = 1;
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_ENABLED),
-               VERIFY_SUCCESS,
-               "Normal Verification (Dev Mode)"))
+  if (!TEST_EQ(VerifyFirmware(root_key_blob, firmware_blob, DEV_MODE_ENABLED),
+               VERIFY_FIRMWARE_SUCCESS,
+               "Normal Firmware Blob Verification (Dev Mode)"))
     success = 0;
 
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_DISABLED),
-               VERIFY_SUCCESS,
-               "Normal Verification (Trusted)"))
+  if (!TEST_EQ(VerifyFirmware(root_key_blob, firmware_blob, DEV_MODE_DISABLED),
+               VERIFY_FIRMWARE_SUCCESS,
+               "Normal Firmware Blob Verification (Trusted)"))
     success = 0;
   return success;
 }
 
-/* Tampered Firmware Verification Tests. */
-int VerifyFirmwareTamperTest(FirmwareImage* image, RSAPublicKey* root_key) {
+
+/* Normal FirmwareImage Verification Tests. */
+int VerifyFirmwareImageTest(FirmwareImage* image,
+                            RSAPublicKey* root_key) {
   int success = 1;
-  fprintf(stderr, "Tampering with firmware preamble....\n");
-  image->firmware_version = 0;
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_ENABLED),
-               VERIFY_PREAMBLE_SIGNATURE_FAILED,
-               "Firmware Preamble Tamper Verification (Dev Mode)"))
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_ENABLED),
+               VERIFY_FIRMWARE_SUCCESS,
+               "Normal FirmwareImage Verification (Dev Mode)"))
     success = 0;
 
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_DISABLED),
-               VERIFY_PREAMBLE_SIGNATURE_FAILED,
-               "Firmware Preamble Tamper Verification (Trusted)"))
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_DISABLED),
+               VERIFY_FIRMWARE_SUCCESS,
+               "Normal FirmwareImage Verification (Trusted)"))
+    success = 0;
+  return success;
+}
+
+/* Tampered FirmwareImage Verification Tests. */
+int VerifyFirmwareImageTamperTest(FirmwareImage* image,
+                                  RSAPublicKey* root_key) {
+  int success = 1;
+  fprintf(stderr, "[[Tampering with firmware preamble....]]\n");
+  image->firmware_version = 0;
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_ENABLED),
+               VERIFY_FIRMWARE_PREAMBLE_SIGNATURE_FAILED,
+               "FirmwareImage Preamble Tamper Verification (Dev Mode)"))
+    success = 0;
+
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_DISABLED),
+               VERIFY_FIRMWARE_PREAMBLE_SIGNATURE_FAILED,
+               "FirmwareImage Preamble Tamper Verification (Trusted)"))
     success = 0;
   image->firmware_version = 1;
 
   image->firmware_data[0] = 'T';
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_ENABLED),
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_ENABLED),
                VERIFY_FIRMWARE_SIGNATURE_FAILED,
-               "Firmware Tamper Verification (Dev Mode)"))
+               "FirmwareImage Tamper Verification (Dev Mode)"))
     success = 0;
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_DISABLED),
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_DISABLED),
                VERIFY_FIRMWARE_SIGNATURE_FAILED,
-               "Firmware Tamper Verification (Trusted)"))
+               "FirmwareImage Tamper Verification (Trusted)"))
     success = 0;
   image->firmware_data[0] = 'F';
 
 
-  fprintf(stderr, "Tampering with root key signature...\n");
+  fprintf(stderr, "[[Tampering with root key signature...]]\n");
   image->key_signature[0] = 0xFF;
   image->key_signature[1] = 0x00;
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_ENABLED),
-               VERIFY_SUCCESS,
-               "Root Signature Tamper Verification (Dev Mode)"))
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_ENABLED),
+               VERIFY_FIRMWARE_SUCCESS,
+               "FirmwareImage Root Signature Tamper Verification (Dev Mode)"))
     success = 0;
-  if (!TEST_EQ(VerifyFirmware(root_key, image, DEV_MODE_DISABLED),
-               VERIFY_ROOT_SIGNATURE_FAILED,
-               "Root Signature Tamper Verification (Trusted)"))
+  if (!TEST_EQ(VerifyFirmwareImage(root_key, image, DEV_MODE_DISABLED),
+               VERIFY_FIRMWARE_ROOT_SIGNATURE_FAILED,
+               "FirmwareImage Root Signature Tamper Verification (Trusted)"))
     success = 0;
 
   return success;
@@ -137,9 +156,12 @@
 int main(int argc, char* argv[]) {
   int len;
   uint8_t* sign_key_buf = NULL;
+  uint8_t* root_key_blob = NULL;
+  uint8_t* firmware_blob = NULL;
   FirmwareImage* image = NULL;
   RSAPublicKey* root_key = NULL;
   int error_code = 1;
+  char* tmp_firmwareblob_file = ".tmpFirmwareBlob";
 
   if(argc != 6) {
     fprintf(stderr, "Usage: %s <algorithm> <root key> <processed root pubkey>"
@@ -149,6 +171,7 @@
 
   /* Read verification keys and create a test image. */
   root_key = RSAPublicKeyFromFile(argv[3]);
+  root_key_blob = BufferFromFile(argv[3], &len);
   sign_key_buf = BufferFromFile(argv[5], &len);
   image = GenerateTestFirmwareImage(atoi(argv[1]), sign_key_buf, 1,
                                     1, 1000);
@@ -171,15 +194,36 @@
     goto failure;
   }
 
-  if (!VerifyFirmwareTest(image, root_key))
+
+  /* Generate a firmware binary blob from image.
+   *
+   * TODO(gauravsh): There should be a function to directly generate a binary
+   * blob buffer from a FirmwareImage instead of indirectly writing to a file
+   * and reading it into a buffer.
+   */
+  if (!WriteFirmwareImage(tmp_firmwareblob_file, image)) {
+    fprintf(stderr, "Couldn't create a temporary firmware blob file.\n");
+    error_code = 1;
+    goto failure;
+  }
+  firmware_blob = BufferFromFile(tmp_firmwareblob_file, &len);
+
+  /* Test Firmware blob verify operations. */
+  if (!VerifyFirmwareTest(firmware_blob, root_key_blob))
     error_code = 255;
-  if (!VerifyFirmwareTamperTest(image, root_key))
+
+  /* Test FirmwareImage verify operations. */
+  if (!VerifyFirmwareImageTest(image, root_key))
+    error_code = 255;
+  if (!VerifyFirmwareImageTamperTest(image, root_key))
     error_code = 255;
 
 failure:
-  Free(root_key);
-  Free(sign_key_buf);
+  Free(firmware_blob);
   Free(image);
+  Free(sign_key_buf);
+  Free(root_key_blob);
+  Free(root_key);
 
   return error_code;
 }
diff --git a/tests/run_rsa_tests.sh b/tests/run_rsa_tests.sh
index 2aa7426..422bf73 100755
--- a/tests/run_rsa_tests.sh
+++ b/tests/run_rsa_tests.sh
@@ -13,6 +13,12 @@
 TEST_FILE=test_file 
 TEST_FILE_SIZE=1000000
 
+COL_RED='\E[31;1m'
+COL_GREEN='\E[32;1m'
+COL_YELLOW='\E[33;1m'
+COL_BLUE='\E[34;1m'
+COL_STOP='\E[0;m'
+
 # Generate public key signatures on an input file for various combinations
 # of message digest algorithms and RSA key sizes.
 function generate_signatures {
@@ -35,7 +41,7 @@
   do
     for hashalgo in ${hash_algos[@]}
     do
-      echo "For RSA-$keylen and $hashalgo:"
+      echo -e "For ${COL_YELLOW}RSA-$keylen and $hashalgo${COL_STOP}:"
       ${UTIL_DIR}/verify_data $algorithmcounter \
         ${KEY_DIR}/key_rsa${keylen}.keyb \
         ${TEST_FILE}.rsa${keylen}_${hashalgo}.sig ${TEST_FILE}
diff --git a/utils/Makefile b/utils/Makefile
index c1967fc..c0de74e 100644
--- a/utils/Makefile
+++ b/utils/Makefile
@@ -3,33 +3,36 @@
 # found in the LICENSE file.
 
 CC ?= gcc
-CFLAGS = -Wall -ggdb -DNDEBUG
+CFLAGS = -Wall -DNDEBUG
 INCLUDES ?= -I../include/
 TOP ?= ../
 
 LIBS = -lcrypto
 FIRMWARELIBS = $(TOP)/crypto/libcrypto.a $(TOP)/common/libcommon.a
 
-all: dumpRSAPublicKey verify_data signature_digest firmware_utility
+all: dumpRSAPublicKey verify_data signature_digest firmware_utility \
+	file_keys.o firmware_image.o
 
 dumpRSAPublicKey: dumpRSAPublicKey.c
 	$(CC) $(CFLAGS) $(LIBS) $< -o $@
 
-verify_data: verify_data.c file_keys.o
-	$(CC) $(CFLAGS) $(INCLUDES) $< -o $@ file_keys.o $(FIRMWARELIBS)
+verify_data: verify_data.c file_keys.c
+	$(CC) $(CFLAGS) $(INCLUDES) $^ -o $@ $(FIRMWARELIBS)
 
 signature_digest: signature_digest.c
 	$(CC) $(CFLAGS) $(INCLUDES) $< -o $@ $(FIRMWARELIBS)
 
-firmware_utility: firmware_utility.c firmware_image.o file_keys.o
-	$(CC) $(CFLAGS) $(INCLUDES) $< -o $@  firmware_image.o $(FIRMWARELIBS)
+firmware_utility: firmware_utility.c firmware_image.o file_keys.c
+	$(CC) $(CFLAGS) $(INCLUDES) $^ -o $@ $(FIRMWARELIBS)
 
+# Used by tests.
 file_keys.o: file_keys.c
 	$(CC) $(CFLAGS) $(INCLUDES) -c $< -o $@
 
 firmware_image.o: firmware_image.c
-	$(CC) $(CFLAGS) $(INCLUDES) -c $< -o $@
+	$(CC) $(CFLAGS) -ansi $(INCLUDES) -c $< -o $@
 
 clean:
-	rm -f dumpRSAPublicKey verify_data signature_digest firmware_image.o file_keys.o
+	rm -f dumpRSAPublicKey verify_data signature_digest firmware_image.o \
+	file_keys.o
 
diff --git a/utils/file_keys.c b/utils/file_keys.c
index 8a8a2cb..bcba749 100644
--- a/utils/file_keys.c
+++ b/utils/file_keys.c
@@ -15,6 +15,7 @@
 #include <sys/types.h>
 #include <unistd.h>
 
+#include "padding.h"
 #include "rsa_utility.h"
 #include "utility.h"
 
@@ -55,3 +56,41 @@
   Free(buf);
   return key;
 }
+
+uint8_t* SignatureFile(char* input_file, char* key_file, int algorithm) {
+  char* sign_utility = "./sign_data.sh";
+  char* cmd;  /* Command line to invoke. */
+  int cmd_len;
+  FILE* cmd_out;  /* File descriptor to command output. */
+  uint8_t* signature = NULL;
+  int signature_size = siglen_map[algorithm] * sizeof(uint32_t);
+
+  /* Build command line:
+   * sign_data.sh <algorithm> <key file> <input file>
+   */
+  cmd_len = (strlen(sign_utility) + 1 + /* +1 for space. */
+             2 + 1 + /* For [algorithm]. */
+             strlen(key_file) + 1 + /* +1 for space. */
+             strlen(input_file) +
+             1);  /* For the trailing '\0'. */
+  cmd = (char*) Malloc(cmd_len);
+  snprintf(cmd, cmd_len, "%s %d %s %s", sign_utility, algorithm, key_file,
+           input_file);
+  cmd_out = popen(cmd, "r");
+  Free(cmd);
+  if (!cmd_out) {
+    fprintf(stderr, "Couldn't execute: %s\n", cmd);
+    return NULL;
+  }
+
+  signature = (uint8_t*) Malloc(signature_size);
+  if (fread(signature, signature_size, 1, cmd_out) != 1) {
+    fprintf(stderr, "Couldn't read signature.\n");
+    pclose(cmd_out);
+    Free(signature);
+    return NULL;
+  }
+
+  pclose(cmd_out);
+  return signature;
+}
diff --git a/utils/firmware_image.c b/utils/firmware_image.c
index ae89490..07aa8bd 100644
--- a/utils/firmware_image.c
+++ b/utils/firmware_image.c
@@ -14,11 +14,15 @@
 #include <sys/stat.h>
 #include <unistd.h>
 
+#include "file_keys.h"
 #include "padding.h"
 #include "rsa_utility.h"
 #include "sha_utility.h"
 #include "utility.h"
 
+/* Macro to determine the size of a field structure in the FirmwareImage
+ * structure. */
+#define FIELD_LEN(field) (sizeof(((FirmwareImage*)0)->field))
 
 FirmwareImage* FirmwareImageNew(void) {
   FirmwareImage* fw = (FirmwareImage*) Malloc(sizeof(FirmwareImage));
@@ -33,9 +37,8 @@
   Free(image->firmware_data);
 }
 
-
-FirmwareImage* ReadFirmware(const char* input_file,
-                            FirmwareImage* image) {
+FirmwareImage* ReadFirmwareImage(const char* input_file,
+                                 FirmwareImage* image) {
   int fd;
   struct stat fd_stat;
 
@@ -78,7 +81,7 @@
   if (!StatefulMemcpy(&st, &image->magic, FIRMWARE_MAGIC_SIZE))
     goto parse_failure;
 
-  if (!SafeMemcmp(image->magic, FIRMWARE_MAGIC, FIRMWARE_MAGIC_SIZE)) {
+  if (SafeMemcmp(image->magic, FIRMWARE_MAGIC, FIRMWARE_MAGIC_SIZE)) {
     fprintf(stderr, "Wrong Firmware Magic.\n");
     goto parse_failure;
   }
@@ -99,7 +102,7 @@
   /* Check whether the header length is correct. */
   header_len = (sizeof(image->header_len) + sizeof(image->sign_algorithm) +
                 sizeof(image->key_version) +
-                sizeof(image->header_hash));
+                sizeof(image->header_checksum));
   if (header_len != image->header_len) {
     fprintf(stderr, "Header length mismatch.");
     goto parse_failure;
@@ -109,7 +112,7 @@
   image->sign_key = (uint8_t*) Malloc(sign_key_len);
   StatefulMemcpy(&st, image->sign_key, sign_key_len);
   StatefulMemcpy(&st, &image->key_version, sizeof(image->key_version));
-  StatefulMemcpy(&st, image->header_hash, sizeof(image->header_hash));
+  StatefulMemcpy(&st, image->header_checksum, sizeof(image->header_checksum));
 
   /* Read key signature. */
   StatefulMemcpy(&st, image->key_signature, sizeof(image->key_signature));
@@ -147,10 +150,10 @@
   sign_key_len = (image->header_len - sizeof(image->header_len) -
                      sizeof(image->sign_algorithm) -
                      sizeof(image->key_version) -
-                     sizeof(image->header_hash));
+                     sizeof(image->header_checksum));
   write(fd, image->sign_key, sign_key_len);
   write(fd, &image->key_version, sizeof(image->key_version));
-  write(fd, &image->header_hash, sizeof(image->header_hash));
+  write(fd, &image->header_checksum, sizeof(image->header_checksum));
 }
 
 void WriteFirmwarePreamble(int fd, FirmwareImage* image) {
@@ -160,31 +163,29 @@
   write(fd, image->preamble, sizeof(image->preamble));
 }
 
+FirmwareImage* WriteFirmwareImage(const char* input_file,
+                                  FirmwareImage* image) {
+  int fd;
+  int signature_len;
 
-FirmwareImage* WriteFirmware(const char* input_file,
-                             FirmwareImage* image) {
-   int fd;
-   int signature_len;
+  if (!image)
+    return NULL;
+  if (-1 == (fd = creat(input_file, S_IRWXU))) {
+    fprintf(stderr, "Couldn't open file for writing.\n");
+    return NULL;
+  }
 
-   if (!image)
-     return NULL;
+  write(fd, image->magic, sizeof(image->magic));
+  WriteFirmwareHeader(fd, image);
+  write(fd, image->key_signature, sizeof(image->key_signature));
+  signature_len = siglen_map[image->sign_algorithm] * sizeof(uint32_t);
+  WriteFirmwarePreamble(fd, image);
+  write(fd, image->preamble_signature, signature_len);
+  write(fd, image->firmware_signature, signature_len);
+  write(fd, image->firmware_data, image->firmware_len);
 
-   if (-1 == (fd = open(input_file, O_WRONLY))) {
-     fprintf(stderr, "Couldn't open file for writing.\n");
-     return NULL;
-   }
-
-   write(fd, &image->magic, sizeof(image->magic));
-   WriteFirmwareHeader(fd, image);
-   write(fd, image->key_signature, sizeof(image->key_signature));
-   signature_len = siglen_map[image->sign_algorithm] * sizeof(uint32_t);
-   WriteFirmwarePreamble(fd, image);
-   write(fd, image->preamble_signature, signature_len);
-   write(fd, image->firmware_signature, signature_len);
-   write(fd, image->firmware_data, image->firmware_len);
-
-   close(fd);
-   return image;
+  close(fd);
+  return image;
 }
 
 void PrintFirmware(const FirmwareImage* image) {
@@ -209,9 +210,169 @@
   /* Output key signature here? */
 }
 
-int VerifyFirmware(const RSAPublicKey* root_key,
-                   const FirmwareImage* image,
+char* kVerifyFirmwareErrors[VERIFY_FIRMWARE_MAX] = {
+  "Success.",
+  "Invalid Image.",
+  "Root Key Signature Failed.",
+  "Invalid Verification Algorithm.",
+  "Preamble Signature Failed.",
+  "Firmware Signature Failed.",
+  "Wrong Firmware Magic.",
+};
+
+int VerifyFirmwareHeader(const uint8_t* root_key_blob,
+                         const uint8_t* header_blob,
+                         const int dev_mode,
+                         int* algorithm,
+                         int* header_len) {
+  int sign_key_len;
+  int root_key_len;
+  uint16_t hlen, algo;
+  uint8_t* header_checksum = NULL;
+
+  /* Base Offset for the header_checksum field. Actual offset is
+   * this + sign_key_len. */
+  int base_header_checksum_offset = (FIELD_LEN(header_len) +
+                                     FIELD_LEN(sign_algorithm) +
+                                     FIELD_LEN(key_version));
+
+
+  root_key_len = RSAProcessedKeySize(ROOT_SIGNATURE_ALGORITHM);
+  Memcpy(&hlen, header_blob, sizeof(hlen));
+  Memcpy(&algo,
+         header_blob + FIELD_LEN(sign_algorithm),
+         sizeof(algo));
+  if (algo >= kNumAlgorithms)
+    return VERIFY_FIRMWARE_INVALID_ALGORITHM;
+  *algorithm = (int) algo;
+  sign_key_len = RSAProcessedKeySize(*algorithm);
+
+  /* Verify if header len is correct? */
+  if (hlen != (base_header_checksum_offset +
+               sign_key_len +
+               FIELD_LEN(header_checksum)))
+    return VERIFY_FIRMWARE_INVALID_IMAGE;
+
+  *header_len = (int) hlen;
+
+  /* Verify if the hash of the header is correct. */
+  header_checksum = DigestBuf(header_blob,
+                              *header_len - FIELD_LEN(header_checksum),
+                              SHA512_DIGEST_ALGORITHM);
+  if (SafeMemcmp(header_checksum,
+                  header_blob + (base_header_checksum_offset + sign_key_len),
+                  FIELD_LEN(header_checksum))) {
+    Free(header_checksum);
+    return VERIFY_FIRMWARE_INVALID_IMAGE;
+  }
+  Free(header_checksum);
+
+  /* Verify root key signature unless we are in dev mode. */
+  if (!dev_mode) {
+    if (!RSAVerifyBinary_f(root_key_blob, NULL,  /* Key to use */
+                           header_blob,  /* Data to verify */
+                           *header_len, /* Length of data */
+                           header_blob + *header_len,  /* Expected Signature */
+                           ROOT_SIGNATURE_ALGORITHM))
+      return VERIFY_FIRMWARE_ROOT_SIGNATURE_FAILED;
+  }
+  return 0;
+}
+
+int VerifyFirmwarePreamble(RSAPublicKey* sign_key,
+                           const uint8_t* preamble_blob,
+                           int algorithm,
+                           int* firmware_len) {
+  uint32_t len;
+  int preamble_len;
+  preamble_len = (FIELD_LEN(firmware_version) +
+                  FIELD_LEN(firmware_len) +
+                  FIELD_LEN(preamble));
+  if (!RSAVerifyBinary_f(NULL, sign_key,  /* Key to use */
+                         preamble_blob,  /* Data to verify */
+                         preamble_len,  /* Length of data */
+                         preamble_blob + preamble_len,  /* Expected Signature */
+                         algorithm))
+    return VERIFY_FIRMWARE_PREAMBLE_SIGNATURE_FAILED;
+
+  Memcpy(&len, preamble_blob + FIELD_LEN(firmware_version),
+         sizeof(len));
+  *firmware_len = (int) len;
+  return 0;
+}
+
+int VerifyFirmwareData(RSAPublicKey* sign_key,
+                       const uint8_t* firmware_data_start,
+                       int firmware_len,
+                       int algorithm) {
+  int signature_len = siglen_map[algorithm] * sizeof(uint32_t);
+  if (!RSAVerifyBinary_f(NULL, sign_key,  /* Key to use. */
+                         firmware_data_start + signature_len,  /* Data to
+                                                                * verify */
+                         firmware_len,  /* Length of data. */
+                         firmware_data_start,  /* Expected Signature */
+                         algorithm))
+    return VERIFY_FIRMWARE_SIGNATURE_FAILED;
+  return 0;
+}
+
+int VerifyFirmware(const uint8_t* root_key_blob,
+                   const uint8_t* firmware_blob,
                    const int dev_mode) {
+  int error_code;
+  int algorithm;  /* Signing key algorithm. */
+  RSAPublicKey* sign_key;
+  int sign_key_len, signature_len, header_len, firmware_len;
+  const uint8_t* header_ptr;  /* Pointer to header. */
+  const uint8_t* sign_key_ptr;  /* Pointer to signing key. */
+  const uint8_t* preamble_ptr;  /* Pointer to preamble block. */
+  const uint8_t* firmware_ptr;  /* Pointer to firmware signature/data. */
+
+  /* Note: All the offset calculations are based on struct FirmwareImage which
+   * is defined in include/firmware_image.h. */
+
+  /* Compare magic bytes. */
+  if (SafeMemcmp(firmware_blob, FIRMWARE_MAGIC, FIRMWARE_MAGIC_SIZE))
+    return VERIFY_FIRMWARE_WRONG_MAGIC;
+  header_ptr = firmware_blob + FIRMWARE_MAGIC_SIZE;
+
+  /* Only continue if header verification succeeds. */
+  if ((error_code = VerifyFirmwareHeader(root_key_blob, header_ptr, dev_mode,
+                                         &algorithm, &header_len)))
+    return error_code;  /* AKA jump to revovery. */
+
+  /* Parse signing key into RSAPublicKey structure since it is required multiple
+   * times. */
+  sign_key_len = RSAProcessedKeySize(algorithm);
+  sign_key_ptr = header_ptr + (FIELD_LEN(header_len) +
+                               FIELD_LEN(sign_algorithm));
+  sign_key = RSAPublicKeyFromBuf(sign_key_ptr, sign_key_len);
+  signature_len = siglen_map[algorithm] * sizeof(uint32_t);
+
+  /* Only continue if preamble verification succeeds. */
+  preamble_ptr = (header_ptr + header_len +
+                  FIELD_LEN(key_signature));
+  if ((error_code = VerifyFirmwarePreamble(sign_key, preamble_ptr, algorithm,
+                                           &firmware_len)))
+    return error_code;  /* AKA jump to recovery. */
+
+  /* Only continue if firmware data verification succeeds. */
+  firmware_ptr = (preamble_ptr +
+                  FIELD_LEN(firmware_version) +
+                  FIELD_LEN(firmware_len) +
+                  FIELD_LEN(preamble) +
+                  signature_len);
+
+  if ((error_code = VerifyFirmwareData(sign_key, firmware_ptr, firmware_len,
+                                       algorithm)))
+    return error_code;  /* AKA jump to recovery. */
+
+  return 0;  /* Success! */
+}
+
+int VerifyFirmwareImage(const RSAPublicKey* root_key,
+                        const FirmwareImage* image,
+                        const int dev_mode) {
   RSAPublicKey* sign_key;
   uint8_t* header_digest = NULL;
   uint8_t* preamble_digest = NULL;
@@ -222,10 +383,15 @@
   DigestContext ctx;
 
   if (!image)
-    return VERIFY_INVALID_IMAGE;
+    return VERIFY_FIRMWARE_INVALID_IMAGE;
 
   /* Verify root key signature on the sign key header if we
-   * are not in dev mode. */
+   * are not in dev mode.
+   *
+   * TODO(gauravsh): Add additional sanity checks here for:
+   *  1) verifying the header length is correct.
+   *  2) header_checksum is correct.
+   */
   if (!dev_mode) {
     DigestInit(&ctx, ROOT_SIGNATURE_ALGORITHM);
     DigestUpdate(&ctx, (uint8_t*) &image->header_len,
@@ -236,14 +402,14 @@
                  RSAProcessedKeySize(image->sign_algorithm));
     DigestUpdate(&ctx, (uint8_t*) &image->key_version,
                  sizeof(image->key_version));
-    DigestUpdate(&ctx, image->header_hash,
-                 sizeof(image->header_hash));
+    DigestUpdate(&ctx, image->header_checksum,
+                 sizeof(image->header_checksum));
     header_digest = DigestFinal(&ctx);
     if (!RSA_verify(root_key, image->key_signature,
                     sizeof(image->key_signature),
                     ROOT_SIGNATURE_ALGORITHM,
                     header_digest)) {
-      error_code =  VERIFY_ROOT_SIGNATURE_FAILED;
+      error_code =  VERIFY_FIRMWARE_ROOT_SIGNATURE_FAILED;
       goto verify_failure;
     }
   }
@@ -255,7 +421,7 @@
   signature_size = siglen_map[image->sign_algorithm] * sizeof(uint32_t);
 
   if (image->sign_algorithm >= kNumAlgorithms)
-    return VERIFY_INVALID_ALGORITHM;
+    return VERIFY_FIRMWARE_INVALID_ALGORITHM;
 
   /* Verify firmware preamble signature. */
   DigestInit(&ctx, image->sign_algorithm);
@@ -269,7 +435,7 @@
   if (!RSA_verify(sign_key, image->preamble_signature,
                   signature_size, image->sign_algorithm,
                   preamble_digest)) {
-    error_code = VERIFY_PREAMBLE_SIGNATURE_FAILED;
+    error_code = VERIFY_FIRMWARE_PREAMBLE_SIGNATURE_FAILED;
     goto verify_failure;
   }
 
@@ -291,52 +457,6 @@
   return error_code;
 }
 
-char* kVerifyFirmwareErrors[VERIFY_MAX] = {
-  "Success.",
-  "Invalid Image.",
-  "Root Key Signature Failed.",
-  "Invalid Verification Algorithm.",
-  "Preamble Signature Failed.",
-  "Firmware Signature Failed.",
-};
-
-uint8_t* SignatureFile(char* input_file, char* key_file, int algorithm) {
-  char* sign_utility = "./sign_data.sh";
-  char* cmd;  /* Command line to invoke. */
-  int cmd_len;
-  FILE* cmd_out;  /* File descriptor to command output. */
-  uint8_t* signature = NULL;
-  int signature_size = siglen_map[algorithm] * sizeof(uint32_t);
-
-  /* Build command line:
-   * sign_data.sh <algorithm> <key file> <input file>
-   */
-  cmd_len = (strlen(sign_utility) + 1 + /* +1 for space. */
-             2 + 1 + /* For [algorithm]. */
-             strlen(key_file) + 1 + /* +1 for space. */
-             strlen(input_file) +
-             1);  /* For the trailing '\0'. */
-  cmd = (char*) Malloc(cmd_len);
-  snprintf(cmd, cmd_len, "%s %d %s %s", sign_utility, algorithm, key_file,
-           input_file);
-  cmd_out = popen(cmd, "r");
-  Free(cmd);
-  if (!cmd_out) {
-    fprintf(stderr, "Couldn't execute: %s\n", cmd);
-    return NULL;
-  }
-
-  signature = (uint8_t*) Malloc(signature_size);
-  if (fread(signature, signature_size, 1, cmd_out) != 1) {
-    fprintf(stderr, "Couldn't read signature.\n");
-    pclose(cmd_out);
-    Free(signature);
-    return NULL;
-  }
-
-  pclose(cmd_out);
-  return signature;
-}
 
 int AddKeySignature(FirmwareImage* image, char* root_key_file) {
   int tmp_hdr_fd;
@@ -371,7 +491,7 @@
   /* Write preamble to a file. */
   if(-1 == (tmp_preamble_fd = creat(tmp_preamble_file, S_IRWXU))) {
     fprintf(stderr, "Could not open temporary file for writing "
-            "firmware praemble.\n");
+            "firmware preamble.\n");
     return 0;
   }
   WriteFirmwarePreamble(tmp_preamble_fd, image);
diff --git a/utils/signature_digest.c b/utils/signature_digest.c
index 8dcd8c9..100f7ec 100644
--- a/utils/signature_digest.c
+++ b/utils/signature_digest.c
@@ -21,7 +21,7 @@
 #include "sha.h"
 #include "sha_utility.h"
 
-uint8_t* prepend_digestinfo(int algorithm, uint8_t* digest) {
+uint8_t* PrependDigestInfo(int algorithm, uint8_t* digest) {
   const int digest_size = hash_size_map[algorithm];
   const int digestinfo_size = digestinfo_size_map[algorithm];
   const uint8_t* digestinfo = hash_digestinfo_map[algorithm];
@@ -55,7 +55,7 @@
   if (!(digest = DigestFile(argv[2], algorithm)))
     goto failure;
 
-  info_digest = prepend_digestinfo(algorithm, digest);
+  info_digest = PrependDigestInfo(algorithm, digest);
   write(1, info_digest, hash_size_map[algorithm] +
         digestinfo_size_map[algorithm]);
 
diff --git a/utils/verify_data.c b/utils/verify_data.c
index 5cb33f4..c58b6fb 100644
--- a/utils/verify_data.c
+++ b/utils/verify_data.c
@@ -22,6 +22,11 @@
 #include "rsa_utility.h"
 #include "verify_data.h"
 
+/* ANSI Color coding sequences. */
+#define COL_GREEN "\e[1;32m"
+#define COL_RED "\e[0;31m]"
+#define COL_STOP "\e[m"
+
 uint8_t* read_signature(char* input_file, int len) {
   int i, sigfd;
   uint8_t* signature = NULL;
@@ -46,7 +51,6 @@
   return signature;
 }
 
-
 int main(int argc, char* argv[]) {
   int i, algorithm, sig_len;
   int return_code = 1;  /* Default to error. */
@@ -80,10 +84,11 @@
     goto failure;
   if(RSA_verify(key, signature, sig_len, algorithm, digest)) {
     return_code = 0;
-    fprintf(stderr, "Signature Verification SUCCEEDED.\n");
-  }
-  else {
-    fprintf(stderr, "Signature Verification FAILED!\n");
+    fprintf(stderr, "Signature Verification "
+            COL_GREEN "SUCCEEDED" COL_STOP "\n");
+  } else {
+    fprintf(stderr, "Signature Verification "
+            COL_RED "FAILED" COL_STOP "\n");
   }
 
 failure: