Add support for HeapReason_GetSkewAligned

Detail:
Similar to HeapReason_GetAligned, GetSkewAligned is used for allocating
aligned blocks (with optional boundary limit). However instead of using
the logical address of the user portion of the block for the alignment
calculation, it uses an arbitrary offset specified in R5. This makes
it useful for clients such as the PCI module, which care about the
physical alignment of blocks rather than logical alignment.

Admin:
Tested with heaptest

Dev/HeapTest/c/testbed

@@ -60,10 +60,8 @@ static void init(void)
 {
 	srand(seed);
 	printf("Seed %08x alloc size %08x\n",seed,allocsize);
-	/* Make heap 4K aligned */
-	heap = (uint32_t *) (((uint32_t) malloc(allocsize+4096)+4095)&0xfffff000);
-	/* Same for backup */
-	backup = (uint32_t *) (((uint32_t) malloc(allocsize+4096)+4095)&0xfffff000);
+	heap = (uint32_t *) malloc(allocsize);
+	backup = (uint32_t *) malloc(allocsize);
 	/* Used space map */
 	usedspace = (uint32_t *) malloc(((allocsize+31*4)>>7)<<2);
 	memset(usedspace,0,((allocsize+31*4)>>7)<<2);
@@ -287,7 +285,7 @@ int main(int argc,char **argv)
 
 	/* TODO - Take parameters from command line */
 	_swix(OS_ReadMonotonicTime,_OUT(0),&seed);
-	allocsize = 8*1024;
+	allocsize = 8*1024; /* 8K is sensible minimum, due to max alignment/boundary used of 2K */
 
 	init();
 
@@ -304,13 +302,13 @@ int main(int argc,char **argv)
 	usedspace[0] = 0xf;
 
 	/* Begin tests */
-	uint32_t temp,temp2,temp3,temp4;
+	uint32_t temp,temp2,temp3,temp4,temp5;
 	while(heapvalid(heap))
 	{
 		if(!opsleft)
 		{
 			opsleft = getrand(128);
-			switch(getrand(4))
+			switch(getrand(5))
 			{
 			case 0:
 				currentop = HeapReason_Get;
@@ -321,9 +319,12 @@ int main(int argc,char **argv)
 			case 2:
 				currentop = HeapReason_ExtendBlock;
 				break;
-			default:
+			case 3:
 				currentop = HeapReason_GetAligned;
 				break;
+			default:
+				currentop = HeapReason_GetSkewAligned;
+				break;
 			}
 		}
 		if(!(sequence&0xffff))
@@ -439,20 +440,23 @@ int main(int argc,char **argv)
 			}
 			break;
 		case HeapReason_GetAligned:
+		case HeapReason_GetSkewAligned:
 			if(numblocks == MAX_ALLOCS)
 			{
 				opsleft = 0;
 				break;
 			}
 			r.r[3] = temp = getrand(allocsize>>4)+1;
-			temp2 = 4<<getrand(9); /* Max 2K alignment (heap 4K aligned) */
+			temp2 = 4<<getrand(9); /* Max 2K alignment */
 			temp3 = temp2*(1<<getrand(5));
-			if(temp3 > 4096) /* Max 2K boundary (heap 4K aligned) */
+			if(temp3 > 4096) /* Max 2K boundary */
 				temp3 = 2048;
 			if(temp3 < temp)
 				temp3 = 0;
 			r.r[2] = temp2;
 			r.r[4] = temp3;
+			r.r[5] = 0;
+			temp5 = (currentop == HeapReason_GetAligned ? r.r[1] : r.r[5]);
 			last = r;
 			err = CallHeap(&r);
 			if(err)
@@ -471,12 +475,13 @@ int main(int argc,char **argv)
 					fprintf(stderr,"Failed to allocate requested block size: %08x bytes at alignment %08x boundary %08x at %08x\n",temp,temp2,temp3,temp4);
 					fail();
 				}
