lwbt/btpbuf.c

   1 #include <stdlib.h>
   2 #include <string.h>
   3
   4 #include "btmemb.h"
   5 #include "btmemr.h"
   6 #include "btpbuf.h"
   7
   8 #if STATISTICS == 1
   9 #define STAT(s)
  10 #else
  11 #define STAT(s)
  12 #endif /* STATISTICS == 1 */
  13
  14 MEMB(pool_pbufs,sizeof(struct pbuf)+PBUF_POOL_BUFSIZE,PBUF_POOL_NUM);
  15 MEMB(rom_pbufs,sizeof(struct pbuf),PBUF_ROM_NUM);
  16
  17 void btpbuf_init()
  18 {
  19         btmemb_init(&pool_pbufs);
  20         btmemb_init(&rom_pbufs);
  21 }
  22
  23 struct pbuf* btpbuf_alloc(pbuf_layer layer,u16_t len,pbuf_flag flag)
  24 {
  25         u16_t offset;
  26         s32_t rem_len;
  27         struct pbuf *p,*q,*r;
  28
  29         offset = 0;
  30         switch(layer)    {
  31                 case PBUF_TRANSPORT:
  32                         offset += TRANSPORT_HLEN;
  33                 case PBUF_LINK:
  34                         offset += LL_HLEN;
  35                         break;
  36                 case PBUF_RAW:
  37                         break;
  38                 default:
  39                         ERROR("btpbuf_alloc: bad pbuf layer.\n");
  40                         return NULL;
  41         }
  42
  43         switch(flag) {
  44                 case PBUF_POOL:
  45                         p = btmemb_alloc(&pool_pbufs);
  46                         if(p==NULL) {
  47                                 ERROR("btbtpbuf_alloc: couldn't allocate pbuf(p) from pool\n");
  48                                 return NULL;
  49                         }
  50
  51                         p->next = NULL;
  52                         p->payload = MEM_ALIGN((void*)((u8_t*)p+(sizeof(struct pbuf)+offset)));
  53                         p->tot_len = len;
  54                         p->len = (len>(PBUF_POOL_BUFSIZE-offset)?(PBUF_POOL_BUFSIZE-offset):len);
  55                         p->flags = PBUF_FLAG_POOL;
  56                         p->ref = 1;
  57
  58                         r = p;
  59                         rem_len = len - p->len;
  60                         while(rem_len>0) {
  61                                 q = btmemb_alloc(&pool_pbufs);
  62                                 if(q==NULL) {
  63                                         ERROR("btpbuf_alloc: couldn't allocate pbuf(q) from pool\n");
  64                                         btpbuf_free(p);
  65                                         return NULL;
  66                                 }
  67
  68                                 q->next = NULL;
  69                                 r->next = q;
  70                                 q->tot_len = rem_len;
  71                                 q->len = (rem_len>PBUF_POOL_BUFSIZE?PBUF_POOL_BUFSIZE:rem_len);
  72                                 q->payload = (void*)((u8_t*)q+sizeof(struct pbuf));
  73                                 q->flags = PBUF_FLAG_POOL;
  74                                 q->ref = 1;
  75
  76                                 rem_len -= q->len;
  77                                 r = q;
  78                         }
  79                         break;
  80                 case PBUF_RAM:
  81                         p = btmemr_malloc(MEM_ALIGN_SIZE(sizeof(struct pbuf)+offset)+MEM_ALIGN_SIZE(len));
  82                         if(p==NULL) {
  83                                 ERROR("btpbuf_alloc: couldn't allocate pbuf from ram\n");
  84                                 return NULL;
  85                         }
  86                         p->payload = MEM_ALIGN((u8_t*)p+sizeof(struct pbuf)+offset);
  87                         p->len = p->tot_len = len;
  88                         p->next = NULL;
  89                         p->flags = PBUF_FLAG_RAM;
  90                         break;
  91                 case PBUF_ROM:
  92                 case PBUF_REF:
  93                         p = btmemb_alloc(&rom_pbufs);
  94                         if(p==NULL) {
  95                                 ERROR("btpbuf_alloc: couldn't allocate pbuf from rom/ref\n");
  96                                 return NULL;
  97                         }
  98                         p->payload = NULL;
  99                         p->next = NULL;
 100                         p->len = p->tot_len = len;
 101                         p->flags = (flag==PBUF_ROM?PBUF_FLAG_ROM:PBUF_FLAG_REF);
 102                         break;
 103                 default:
 104                         ERROR("btpbuf_alloc: bad flag value.\n");
 105                         return NULL;
 106         }
 107
 108         p->ref = 1;
 109         return p;
 110 }
 111
 112 u8_t btpbuf_free(struct pbuf *p)
 113 {
 114         u8_t cnt;
 115         u32 level;
 116         struct pbuf *q;
 117
 118         if(p==NULL) return 0;
 119
 120         cnt = 0;
 121
 122         _CPU_ISR_Disable(level);
 123         while(p!=NULL) {
 124                 p->ref--;
 125                 if(p->ref==0) {
 126                         q = p->next;
 127                         if(p->flags==PBUF_FLAG_POOL) {
 128                                 btmemb_free(&pool_pbufs,p);
 129                         } else if(p->flags==PBUF_FLAG_ROM || p->flags==PBUF_FLAG_REF) {
 130                                 btmemb_free(&rom_pbufs,p);
 131                         } else {
 132                                 btmemr_free(p);
 133                         }
 134                         cnt++;
 135                         p = q;
 136                 } else
 137                         p = NULL;
 138         }
 139         _CPU_ISR_Restore(level);
 140
 141         return cnt;
 142 }
 143
 144 void btpbuf_realloc(struct pbuf *p,u16_t new_len)
 145 {
 146         u16_t rem_len;
 147         s16_t grow;
 148         struct pbuf *q;
 149
 150         if(new_len>=p->tot_len) return;
 151
 152         grow = new_len - p->tot_len;
 153         rem_len = new_len;
 154         q = p;
 155         while(rem_len>q->len) {
 156                 rem_len -= q->len;
 157                 q->tot_len += grow;
 158                 q = q->next;
 159         }
 160
 161         if(q->flags==PBUF_FLAG_RAM && rem_len!=q->len)
 162                 btmemr_realloc(q,(u8_t*)q->payload-(u8_t*)q+rem_len);
 163
 164         q->len = rem_len;
 165         q->tot_len = q->len;
 166
 167         if(q->next!=NULL) btpbuf_free(q->next);
 168         q->next = NULL;
 169 }
 170
 171 u8_t btpbuf_header(struct pbuf *p,s16_t hdr_size_inc)
 172 {
 173         void *payload;
 174
 175         if(hdr_size_inc==0 || p==NULL) return 0;
 176
 177
 178         payload = p->payload;
 179         if(p->flags==PBUF_FLAG_POOL || p->flags==PBUF_FLAG_RAM) {
 180                 p->payload = (u8_t*)p->payload-hdr_size_inc;
 181                 if((u8_t*)p->payload<(u8_t*)p+sizeof(struct pbuf)) {
 182                         p->payload = payload;
 183                         return 1;
 184                 }
 185         } else if(p->flags==PBUF_FLAG_ROM || p->flags==PBUF_FLAG_REF) {
 186                 if(hdr_size_inc<0 && hdr_size_inc-p->len<=0) p->payload = (u8_t*)p->payload-hdr_size_inc;
 187                 else return 1;
 188         }
 189         p->tot_len += hdr_size_inc;
 190         p->len += hdr_size_inc;
 191
 192         return 0;
 193 }
 194
 195 u8_t btpbuf_clen(struct pbuf *p)
 196 {
 197         u8_t len;
 198
 199         len = 0;
 200         while(p!=NULL) {
 201                 len++;
 202                 p = p->next;
 203         }
 204         return len;
 205 }
 206
 207 void btpbuf_ref(struct pbuf *p)
 208 {
 209         u32 level;
 210
 211         if(p!=NULL) {
 212                 _CPU_ISR_Disable(level);
 213                 ++(p->ref);
 214                 _CPU_ISR_Restore(level);
 215         }
 216 }
 217
 218 void btpbuf_cat(struct pbuf *h,struct pbuf *t)
 219 {
 220         struct pbuf *p;
 221
 222         if(h==NULL || t==NULL) return;
 223
 224         for(p=h;p->next!=NULL;p=p->next) {
 225                 p->tot_len += t->tot_len;
 226         }
 227         p->tot_len += t->tot_len;
 228         p->next = t;
 229 }
 230
 231 void btpbuf_queue(struct pbuf *p,struct pbuf *n)
 232 {
 233         if(p==NULL || n==NULL || p==n) return;
 234
 235         while(p->next!=NULL) p = p->next;
 236
 237         p->next = n;
 238         btpbuf_ref(n);
 239 }
 240
 241 struct pbuf* btpbuf_dequeue(struct pbuf *p)
 242 {
 243         struct pbuf *q;
 244
 245         if(p==NULL) return NULL;
 246
 247         while(p->tot_len!=p->len) p = p->next;
 248
 249         q = p->next;
 250         p->next = NULL;
 251
 252         return q;
 253 }
 254
 255 void btpbuf_chain(struct pbuf *h,struct pbuf *t)
 256 {
 257         btpbuf_cat(h,t);
 258         btpbuf_ref(t);
 259 }
 260
 261 struct pbuf* btpbuf_dechain(struct pbuf *p)
 262 {
 263         struct pbuf *q;
 264         u8_t tail_gone = 1;
 265
 266         q = p->next;
 267         if(q!=NULL) {
 268                 q->tot_len = p->tot_len - p->len;
 269                 p->next = NULL;
 270                 p->tot_len = p->len;
 271
 272                 tail_gone = btpbuf_free(q);
 273         }
 274
 275         return (tail_gone>0?NULL:q);
 276 }
 277
 278 struct pbuf* btpbuf_take(struct pbuf *p)
 279 {
 280         struct pbuf *q , *prev, *head;
 281
 282         prev = NULL;
 283         head = p;
 284         /* iterate through pbuf chain */
 285         do
 286         {
 287                 /* pbuf is of type PBUF_REF? */
 288                 if (p->flags == PBUF_FLAG_REF) {
 289                         LOG("pbuf_take: encountered PBUF_REF %p\n", (void *)p);
 290                         /* allocate a pbuf (w/ payload) fully in RAM */
 291                         /* PBUF_POOL buffers are faster if we can use them */
 292                         if (p->len <= PBUF_POOL_BUFSIZE) {
 293                                 q = btpbuf_alloc(PBUF_RAW, p->len, PBUF_POOL);
 294                                 if (q == NULL) {
 295                                         LOG("pbuf_take: Could not allocate PBUF_POOL\n");
 296                                 }
 297                         } else {
 298                                 /* no replacement pbuf yet */
 299                                 q = NULL;
 300                                 LOG("pbuf_take: PBUF_POOL too small to replace PBUF_REF\n");
 301                         }
 302                         /* no (large enough) PBUF_POOL was available? retry with PBUF_RAM */
 303                         if (q == NULL) {
 304                                 q = btpbuf_alloc(PBUF_RAW, p->len, PBUF_RAM);
 305                                 if (q == NULL) {
 306                                         LOG("pbuf_take: Could not allocate PBUF_RAM\n");
 307                                 }
 308                         }
 309                         /* replacement pbuf could be allocated? */
 310                         if (q != NULL)
 311                         {
 312                                 /* copy p to q */
 313                                 /* copy successor */
 314                                 q->next = p->next;
 315                                 /* remove linkage from original pbuf */
 316                                 p->next = NULL;
 317                                 /* remove linkage to original pbuf */
 318                                 if (prev != NULL) {
 319                                         /* break chain and insert new pbuf instead */
 320                                         prev->next = q;
 321                                         /* prev == NULL, so we replaced the head pbuf of the chain */
 322                                 } else {
 323                                         head = q;
 324                                 }
 325                                 /* copy pbuf payload */
 326                                 memcpy(q->payload, p->payload, p->len);
 327                                 q->tot_len = p->tot_len;
 328                                 q->len = p->len;
 329                                 /* in case p was the first pbuf, it is no longer refered to by
 330                                  * our caller, as the caller MUST do p = pbuf_take(p);
 331                                  * in case p was not the first pbuf, it is no longer refered to
 332                                  * by prev. we can safely free the pbuf here.
 333                                  * (note that we have set p->next to NULL already so that
 334                                  * we will not free the rest of the chain by accident.)
 335                                  */
 336                                 btpbuf_free(p);
 337                                 /* do not copy ref, since someone else might be using the old buffer */
 338                                 LOG("pbuf_take: replaced PBUF_REF %p with %p\n", (void *)p, (void *)q);
 339                                 p = q;
 340                         } else {
 341                                 /* deallocate chain */
 342                                 btpbuf_free(head);
 343                                 LOG("pbuf_take: failed to allocate replacement pbuf for %p\n", (void *)p);
 344                                 return NULL;
 345                         }
 346                 /* p->flags != PBUF_FLAG_REF */
 347                 } else {
 348                         LOG("pbuf_take: skipping pbuf not of type PBUF_REF\n");
 349                 }
 350                 /* remember this pbuf */
 351                 prev = p;
 352                 /* proceed to next pbuf in original chain */
 353                 p = p->next;
 354         } while (p);
 355         LOG("pbuf_take: end of chain reached.\n");
 356
 357         return head;
 358 }