1 files changed, 24 insertions, 8 deletions

diff --git a/src/memory.cpp b/src/memory.cpp
index 793c4d0..538ffd8 100644
--- a/src/memory.cpp
+++ b/src/memory.cpp
@@ -1,6 +1,9 @@
 
 #include "memory.hpp"
 
+const std::size_t MINIMUM_ALIGNMENT = 1;
+const std::size_t MAXIMUM_ALIGNMENT = 64;
+
 struct MemBlock {
 	char *data;
 	std::size_t size;
@@ -52,12 +55,16 @@ void DestroyBucket(MemBucket *bkt) {
 	bkt->transient = false;
 }
 
-void *Pke_New(std::size_t sz, MemBucket *bkt) {
+void *Pke_New(std::size_t sz, std::size_t alignment, MemBucket *bkt) {
 	MemBlock *blocks = reinterpret_cast<MemBlock *>(bkt->blocks);
+	std::size_t calculatedAlignment = alignment < MINIMUM_ALIGNMENT ? MINIMUM_ALIGNMENT : alignment;
 	int64_t availBlockIndex = -1;
 	void *data = nullptr;
 	for (int64_t i = 0; i <= bkt->lastEmptyBlockIndex; ++i) {
 		auto blk = blocks[i];
+		std::size_t misalignment = reinterpret_cast<std::size_t>(blk.data) % calculatedAlignment;
+		if (misalignment != 0)
+			continue;
 		if (blk.size > sz) {
 			availBlockIndex = i;
 			break;
@@ -77,24 +84,31 @@ void *Pke_New(std::size_t sz, MemBucket *bkt) {
 		}
 	} else {
 		assert(bkt->head + sz <= bkt->size && "memory bucket specified, but full");
-		data = bkt->ptr + bkt->head;
-		bkt->head += sz;
+		std::size_t misalignment = reinterpret_cast<std::size_t>(bkt->ptr + bkt->head) % calculatedAlignment;
+		std::size_t padding = calculatedAlignment - misalignment;
+		if (misalignment != 0) {
+			bkt->lastEmptyBlockIndex += 1;
+			blocks[bkt->lastEmptyBlockIndex].data = bkt->ptr + bkt->head;
+			blocks[bkt->lastEmptyBlockIndex].size = padding;
+		}
+		data = bkt->ptr + bkt->head + padding;
+		bkt->head += sz + padding;
 	}
 	bkt->allocs++;
 	return data;
 }
 
-void *Pke_New(std::size_t sz) {
+void *Pke_New(std::size_t sz, std::size_t alignment) {
 	MemBucket *bkt = nullptr;
 	for (long i = 0; i < bucketHead; ++i) {
-		if (buckets[i].transient == false && buckets[i].size - buckets[i].head > sz) {
+		if (buckets[i].transient == false && buckets[i].size - buckets[i].head > sz + MAXIMUM_ALIGNMENT) {
 			bkt = &buckets[i];
 		}
 	}
 	if (bkt == nullptr) {
 		bkt = &buckets[InitNewBucket(DEFAULT_BUCKET_SIZE)];
 	}
-	return Pke_New(sz, bkt);
+	return Pke_New(sz, alignment, bkt);
 }
 
 void Pke_Delete(const void *ptr, std::size_t sz, MemBucket *bkt) {
@@ -170,9 +184,11 @@ void Pke_MemoryFlush() {
 	}
 }
 
-uint64_t Buckets_NewHandle(std::size_t bucketBytes, uint64_t bucketItemCount, uint64_t &bucketIncrementer, uint64_t &bucketCounter, uint64_t &itemCounter, void*& buckets) {
+uint64_t Buckets_NewHandle(std::size_t bucketBytes, std::size_t alignment, uint64_t bucketItemCount, uint64_t &bucketIncrementer, uint64_t &bucketCounter, uint64_t &itemCounter, void*& buckets) {
 	uint64_t newHandle{itemCounter | bucketCounter};
 
+	std::size_t calculatedAlignment = alignment < MINIMUM_ALIGNMENT ? MINIMUM_ALIGNMENT : alignment;
+
 	itemCounter += uint64_t{1ULL << 32};
 	if (itemCounter > uint64_t{(bucketItemCount - 1) << 32}) {
 		itemCounter = 0ULL;
@@ -180,7 +196,7 @@ uint64_t Buckets_NewHandle(std::size_t bucketBytes, uint64_t bucketItemCount, ui
 	}
 	if (bucketCounter > bucketIncrementer) {
 		int64_t newIncrement = bucketIncrementer * 1.5;
-		char * newBuckets = reinterpret_cast<char *>(Pke_New(bucketBytes * newIncrement));
+		char * newBuckets = reinterpret_cast<char *>(Pke_New(bucketBytes * newIncrement, calculatedAlignment));
 		std::memcpy(newBuckets, buckets, bucketBytes * bucketIncrementer);
 		Pke_Delete(buckets, bucketBytes * bucketIncrementer);
 		buckets = newBuckets;