-				if(temp4 & (temp2-1))
+				temp5 = temp4+temp5;
+				if(temp5 & (temp2-1))
 				{
 					fprintf(stderr,"Block allocated at wrong alignment: %08x bytes at alignment %08x boundary %08x at %08x\n",temp,temp2,temp3,temp4);
 					fail();
 				}
-				if(temp3 && ((temp4 & ~(temp3-1)) != ((temp4+temp-1) & ~(temp3-1))))
+				if(temp3 && ((temp5 & ~(temp3-1)) != ((temp5+temp-1) & ~(temp3-1))))
 				{
 					fprintf(stderr,"Block crosses boundary: %08x bytes at alignment %08x boundary %08x at %08x\n",temp,temp2,temp3,temp4);
 					fail();

Dev/HeapTest/s/asm

@@ -50,12 +50,14 @@ HeapSavedReg_R1     # 4
 HeapSavedReg_R2     # 4
 HeapSavedReg_R3     # 4
 HeapSavedReg_R4     # 4
+HeapSavedReg_R5     # 4
 HeapSavedReg_R13    # 4
 HeapReturnedReg_R0  # 4
 HeapReturnedReg_R1  # 4
 HeapReturnedReg_R2  # 4
 HeapReturnedReg_R3  # 4
 HeapReturnedReg_R4  # 4
+HeapReturnedReg_R5  # 4
 HeapReturnedReg_R13 # 4
 HeapReturnedReg_PSR # 4
 ZeroPageSize        * @

hdr/KernelWS

@@ -1538,18 +1538,17 @@ HeapSavedReg_R1     # 4
 HeapSavedReg_R2     # 4
 HeapSavedReg_R3     # 4
 HeapSavedReg_R4     # 4
+HeapSavedReg_R5     # 4
 HeapSavedReg_R13    # 4
 HeapReturnedReg_R0  # 4
 HeapReturnedReg_R1  # 4
 HeapReturnedReg_R2  # 4
 HeapReturnedReg_R3  # 4
 HeapReturnedReg_R4  # 4
+HeapReturnedReg_R5  # 4
 HeapReturnedReg_R13 # 4
 HeapReturnedReg_PSR # 4                 ; also acts as interlock
 
-PrinterBufferAddr   #  4                ; holds address of printer buffer
-PrinterBufferSize   #  4                ; size of printer buffer - not to be confused with PrintBuffSize
-                                        ; which is the (constant) default size for the MOS's smallish buffer
 RawMachineID        #  8                ; 64 bits for unique machine ID
 KernelMessagesBlock #  20               ; 5 Words for messagetrans message block.
 ErrorSemaphore      #  1                ; Error semaphore to avoid looping on error translation.
@@ -1566,6 +1565,10 @@ PreVeneerRegDump    #  17*4             ; room for r0-r15, spsr
 CachedErrorBlocks   #  4                ; pointer to sysheap node holding the error block cache
  ]
 
+PrinterBufferAddr   #  4                ; holds address of printer buffer
+PrinterBufferSize   #  4                ; size of printer buffer - not to be confused with PrintBuffSize
+                                        ; which is the (constant) default size for the MOS's smallish buffer
+
  [ :DEF: ShowWS
  ! 0, "low space free ":CC::STR:(&FE8-@)
  ]

s/HeapMan

@@ -42,10 +42,11 @@ UsedSpaceDebugMask * &08000000
 
 Nil     *       0
 
-hpd     RN      r1      ; The punter sees these
-addr    RN      r2
-size    RN      r3
-work    RN      r4
+hpd      RN     r1      ; The punter sees these
+addr     RN     r2
+size     RN     r3
+work     RN     r4
+alignrel RN     r5
 
 HpTemp  RN      r10     ; But not these
 tp      RN      r11
@@ -236,7 +237,7 @@ SafeNaffExtension
 heapopdoneinbackground ROUT
         LDR        R12, =ZeroPage+HeapReturnedReg_R0
 
