1 files changed, 58 insertions, 0 deletions
diff --git a/src/kservice.c b/src/kservice.c
index 14fb19b225..0a96de77c8 100644
--- a/src/kservice.c
+++ b/src/kservice.c
@@ -1042,6 +1042,64 @@ void rt_kprintf(const char *fmt, ...)
 }
 #endif
 
+#ifdef RT_USING_HEAP
+/**
+ * This function allocates a memory block, which address is aligned to the
+ * specified alignment size.
+ *
+ * @param size the allocated memory block size
+ * @param align the alignment size
+ *
+ * @return the allocated memory block on successful, otherwise returns RT_NULL
+ */
+void* rt_malloc_align(rt_size_t size, rt_size_t align)
+{
+	void *align_ptr;
+	void *ptr;
+	rt_size_t align_size;
+
+	/* align the alignment size to 4 byte */
+	align = ((align + 0x03) & ~0x03);
+
+	/* get total aligned size */
+	align_size = ((size + 0x03) & ~0x03) + align;
+	/* allocate memory block from heap */
+	ptr = rt_malloc(align_size);
+	if (ptr != RT_NULL)
+	{
+		if (((rt_uint32_t)ptr & (align - 1)) == 0) /* the allocated memory block is aligned */
+		{
+			align_ptr = (void*) ((rt_uint32_t)ptr + align);
+		}
+		else
+		{
+			align_ptr = (void*) (((rt_uint32_t)ptr + (align - 1)) & ~(align - 1));
+		}
+
+		/* set the pointer before alignment pointer to the real pointer */
+		*((rt_uint32_t*)((rt_uint32_t)align_ptr - sizeof(void*))) = (rt_uint32_t)ptr;
+
+		ptr = align_ptr;
+	}
+
+	return ptr;
+}
+
+/**
+ * This function release the memory block, which is allocated by rt_malloc_align
+ * function and address is aligned.
+ *
+ * @param ptr the memory block pointer
+ */
+void rt_free_align(void* ptr)
+{
+	void* real_ptr;
+
+	real_ptr = (void*)*(rt_uint32_t*)((rt_uint32_t)ptr - sizeof(void*));
+	rt_free(real_ptr);
+}
+#endif
+
 #if !defined (RT_USING_NEWLIB) && defined (RT_USING_MINILIBC) && defined (__GNUC__)
 #include <sys/types.h>
 void *memcpy(void *dest, const void *src, size_t n) __attribute__((weak, alias("rt_memcpy")));