#endif
 }
 
-void main(void)
+static void init_heap(void)
 {
-       /* First, copy the boot header into the "zeropage" */
-       copy_boot_params();
+       char *stack_end;
 
-       /* End of heap check */
        if (boot_params.hdr.loadflags & CAN_USE_HEAP) {
-               heap_end = (char *)(boot_params.hdr.heap_end_ptr
-                                   +0x200-STACK_SIZE);
+               asm("leal %P1(%%esp),%0"
+                   : "=r" (stack_end) : "i" (-STACK_SIZE));
+
+               heap_end = (char *)
+                       ((size_t)boot_params.hdr.heap_end_ptr + 0x200);
+               if (heap_end > stack_end)
+                       heap_end = stack_end;
        } else {
                /* Boot protocol 2.00 only, no heap available */
                puts("WARNING: Ancient bootloader, some functionality "
                     "may be limited!\n");
        }
+}
+
+void main(void)
+{
+       /* First, copy the boot header into the "zeropage" */
+       copy_boot_params();
+
+       /* End of heap check */
+       init_heap();
 
        /* Make sure we have all the proper CPU support */
        if (validate_cpu()) {