zinger: check RW firmware signature

The Zinger RW is now signed with 2048-bit RSA key (using SHA-256 as
digest).
This CL implements the verification mechanism.

note: the RSA key used for signing must be provided as a .pem file.
The path to .pem file must be provided in the PEM environment variable.
By default, it's using the dev key stored in zinger_dev_key.pem.

Signed-off-by: Vincent Palatin <vpalatin@chromium.org>

BRANCH=samus
BUG=chrome-os-partner:28336
TEST=on Zinger, run with properly signed RW firmware and corrupted
firmware and check the serial traces.

Change-Id: Ia58482458904a3ed72d6b0e95996cae86a0ead83
Reviewed-on: https://chromium-review.googlesource.com/220178
Commit-Queue: Vincent Palatin <vpalatin@chromium.org>
Tested-by: Vincent Palatin <vpalatin@chromium.org>
Reviewed-by: Alec Berg <alecaberg@chromium.org>

14 files changed, 402 insertions, 31 deletions

diff --git a/Makefile b/Makefile
index 204dbdfdbf..92440e4c36 100644
--- a/Makefile
+++ b/Makefile
@@ -30,8 +30,13 @@ PROJECT?=ec
 # Output directory for build objects
 out?=build/$(BOARD)
 
+# If no key file is provided, use the default dev key
+PEM ?= board/$(BOARD)/dev_key.pem
+
 include Makefile.toolchain
 
+all: $(out)/$(PROJECT).bin utils
+
 # The board makefile sets $CHIP and the chip makefile sets $CORE.
 # Include those now, since they must be defined for _flag_cfg below.
 include board/$(BOARD)/build.mk
@@ -79,6 +84,14 @@ ifneq "$(CONFIG_COMMON_RUNTIME)" "y"
 		    -D"irq_$(irq)_handler_optional=irq_$(irq)_handler")
 endif
 
+# Compute RW firmware size and offset
+_rw_off_str:=$(shell echo "CONFIG_FW_RW_OFF" | $(CPP) $(CPPFLAGS) -P \
+		-Ichip/$(CHIP) -Iboard/$(BOARD) -imacros include/config.h)
+_rw_off:=$(shell echo "$$(($(_rw_off_str)))")
+_rw_size_str:=$(shell echo "CONFIG_FW_RW_SIZE" | $(CPP) $(CPPFLAGS) -P \
+		-Ichip/$(CHIP) -Iboard/$(BOARD) -imacros include/config.h)
+_rw_size:=$(shell echo "$$(($(_rw_size_str)))")
+
 # Get build configuration from sub-directories
 # Note that this re-includes the board and chip makefiles
 include board/$(BOARD)/build.mk
diff --git a/Makefile.rules b/Makefile.rules
index e5279d8097..4a19e0a3b3 100644
--- a/Makefile.rules
+++ b/Makefile.rules
@@ -30,6 +30,7 @@ section_is = $(subst .,,SECTION_IS_$(suffix $(1)))
 # Decrease verbosity unless you pass V=1
 quiet = $(if $(V),,@echo '  $(2)' $(subst $(out)/,,$@) ; )$(cmd_$(1))
 silent = $(if $(V),,1>/dev/null)
+silent_err = $(if $(V),,2>/dev/null)
 
 # commands to build all targets
 cmd_lds = $(CPP) -P -C -MMD -MF $@.d -MT $@ $(CPPFLAGS) \
@@ -52,11 +53,14 @@ cmd_c_to_host = $(HOSTCC) $(HOST_CFLAGS) -MMD -MF $@.d  -o $@ \
 cmd_host_test = ./util/run_host_test $* $(silent)
 cmd_version = ./util/getversion.sh > $@
 cmd_mv_from_tmp = mv $(out)/$*.bin.tmp $(out)/$*.bin
