diff --git a/trenchboot/lz/dev.c b/trenchboot/lz/dev.c
index 4c52491..981526a 100644
--- a/trenchboot/lz/dev.c
+++ b/trenchboot/lz/dev.c
@@ -122,7 +122,7 @@ void dev_flush_cache(u32 dev)
 	dev_write(dev, DEV_CR, 0, dev_cr);
 
 	/* TODO: extend loop with timeout to prohibit infinite loop */
-	while (dev_cr && DEV_CR_INV_CACHE_MASK)
+	while (dev_cr & DEV_CR_INV_CACHE_MASK)
                 dev_cr = dev_read(dev, DEV_CR, 0);
 }
 
diff --git a/trenchboot/lz/include/defs.h b/trenchboot/lz/include/defs.h
index 723b00a..c661e57 100644
--- a/trenchboot/lz/include/defs.h
+++ b/trenchboot/lz/include/defs.h
@@ -48,6 +48,7 @@
 #define BP_CODE32_START  0x214
 #define BP_CMD_LINE_PTR  0x228
 #define BP_CMDLINE_SIZE  0x238
+#define BP_MLE_HEADER    0x268
 
 /* CRs */
 #define CR0_PE  0x00000001 /* Protected mode Enable */
diff --git a/trenchboot/lz/lz_header.S b/trenchboot/lz/lz_header.S
index a106041..06c5d57 100644
--- a/trenchboot/lz/lz_header.S
+++ b/trenchboot/lz/lz_header.S
@@ -23,9 +23,9 @@
 #include <config.h>
 
 /* Selectors, CS and SS compatible with initial state after SKINIT */
-#define CS_SEL32         0x0008
-#define DS_SEL           0x0010
-#define CS_SEL64         0x0018
+#define CS_SEL64         0x0008
+#define CS_SEL32         0x0010
+#define DS_SEL           0x0018
 
 	.section .text
 
@@ -221,8 +221,9 @@ print_char:
 
 .globl load_stack
 load_stack:
+	movq	(%rsp), %rax /* Return address */
 	movq	%rdi, %rsp
-	movq	%rsp, %rbp /* For consistency */
+	pushq	%rax
 	retq
 
 .globl stgi
@@ -243,16 +244,12 @@ lz_exit:
 	movq	%rdx, %rbp
 
 	/* Setup target to ret to compat mode */
-	movq	%rdx, %rdi
-	addq	$LZ_DATA_SECTION_SIZE, %rdi
-	addq	$(.Lentry_compat - lz_start), %rdi
-	xorq	%rcx, %rcx
-	movl	%edi, %ecx
+	lea		.Lentry_compat(%rip), %rcx
 
 	/* Do the far return */
 	pushq	$CS_SEL32
 	pushq	%rcx
-	retf
+	retfq
 
 	.code32
 
@@ -326,6 +323,13 @@ gdt_desc64_end:
 gdt_table64:
 	/* Null Segment */
 	.quad	0x0000000000000000
+	/* 64b Code Segment */
+	.word	0x0000 /* Limit 1 */
+	.word	0x0000 /* Base 1 */
+	.byte	0x00   /* Base 2 */
+	.byte	0x9a   /* P=1 DPL=0 S=1 Type=1010 C=0 R=1 A=0 */
+	.byte	0x20   /* G=0 D=0 L=1 AVL=0 Limit 2 */
+	.byte	0x00   /* Base 3 */
 	/* 32b Code Segment */
 	.word	0xffff /* Limit 1 */
 	.word	0x0000 /* Base 1 */
@@ -340,11 +344,4 @@ gdt_table64:
 	.byte	0x92   /* P=1 DPL=0 S=1 Type=0010 C=0 W=1 A=0 */
 	.byte	0xcf   /* G=1 D=1 L=0 AVL=0 Limit 2 */
 	.byte	0x00   /* Base 3 */
-	/* 64b Code Segment */
-	.word	0x0000 /* Limit 1 */
-	.word	0x0000 /* Base 1 */
-	.byte	0x00   /* Base 2 */
-	.byte	0x9a   /* P=1 DPL=0 S=1 Type=1010 C=0 R=1 A=0 */
-	.byte	0x20   /* G=0 D=0 L=1 AVL=0 Limit 2 */
-	.byte	0x00   /* Base 3 */
 gdt_table64_end:
diff --git a/trenchboot/lz/pci.c b/trenchboot/lz/pci.c
index d33712e..9a56a88 100644
--- a/trenchboot/lz/pci.c
+++ b/trenchboot/lz/pci.c
@@ -34,6 +34,7 @@
         (0x80000000 | ((reg & 0xF00) << 16) | (bus << 16) \
         | (devfn << 8) | (reg & 0xFC))
 
