Daniele Lacamera / PicoTCP-Experimental_CDC_ECM_Branch

Free (GPLv2) TCP/IP stack developed by TASS Belgium

Fork of PicoTCP by Daniele Lacamera

stack/pico_frame.c@51:18637a3d071f, 2013-08-03 (annotated)

Committer:
daniele
Date:
Sat Aug 03 08:50:27 2013 +0000
Revision:
51:18637a3d071f
Parent:
45:ca30069e49bb 
Branch for CDC-ECM: Work in progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniele 10:dd7111d4279f 1 /*********************************************************************
daniele 10:dd7111d4279f 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
daniele 10:dd7111d4279f 3 See LICENSE and COPYING for usage.
daniele 10:dd7111d4279f 4
daniele 10:dd7111d4279f 5 .
daniele 10:dd7111d4279f 6
daniele 10:dd7111d4279f 7 Authors: Daniele Lacamera
daniele 10:dd7111d4279f 8 *********************************************************************/
daniele 10:dd7111d4279f 9
daniele 10:dd7111d4279f 10
daniele 10:dd7111d4279f 11 #include "pico_config.h"
daniele 10:dd7111d4279f 12 #include "pico_frame.h"
daniele 10:dd7111d4279f 13
daniele 10:dd7111d4279f 14 #ifdef PICO_SUPPORT_DEBUG_MEMORY
daniele 10:dd7111d4279f 15 static int n_frames_allocated;
daniele 10:dd7111d4279f 16 #endif
daniele 10:dd7111d4279f 17
daniele 10:dd7111d4279f 18 /** frame alloc/dealloc/copy **/
daniele 10:dd7111d4279f 19 void pico_frame_discard(struct pico_frame *f)
daniele 10:dd7111d4279f 20 {
daniele 10:dd7111d4279f 21 (*f->usage_count)--;
daniele 10:dd7111d4279f 22 if (*f->usage_count <= 0) {
daniele 10:dd7111d4279f 23 pico_free(f->usage_count);
daniele 10:dd7111d4279f 24 #ifdef PICO_SUPPORT_DEBUG_MEMORY
daniele 10:dd7111d4279f 25 dbg("Discarded buffer @%p, caller: %p\n", f->buffer, __builtin_return_address(3));
daniele 10:dd7111d4279f 26 dbg("DEBUG MEMORY: %d frames in use.\n", --n_frames_allocated);
daniele 10:dd7111d4279f 27 #endif
daniele 10:dd7111d4279f 28 pico_free(f->buffer);
daniele 10:dd7111d4279f 29 if (f->info)
daniele 10:dd7111d4279f 30 pico_free(f->info);
daniele 10:dd7111d4279f 31 }
daniele 10:dd7111d4279f 32 #ifdef PICO_SUPPORT_DEBUG_MEMORY
daniele 10:dd7111d4279f 33 else {
daniele 10:dd7111d4279f 34 dbg("Removed frame @%p(copy), usage count now: %d\n", f, *f->usage_count);
daniele 10:dd7111d4279f 35 }
daniele 10:dd7111d4279f 36 #endif
daniele 10:dd7111d4279f 37 pico_free(f);
daniele 10:dd7111d4279f 38 }
daniele 10:dd7111d4279f 39
daniele 10:dd7111d4279f 40 struct pico_frame *pico_frame_copy(struct pico_frame *f)
daniele 10:dd7111d4279f 41 {
daniele 10:dd7111d4279f 42 struct pico_frame *new = pico_zalloc(sizeof(struct pico_frame));
daniele 10:dd7111d4279f 43 if (!new)
daniele 10:dd7111d4279f 44 return NULL;
daniele 10:dd7111d4279f 45 memcpy(new, f, sizeof(struct pico_frame));
daniele 10:dd7111d4279f 46 *(new->usage_count) += 1;
daniele 10:dd7111d4279f 47 #ifdef PICO_SUPPORT_DEBUG_MEMORY
daniele 10:dd7111d4279f 48 dbg("Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
daniele 10:dd7111d4279f 49 #endif
daniele 10:dd7111d4279f 50 new->next = NULL;
daniele 10:dd7111d4279f 51 return new;
daniele 10:dd7111d4279f 52 }
daniele 10:dd7111d4279f 53
daniele 10:dd7111d4279f 54
daniele 10:dd7111d4279f 55 struct pico_frame *pico_frame_alloc(int size)
daniele 10:dd7111d4279f 56 {
daniele 10:dd7111d4279f 57 struct pico_frame *p = pico_zalloc(sizeof(struct pico_frame));
daniele 10:dd7111d4279f 58 if (!p)
daniele 10:dd7111d4279f 59 return NULL;
daniele 10:dd7111d4279f 60 p->buffer = pico_zalloc(size);
daniele 10:dd7111d4279f 61 if (!p->buffer) {
daniele 10:dd7111d4279f 62 pico_free(p);
daniele 10:dd7111d4279f 63 return NULL;
daniele 10:dd7111d4279f 64 }
daniele 10:dd7111d4279f 65 p->usage_count = pico_zalloc(sizeof(uint32_t));
daniele 10:dd7111d4279f 66 if (!p->usage_count) {
daniele 10:dd7111d4279f 67 pico_free(p->buffer);
daniele 10:dd7111d4279f 68 pico_free(p);
daniele 10:dd7111d4279f 69 return NULL;
daniele 10:dd7111d4279f 70 }
daniele 10:dd7111d4279f 71 p->buffer_len = size;
daniele 10:dd7111d4279f 72
daniele 10:dd7111d4279f 73
daniele 10:dd7111d4279f 74 /* By default, frame content is the full buffer. */
daniele 10:dd7111d4279f 75 p->start = p->buffer;
daniele 10:dd7111d4279f 76 p->len = p->buffer_len;
daniele 10:dd7111d4279f 77 *p->usage_count = 1;
daniele 10:dd7111d4279f 78 #ifdef PICO_SUPPORT_DEBUG_MEMORY
daniele 10:dd7111d4279f 79 dbg("Allocated buffer @%p, len= %d caller: %p\n", p->buffer, p->buffer_len, __builtin_return_address(2));
daniele 10:dd7111d4279f 80 dbg("DEBUG MEMORY: %d frames in use.\n", ++n_frames_allocated);
daniele 10:dd7111d4279f 81 #endif
daniele 10:dd7111d4279f 82 return p;
daniele 10:dd7111d4279f 83 }
daniele 10:dd7111d4279f 84
daniele 10:dd7111d4279f 85 struct pico_frame *pico_frame_deepcopy(struct pico_frame *f)
daniele 10:dd7111d4279f 86 {
daniele 10:dd7111d4279f 87 struct pico_frame *new = pico_frame_alloc(f->buffer_len);
daniele 10:dd7111d4279f 88 int addr_diff;
daniele 10:dd7111d4279f 89 unsigned char *buf;
daniele 10:dd7111d4279f 90 uint32_t *uc;
daniele 10:dd7111d4279f 91 if (!new)
daniele 10:dd7111d4279f 92 return NULL;
daniele 10:dd7111d4279f 93
daniele 10:dd7111d4279f 94 /* Save the two key pointers... */
daniele 10:dd7111d4279f 95 buf = new->buffer;
daniele 10:dd7111d4279f 96 uc = new->usage_count;
daniele 10:dd7111d4279f 97
daniele 10:dd7111d4279f 98 /* Overwrite all fields with originals */
daniele 10:dd7111d4279f 99 memcpy(new, f, sizeof(struct pico_frame));
daniele 10:dd7111d4279f 100
daniele 10:dd7111d4279f 101 /* ...restore the two key pointers */
daniele 10:dd7111d4279f 102 new->buffer = buf;
daniele 10:dd7111d4279f 103 new->usage_count = uc;
daniele 10:dd7111d4279f 104
daniele 10:dd7111d4279f 105 /* Update in-buffer pointers with offset */
daniele 10:dd7111d4279f 106 addr_diff = (int)new->buffer - (int)f->buffer;
daniele 10:dd7111d4279f 107 new->net_hdr += addr_diff;
daniele 10:dd7111d4279f 108 new->transport_hdr += addr_diff;
daniele 10:dd7111d4279f 109 new->app_hdr += addr_diff;
daniele 10:dd7111d4279f 110 new->start += addr_diff;
daniele 10:dd7111d4279f 111 new->payload += addr_diff;
daniele 10:dd7111d4279f 112
daniele 10:dd7111d4279f 113 #ifdef PICO_SUPPORT_DEBUG_MEMORY
daniele 10:dd7111d4279f 114 dbg("Deep-Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
daniele 10:dd7111d4279f 115 #endif
daniele 10:dd7111d4279f 116 new->next = NULL;
daniele 10:dd7111d4279f 117 return new;
daniele 10:dd7111d4279f 118 }
daniele 10:dd7111d4279f 119
daniele 10:dd7111d4279f 120 /**
daniele 10:dd7111d4279f 121 * Calculate checksum of a given string
daniele 10:dd7111d4279f 122 */
daniele 10:dd7111d4279f 123 uint16_t pico_checksum(void *inbuf, int len)
daniele 10:dd7111d4279f 124 {
daniele 10:dd7111d4279f 125 uint8_t *buf = (uint8_t *) inbuf;
daniele 10:dd7111d4279f 126 uint16_t tmp = 0;
tass 45:ca30069e49bb 127 uint32_t sum = 0;
tass 45:ca30069e49bb 128 int i = 0;
tass 45:ca30069e49bb 129
tass 45:ca30069e49bb 130 for(i=0; i < len; i++) {
tass 45:ca30069e49bb 131 if (i%2) {
tass 45:ca30069e49bb 132 sum += buf[i];
tass 45:ca30069e49bb 133 } else {
daniele 10:dd7111d4279f 134 tmp = buf[i];
tass 45:ca30069e49bb 135 sum += (tmp << 8);
daniele 10:dd7111d4279f 136 }
daniele 10:dd7111d4279f 137 }
tass 45:ca30069e49bb 138
tass 45:ca30069e49bb 139 while (sum >> 16) { /* a second carry is possible! */
tass 45:ca30069e49bb 140 sum = (sum & 0x0000FFFF) + (sum >> 16);
tass 45:ca30069e49bb 141 }
tass 45:ca30069e49bb 142 return (uint16_t) (~sum);
daniele 10:dd7111d4279f 143 }
daniele 10:dd7111d4279f 144
daniele 10:dd7111d4279f 145 uint16_t pico_dualbuffer_checksum(void *inbuf1, int len1, void *inbuf2, int len2)
daniele 10:dd7111d4279f 146 {
daniele 10:dd7111d4279f 147 uint8_t *b1 = (uint8_t *) inbuf1;
daniele 10:dd7111d4279f 148 uint8_t *b2 = (uint8_t *) inbuf2;
daniele 10:dd7111d4279f 149 uint16_t tmp = 0;
tass 45:ca30069e49bb 150 uint32_t sum = 0;
tass 45:ca30069e49bb 151 int i = 0, j = 0;
tass 45:ca30069e49bb 152
tass 45:ca30069e49bb 153 for(i=0; i < len1; i++) {
tass 45:ca30069e49bb 154 if (j%2) {
tass 45:ca30069e49bb 155 sum += b1[i];
tass 45:ca30069e49bb 156 } else {
daniele 10:dd7111d4279f 157 tmp = b1[i];
tass 45:ca30069e49bb 158 sum += (tmp << 8);
daniele 10:dd7111d4279f 159 }
daniele 10:dd7111d4279f 160 j++;
daniele 10:dd7111d4279f 161 }
daniele 10:dd7111d4279f 162
tass 45:ca30069e49bb 163 j = 0; /* j has to be reset if len1 is odd */
tass 45:ca30069e49bb 164 for(i=0; i < len2; i++) {
tass 45:ca30069e49bb 165 if (j%2) {
tass 45:ca30069e49bb 166 sum += b2[i];
tass 45:ca30069e49bb 167 } else {
daniele 10:dd7111d4279f 168 tmp = b2[i];
tass 45:ca30069e49bb 169 sum += (tmp << 8);
daniele 10:dd7111d4279f 170 }
daniele 10:dd7111d4279f 171 j++;
daniele 10:dd7111d4279f 172 }
tass 45:ca30069e49bb 173 while (sum >> 16) { /* a second carry is possible! */
tass 45:ca30069e49bb 174 sum = (sum & 0x0000FFFF) + (sum >> 16);
tass 45:ca30069e49bb 175 }
tass 45:ca30069e49bb 176 return (uint16_t) (~sum);
daniele 10:dd7111d4279f 177 }
daniele 10:dd7111d4279f 178
daniele 10:dd7111d4279f 179 uint16_t pico_dualbuffer_checksum_broken(void *inbuf1, int len1, void *inbuf2, int len2)
daniele 10:dd7111d4279f 180 {
daniele 10:dd7111d4279f 181 uint16_t *b1 = (uint16_t *) inbuf1;
daniele 10:dd7111d4279f 182 uint16_t *b2 = (uint16_t *) inbuf2;
daniele 10:dd7111d4279f 183 uint32_t sum = 0;
daniele 10:dd7111d4279f 184 int i=0, j=0;
daniele 10:dd7111d4279f 185 for(i=0; i<(len1>>1); i++){
daniele 10:dd7111d4279f 186 sum += short_be(b1[i]);
daniele 10:dd7111d4279f 187 j++;
daniele 10:dd7111d4279f 188 }
daniele 10:dd7111d4279f 189 for(i=0; i<(len2>>1); i++){
daniele 10:dd7111d4279f 190 sum += short_be(b2[i]);
daniele 10:dd7111d4279f 191 j++;
daniele 10:dd7111d4279f 192 }
daniele 10:dd7111d4279f 193 sum = (sum & 0xFFFF) + (sum >> 16);
daniele 10:dd7111d4279f 194 sum += (sum >> 16);
daniele 10:dd7111d4279f 195
daniele 10:dd7111d4279f 196 // Take the bitwise complement of sum
daniele 10:dd7111d4279f 197 sum = ~sum;
daniele 10:dd7111d4279f 198 return (uint16_t) (sum) ;
daniele 10:dd7111d4279f 199 }
daniele 10:dd7111d4279f 200
daniele 10:dd7111d4279f 201