-cmd_extractrw-y = cd $(out) && \
-	dump_fmap -x $(PROJECT).bin.tmp RW_SECTION_A $(silent) && \
-	mv RW_SECTION_A $(PROJECT).RW.bin
+cmd_extractrw-y = dd if=$(out)/$(PROJECT).bin.tmp of=$(out)/$(PROJECT).RW.bin \
+	       bs=1 count=$(_rw_size) skip=$(_rw_off) $(silent_err)
 cmd_copyrw-y = cd $(out) && cp $(PROJECT).RW.flat $(PROJECT).RW.bin
 
+# commands for RSA signature
+cmd_pubkey = ./util/pem_extract_pubkey.py $(PEM) > $@
+cmd_rsasign = ./util/ec_sign_rsa.py $(PEM) $(out)/$*.bin.tmp
+
 # commands to build optional xref files
 cmd_deps_to_list = cat $(deps) | tr -d ':\\' | tr ' ' '\012' \
 	| egrep '\.[chS]$$' | sort | uniq > $@
@@ -65,7 +69,6 @@ cmd_ctags = ctags -o $@ $(shell cat $<)
 targ_if_prog = $(if $(shell which $(1) 2>/dev/null),$(2),)
 
 .PHONY: all tests utils hosttests
-all: $(out)/$(PROJECT).bin utils
 
 buildall: $(foreach b, $(boards), proj-$(b)) runtests
 	@touch .tests-passed
@@ -138,8 +141,8 @@ $(out)/%.lds: core/$(CORE)/ec.lds.S
 
 $(out)/%.bin: $(out)/%.obj
 	$(call quiet,obj_to_bin,OBJCOPY)
