pico_frame.c

00001 /*********************************************************************
00002    PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
00003    See LICENSE and COPYING for usage.
00004 
00005    .
00006 
00007    Authors: Daniele Lacamera
00008  *********************************************************************/
00009 
00010 
00011 #include "pico_config.h"
00012 #include "pico_frame.h"
00013 #include "pico_protocol.h"
00014 #include "pico_stack.h"
00015 
00016 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00017 static int n_frames_allocated;
00018 #endif
00019 
00020 /** frame alloc/dealloc/copy **/
00021 void pico_frame_discard(struct pico_frame *f)
00022 {
00023     if (!f)
00024         return;
00025 
00026     (*f->usage_count)--;
00027     if (*f->usage_count == 0) {
00028         if (f->flags & PICO_FRAME_FLAG_EXT_USAGE_COUNTER)
00029             PICO_FREE(f->usage_count);
00030 
00031 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00032         dbg("Discarded buffer @%p, caller: %p\n", f->buffer, __builtin_return_address(3));
00033         dbg("DEBUG MEMORY: %d frames in use.\n", --n_frames_allocated);
00034 #endif
00035         if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER))
00036             PICO_FREE(f->buffer);
00037         else if (f->notify_free)
00038             f->notify_free(f->buffer);
00039 
00040         if (f->info)
00041             PICO_FREE(f->info);
00042     }
00043 
00044 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00045     else {
00046         dbg("Removed frame @%p(copy), usage count now: %d\n", f, *f->usage_count);
00047     }
00048 #endif
00049     PICO_FREE(f);
00050 }
00051 
00052 struct pico_frame *pico_frame_copy(struct pico_frame *f)
00053 {
00054     struct pico_frame *new = PICO_ZALLOC(sizeof(struct pico_frame));
00055     if (!new)
00056         return NULL;
00057 
00058     memcpy(new, f, sizeof(struct pico_frame));
00059     *(new->usage_count) += 1;
00060 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00061     dbg("Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
00062 #endif
00063     new->next = NULL;
00064     return new;
00065 }
00066 
00067 
00068 static struct pico_frame *pico_frame_do_alloc(uint32_t size, int zerocopy, int ext_buffer)
00069 {
00070     struct pico_frame *p = PICO_ZALLOC(sizeof(struct pico_frame));
00071     uint32_t frame_buffer_size = size;
00072     if (!p)
00073         return NULL;
00074 
00075     if (ext_buffer && !zerocopy) {
00076         /* external buffer implies zerocopy flag! */
00077         PICO_FREE(p);
00078         return NULL;
00079     }
00080 
00081     if (!zerocopy) {
00082         unsigned int align = size % sizeof(uint32_t);
00083         /* Ensure that usage_count starts on an aligned address */
00084         if (align) {
00085             frame_buffer_size += (uint32_t)sizeof(uint32_t) - align;
00086         }
00087 
00088         p->buffer = PICO_ZALLOC(frame_buffer_size + sizeof(uint32_t));
00089         if (!p->buffer) {
00090             PICO_FREE(p);
00091             return NULL;
00092         }
00093 
00094         p->usage_count = (uint32_t *)(((uint8_t*)p->buffer) + frame_buffer_size);
00095     } else {
00096         p->buffer = NULL;
00097         p->flags |= PICO_FRAME_FLAG_EXT_USAGE_COUNTER;
00098         p->usage_count = PICO_ZALLOC(sizeof(uint32_t));
00099         if (!p->usage_count) {
00100             PICO_FREE(p);
00101             return NULL;
00102         }
00103     }
00104 
00105 
00106     p->buffer_len = size;
00107 
00108     /* By default, frame content is the full buffer. */
00109     p->start = p->buffer;
00110     p->len = p->buffer_len;
00111     *p->usage_count = 1;
00112 
00113     if (ext_buffer)
00114         p->flags |= PICO_FRAME_FLAG_EXT_BUFFER;
00115 
00116 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00117     dbg("Allocated buffer @%p, len= %d caller: %p\n", p->buffer, p->buffer_len, __builtin_return_address(2));
00118     dbg("DEBUG MEMORY: %d frames in use.\n", ++n_frames_allocated);
00119 #endif
00120     return p;
00121 }
00122 
00123 struct pico_frame *pico_frame_alloc(uint32_t size)
00124 {
00125     return pico_frame_do_alloc(size, 0, 0);
00126 }
00127 
00128 int pico_frame_grow(struct pico_frame *f, uint32_t size)
00129 {
00130     uint8_t *oldbuf;
00131     uint32_t usage_count, *p_old_usage;
00132     uint32_t frame_buffer_size;
00133     uint32_t oldsize;
00134     unsigned int align;
00135     int addr_diff = 0;
00136 
00137     if (!f || (size < f->buffer_len)) {
00138         return -1;
00139     }
00140 
00141     align = size % sizeof(uint32_t);
00142     frame_buffer_size = size;
00143     if (align) {
00144         frame_buffer_size += (uint32_t)sizeof(uint32_t) - align;
00145     }
00146 
00147     oldbuf = f->buffer;
00148     oldsize = f->buffer_len;
00149     usage_count = *(f->usage_count);
00150     p_old_usage = f->usage_count;
00151     f->buffer = PICO_ZALLOC(frame_buffer_size + sizeof(uint32_t));
00152     if (!f->buffer) {
00153         f->buffer = oldbuf;
00154         return -1;
00155     }
00156 
00157     f->usage_count = (uint32_t *)(((uint8_t*)f->buffer) + frame_buffer_size);
00158     *f->usage_count = usage_count;
00159     f->buffer_len = size;
00160     memcpy(f->buffer, oldbuf, oldsize);
00161 
00162     /* Update hdr fields to new buffer*/
00163     addr_diff = (int)(f->buffer - oldbuf);
00164     f->net_hdr += addr_diff;
00165     f->datalink_hdr += addr_diff;
00166     f->transport_hdr += addr_diff;
00167     f->app_hdr += addr_diff;
00168     f->start += addr_diff;
00169     f->payload += addr_diff;
00170 
00171     if (f->flags & PICO_FRAME_FLAG_EXT_USAGE_COUNTER)
00172         PICO_FREE(p_old_usage);
00173 
00174     if (!(f->flags & PICO_FRAME_FLAG_EXT_BUFFER))
00175         PICO_FREE(oldbuf);
00176     else if (f->notify_free)
00177         f->notify_free(oldbuf);
00178 
00179     f->flags = 0;
00180     /* Now, the frame is not zerocopy anymore, and the usage counter has been moved within it */
00181     return 0;
00182 }
00183 
00184 struct pico_frame *pico_frame_alloc_skeleton(uint32_t size, int ext_buffer)
00185 {
00186     return pico_frame_do_alloc(size, 1, ext_buffer);
00187 }
00188 
00189 int pico_frame_skeleton_set_buffer(struct pico_frame *f, void *buf)
00190 {
00191     if (!buf)
00192         return -1;
00193 
00194     f->buffer = (uint8_t *) buf;
00195     f->start = f->buffer;
00196     return 0;
00197 }
00198 
00199 struct pico_frame *pico_frame_deepcopy(struct pico_frame *f)
00200 {
00201     struct pico_frame *new = pico_frame_alloc(f->buffer_len);
00202     int addr_diff;
00203     unsigned char *buf;
00204     uint32_t *uc;
00205     if (!new)
00206         return NULL;
00207 
00208     /* Save the two key pointers... */
00209     buf = new->buffer;
00210     uc  = new->usage_count;
00211 
00212     /* Overwrite all fields with originals */
00213     memcpy(new, f, sizeof(struct pico_frame));
00214 
00215     /* ...restore the two key pointers */
00216     new->buffer = buf;
00217     new->usage_count = uc;
00218 
00219     /* Update in-buffer pointers with offset */
00220     addr_diff = (int)(new->buffer - f->buffer);
00221     new->datalink_hdr += addr_diff;
00222     new->net_hdr += addr_diff;
00223     new->transport_hdr += addr_diff;
00224     new->app_hdr += addr_diff;
00225     new->start += addr_diff;
00226     new->payload += addr_diff;
00227 
00228 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00229     dbg("Deep-Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
00230 #endif
00231     new->next = NULL;
00232     return new;
00233 }
00234 
00235 
00236 static inline uint32_t pico_checksum_adder(uint32_t sum, void *data, uint32_t len)
00237 {
00238     uint16_t *buf = (uint16_t *)data;
00239     uint16_t *stop;
00240 
00241     if (len & 0x01) {
00242         --len;
00243 #ifdef PICO_BIGENDIAN
00244         sum += (((uint8_t *)data)[len]) << 8;
00245 #else
00246         sum += ((uint8_t *)data)[len];
00247 #endif
00248     }
00249 
00250     stop = (uint16_t *)(((uint8_t *)data) + len);
00251 
00252     while (buf < stop) {
00253         sum += *buf++;
00254     }
00255     return sum;
00256 }
00257 
00258 static inline uint16_t pico_checksum_finalize(uint32_t sum)
00259 {
00260     while (sum >> 16) { /* a second carry is possible! */
00261         sum = (sum & 0x0000FFFF) + (sum >> 16);
00262     }
00263     return short_be((uint16_t) ~sum);
00264 }
00265 
00266 /**
00267  * Calculate checksum of a given string
00268  */
00269 uint16_t pico_checksum(void *inbuf, uint32_t len)
00270 {
00271     uint32_t sum;
00272 
00273     sum = pico_checksum_adder(0, inbuf, len);
00274     return pico_checksum_finalize(sum);
00275 }
00276 
00277 /* WARNING: len1 MUST be an EVEN number */
00278 uint16_t pico_dualbuffer_checksum(void *inbuf1, uint32_t len1, void *inbuf2, uint32_t len2)
00279 {
00280     uint32_t sum;
00281 
00282     sum = pico_checksum_adder(0, inbuf1, len1);
00283     sum = pico_checksum_adder(sum, inbuf2, len2);
00284     return pico_checksum_finalize(sum);
00285 }
00286