Fix gc_realloc to expand in place

* Issue #322

py/gc.c

@@ -326,20 +326,72 @@ machine_uint_t gc_nbytes(void *ptr_in) {
     return 0;
 }
 
-void *gc_realloc(void *ptr, machine_uint_t n_bytes) {
-    machine_uint_t n_existing = gc_nbytes(ptr);
-    if (n_bytes <= n_existing) {
-        return ptr;
-    } else {
-        // TODO check if we can grow inplace
-        void *ptr2 = gc_alloc(n_bytes);
-        if (ptr2 == NULL) {
-            return ptr2;
+void *gc_realloc(void *ptr_in, machine_uint_t n_bytes) {
+    void *ptr_out = NULL;
+    machine_uint_t block =0;
+    machine_uint_t ptr = (machine_uint_t)ptr_in;
+
+    if (ptr_in == NULL) {
+        return gc_alloc(n_bytes);
+    }
+
+    if (VERIFY_PTR(ptr)                         /* verify pointer */
+        && (block = BLOCK_FROM_PTR(ptr))        /* get first block */
+        && ATB_GET_KIND(block) == AT_HEAD) {    /* make sure it's a HEAD block */
+
+        byte block_type;
+        machine_uint_t n_free   = 0;
+        machine_uint_t n_blocks = 1; /* counting HEAD block */
+
+        /* get the number of consecutive tail blocks and
+           the number of free blocks after last tail block */
+        do  {
+            block_type = ATB_GET_KIND(block + n_blocks + n_free);
+            switch (block_type) {
+                case AT_FREE: n_free++; break;
+                case AT_TAIL: n_blocks++; break;
+                default: break;
+            }
+        /* stop as soon as we find enough blocks for n_bytes */
+        } while (block_type != AT_HEAD && (n_bytes > (n_blocks * BYTES_PER_BLOCK)));
+
+        /* number of allocated bytes */
+        machine_uint_t n_existing = n_blocks * BYTES_PER_BLOCK;
+
+        /* check if realloc'ing to a smaller size */
+        if (n_bytes <= n_existing) {
+            ptr_out = ptr_in;
+            /* TODO should free remaining tail blocks ? */
+            machine_uint_t last_block = block + n_blocks;
+            while (ATB_GET_KIND(last_block) == AT_TAIL) {
+                ATB_ANY_TO_FREE(last_block);
+                  last_block++;
+            }
+
+        /* check if we can expand in place */
+        } else if (n_bytes <= (n_existing + (n_free * BYTES_PER_BLOCK))) {
+            /* number of blocks needed to expand +1 if there's a remainder */
+            machine_uint_t n_diff = ( n_bytes - n_existing)/BYTES_PER_BLOCK+
+                                    ((n_bytes - n_existing)%BYTES_PER_BLOCK!=0);
+
+            DEBUG_printf("gc_realloc: expanding " UINT_FMT " blocks (" UINT_FMT " bytes) to " UINT_FMT " blocks (" UINT_FMT " bytes)\n",
+                    n_existing/BYTES_PER_BLOCK, n_existing, n_existing/BYTES_PER_BLOCK+n_diff, n_existing + n_diff*BYTES_PER_BLOCK);
+
+            /* mark rest of blocks as used tail */
+            for (machine_uint_t bl = block+n_blocks; bl < (block+n_blocks+n_diff); bl++) {
+                ATB_FREE_TO_TAIL(bl);
+            }
+            ptr_out = ptr_in;
+
+        /* try to find a new contiguous chain */
+        } else if ((ptr_out = gc_alloc(n_bytes)) != NULL) {
+            printf("gc_realloc: allocated new block \n");
+            memcpy(ptr_out, ptr_in, n_existing);
+            gc_free(ptr_in);
         }
-        memcpy(ptr2, ptr, n_existing);
-        gc_free(ptr);
-        return ptr2;
     }
+
+    return ptr_out;
 }
 
 void gc_dump_info() {