-	$(if $(sign-y),$(call quiet,sign,SIGN   ),)
-	$(if $(sign-y),$(call quiet,extractrw-y,EXTR_RW), \
+	$(if $(wildcard $(PEM)),$(call quiet,rsasign,SIGN   ),)
+	$(if $(wildcard $(PEM)),$(call quiet,extractrw-y,EXTR_RW), \
 			$(call quiet,copyrw-y,COPY_RW))
 	$(call quiet,mv_from_tmp,MV     )
 
@@ -173,6 +176,9 @@ $(out)/common/version.o: $(out)/ec_version.h
 $(out)/ec_version.h: $(filter-out $(out)/common/version.o,$(objs))
 	$(call quiet,version,VERSION)
 
+$(out)/gen_pub_key.h: $(PEM)
+	$(call quiet,pubkey,PUBKEY )
+
 $(build-utils): $(out)/%:$(build-srcs)
 	$(call quiet,c_to_build,BUILDCC)
 
diff --git a/board/zinger/board.c b/board/zinger/board.c
index faab419cc4..bf974cef24 100644
--- a/board/zinger/board.c
+++ b/board/zinger/board.c
@@ -7,11 +7,22 @@
 #include "common.h"
 #include "debug.h"
 #include "registers.h"
+#include "rsa.h"
 #include "sha1.h"
+#include "sha256.h"
 #include "task.h"
 #include "usb_pd.h"
 #include "util.h"
 
+/* Insert the RSA public key definition */
+const struct rsa_public_key pkey __attribute__((section(".rsa_pubkey"))) =
+#include "gen_pub_key.h"
+/* The RSA signature is stored at the end of the RW firmware */
+static const void *rw_sig = (void *)CONFIG_FLASH_BASE + CONFIG_FW_RW_OFF
+				 + CONFIG_FW_RW_SIZE - RSANUMBYTES;
+/* Large 768-Byte buffer for RSA computation : could be re-use afterwards... */
+static uint32_t rsa_workbuf[3 * RSANUMWORDS];
+
 extern void pd_rx_handler(void);
 
 /* RW firmware reset vector */
@@ -44,23 +55,17 @@ int is_ro_mode(void)
 
 static int check_rw_valid(void)
 {
-	uint32_t *hash;
-	uint32_t *fw_hash = (uint32_t *)
-		(CONFIG_FLASH_BASE + CONFIG_FLASH_SIZE - 32);
+	int good;
+	uint8_t *hash;
 
 	/* Check if we have a RW firmware flashed */
 	if (*rw_rst == 0xffffffff)
 		return 0;
 
-	hash = (uint32_t *)flash_hash_rw();
-	/* TODO(crosbug.com/p/28336) use secret key to check RW */
-	if (memcmp(hash, fw_hash, SHA1_DIGEST_SIZE) != 0) {
-		/* Firmware doesn't match the recorded hash */
-		debug_printf("SHA-1 %08x %08x %08x %08x %08x\n",
-			hash[0], hash[1], hash[2], hash[3], hash[4]);
-		debug_printf("FW SHA-1 %08x %08x %08x %08x %08x\n",
-			fw_hash[0], fw_hash[1], fw_hash[2],
-			fw_hash[3], fw_hash[4]);
+	hash = flash_hash_rw();
+	good = rsa_verify(&pkey, (void *)rw_sig, (void *)hash, rsa_workbuf);
+	if (!good) {
+		debug_printf("RSA verify FAILED\n");
 		return 0;
 	}
 
diff --git a/board/zinger/board.h b/board/zinger/board.h
index daca59de8f..2b420e496a 100644
--- a/board/zinger/board.h
+++ b/board/zinger/board.h
@@ -24,7 +24,8 @@
 #define CONFIG_USB_PD_CUSTOM_VDM
 #undef CONFIG_USB_PD_RX_COMP_IRQ
 #define CONFIG_HW_CRC
-#define CONFIG_SHA1
+#define CONFIG_RSA
+#define CONFIG_SHA256
 #undef CONFIG_WATCHDOG_HELP
 #undef CONFIG_WATCHDOG_PERIOD_MS
 #define CONFIG_WATCHDOG_PERIOD_MS 2300
diff --git a/board/zinger/build.mk b/board/zinger/build.mk
index 4e9e2f1f6d..885b0c5c0a 100644
--- a/board/zinger/build.mk
+++ b/board/zinger/build.mk
@@ -12,3 +12,6 @@ CHIP_VARIANT:=stm32f03x
 
 board-y=board.o hardware.o runtime.o usb_pd_policy.o
 board-$(CONFIG_DEBUG_PRINTF)+=debug.o
+
+# Add dependency to generate the public key coefficients header
+$(out)/board/$(BOARD)/board.o: $(out)/gen_pub_key.h
diff --git a/board/zinger/dev_key.pem b/board/zinger/dev_key.pem
new file mode 100644
index 0000000000..6912b1f44e
--- /dev/null
+++ b/board/zinger/dev_key.pem
@@ -0,0 +1,27 @@
+-----BEGIN RSA PRIVATE KEY-----
+MIIEpgIBAAKCAQEAvI3KBubudlZyX1oBNzWhQ+bNemHNVC5bO7TjJMTYTIJeKTaW
+IyHCFLF9ztpe8tT9Y+ga4VO9PEktP1WJpdU0ecG6VwK3951/cElosfnIPmIY7dVp
+PQtGtGS/Zih1nTMRV5hqtGa9SRg0i2kdph+quFXGQoMriTl0StmvNVtD51nRPGwc
+ZsG9+P0yfnyo7l3qXtKu5gBx/jDne2kl5/isPHkKfl9le+aNQaNjWeJyB4XDqHXM
+AjuW1G7FxoKlU4b363gQbE84Q80X1Qd9iAoRj/HLmDrx9h6FDFs+HbKDfCYtg2fK
+upEHr1bFUCxjc6AWuFglSq0yn5kyp1Bh5CD3PQIDAQABAoIBAQCNO2NlEhrx9sSK
+mX8pnHkjxECK97D16hcaPN6azvr5K/ldw25n+ERIfb4vb7AJEfKOn+9qce/ftSw4
+MVj+Jxm8TZjGzdmAlq87KzFJhkAwQghMNTszpzuZqZEX8xxN2E+YHilm4UHM+114
+Qw8bPMMCefUcIuY8ThXGbxFm1Oqi4YHHfqE6waRc0XXnLZOYr7NDCmhgyUG1dpAH
+kW0EYuJ1UNGaKE4LsWKvi0SYBQ48Mqh1XPkyiL/5I2whewsU2K4KEjynZp0+ULUG
+Dxfv3uCywsSsLuNR+EV5tTUp4eY1BLKULJQTMH3hyV1Xf1qEt2YN/3ZHsv7MPQzS
+sPIdN+LhAoGBAN6wbcxPnfkJROOVRUzE05IEEBalVULLo1cA1ss/7RjIeUvdRCAa
+12OxF4LSNzrSxcPCLsDnYq+j4HoS6KZ31c1TbaKcaUOPfRohrtGBZMxPgDTZgEBa
+JlsVtD2vzYvfUIpVQFz6Tnix3F29Gq5RaZdW5/qwOYyx0wtUrPf+pwZ5AoGBANjC
+MjgoGtcubR6chDhZcFU4vopdL7IEhMOZ1qxLFTQnINGXXDJpgVvdJRKdDV29DjNZ
+zF9wgmoiVm+uM/344bquUV7KHl5bEsZ+4KH6EA4y3IKVgxaxU7dpF6Q6L+rAuYp/
+j0N9XoVnS3aq30HkTkt+jQe0Hl6eEDOJqHEjolXlAoGBAJbMqs3cbIGkQT5May1d
+bFhI4Aw10dL1y5qzOsFQfOJ3f4xcPjHve0RLPDye1j/DU6EI8lg3WKDQPMbt3xY7
+uFDe2jNv7+iMVo9Hl/bPxM6GV69ySmNJqQetXu0XC/5YL1Y9/OP5rQIWj7/6uwKo
+pvSRKW6dv5sDIINfx/H4RGshAoGBAMIs7Tn7S1gaoev7QEMOdCAT7jUbF3/8pkZn
+SLUdqcgHiVHYquIKO7TknbJX+MJReygrOHcC3gFf81imkLLiQqyuPfyRSbUzFtW0
+kVzpG3rsuzdL4pvwjNNQFLqs2YIN1eipLtjBtWwCRcrvdYKcmDrvCj2tcEtIg7D3
+j2qTBni1AoGBAI58xPHxB0cNclhWiFHPNgk98GkwADWxfeTZduoyfpraSrpbseu8
+Cfgq1p5E2nM9jWx4jdKA/fxdD40bneupPi5w5SE2gmwtmQFR3TehI8gxNbEL2Gq6
+6ZkgxnGNxFaE6saHVDHKU8Q2bgzCI8JlOOtSjzKvbr+hsQMYHcEJxom6
+-----END RSA PRIVATE KEY-----
diff --git a/board/zinger/hardware.c b/board/zinger/hardware.c
index 1b22ec64c4..061f3bdf89 100644
--- a/board/zinger/hardware.c
+++ b/board/zinger/hardware.c
@@ -9,7 +9,8 @@
 #include "common.h"
 #include "cpu.h"
 #include "registers.h"
-#include "sha1.h"
+#include "rsa.h"
+#include "sha256.h"
 #include "task.h"
 #include "timer.h"
 #include "util.h"
@@ -374,11 +375,11 @@ exit_er:
 	return res;
 }
 
-static struct sha1_ctx ctx;
+static struct sha256_ctx ctx;
 uint8_t *flash_hash_rw(void)
 {
-	sha1_init(&ctx);
-	sha1_update(&ctx, (void *)CONFIG_FLASH_BASE + CONFIG_FW_RW_OFF,
-		    CONFIG_FW_RW_SIZE - 32);
-	return sha1_final(&ctx);
+	SHA256_init(&ctx);
+	SHA256_update(&ctx, (void *)CONFIG_FLASH_BASE + CONFIG_FW_RW_OFF,
+		    CONFIG_FW_RW_SIZE - RSANUMBYTES);
+	return SHA256_final(&ctx);
 }
diff --git a/board/zinger/usb_pd_policy.c b/board/zinger/usb_pd_policy.c
index bc2a0ede3b..f54841bb72 100644
--- a/board/zinger/usb_pd_policy.c
+++ b/board/zinger/usb_pd_policy.c
@@ -378,13 +378,12 @@ int pd_custom_vdm(int port, int cnt, uint32_t *payload, uint32_t **rpayload)
 		break;
 	case VDO_CMD_READ_INFO:
 		hash = flash_hash_rw();
-		/* copy hash into response */
-		memcpy(payload + 1, hash, SHA1_DIGEST_SIZE);
+		/* copy the 20 first bytes of the hash into response */
+		memcpy(payload + 1, hash, 5 * sizeof(uint32_t));
 		/* copy other info into response */
-		payload[SHA1_DIGEST_SIZE/4 + 1] = VDO_INFO(
-						USB_PD_HARDWARE_DEVICE_ID,
-						ver_get_numcommits(),
-						!is_ro_mode());
+		payload[6] = VDO_INFO(USB_PD_HARDWARE_DEVICE_ID,
+					ver_get_numcommits(),
+					!is_ro_mode());
 		rsize = 7;
 		break;
 	case VDO_CMD_FLASH_ERASE:
diff --git a/common/build.mk b/common/build.mk
index cd77040a3d..189a44a5c1 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -61,6 +61,7 @@ common-$(CONFIG_PWM)+=pwm.o
 common-$(CONFIG_PWM_KBLIGHT)+=pwm_kblight.o
 common-$(CONFIG_RSA)+=rsa.o
 common-$(CONFIG_SHA1)+=sha1.o
+common-$(CONFIG_SHA256)+=sha256.o
 common-$(CONFIG_SMBUS)+= smbus.o
 common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
 common-$(CONFIG_SPI_FLASH)+=spi_flash.o
diff --git a/core/cortex-m/ec.lds.S b/core/cortex-m/ec.lds.S
index dcadd43a6a..ad0de62289 100644
--- a/core/cortex-m/ec.lds.S
+++ b/core/cortex-m/ec.lds.S
@@ -209,6 +209,13 @@ SECTIONS
 	/* NOTHING MAY GO AFTER THIS! */
     } > IRAM
 
+    .flash_suffix  : AT(LOADADDR(.data) + SIZEOF(.data)) {
+        FILL(0xff);
+        /* Put the public key coefficients at the end of the partition */
+        . = ORIGIN(FLASH) + LENGTH(FLASH) - 528;
+        *(.rsa_pubkey)
+    } > FLASH
+
     /* The linker won't notice if the .data section is too big to fit,
      * apparently because we're sending it into IRAM, not FLASH. The following
      * symbol isn't used by the code, but running "objdump -t *.elf | grep hey"
diff --git a/core/cortex-m0/ec.lds.S b/core/cortex-m0/ec.lds.S
index 0eb80252e1..a2e372ad8a 100644
--- a/core/cortex-m0/ec.lds.S
+++ b/core/cortex-m0/ec.lds.S
@@ -197,6 +197,13 @@ SECTIONS
 	/* NOTHING MAY GO AFTER THIS! */
     } > IRAM
 