-        LDMIA     R12, {R0-R4, R10, R11}
+        LDMIA     R12, {R0-R5, R10, R11}
         MOV       stack, R10
         MOV       R10, #0
         STR       R10, [R12, #HeapReturnedReg_PSR-HeapReturnedReg_R0]
@@ -298,7 +299,7 @@ inspect_IRQ_stack
          ORR    R10, R10, #I32_bit:OR:SVC2632
          STR    R10, [R11, #4*6]               ; return into SVC26/32 mode with IRQs disabled
 
-         Push  "R0-R4, lr"
+         Push  "R0-R5, lr"
 
          LDR    R10, =ZeroPage+HeapSavedReg_R0
 
@@ -308,14 +309,14 @@ inspect_IRQ_stack
 ;         CMP    R12, #0
 ;         BNE    HeapInUse
 
-         LDMIA  R10, {R0-R4, R11}
+         LDMIA  R10, {R0-R5, R11}
          SWI    XOS_Heap                ; with interrupts off!
          LDR    R12, =ZeroPage+HeapReturnedReg_R0
 
    ; Could we poke these into the IRQ stack too...?
    ; would allow interruptible IRQ processes to do heap ops!!!
          MRS    lr, CPSR
-         STMIA  R12, {R0-R4, R11, lr}
+         STMIA  R12, {R0-R5, R11, lr}
 ; Any errors that were generated by the foreground operation may have ended up
 ; using one of MessageTrans' IRQ buffers. Trouble is, any number of IRQ errors
 ; could occur between now and when the foreground task gets the error. Avoid
@@ -331,12 +332,12 @@ heapbackgrounderrorloop
          BNE    heapbackgrounderrorloop
 
 noheapbackgrounderror
-         Pull  "R0-R4, lr"
+         Pull  "R0-R5, lr"
 
 iis_end                                 ; store the registers in the info block
         LDR     R12, =ZeroPage+HeapSavedReg_R0
-        STMIA   R12, {R0-R4}
-        STR     stack, [R12, #5*4]
+        STMIA   R12, {R0-R5}
+        STR     stack, [R12, #HeapSavedReg_R13-HeapSavedReg_R0]
 
 first_heap_address_to_trap              ; because register saveblock now set.
         LDR     R12, [R12, #HeapReturnedReg_PSR-HeapSavedReg_R0]
@@ -367,6 +368,8 @@ HeapJumpTable ; Check reason codes against Hdr:Heap defs
         B       ReadBlockSize
  assert ((.-HeapJumpTable) :SHR: 2) = HeapReason_GetAligned
         B       GetAreaAligned
+ assert ((.-HeapJumpTable) :SHR: 2) = HeapReason_GetSkewAligned
+        B       GetAreaAligned
  [ debheap
         B       ShowHeap
  ]
@@ -767,23 +770,26 @@ garfailed_zero * garfailed
 ; GetAreaAligned. Top level HeapEntry
 ; ==============
 ;
-; Allocate an aligned block of memory from the heap
+; Allocate an aligned block of memory from the heap.
 
 ; This is the same as GetArea, except it will only allocate areas with the given
 ; (power-of-two) alignment.
+; HeapReason_GetAligned aligns block user area using the logical address.
+; HeapReason_GetSkewAligned aligns the area using the value in R5 (alignrel).
 ; Fails if requesting size = 0
 
 ; In : hpd -> heap pointer
 ;      size = size of block required
 ;      addr = alignment (power of 2)
 ;      work = boundary (power of 2, 0 for none)
+;      alignrel = offset to use for alignment/boundary (GetSkewAligned)
 
 ; Out : VClear : addr -> got a block
 ;       VSet   : addr = 0, couldn't get block
 ;       Rest of universe ok
 
 GetAreaAligned ROUT
-        Push   "size,work"
+        Push    "r0,size,work,alignrel"
         ValidateHpd garafailed
 
  [ debheap
@@ -816,6 +822,14 @@ GetAreaAligned ROUT
         CMP     bp, #3
         MOVLT   bp, #3                          ; Minimum alignment is 4
 
+        ; Set up alignrel to be the offset for converting logical address to
+        ; alignment address
+        CMP     r0, #HeapReason_GetAligned
+        MOVEQ   alignrel, #0
+        SUBNE   alignrel, alignrel, hpd
+        TST     alignrel, #3                    ; Alignment offset must be multiple of 4
+        BNE     garafailed
+
         SUB     r0, work, #1                    ; Store boundary-1 in r0
         TST     r0, work
         BNE     garafailed_boundary             ; Must be power of 2!
@@ -840,8 +854,10 @@ garaloop
         ADD     work,addr,#4 ; 4 bytes for storing block size
         ADD     HpTemp,HpTemp,addr ; End of free block
         ADD     work,work,bp
+        ADD     work,work,alignrel
+        ADD     addr,addr,alignrel
 garaloop2
-        BIC     work,work,bp ; work = start of user block
+        BIC     work,work,bp ; work = start of user block (in alignment space)
         SUB     lr,work,addr
         CMP     lr,#4
         BEQ     garastartok ; Start alignment is exact
@@ -855,6 +871,8 @@ garaloop2
         B       garaloop2
 
 garastartok
+        SUB     work,work,alignrel
+        SUB     addr,addr,alignrel
 ; Calculate block end address
         ADD     lr,work,size ; End of user block
         SUBS    lr,HpTemp,lr ; Gap after user block
@@ -863,13 +881,17 @@ garastartok
 ; Check boundary requirement
         CMP     r0,#-1
         BEQ     garaboundaryok
+        ADD     work,work,alignrel
         AND     lr,work,r0 ; Start offset within boundary
+        SUB     work,work,alignrel
         ADD     lr,lr,size
         SUB     lr,lr,#1 ; Last byte of allocation
         CMP     lr,r0
         BLS     garaboundaryok
 
 ; This allocation crosses a boundary. Shift 'work' up to be boundary aligned.
+        ADD     work,work,alignrel
+        ADD     addr,addr,alignrel
         ADD     work,work,r0
         BIC     work,work,r0
         B       garaloop2 ; Loop back round to recheck everything (with small boundary sizes, we may have created a situation where we can't fit an initial free block)
@@ -938,10 +960,11 @@ garamore
  ]
         LDR     work, hpdbase
         ADD     work, work, hpd
+        ADD     work, work, alignrel
         ADD     tp, work, #4
         ADD     tp, tp, bp
 garamoreloop
-        BIC     tp, tp, bp            ; tp = pointer to return to user
+        BIC     tp, tp, bp            ; tp = pointer (in alignment space) to return to user
 
 ; Make sure there's enough space for a free block if necessary
         SUB     HpTemp, tp, work      ; HpTemp = tp-(hpd+hpdbase)
@@ -967,6 +990,8 @@ garamoreok
         B       garamoreloop
 
 garamoreboundaryok
+        SUB     tp, tp, alignrel
+        SUB     work, work, alignrel
         ADD     HpTemp, tp, size      ; New heap end
         SUB     HpTemp, HpTemp, hpd   ; New heap size
         LDR     lr, hpdend
@@ -998,16 +1023,14 @@ garamoreboundaryok
 ResultIsTpPlus4
 ; Block size is already stored
         ADD     addr, tp, #4
-        Pull    "size,work"
-        MOV     r0,#HeapReason_GetAligned
+        Pull    "r0,size,work,alignrel"
         CLRV
         B       GoodHeapExit
 
 ResultIsWorkPlus4
         STR     size, [work]           ; Store block size
         ADD     addr, work, #4         ; Move to correct return reg & add offset
-        Pull    "size,work"
-        MOV     r0,#HeapReason_GetAligned
+        Pull    "r0,size,work,alignrel"
         CLRV
         B       GoodHeapExit
 
@@ -1021,7 +1044,8 @@ garafailed
  ]
 garafail_common
         MOV     addr, #0                ; addr := 0 if we couldn't allocate
-        Pull    "size,work"             ; RESULTIS 0
+        ADD     sp, sp, #4              ; junk R0
+        Pull    "size,work,alignrel"    ; RESULTIS 0
         B       NaffHeapExit            ; VSet Exit
 
 garafailed_badhpd