Added "pico_frame_grow"

tass-belgium · Jun 19, 2015 · 6e9283b · 6e9283b
1 parent 79b2848
commit 6e9283b
Show file tree

Hide file tree

Showing 3 changed files with 85 additions and 1 deletion.
diff --git a/include/pico_frame.h b/include/pico_frame.h
@@ -91,6 +91,7 @@ void pico_frame_discard(struct pico_frame *f);
 struct pico_frame *pico_frame_copy(struct pico_frame *f);
 struct pico_frame *pico_frame_deepcopy(struct pico_frame *f);
 struct pico_frame *pico_frame_alloc(uint32_t size);
+int pico_frame_grow(struct pico_frame *f, uint32_t size);
 struct pico_frame *pico_frame_alloc_skeleton(uint32_t size, int ext_buffer);
 int pico_frame_skeleton_set_buffer(struct pico_frame *f, void *buf);
 uint16_t pico_checksum(void *inbuf, uint32_t len);

diff --git a/stack/pico_frame.c b/stack/pico_frame.c
@@ -10,6 +10,8 @@
 
 #include "pico_config.h"
 #include "pico_frame.h"
+#include "pico_protocol.h"
+#include "pico_stack.h"
 
 #ifdef PICO_SUPPORT_DEBUG_MEMORY
 static int n_frames_allocated;
@@ -89,7 +91,7 @@ static struct pico_frame *pico_frame_do_alloc(uint32_t size, int zerocopy, int e
             return NULL;
         }
 
-        p->usage_count = (uint32_t *)(((uint8_t*)p->buffer) + size);
+        p->usage_count = (uint32_t *)(((uint8_t*)p->buffer) + frame_buffer_size);
     } else {
         p->buffer = NULL;
         p->flags = PICO_FRAME_FLAG_EXT_USAGE_COUNTER;
@@ -123,6 +125,42 @@ struct pico_frame *pico_frame_alloc(uint32_t size)
     return pico_frame_do_alloc(size, 0, 0);
 }
 
+int pico_frame_grow(struct pico_frame *f, uint32_t size)
+{
+    uint8_t *oldbuf;
+    uint32_t usage_count;
+    uint32_t frame_buffer_size;
+    uint32_t oldsize;
+    unsigned int align, oldalign;
+
+    if (!f || (size < f->buffer_len) || (f->flags & (PICO_FRAME_FLAG_EXT_BUFFER | PICO_FRAME_FLAG_EXT_USAGE_COUNTER)) ) {
+        return -1;
+    }
+    align = size % sizeof(uint32_t);
+    frame_buffer_size = size;
+    if (align) {
+        frame_buffer_size += (uint32_t)sizeof(uint32_t) - align;
+    }
+    oldbuf = f->buffer;
+    oldsize = f->buffer_len;
+    oldalign = oldsize % sizeof(uint32_t);
+    if (oldalign) {
+        oldsize += (uint32_t)sizeof(uint32_t) - oldalign;
+    }
+    usage_count = *((uint32_t *)(oldbuf + oldsize));
+    f->buffer = PICO_ZALLOC(frame_buffer_size + sizeof(uint32_t));
+    if (!f->buffer) {
+        f->buffer = oldbuf;
+        return -1;
+    }
+    f->usage_count = (uint32_t *)(((uint8_t*)f->buffer) + frame_buffer_size);
+    *f->usage_count = usage_count;
+    f->buffer_len = size;
+    memcpy(f->buffer, oldbuf, oldsize);
+    PICO_FREE(oldbuf);
+    return 0;
+}
+
 struct pico_frame *pico_frame_alloc_skeleton(uint32_t size, int ext_buffer)
 {
     return pico_frame_do_alloc(size, 1, ext_buffer);

diff --git a/test/unit/modunit_pico_frame.c b/test/unit/modunit_pico_frame.c
@@ -49,6 +49,48 @@ START_TEST(tc_pico_frame_alloc_discard)
 }
 END_TEST
 
+START_TEST(tc_pico_frame_grow)
+{
+    struct pico_frame *f = pico_frame_alloc(3);
+    fail_if(f->buffer_len != 3);
+    /* Ensure that the usage_count starts at byte 4, for good alignment */
+    fail_if(((void*)f->usage_count - (void *)f->buffer ) != 4);
+
+    ((uint8_t *)f->buffer)[0] = 'a';
+    ((uint8_t *)f->buffer)[1] = 'b';
+    ((uint8_t *)f->buffer)[2] = 'c';
+    *f->usage_count = 12; 
+
+
+    /* First, the failing cases. */
+    fail_if(pico_frame_grow(NULL, 30) == 0);
+    fail_if(pico_frame_grow(f, 2) == 0);
+    f->flags = PICO_FRAME_FLAG_EXT_BUFFER;
+    fail_if(pico_frame_grow(f, 21) == 0);
+    f->flags = PICO_FRAME_FLAG_EXT_USAGE_COUNTER;
+    fail_if(pico_frame_grow(f, 21) == 0);
+    f->flags = 0;
+
+    pico_set_mm_failure(1);
+    fail_if(pico_frame_grow(f, 21) == 0);
+
+    /* Now, the good one. */
+    fail_if(pico_frame_grow(f, 21) != 0);
+    fail_if(f->buffer_len != 21);
+    fail_if(((void *)f->usage_count - (void *)f->buffer) != 24);
+
+
+    fail_if(((uint8_t *)f->buffer)[0] != 'a');
+    fail_if(((uint8_t *)f->buffer)[1] != 'b');
+    fail_if(((uint8_t *)f->buffer)[2] != 'c');
+    fail_if(*f->usage_count != 12);
+
+    *f->usage_count = 1;
+    pico_frame_discard(f);
+
+}
+END_TEST
+
 START_TEST(tc_pico_frame_copy)
 {
     struct pico_frame *f = pico_frame_alloc(FRAME_SIZE);
@@ -156,16 +198,19 @@ Suite *pico_suite(void)
     Suite *s = suite_create("pico_frame.c");
     TCase *TCase_pico_frame_alloc_discard = tcase_create("Unit test for pico_frame_alloc_discard");
     TCase *TCase_pico_frame_copy = tcase_create("Unit test for pico_frame_copy");
+    TCase *TCase_pico_frame_grow = tcase_create("Unit test for pico_frame_grow");
     TCase *TCase_pico_frame_deepcopy = tcase_create("Unit test for pico_frame_deepcopy");
     TCase *TCase_pico_is_digit = tcase_create("Unit test for pico_is_digit");
     TCase *TCase_pico_is_hex = tcase_create("Unit test for pico_is_hex");
     tcase_add_test(TCase_pico_frame_alloc_discard, tc_pico_frame_alloc_discard);
     tcase_add_test(TCase_pico_frame_copy, tc_pico_frame_copy);
+    tcase_add_test(TCase_pico_frame_grow, tc_pico_frame_grow);
     tcase_add_test(TCase_pico_frame_deepcopy, tc_pico_frame_deepcopy);
     tcase_add_test(TCase_pico_is_digit, tc_pico_is_digit);
     tcase_add_test(TCase_pico_is_hex, tc_pico_is_hex);
     suite_add_tcase(s, TCase_pico_frame_alloc_discard);
     suite_add_tcase(s, TCase_pico_frame_copy);
+    suite_add_tcase(s, TCase_pico_frame_grow);
     suite_add_tcase(s, TCase_pico_frame_deepcopy);
     return s;
 }