arch/m68k-amiga/boot/early.c

   1 /*
   2  * Copyright (C) 2011-2017, The AROS Development Team.  All rights reserved.
   3  * Author: Jason S. McMullan <jason.mcmullan@gmail.com>
   4  *
   5  * Licensed under the AROS PUBLIC LICENSE (APL) Version 1.1
   6  */
   7
   8 #define DEBUG 0
   9
  10 #include <proto/exec.h>
  11 #include <exec/memory.h>
  12
  13 #include "amiga_hwreg.h"
  14
  15 #include "early.h"
  16 #include "debug.h"
  17
  18 void Early_ScreenCode(ULONG code)
  19 {
  20         reg_w(BPLCON0, 0x0200);
  21         reg_w(BPL1DAT, 0x0000);
  22         reg_w(COLOR00, code & RGB_MASK);
  23 }
  24
  25 void Early_Alert(ULONG alert)
  26 {
  27     const int bright = ((alert >> 4) & 1) ? 0xf : 0x7;
  28     const int color =
  29                 RGB(((alert >> 2) & 1) * bright,
  30                     ((alert >> 1) & 1) * bright,
  31                     ((alert >> 0) & 1) * bright);
  32
  33     DEBUGPUTHEX(("Early_Alert", alert));
  34
  35     for (;;) {
  36         volatile int i;
  37         Early_ScreenCode(color);
  38         for (i = 0; i < 100000; i++);
  39         Early_ScreenCode(0x000);
  40         for (i = 0; i < 100000; i++);
  41
  42         if (!(alert & AT_DeadEnd))
  43             break;
  44     }
  45 }
  46
  47 /* Fatal trap for early problems */
  48 extern void Exec_MagicResetCode(void);
  49 void __attribute__((interrupt)) Early_TrapHandler(void)
  50 {
  51     volatile int i;
  52     Early_ScreenCode(CODE_TRAP_FAIL);
  53
  54     /* If we have a valid SysBase, then
  55      * we can run the debugger.
  56      */
  57     if (SysBase != NULL)
  58         Debug(0);
  59     else
  60         Early_Alert(AT_DeadEnd | 1);
  61
  62     /* Sleep for a while */
  63     for (i = 0; i < 100000; i++);
  64
  65     /* Reset everything but the CPU, then restart
  66      * at the ROM exception vector
  67      */
  68     Exec_MagicResetCode();
  69 }
  70
  71 APTR Early_AllocAbs(struct MemHeader *mh, APTR location, IPTR byteSize)
  72 {
  73     APTR ret = NULL;
  74     APTR endlocation = location + byteSize;
  75     struct MemChunk *p1, *p2, *p3, *p4;
  76
  77     if (mh->mh_Lower > location || mh->mh_Upper < endlocation)
  78         return (APTR)1;
  79
  80     /* Align size to the requirements */
  81     byteSize += (IPTR)location&(MEMCHUNK_TOTAL - 1);
  82     byteSize  = (byteSize + MEMCHUNK_TOTAL-1) & ~(MEMCHUNK_TOTAL-1);
  83
  84     /* Align the location as well */
  85     location=(APTR)((IPTR)location & ~(MEMCHUNK_TOTAL-1));
  86
  87     /* Start and end(+1) of the block */
  88     p3=(struct MemChunk *)location;
  89     p4=(struct MemChunk *)((UBYTE *)p3+byteSize);
  90
  91     /*
  92         The free memory list is only single linked, i.e. to remove
  93         elements from the list I need the node's predessor. For the
  94         first element I can use freeList->mh_First instead of a real
  95         predecessor.
  96     */
  97     p1 = (struct MemChunk *)&mh->mh_First;
  98     p2 = p1->mc_Next;
  99
 100     /* Follow the list to find a chunk with our memory. */
 101     while (p2 != NULL)
 102     {
 103         /* Found a chunk that fits? */
 104         if((UBYTE *)p2+p2->mc_Bytes>=(UBYTE *)p4&&p2<=p3)
 105         {
 106             /* Check if there's memory left at the end. */
 107             if((UBYTE *)p2+p2->mc_Bytes!=(UBYTE *)p4)
 108             {
 109                 /* Yes. Add it to the list */
 110                 p4->mc_Next  = p2->mc_Next;
 111                 p4->mc_Bytes = (UBYTE *)p2+p2->mc_Bytes-(UBYTE *)p4;
 112                 p2->mc_Next  = p4;
 113             }
 114
 115             /* Check if there's memory left at the start. */
 116             if(p2!=p3)
 117                 /* Yes. Adjust the size */
 118                 p2->mc_Bytes=(UBYTE *)p3-(UBYTE *)p2;
 119             else
 120                 /* No. Skip the old chunk */
 121                 p1->mc_Next=p2->mc_Next;
 122
 123             /* Adjust free memory count */
 124             mh->mh_Free-=byteSize;
 125
 126             /* Return the memory */
 127             ret = p3;
 128             break;
 129         }
 130         /* goto next chunk */
 131
 132         p1=p2;
 133         p2=p2->mc_Next;
 134     }
 135
 136     return ret;
 137 }