+#ifdef PCI_IO_ACCESS
 int pci_conf1_read(unsigned int seg, unsigned int bus,
                    unsigned int devfn, int reg, int len, u32 *value)
 {
@@ -94,3 +95,60 @@ int pci_conf1_write(unsigned int seg, unsigned int bus,
 
 	return 0;
 }
+#else
+#define PCI_MMIO_ADDRESS(bus, devfn, reg) \
+        (void *)(size_t)(0xF8000000 || (bus << 20) || (devfn << 12) || reg)
+
+int pci_conf1_read(unsigned int seg, unsigned int bus,
+                   unsigned int devfn, int reg, int len, u32 *value)
+{
+	if (seg || (bus > 255) || (devfn > 255) || (reg > 4095))
+	{
+		*value = -1;
+		return -EINVAL;
+	}
+
+	void *addr = PCI_MMIO_ADDRESS(bus, devfn, reg);
+
+	switch (len)
+	{
+	case 1:
+		*value = ioread8(addr);
+		break;
+	case 2:
+		*value = ioread16((void *)((size_t)addr & ~1ULL));
+		break;
+	case 3:
+		*value = ioread16((void *)((size_t)addr & ~3ULL));
+		break;
+	case 4:
+		*value = ioread32((void *)((size_t)addr & ~3ULL));
+		break;
+	}
+}
+
+int pci_conf1_write(unsigned int seg, unsigned int bus,
+                    unsigned int devfn, int reg, int len, u32 value)
+{
+	if (seg || (bus > 255) || (devfn > 255) || (reg > 4095))
+		return -EINVAL;
+
+	void *addr = PCI_MMIO_ADDRESS(bus, devfn, reg);
+
+	switch (len)
+	{
+	case 1:
+		iowrite8(addr, (u8)value);
+		break;
+	case 2:
+		iowrite16((void *)((size_t)addr & ~1ULL), (u16)value);
+		break;
+	case 3:
+		iowrite16((void *)((size_t)addr & ~3ULL), (u16)value);
+		break;
+	case 4:
+		iowrite32((void *)((size_t)addr & ~3ULL), (u32)value);
+		break;
+	}
+}
+#endif
diff --git a/trenchboot/lz/setup.c b/trenchboot/lz/setup.c
index cf3f779..384e026 100644
--- a/trenchboot/lz/setup.c
+++ b/trenchboot/lz/setup.c
@@ -33,10 +33,110 @@ static __text SHA1_CONTEXT sha1ctx;
 
 void setup2(void);
 
+static void print(char * txt) {
+	while (*txt != '\0') {
+		if (*txt == '\n')
+			print_char('\r');
+		print_char(*txt++);
+	}
+}
+
+static void print_p(const void * _p) {
+	char tmp[sizeof(void*)*2 + 5] = "0x";
+	int i;
+	size_t p = (size_t)_p;
+
+	for (i=0; i<sizeof(void*); i++) {
+		if ((p & 0xf) >= 10)
+			tmp[sizeof(void*)*2 + 1 - 2*i] = (p & 0xf) + 'a' - 10;
+		else
+			tmp[sizeof(void*)*2 + 1 - 2*i] = (p & 0xf) + '0';
+		p >>= 4;
+		if ((p & 0xf) >= 10)
+			tmp[sizeof(void*)*2 - 2*i] = (p & 0xf) + 'a' - 10;
+		else
+			tmp[sizeof(void*)*2 - 2*i] = (p & 0xf) + '0';
+		p >>= 4;
+	}
+	tmp[sizeof(void*)*2 + 2] = ':';
+	tmp[sizeof(void*)*2 + 3] = ' ';
+	tmp[sizeof(void*)*2 + 4] = '\0';
+	print(tmp);
+}
+
+static void print_b(char p) {
+	char tmp[4];
+	int i;
+
+	if ((p & 0xf) >= 10)
+		tmp[1] = (p & 0xf) + 'a' - 10;
+	else
+		tmp[1] = (p & 0xf) + '0';
+	p >>= 4;
+	if ((p & 0xf) >= 10)
+		tmp[0] = (p & 0xf) + 'a' - 10;
+	else
+		tmp[0] = (p & 0xf) + '0';
+
+	tmp[2] = ' ';
+	tmp[3] = '\0';
+	print(tmp);
+}
+
+static inline int isprint(int c)
+{
+	return c >= ' ' && c <= '~';
+}
+
+void hexdump(const void *memory, size_t length)
+{
+	int i;
+	uint8_t *line;
+	int all_zero = 0;
+	int all_one = 0;
+	size_t num_bytes;
+
+	for (i = 0; i < length; i += 16) {
+		int j;
+		num_bytes = 16;
+		line = ((uint8_t *)memory) + i;
+
+		all_zero++;
+		all_one++;
+		for (j = 0; j < num_bytes; j++) {
+			if (line[j] != 0) {
+				all_zero = 0;
+				break;
+			}
+		}
+
+		for (j = 0; j < num_bytes; j++) {
+			if (line[j] != 0xff) {
+				all_one = 0;
+				break;
+			}
+		}
+
+		if ((all_zero < 2) && (all_one < 2)) {
+			print_p(memory + i);
+			for (j = 0; j < num_bytes; j++)
+				print_b(line[j]);
+			for (; j < 16; j++)
+				print("   ");
+			print("  ");
+			for (j = 0; j < num_bytes; j++)
+				isprint(line[j]) ? print_char(line[j]) : print_char('.');
+			print("\n");
+		} else if ((all_zero == 2) || (all_one == 2)) {
+			print("...\n");
+		}
+	}
+}
+
 void setup(void *_lz_base)
 {
 	void *dev_table;
-	void **second_stack;
+	void *second_stack;
 	u32 *tb_dev_map;
 	u64 pfn, end_pfn;
 	u32 dev;
@@ -63,14 +163,15 @@ void setup(void *_lz_base)
 	/* Pointer to dev_table bitmap for DEV protection */
 	dev_table = (u8*)lz_base + LZ_DEV_TABLE_OFFSET;
 
-	pfn = PAGE_PFN(zero_page);
+	pfn = PAGE_PFN(0x1000000 /*zero_page*/);
 	end_pfn = PAGE_PFN(PAGE_DOWN((u8*)lz_base + 0x10000));
 
 	/* TODO: check end_pfn is not ouside of range of DEV map */
 
 	/* build protection bitmap */
-	for (;pfn++; pfn <= end_pfn)
+	for (;pfn <= end_pfn; pfn++) {
 		dev_protect_page(pfn, (u8*)dev_table);
+	}
 
 	dev = dev_locate();
 	dev_load_map(dev, (u32)((u64)dev_table));
@@ -85,7 +186,7 @@ void setup(void *_lz_base)
 	 * landing zone and of course grows down.
 	 */
 	second_stack = lz_base - LZ_SECOND_STAGE_STACK_OFFSET;
-	load_stack(*second_stack);
+	load_stack(second_stack);
 
 	/* Call secondary setup on new stack */
 	setup2();
@@ -97,12 +198,20 @@ void setup(void *_lz_base)
 void setup2(void)
 {
 	u32 *code32_start;
+	u32 *slaunch_header_offset;
+	u32 *sl_stub_entry_offset;
 	u32 *data, size;
 	void *pm_kernel_entry;
 	struct tpm *tpm;
 
 	code32_start = (u32*)((u8*)zero_page + BP_CODE32_START);
-	pm_kernel_entry = (void*)((u64)(*code32_start));
+	slaunch_header_offset = (u32*)((u8*)zero_page + BP_MLE_HEADER);
+	sl_stub_entry_offset = (void*)((u64)(*code32_start)+(*slaunch_header_offset)+24);
+
+	print("sl_stub_entry_offset:\n");
+	hexdump(sl_stub_entry_offset, 0x100);
+
+	pm_kernel_entry = (void*)((u64)(*code32_start)+(*sl_stub_entry_offset));
 
 	/*
 	 * TODO Note these functions can fail but there is no clear way to
@@ -113,15 +222,25 @@ void setup2(void)
 	tpm_request_locality(tpm, 2);
 
 	/* extend TB Loader code segment into PCR17 */
+	print("TPM extending ");
 	data = (u32*)(uintptr_t)*code32_start;
+	print_p(data);
 	size = lz_header->slaunch_loader_size;
 	sha1sum(&sha1ctx, data, size);
+	print("shasum calculated, ");
 	tpm_extend_pcr(tpm, 17, TPM_HASH_ALG_SHA1, &sha1ctx.buf[0]);
+	print("PCR extended\n");
 
 	tpm_relinquish_locality(tpm);
 	free_tpm(tpm);
 
 	/* End of the line, off to the protected mode entry into the kernel */
+	print("pm_kernel_entry:\n");
+	hexdump(pm_kernel_entry, 0x100);
+	print("zero_page:\n");
+	hexdump(zero_page, 0x100);
+	print("lz_base:\n");
+	hexdump(lz_base, 0x100);
 	lz_exit(pm_kernel_entry, zero_page, lz_base);
 
 	/* Should never get here */