diff --git a/src/queue.c b/src/queue.c
index 050b6ed..da0d515 100644
--- a/src/queue.c
+++ b/src/queue.c
@@ -10,6 +10,30 @@
  */
 #include "queue.h"
 
+static struct _queue_node * queue_node_new(void* obj, uint32_t obj_size)
+{
+    void * obj_new = malloc(obj_size);
+    if (obj_new == NULL)
+    {
+        goto done;
+    }
+    memmove(obj_new, obj, obj_size);
+
+    struct _queue_node* node_new = (struct _queue_node*)malloc(sizeof(struct _queue_node));
+    if(node_new == NULL)
+    {
+        goto done1;
+    }
+    node_new->obj = obj_new;
+    node_new->next = NULL;
+
+    return node_new;
+done1:
+    free(obj_new);
+done:
+    return NULL;
+}
+
 bool queue_push(struct _queue* self, void* obj)
 {
     assert(self != NULL);
@@ -18,14 +42,14 @@ bool queue_push(struct _queue* self, void* obj)
     void * obj_new = malloc(self->_obj_size);
     if (obj_new == NULL)
     {
-        return false;
+        goto done;
     }
     memmove(obj_new, obj, self->_obj_size);
 
     struct _queue_node* node_new = (struct _queue_node*)malloc(sizeof(struct _queue_node));
     if(node_new == NULL)
     {
-        return false;
+        goto done1;
     }
     node_new->obj = obj_new;
     node_new->next = NULL;
@@ -41,7 +65,12 @@ bool queue_push(struct _queue* self, void* obj)
         self->_back = node_new;
     }
     self->_size++;
+
     return true;
+done1:
+    free(obj_new);
+done:
+    return false;
 }
 
 bool queue_pop(struct _queue* self, void* obj)
diff --git a/test/test_queue.c b/test/test_queue.c
index d8990e8..74440aa 100644
--- a/test/test_queue.c
+++ b/test/test_queue.c
@@ -40,10 +40,9 @@ static void test_queue_init(void)
     TEST_ASSERT_FALSE(queue_init2(queue, 0, 1));
     TEST_ASSERT_FALSE(queue_init2(queue, sizeof(int), 0));
     queue_free(&queue);
-
-    queue_init2(queue, sizeof(int), 1);
 }
 
+
 static void test_queue_push(void)
 {
     int temp = 0;
@@ -59,13 +58,55 @@ static void test_queue_push(void)
     for(i = 0; i < len; i++)
     {
         TEST_ASSERT_TRUE(queue->push(queue, &data[i]));
+        TEST_ASSERT_EQUAL_INT(i + 1, queue->size(queue));
+
+        TEST_ASSERT_TRUE(queue->front(queue, &temp));
+        TEST_ASSERT_EQUAL_INT(data[0], temp);
+
+        TEST_ASSERT_TRUE(queue->back(queue, &temp));
+        TEST_ASSERT_EQUAL_INT(data[i], temp);
     }
     queue_free(&queue);
 
     // ------------------------------
     queue = queue_new();
-    queue_init2(queue, sizeof(int), 10);
+    queue_init2(queue, sizeof(int), len);
+    for(i = 0; i < len; i++)
+    {
+        TEST_ASSERT_TRUE(queue->push(queue, &data[i]));
+        TEST_ASSERT_EQUAL_INT(i + 1, queue->size(queue));
 
+        TEST_ASSERT_TRUE(queue->front(queue, &temp));
+        TEST_ASSERT_EQUAL_INT(data[0], temp);
+
+        TEST_ASSERT_TRUE(queue->back(queue, &temp));
+        TEST_ASSERT_EQUAL_INT(data[i], temp);
+    }
+    queue_free(&queue);
+
+    // ------------------------------
+    // if capacity is less than data len
+    uint32_t capacity = len - 1;
+    queue = queue_new();
+    queue_init2(queue, sizeof(int), capacity);
+    for(i = 0; i < len; i++)
+    {
+        if(i == capacity - 1)
+        {
+            TEST_ASSERT_TRUE(queue->push(queue, &data[i]));
+            TEST_ASSERT_EQUAL_INT(i + 1, queue->size(queue));
+
+            TEST_ASSERT_TRUE(queue->front(queue, &temp));
+            TEST_ASSERT_EQUAL_INT(data[0], temp);
+
+            TEST_ASSERT_TRUE(queue->back(queue, &temp));
+            TEST_ASSERT_EQUAL_INT(data[i], temp);
+        }
+        else
+        {
+            
+        }
+    }
     queue_free(&queue);
 }
 
@@ -313,6 +354,7 @@ void test_queue(void)
     // TEST_MESSAGE("----- test_queue -----");
     RUN_TEST(test_queue_new);
     RUN_TEST(test_queue_init);
+    RUN_TEST(test_queue_push);
 
     RUN_TEST(test_queue_num);
     RUN_TEST(test_queue_struct);