+    .flash_suffix  : AT(LOADADDR(.data) + SIZEOF(.data)) {
+        FILL(0xff);
+        /* Put the public key coefficients at the end of the partition */
+        . = ORIGIN(FLASH) + LENGTH(FLASH) - 528;
+        *(.rsa_pubkey)
+    } > FLASH
+
     /* The linker won't notice if the .data section is too big to fit,
      * apparently because we're sending it into IRAM, not FLASH. The following
      * symbol isn't used by the code, but running "objdump -t *.elf | grep hey"
diff --git a/include/config.h b/include/config.h
index 897478ac18..87817768d9 100644
--- a/include/config.h
+++ b/include/config.h
@@ -834,6 +834,9 @@
 /* Support computing SHA-1 hash */
 #undef CONFIG_SHA1
 
+/* Support computing SHA-256 hash (without the VBOOT code) */
+#undef CONFIG_SHA256
+
 /* Emulate the CLZ (Count Leading Zeros) in software for CPU lacking support */
 #undef CONFIG_SOFTWARE_CLZ
 
diff --git a/util/ec_sign_rsa.py b/util/ec_sign_rsa.py
new file mode 100755
index 0000000000..661ed37653
--- /dev/null
+++ b/util/ec_sign_rsa.py
@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+# Copyright (c) 2014 The Chromium OS Authors. All rights reserved.
+# Use of this source code is governed by a BSD-style license that can be
+# found in the LICENSE file.
+"""Sign EC firmware with 2048-bit RSA signature.
+
+  Insert the RSA signature (256 bytes) at the end of the RW firmware
+  and replace the public key constants with the new key in RO firmware.
+
+  Example:
+    ./util/sign_rsa [--rw] <pem> <ecfile>
+
+    ./util/sign_rsa board/zinger/zinger_dev_key.pem build/zinger/ec.bin
+"""
+import logging
+import sys
+
+from subprocess import Popen, PIPE
+from pem_extract_pubkey import extract_pubkey
+
+# Size of a 2048-bit RSA signature
+RSANUMBYTES = 256
+# OpenSSL command to sign with SHA256andRSA
+RSA_CMD = ["openssl", "dgst", "-sha256", "-sign"]
+
+# Length reserved at the end of the RO partition for the public key
+PUBKEY_RESERVED_SPACE = 528
+
+def main():
+  # Parse command line arguments
+  if len(sys.argv) < 3:
+    sys.stderr.write("Usage: %s [--rw] <pem> <ecfile>\n" % sys.argv[0])
+    sys.exit(-1)
+  if "--rw" in sys.argv:
+    sys.argv.remove("--rw")
+    has_ro = False
+  else:
+    has_ro = True
+  pemfile = sys.argv[1]
+  ecfile = sys.argv[2]
+
+  # Get EC firmware content
+  try:
+    ec = file(ecfile).read()
+  except:
+    logging.error('cannot read firmware binary %s', ecfile)
+    sys.exit(-1)
+
+  # Extract the padded RW firmware to sign
+  imglen = len(ec)/2
+  rwdata = ec[imglen:-RSANUMBYTES] if has_ro else ec[:-RSANUMBYTES]
+  # Compute the RSA signature using the OpenSSL binary
+  RSA_CMD.append(pemfile)
+  openssl = Popen(RSA_CMD, stdin=PIPE, stdout=PIPE)
+  signature,_ = openssl.communicate(rwdata)
+
+  if has_ro:
+    # Get the public key values from the .pem file
+    pubkey = extract_pubkey(pemfile, headerMode=False)
+    # Add padding
+    pubkey = pubkey + "\xff" * (PUBKEY_RESERVED_SPACE - len(pubkey))
+
+  # Write back the signed EC firmware
+  with open(ecfile, 'w') as fd:
+    if has_ro:
+      fd.write(ec[:imglen-len(pubkey)])
+      fd.write(pubkey)
+    fd.write(rwdata)
+    fd.write(signature)
+
+if __name__ == '__main__':
+  try:
+    main()
+  except KeyboardInterrupt:
+    sys.exit()
diff --git a/util/pem_extract_pubkey.py b/util/pem_extract_pubkey.py
new file mode 100755
index 0000000000..739e1bea57
--- /dev/null
+++ b/util/pem_extract_pubkey.py
@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+# Copyright (c) 2014 The Chromium OS Authors. All rights reserved.
+# Use of this source code is governed by a BSD-style license that can be
+# found in the LICENSE file.
+"""Extract the public key from a .pem RSA private key file (PKCS#1),
+  and compute the public key coefficients used by the signature verification
+  code.
+
+  Example:
+    ./util/pem_extract_pubkey board/zinger/zinger_dev_key.pem
+
+  Note: to generate a suitable private key :
+  RSA 2048-bit with public exponent F4 (65537)
+  you can use the following OpenSSL command :
+  openssl genrsa -F4 -out private.pem 2048
+"""
+
+import array
+import base64
+import sys
+
+VERSION = '0.0.1'
+
+"""
+RSA Private Key file (PKCS#1) encoding :
+
+It starts and ends with the tags:
+-----BEGIN RSA PRIVATE KEY-----
+BASE64 ENCODED DATA
+-----END RSA PRIVATE KEY-----
+
+The base64 encoded data is using an ASN.1 / DER structure :
+
+RSAPrivateKey ::= SEQUENCE {
+  version           Version,
+  modulus           INTEGER,  -- n
+  publicExponent    INTEGER,  -- e
+  privateExponent   INTEGER,  -- d
+  prime1            INTEGER,  -- p
+  prime2            INTEGER,  -- q
+  exponent1         INTEGER,  -- d mod (p-1)
+  exponent2         INTEGER,  -- d mod (q-1)
+  coefficient       INTEGER,  -- (inverse of q) mod p
+  otherPrimeInfos   OtherPrimeInfos OPTIONAL
+}
+"""
+
+PEM_HEADER='-----BEGIN RSA PRIVATE KEY-----'
+PEM_FOOTER='-----END RSA PRIVATE KEY-----'
+
+class PEMError(Exception):
+  """Exception class for pem_extract_pubkey utility."""
+
+# "Constructed" bit in DER tag
+DER_C=0x20
+# DER Sequence tag (always constructed)
+DER_SEQUENCE=DER_C|0x10
+# DER Integer tag
+DER_INTEGER=0x02
+
+class DER:
+  """DER encoded binary data storage and parser."""
+  def __init__(self, data):
+    # DER encoded binary data
+    self._data = data
+    # Initialize index in the data stream
+    self._idx = 0
+
+  def get_byte(self):
+    octet = ord(self._data[self._idx])
+    self._idx += 1
+    return octet
+
+  def get_len(self):
+    octet = self.get_byte()
+    if octet == 0x80:
+      raise PEMError('length indefinite form not supported')
+    if octet & 0x80: # Length long form
+      bytecnt = octet & ~0x80
+      total = 0
+      for i in range(bytecnt):
+        total = (total << 8) | self.get_byte()
+      return total
+    else: # Length short form
+      return octet
+
+  def get_tag(self):
+    tag = self.get_byte()
+    length = self.get_len()
+    data = self._data[self._idx:self._idx + length]
+    self._idx += length
+    return {"tag" : tag, "length" : length, "data" : data}
+
+def pem_get_mod(filename):
+  """Extract the modulus from a PEM private key file.
+
+  the PEM file is DER encoded according the structure quoted above.
+
+  Args:
+    filename : Full path to the .pem private key file.
+
+  Raises:
+    PEMError: If unable to parse .pem file or invalid file format.
+  """
+  # Read all the content of the .pem file
+  content = file(filename).readlines()
+  # Check the PEM RSA Private key tags
+  if content[0].strip() != PEM_HEADER:
+    raise PEMError('invalid PEM private key header')
+  if content[-1].strip() != PEM_FOOTER:
+    raise PEMError('invalid PEM private key footer')
+  # Decode the DER binary stream from the base64 data
+  b64 = "".join([l.strip() for l in content[1:-1]])
+  der = DER(base64.b64decode(b64))
+
+  # Parse the DER and fail at the first error
+  # The private key should be a (constructed) sequence
+  seq = der.get_tag()
+  if seq["tag"] != DER_SEQUENCE:
+    raise PEMError('expecting an ASN.1 sequence')
+  seq = DER(seq["data"])
+
+  # 1st field is Version
+  ver = seq.get_tag()
+  if ver["tag"] != DER_INTEGER:
+    raise PEMError('version field should be an integer')
+
+  # 2nd field is Modulus
+  mod = seq.get_tag()
+  if mod["tag"] != DER_INTEGER:
+    raise PEMError('modulus field should be an integer')
+  # 2048 bits + mandatory ASN.1 sign (0) => 257 Bytes
+  if mod["length"] != 257 or mod["data"][0] != '\x00':
+    raise PEMError('Invalid key length (expecting 2048 bits)')
+
+  # 3rd field is Public Exponent
+  exp = seq.get_tag()
+  if exp["tag"] != DER_INTEGER:
+    raise PEMError('exponent field should be an integer')
+  if exp["length"] != 3 or exp["data"] != "\x01\x00\x01":
+    raise PEMError('the public exponent must be F4 (65537)')
+
+  return mod["data"]
+
+def modinv(a, m):
+  """ The multiplicitive inverse of a in the integers modulo m.
+
+  Return b when a * b == 1 mod m
+  """
+  # Extended GCD
+  lastrem, rem = abs(a), abs(m)
+  x, lastx, y, lasty = 0, 1, 1, 0
+  while rem:
+    lastrem, (quotient, rem) = rem, divmod(lastrem, rem)
+    x, lastx = lastx - quotient*x, x
+    y, lasty = lasty - quotient*y, y
+  #
+  if lastrem != 1:
+    raise ValueError
+  x = lastx * (-1 if a < 0 else 1)
+  return x % m
+
+def to_words(n, count):
+  h = '%x' % n
+  s = ('0'*(len(h) % 2) + h).zfill(count*8).decode('hex')
+  return array.array("I", s[::-1])
+
+def compute_mod_parameters(modulus):
+  ''' Prepare/pre-compute coefficients for the RSA public key signature
+    verification code.
+  '''
+  # create an array of uint32_t to store the modulus but skip the sign byte
+  w = array.array("I",modulus[1:])
+  # all integers in DER encoded .pem file are big endian.
+  w.reverse()
+  w.byteswap()
+  # convert the big-endian modulus to a big integer for the computations
+  N = 0
+  for i in range(len(modulus)):
+    N = (N << 8) | ord(modulus[i])
+  # -1 / N[0] mod 2^32
+  B = 0x100000000L
+  n0inv = B - modinv(w[0], B)
+  # R = 2^(modulo size); RR = (R * R) % N
+  RR = pow(2, 4096, N)
+  rr_words = to_words(RR, 64)
+
+  return {'mod':w, 'rr':rr_words, 'n0inv':n0inv}
+
+def print_header(params):
+  print "{\n\t.n = {%s}," % (",".join(["0x%08x" % (i) for i in params['mod']]))
+  print "\t.rr = {%s}," % (",".join(["0x%08x" % (i) for i in params['rr']]))
+  print "\t.n0inv = 0x%08x\n};" % (params['n0inv'])
+
+def dump_blob(params):
+  mod_bin = params['mod'].tostring()
+  rr_bin = params['rr'].tostring()
+  n0inv_bin = array.array("I",[params['n0inv']]).tostring()
+  return mod_bin + rr_bin + n0inv_bin
+
+def extract_pubkey(pemfile, headerMode=True):
+  # Read the modulus in the .pem file
+  mod = pem_get_mod(sys.argv[1])
+  # Pre-compute the parameters used by the verification code
+  p = compute_mod_parameters(mod)
+
+  if headerMode:
+    # Generate a C header file with the parameters
+    print_header(p)
+  else:
+    # Generate the packed structure as a binary blob
+    return dump_blob(p)
+
+if __name__ == '__main__':
+  try:
+    if len(sys.argv) < 2:
+      raise PEMError('Invalid arguments. Usage: ./pem_extract_pubkey priv.pem')
+    extract_pubkey(sys.argv[1])
+  except KeyboardInterrupt:
+    sys.exit(0)
+  except PEMError as e:
+    sys.stderr.write("Error: %s\n" % (e.message))
+    sys.exit(1)