arch/m68k-amiga/boot/early.c

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