pico_frame.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. 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 
00014 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00015 static int n_frames_allocated;
00016 #endif
00017 
00018 /** frame alloc/dealloc/copy **/
00019 void pico_frame_discard(struct pico_frame *f)
00020 {
00021   (*f->usage_count)--;
00022   if (*f->usage_count <= 0) {
00023     pico_free(f->usage_count);
00024 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00025     dbg("Discarded buffer @%p, caller: %p\n", f->buffer, __builtin_return_address(3));
00026     dbg("DEBUG MEMORY: %d frames in use.\n", --n_frames_allocated);
00027 #endif
00028     pico_free(f->buffer);
00029     if (f->info)
00030       pico_free(f->info);
00031   }
00032 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00033   else {
00034     dbg("Removed frame @%p(copy), usage count now: %d\n", f, *f->usage_count);
00035   }
00036 #endif
00037   pico_free(f);
00038 }
00039 
00040 struct pico_frame *pico_frame_copy(struct pico_frame *f)
00041 {
00042   struct pico_frame *new = pico_zalloc(sizeof(struct pico_frame));
00043   if (!new)
00044     return NULL;
00045   memcpy(new, f, sizeof(struct pico_frame));
00046   *(new->usage_count) += 1;
00047 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00048   dbg("Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
00049 #endif
00050   new->next = NULL;
00051   return new;
00052 }
00053 
00054 
00055 struct pico_frame *pico_frame_alloc(int size)
00056 {
00057   struct pico_frame *p = pico_zalloc(sizeof(struct pico_frame));
00058   if (!p)
00059     return NULL;
00060   p->buffer = pico_zalloc(size);
00061   if (!p->buffer) {
00062     pico_free(p);
00063     return NULL;
00064   }
00065   p->usage_count = pico_zalloc(sizeof(uint32_t));
00066   if (!p->usage_count) {
00067     pico_free(p->buffer);
00068     pico_free(p);
00069     return NULL;
00070   }
00071   p->buffer_len = size;
00072 
00073 
00074   /* By default, frame content is the full buffer. */
00075   p->start = p->buffer;
00076   p->len = p->buffer_len;
00077   *p->usage_count = 1;
00078 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00079     dbg("Allocated buffer @%p, len= %d caller: %p\n", p->buffer, p->buffer_len, __builtin_return_address(2));
00080     dbg("DEBUG MEMORY: %d frames in use.\n", ++n_frames_allocated);
00081 #endif
00082   return p;
00083 }
00084 
00085 struct pico_frame *pico_frame_deepcopy(struct pico_frame *f)
00086 {
00087   struct pico_frame *new = pico_frame_alloc(f->buffer_len);
00088   int addr_diff;
00089   unsigned char *buf;
00090   uint32_t *uc;
00091   if (!new)
00092     return NULL;
00093 
00094   /* Save the two key pointers... */
00095   buf = new->buffer;
00096   uc  = new->usage_count;
00097 
00098   /* Overwrite all fields with originals */
00099   memcpy(new, f, sizeof(struct pico_frame));
00100 
00101   /* ...restore the two key pointers */
00102   new->buffer = buf;
00103   new->usage_count = uc;
00104 
00105   /* Update in-buffer pointers with offset */
00106   addr_diff = (int)new->buffer - (int)f->buffer;
00107   new->net_hdr += addr_diff;
00108   new->transport_hdr += addr_diff;
00109   new->app_hdr += addr_diff;
00110   new->start += addr_diff;
00111   new->payload += addr_diff;
00112 
00113 #ifdef PICO_SUPPORT_DEBUG_MEMORY
00114   dbg("Deep-Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
00115 #endif
00116   new->next = NULL;
00117   return new;
00118 }
00119 
00120 /**
00121  * Calculate checksum of a given string
00122  */
00123 uint16_t pico_checksum(void *inbuf, int len)
00124 {
00125   uint8_t *buf = (uint8_t *) inbuf;
00126   uint16_t tmp = 0;
00127   uint32_t sum = 0;
00128   int i = 0;
00129 
00130   for(i=0; i < len; i++) {
00131     if (i%2) {
00132       sum += buf[i];
00133     } else {
00134       tmp = buf[i];
00135       sum += (tmp << 8);
00136     }
00137   }
00138 
00139   while (sum >> 16) { /* a second carry is possible! */
00140     sum = (sum & 0x0000FFFF) + (sum >> 16);
00141   } 
00142   return (uint16_t) (~sum);
00143 }
00144 
00145 uint16_t pico_dualbuffer_checksum(void *inbuf1, int len1, void *inbuf2, int len2)
00146 {
00147   uint8_t *b1 = (uint8_t *) inbuf1;
00148   uint8_t *b2 = (uint8_t *) inbuf2;
00149   uint16_t tmp = 0;
00150   uint32_t sum = 0;
00151   int i = 0, j = 0;
00152 
00153   for(i=0; i < len1; i++) {
00154     if (j%2) {
00155       sum += b1[i];
00156     } else {
00157       tmp = b1[i];
00158       sum += (tmp << 8);
00159     }
00160     j++;
00161   }
00162 
00163   j = 0; /* j has to be reset if len1 is odd */
00164   for(i=0; i < len2; i++) {
00165     if (j%2) {
00166       sum += b2[i];
00167     } else {
00168       tmp = b2[i];
00169       sum += (tmp << 8);
00170     }
00171     j++;
00172   }
00173   while (sum >> 16) { /* a second carry is possible! */
00174     sum = (sum & 0x0000FFFF) + (sum >> 16);
00175   } 
00176   return (uint16_t) (~sum);
00177 }
00178 
00179 uint16_t pico_dualbuffer_checksum_broken(void *inbuf1, int len1, void *inbuf2, int len2)
00180 {
00181   uint16_t *b1 = (uint16_t *) inbuf1;
00182   uint16_t *b2 = (uint16_t *) inbuf2;
00183   uint32_t sum = 0;
00184   int i=0, j=0;
00185   for(i=0; i<(len1>>1); i++){
00186     sum += short_be(b1[i]);
00187     j++;
00188   }
00189   for(i=0; i<(len2>>1); i++){
00190     sum += short_be(b2[i]);
00191     j++;
00192   }
00193   sum = (sum & 0xFFFF) + (sum >> 16);
00194   sum += (sum >> 16);
00195   
00196   // Take the bitwise complement of sum
00197   sum = ~sum;
00198   return (uint16_t) (sum)  ;
00199 }
00200 
00201