Diff: stack/pico_frame.c

Revision:

152:a3d286bf94e5

Parent:

149:5f4cb161cec3

Child:

154:6c0e92a80c4a

--- a/stack/pico_frame.c	Wed Apr 09 17:32:25 2014 +0200
+++ b/stack/pico_frame.c	Mon Sep 28 13:16:18 2015 +0200
@@ -1,5 +1,5 @@
 /*********************************************************************
-   PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+   PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
    See LICENSE and COPYING for usage.
 
    .
@@ -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;
@@ -23,12 +25,18 @@
 
     (*f->usage_count)--;
     if (*f->usage_count <= 0) {
-        PICO_FREE(f->usage_count);
+        if (f->flags & PICO_FRAME_FLAG_EXT_USAGE_COUNTER)
+            PICO_FREE(f->usage_count);
+
 #ifdef PICO_SUPPORT_DEBUG_MEMORY
         dbg("Discarded buffer @%p, caller: %p\n", f->buffer, __builtin_return_address(3));
         dbg("DEBUG MEMORY: %d frames in use.\n", --n_frames_allocated);
 #endif
-        PICO_FREE(f->buffer);
+        if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER))
+            PICO_FREE(f->buffer);
+        else if (f->notify_free)
+            f->notify_free(f->buffer);
+
         if (f->info)
             PICO_FREE(f->info);
     }
@@ -57,38 +65,54 @@
 }
 
 
-static struct pico_frame *pico_frame_do_alloc(uint32_t size, int zerocopy)
+static struct pico_frame *pico_frame_do_alloc(uint32_t size, int zerocopy, int ext_buffer)
 {
     struct pico_frame *p = PICO_ZALLOC(sizeof(struct pico_frame));
+    uint32_t frame_buffer_size = size;
     if (!p)
         return NULL;
 
+    if (ext_buffer && !zerocopy) {
+        /* external buffer implies zerocopy flag! */
+        PICO_FREE(p);
+        return NULL;
+    }
+
     if (!zerocopy) {
-        p->buffer = PICO_ZALLOC(size);
+        unsigned int align = size % sizeof(uint32_t);
+        /* Ensure that usage_count starts on an aligned address */
+        if (align) {
+            frame_buffer_size += (uint32_t)sizeof(uint32_t) - align;
+        }
+
+        p->buffer = PICO_ZALLOC(frame_buffer_size + sizeof(uint32_t));
         if (!p->buffer) {
             PICO_FREE(p);
             return NULL;
         }
+
+        p->usage_count = (uint32_t *)(((uint8_t*)p->buffer) + frame_buffer_size);
     } else {
         p->buffer = NULL;
+        p->flags |= PICO_FRAME_FLAG_EXT_USAGE_COUNTER;
+        p->usage_count = PICO_ZALLOC(sizeof(uint32_t));
+        if (!p->usage_count) {
+            PICO_FREE(p);
+            return NULL;
+        }
     }
 
-    p->usage_count = PICO_ZALLOC(sizeof(uint32_t));
-    if (!p->usage_count) {
-        if (p->buffer)
-            PICO_FREE(p->buffer);
-
-        PICO_FREE(p);
-        return NULL;
-    }
 
     p->buffer_len = size;
 
-
     /* By default, frame content is the full buffer. */
     p->start = p->buffer;
     p->len = p->buffer_len;
     *p->usage_count = 1;
+
+    if (ext_buffer)
+        p->flags |= PICO_FRAME_FLAG_EXT_BUFFER;
+
 #ifdef PICO_SUPPORT_DEBUG_MEMORY
     dbg("Allocated buffer @%p, len= %d caller: %p\n", p->buffer, p->buffer_len, __builtin_return_address(2));
     dbg("DEBUG MEMORY: %d frames in use.\n", ++n_frames_allocated);
@@ -98,12 +122,68 @@
 
 struct pico_frame *pico_frame_alloc(uint32_t size)
 {
-    return pico_frame_do_alloc(size, 0);
+    return pico_frame_do_alloc(size, 0, 0);
 }
 
