;*************************************************************************
;** mmu.library                                                         **
;**                                                                     **
;** a system library for arbitration and control of the MC68K MMUs      **
;**                                                                     **
;** © 1998 THOR-Software, Thomas Richter                                **
;** No commercial use, reassembly, modification without prior, written  **
;** permission of the authors.                                          **
;** Including this library in any commercial software REQUIRES a        **
;** written permission and the payment of a small fee.                  **
;**                                                                     **
;** This is an internal header file, do not depend on anything here.    **
;** Use the official include files.                                     **
;** Distributed only for the mmu.library development group for private  **
;** use.                                                                **
;**                                                                     **
;**---------------------------------------------------------------------**
;** Block: Service routines                                             **
;*************************************************************************

;FOLD Includes
        include mu_lib.i
        include INC:exec_lib.asm
        include INC:macros.asm
;ENDFOLD
;FOLD Defines
memf_clear      =       16
;ENDFOLD

        section main_code,code

;FOLD AllocAligned
;*************************************************
;** AllocAligned                                **
;** Allocate d0 bytes of type d1, aligned to a0 **
;** boundaries                                  **
;** return NULL on failure or the memory block  **
;*************************************************
        xdef AllocAligned
AllocAligned:
        saveregs d2-d4/a6

        move.l a0,d2            ;alignment present?
        beq .fail

        move.l mulib_SysBase(a6),a6
        subq.l #1,d2            ;reduce
        move.l d1,d4            ;keep type
        move.l d0,d3            ;saveback size
        bclr #memf_clear,d1     ;MEMF_CLEAR doesn't matter anyways
        add.l d2,d0             ;bytes + alignment - 1
        jsr AllocMem(a6)        ;get the memory
        tst.l d0
        beq.s .exit             ;on failure
        jsr Forbid(a6)          ;does not alter the registers
        move.l d0,a1            ;keep it
        move.l d3,d1            ;restore bytesize
        add.l d2,d0             ;address plus alignment
        add.l d2,d1
        not.l d2
        and.l d0,d2             ;align to next boundary
        move.l d1,d0
        jsr FreeMem(a6)         ;release it

        move.l d3,d0            ;bytesize
        move.l d2,a1            ;location
        jsr AllocAbs(a6)        ;allocate it

        jsr Permit(a6)          ;does not alter registers

        tst.l d0                ;worked?
        beq.s .fail             ;pass thru failure

        move.l d2,d0            ;location in d2
        btst #memf_clear,d4     ;clear it ?
        beq.s .exit
                                ;here: Yes, please
        move.l d2,a0
        moveq #0,d1             ;Clear
        sub.l a1,a1
        moveq #0,d2
        sub.l a6,a6
        adda.l d3,a0            ;Poiner BEHIND the memory

        lsr.l #1,d3
        bcc.s .nobyte
        move.b d1,-(a0)         ;One byte at the end?
.nobyte:
        lsr.l #1,d3
        bcc.s .noword
        move.w d1,-(a0)
.noword:
        lsr.l #1,d3
        bcc.s .nolong
        move.l d1,-(a0)
.nolong:
        lsr.l #1,d3
        bcc.s .noquad
        movem.l d1/d2,-(a0)
.noquad:
        lsr.l #1,d3
        bcc.s .no2quad
        movem.l d1/d2/a1/a6,-(a0)
.no2quad:
        do
         subq.l #1,d3
         bcs.s .exit
         movem.l d1/d2/a1/a6,-(a0)
         movem.l d1/d2/a1/a6,-(a0)
        loop.s
.fail:
        moveq #0,d0
.exit:
        loadregs
        rts
;ENDFOLD