-struct pico_frame *pico_frame_alloc_skeleton(uint32_t size)
+int pico_frame_grow(struct pico_frame *f, uint32_t size)
 {
-    return pico_frame_do_alloc(size, 1);
+    uint8_t *oldbuf;
+    uint32_t usage_count, *p_old_usage;
+    uint32_t frame_buffer_size;
+    uint32_t oldsize;
+    unsigned int align;
+    int addr_diff = 0;
+
+    if (!f || (size < f->buffer_len)) {
+        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;
+    usage_count = *(f->usage_count);
+    p_old_usage = f->usage_count;
+    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);
+
+    /* Update hdr fields to new buffer*/
+    addr_diff = (int)(f->buffer - oldbuf);
+    f->net_hdr += addr_diff;
+    f->datalink_hdr += addr_diff;
+    f->transport_hdr += addr_diff;
+    f->app_hdr += addr_diff;
+    f->start += addr_diff;
+    f->payload += addr_diff;
+
+    if (f->flags & PICO_FRAME_FLAG_EXT_USAGE_COUNTER)
+        PICO_FREE(p_old_usage);
+
+    if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER))
+        PICO_FREE(oldbuf);
+    else if (f->notify_free)
+        f->notify_free(oldbuf);
+
+    f->flags = 0;
+    /* Now, the frame is not zerocopy anymore, and the usage counter has been moved within it */
+    return 0;
+}
+
+struct pico_frame *pico_frame_alloc_skeleton(uint32_t size, int ext_buffer)
+{
+    return pico_frame_do_alloc(size, 1, ext_buffer);
 }
 
 int pico_frame_skeleton_set_buffer(struct pico_frame *f, void *buf)
@@ -138,6 +218,7 @@
 
     /* Update in-buffer pointers with offset */
     addr_diff = (int)(new->buffer - f->buffer);
+    new->datalink_hdr += addr_diff;
     new->net_hdr += addr_diff;
     new->transport_hdr += addr_diff;
     new->app_hdr += addr_diff;
@@ -151,51 +232,55 @@
     return new;
 }
 
+
+static inline uint32_t pico_checksum_adder(uint32_t sum, void *data, uint32_t len)
+{
+    uint16_t *buf = (uint16_t *)data;
+    uint16_t *stop;
+
+    if (len & 0x01) {
+        --len;
+#ifdef PICO_BIGENDIAN
+        sum += (((uint8_t *)data)[len]) << 8;
+#else
+        sum += ((uint8_t *)data)[len];
+#endif
+    }
+
+    stop = (uint16_t *)(((uint8_t *)data) + len);
+
+    while (buf < stop) {
+        sum += *buf++;
+    }
+    return sum;
+}
+
+static inline uint16_t pico_checksum_finalize(uint32_t sum)
+{
+    while (sum >> 16) { /* a second carry is possible! */
+        sum = (sum & 0x0000FFFF) + (sum >> 16);
+    }
+    return short_be((uint16_t) ~sum);
+}
+
 /**
  * Calculate checksum of a given string
  */
 uint16_t pico_checksum(void *inbuf, uint32_t len)
 {
-    uint8_t *buf = (uint8_t *) inbuf;
-    uint32_t tmp = 0;
-    uint32_t sum = 0;
-    uint32_t i = 0;
+    uint32_t sum;
 
-    for(i = 0; i < len; i += 2u) {
-        tmp = buf[i];
-        sum += (tmp << 8lu);
-        if (len > (i + 1))
-            sum += buf[i + 1];
-    }
-    while (sum >> 16) { /* a second carry is possible! */
-        sum = (sum & 0x0000FFFF) + (sum >> 16);
-    }
-    return (uint16_t) (~sum);
+    sum = pico_checksum_adder(0, inbuf, len);
+    return pico_checksum_finalize(sum);
 }
 
+/* WARNING: len1 MUST be an EVEN number */
 uint16_t pico_dualbuffer_checksum(void *inbuf1, uint32_t len1, void *inbuf2, uint32_t len2)
 {
-    uint8_t *b1 = (uint8_t *) inbuf1;
-    uint8_t *b2 = (uint8_t *) inbuf2;
-    uint32_t tmp = 0;
-    uint32_t sum = 0;
-    uint32_t i = 0;
+    uint32_t sum;
 
-    for(i = 0; i < len1; i += 2u) {
-        tmp = b1[i];
-        sum += (tmp << 8lu);
-        if (len1 > (i + 1))
-            sum += b1[i + 1];
-    }
-    for(i = 0; i < len2; i += 2u) {
-        tmp = b2[i];
-        sum += (tmp << 8lu);
-        if (len2 > (i + 1))
-            sum += b2[i + 1];
-    }
-    while (sum >> 16) { /* a second carry is possible! */
-        sum = (sum & 0x0000FFFF) + (sum >> 16);
-    }
-    return (uint16_t) (~sum);
+    sum = pico_checksum_adder(0, inbuf1, len1);
+    sum = pico_checksum_adder(sum, inbuf2, len2);
+    return pico_checksum_finalize(sum);
 }