1 /****************************************************************************** 2 * 3 * Copyright (C) 1999-2012 Broadcom Corporation 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at: 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 * 17 ******************************************************************************/ 18 #include <stdio.h> 19 #include "gki_int.h" 20 21 #if (GKI_NUM_TOTAL_BUF_POOLS > 16) 22 #error Number of pools out of range (16 Max)! 23 #endif 24 25 #if (BTU_STACK_LITE_ENABLED == FALSE) 26 static void gki_add_to_pool_list(uint8_t pool_id); 27 static void gki_remove_from_pool_list(uint8_t pool_id); 28 #endif /* BTU_STACK_LITE_ENABLED == FALSE */ 29 30 /******************************************************************************* 31 ** 32 ** Function gki_init_free_queue 33 ** 34 ** Description Internal function called at startup to initialize a free 35 ** queue. It is called once for each free queue. 36 ** 37 ** Returns void 38 ** 39 *******************************************************************************/ 40 static void gki_init_free_queue(uint8_t id, uint16_t size, uint16_t total, 41 void* p_mem) { 42 uint16_t i; 43 uint16_t act_size; 44 BUFFER_HDR_T* hdr; 45 BUFFER_HDR_T* hdr1 = NULL; 46 uint32_t* magic; 47 int32_t tempsize = size; 48 tGKI_COM_CB* p_cb = &gki_cb.com; 49 50 /* Ensure an even number of longwords */ 51 tempsize = (int32_t)ALIGN_POOL(size); 52 act_size = (uint16_t)(tempsize + BUFFER_PADDING_SIZE); 53 54 /* Remember pool start and end addresses */ 55 if (p_mem) { 56 p_cb->pool_start[id] = (uint8_t*)p_mem; 57 p_cb->pool_end[id] = (uint8_t*)p_mem + (act_size * total); 58 } 59 60 p_cb->pool_size[id] = act_size; 61 62 p_cb->freeq[id].size = (uint16_t)tempsize; 63 p_cb->freeq[id].total = total; 64 p_cb->freeq[id].cur_cnt = 0; 65 p_cb->freeq[id].max_cnt = 0; 66 67 /* Initialize index table */ 68 if (p_mem) { 69 hdr = (BUFFER_HDR_T*)p_mem; 70 p_cb->freeq[id].p_first = hdr; 71 for (i = 0; i < total; i++) { 72 hdr->task_id = GKI_INVALID_TASK; 73 hdr->q_id = id; 74 hdr->status = BUF_STATUS_FREE; 75 magic = (uint32_t*)((uint8_t*)hdr + BUFFER_HDR_SIZE + tempsize); 76 *magic = MAGIC_NO; 77 hdr1 = hdr; 78 hdr = (BUFFER_HDR_T*)((uint8_t*)hdr + act_size); 79 hdr1->p_next = hdr; 80 } 81 if (hdr1 != NULL) hdr = hdr1; 82 hdr->p_next = NULL; 83 p_cb->freeq[id].p_last = hdr; 84 } 85 return; 86 } 87 88 static bool gki_alloc_free_queue(uint8_t id) { 89 FREE_QUEUE_T* Q; 90 tGKI_COM_CB* p_cb = &gki_cb.com; 91 92 Q = &p_cb->freeq[p_cb->pool_list[id]]; 93 94 if (Q->p_first == 0) { 95 void* p_mem = GKI_os_malloc((Q->size + BUFFER_PADDING_SIZE) * Q->total); 96 if (p_mem) { 97 // re-initialize the queue with allocated memory 98 gki_init_free_queue(id, Q->size, Q->total, p_mem); 99 return true; 100 } 101 GKI_exception(GKI_ERROR_BUF_SIZE_TOOBIG, 102 "gki_alloc_free_queue: Not enough memory"); 103 } 104 return false; 105 } 106 107 /******************************************************************************* 108 ** 109 ** Function gki_buffer_init 110 ** 111 ** Description Called once internally by GKI at startup to initialize all 112 ** buffers and free buffer pools. 113 ** 114 ** Returns void 115 ** 116 *******************************************************************************/ 117 void gki_buffer_init(void) { 118 uint8_t i, tt, mb; 119 tGKI_COM_CB* p_cb = &gki_cb.com; 120 121 /* Initialize mailboxes */ 122 for (tt = 0; tt < GKI_MAX_TASKS; tt++) { 123 for (mb = 0; mb < NUM_TASK_MBOX; mb++) { 124 p_cb->OSTaskQFirst[tt][mb] = NULL; 125 p_cb->OSTaskQLast[tt][mb] = NULL; 126 } 127 } 128 129 for (tt = 0; tt < GKI_NUM_TOTAL_BUF_POOLS; tt++) { 130 p_cb->pool_start[tt] = NULL; 131 p_cb->pool_end[tt] = NULL; 132 p_cb->pool_size[tt] = 0; 133 134 p_cb->freeq[tt].p_first = 0; 135 p_cb->freeq[tt].p_last = 0; 136 p_cb->freeq[tt].size = 0; 137 p_cb->freeq[tt].total = 0; 138 p_cb->freeq[tt].cur_cnt = 0; 139 p_cb->freeq[tt].max_cnt = 0; 140 } 141 142 /* Use default from target.h */ 143 p_cb->pool_access_mask = GKI_DEF_BUFPOOL_PERM_MASK; 144 145 #if (GKI_NUM_FIXED_BUF_POOLS > 0) 146 gki_init_free_queue(0, GKI_BUF0_SIZE, GKI_BUF0_MAX, p_cb->bufpool0); 147 #endif 148 149 #if (GKI_NUM_FIXED_BUF_POOLS > 1) 150 gki_init_free_queue(1, GKI_BUF1_SIZE, GKI_BUF1_MAX, p_cb->bufpool1); 151 #endif 152 153 #if (GKI_NUM_FIXED_BUF_POOLS > 2) 154 gki_init_free_queue(2, GKI_BUF2_SIZE, GKI_BUF2_MAX, p_cb->bufpool2); 155 #endif 156 157 #if (GKI_NUM_FIXED_BUF_POOLS > 3) 158 gki_init_free_queue(3, GKI_BUF3_SIZE, GKI_BUF3_MAX, p_cb->bufpool3); 159 #endif 160 161 #if (GKI_NUM_FIXED_BUF_POOLS > 4) 162 gki_init_free_queue(4, GKI_BUF4_SIZE, GKI_BUF4_MAX, p_cb->bufpool4); 163 #endif 164 165 #if (GKI_NUM_FIXED_BUF_POOLS > 5) 166 gki_init_free_queue(5, GKI_BUF5_SIZE, GKI_BUF5_MAX, p_cb->bufpool5); 167 #endif 168 169 #if (GKI_NUM_FIXED_BUF_POOLS > 6) 170 gki_init_free_queue(6, GKI_BUF6_SIZE, GKI_BUF6_MAX, p_cb->bufpool6); 171 #endif 172 173 #if (GKI_NUM_FIXED_BUF_POOLS > 7) 174 gki_init_free_queue(7, GKI_BUF7_SIZE, GKI_BUF7_MAX, p_cb->bufpool7); 175 #endif 176 177 #if (GKI_NUM_FIXED_BUF_POOLS > 8) 178 gki_init_free_queue(8, GKI_BUF8_SIZE, GKI_BUF8_MAX, p_cb->bufpool8); 179 #endif 180 181 #if (GKI_NUM_FIXED_BUF_POOLS > 9) 182 gki_init_free_queue(9, GKI_BUF9_SIZE, GKI_BUF9_MAX, p_cb->bufpool9); 183 #endif 184 185 #if (GKI_NUM_FIXED_BUF_POOLS > 10) 186 gki_init_free_queue(10, GKI_BUF10_SIZE, GKI_BUF10_MAX, p_cb->bufpool10); 187 #endif 188 189 #if (GKI_NUM_FIXED_BUF_POOLS > 11) 190 gki_init_free_queue(11, GKI_BUF11_SIZE, GKI_BUF11_MAX, p_cb->bufpool11); 191 #endif 192 193 #if (GKI_NUM_FIXED_BUF_POOLS > 12) 194 gki_init_free_queue(12, GKI_BUF12_SIZE, GKI_BUF12_MAX, p_cb->bufpool12); 195 #endif 196 197 #if (GKI_NUM_FIXED_BUF_POOLS > 13) 198 gki_init_free_queue(13, GKI_BUF13_SIZE, GKI_BUF13_MAX, p_cb->bufpool13); 199 #endif 200 201 #if (GKI_NUM_FIXED_BUF_POOLS > 14) 202 gki_init_free_queue(14, GKI_BUF14_SIZE, GKI_BUF14_MAX, p_cb->bufpool14); 203 #endif 204 205 #if (GKI_NUM_FIXED_BUF_POOLS > 15) 206 gki_init_free_queue(15, GKI_BUF15_SIZE, GKI_BUF15_MAX, p_cb->bufpool15); 207 #endif 208 209 /* add pools to the pool_list which is arranged in the order of size */ 210 for (i = 0; i < GKI_NUM_FIXED_BUF_POOLS; i++) { 211 p_cb->pool_list[i] = i; 212 } 213 214 p_cb->curr_total_no_of_pools = GKI_NUM_FIXED_BUF_POOLS; 215 216 return; 217 } 218 219 /******************************************************************************* 220 ** 221 ** Function GKI_init_q 222 ** 223 ** Description Called by an application to initialize a buffer queue. 224 ** 225 ** Returns void 226 ** 227 *******************************************************************************/ 228 void GKI_init_q(BUFFER_Q* p_q) { 229 p_q->p_first = p_q->p_last = NULL; 230 p_q->count = 0; 231 232 return; 233 } 234 235 /******************************************************************************* 236 ** 237 ** Function GKI_getbuf 238 ** 239 ** Description Called by an application to get a free buffer which 240 ** is of size greater or equal to the requested size. 241 ** 242 ** Note: This routine only takes buffers from public pools. 243 ** It will not use any buffers from pools 244 ** marked GKI_RESTRICTED_POOL. 245 ** 246 ** Parameters size - (input) number of bytes needed. 247 ** 248 ** Returns A pointer to the buffer, or NULL if none available 249 ** 250 *******************************************************************************/ 251 void* GKI_getbuf(uint16_t size) 252 { 253 uint8_t i; 254 FREE_QUEUE_T* Q; 255 BUFFER_HDR_T* p_hdr; 256 tGKI_COM_CB* p_cb = &gki_cb.com; 257 258 if (size == 0) { 259 GKI_exception(GKI_ERROR_BUF_SIZE_ZERO, "getbuf: Size is zero"); 260 return (NULL); 261 } 262 263 /* Find the first buffer pool that is public that can hold the desired size */ 264 for (i = 0; i < p_cb->curr_total_no_of_pools; i++) { 265 if (size <= p_cb->freeq[p_cb->pool_list[i]].size) break; 266 } 267 268 if (i == p_cb->curr_total_no_of_pools) { 269 GKI_exception(GKI_ERROR_BUF_SIZE_TOOBIG, "getbuf: Size is too big"); 270 return (NULL); 271 } 272 273 /* Make sure the buffers aren't disturbed til finished with allocation */ 274 GKI_disable(); 275 276 /* search the public buffer pools that are big enough to hold the size 277 * until a free buffer is found */ 278 for (; i < p_cb->curr_total_no_of_pools; i++) { 279 /* Only look at PUBLIC buffer pools (bypass RESTRICTED pools) */ 280 if (((uint16_t)1 << p_cb->pool_list[i]) & p_cb->pool_access_mask) continue; 281 282 Q = &p_cb->freeq[p_cb->pool_list[i]]; 283 if (Q->cur_cnt < Q->total) { 284 if (Q->p_first == 0 && gki_alloc_free_queue(i) != true) { 285 GKI_TRACE_ERROR_0("GKI_getbuf() out of buffer"); 286 GKI_enable(); 287 return NULL; 288 } 289 290 if (Q->p_first == 0) { 291 /* gki_alloc_free_queue() failed to alloc memory */ 292 GKI_TRACE_ERROR_0("GKI_getbuf() fail alloc free queue"); 293 GKI_enable(); 294 return NULL; 295 } 296 297 p_hdr = Q->p_first; 298 Q->p_first = p_hdr->p_next; 299 300 if (!Q->p_first) Q->p_last = NULL; 301 302 if (++Q->cur_cnt > Q->max_cnt) Q->max_cnt = Q->cur_cnt; 303 304 GKI_enable(); 305 306 p_hdr->task_id = GKI_get_taskid(); 307 308 p_hdr->status = BUF_STATUS_UNLINKED; 309 p_hdr->p_next = NULL; 310 p_hdr->Type = 0; 311 return ((void*)((uint8_t*)p_hdr + BUFFER_HDR_SIZE)); 312 } 313 } 314 315 GKI_TRACE_ERROR_0("GKI_getbuf() unable to allocate buffer!!!!!"); 316 317 GKI_enable(); 318 319 return (NULL); 320 } 321 322 /******************************************************************************* 323 ** 324 ** Function GKI_getpoolbuf 325 ** 326 ** Description Called by an application to get a free buffer from 327 ** a specific buffer pool. 328 ** 329 ** Note: If there are no more buffers available from the pool, 330 ** the public buffers are searched for an available 331 ** buffer. 332 ** 333 ** Parameters pool_id - (input) pool ID to get a buffer out of. 334 ** 335 ** Returns A pointer to the buffer, or NULL if none available 336 ** 337 *******************************************************************************/ 338 void* GKI_getpoolbuf(uint8_t pool_id) 339 { 340 FREE_QUEUE_T* Q; 341 BUFFER_HDR_T* p_hdr; 342 tGKI_COM_CB* p_cb = &gki_cb.com; 343 344 if (pool_id >= GKI_NUM_TOTAL_BUF_POOLS) return (NULL); 345 346 /* Make sure the buffers aren't disturbed til finished with allocation */ 347 GKI_disable(); 348 349 Q = &p_cb->freeq[pool_id]; 350 if (Q->cur_cnt < Q->total) { 351 if (Q->p_first == 0 && gki_alloc_free_queue(pool_id) != true) return NULL; 352 353 if (Q->p_first == 0) { 354 /* gki_alloc_free_queue() failed to alloc memory */ 355 GKI_TRACE_ERROR_0("GKI_getpoolbuf() fail alloc free queue"); 356 return NULL; 357 } 358 359 p_hdr = Q->p_first; 360 Q->p_first = p_hdr->p_next; 361 362 if (!Q->p_first) Q->p_last = NULL; 363 364 if (++Q->cur_cnt > Q->max_cnt) Q->max_cnt = Q->cur_cnt; 365 366 GKI_enable(); 367 368 p_hdr->task_id = GKI_get_taskid(); 369 370 p_hdr->status = BUF_STATUS_UNLINKED; 371 p_hdr->p_next = NULL; 372 p_hdr->Type = 0; 373 374 return ((void*)((uint8_t*)p_hdr + BUFFER_HDR_SIZE)); 375 } 376 377 /* If here, no buffers in the specified pool */ 378 GKI_enable(); 379 380 /* try for free buffers in public pools */ 381 return (GKI_getbuf(p_cb->freeq[pool_id].size)); 382 } 383 384 /******************************************************************************* 385 ** 386 ** Function GKI_freebuf 387 ** 388 ** Description Called by an application to return a buffer to the free 389 ** pool. 390 ** 391 ** Parameters p_buf - (input) address of the beginning of a buffer. 392 ** 393 ** Returns void 394 ** 395 *******************************************************************************/ 396 void GKI_freebuf(void* p_buf) { 397 FREE_QUEUE_T* Q; 398 BUFFER_HDR_T* p_hdr; 399 400 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE) 401 if (!p_buf || gki_chk_buf_damage(p_buf)) { 402 GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Free - Buf Corrupted"); 403 return; 404 } 405 #endif 406 407 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 408 409 if (p_hdr->status != BUF_STATUS_UNLINKED) { 410 GKI_exception(GKI_ERROR_FREEBUF_BUF_LINKED, "Freeing Linked Buf"); 411 return; 412 } 413 414 if (p_hdr->q_id >= GKI_NUM_TOTAL_BUF_POOLS) { 415 GKI_exception(GKI_ERROR_FREEBUF_BAD_QID, "Bad Buf QId"); 416 return; 417 } 418 419 GKI_disable(); 420 421 /* 422 ** Release the buffer 423 */ 424 Q = &gki_cb.com.freeq[p_hdr->q_id]; 425 if (Q->p_last) 426 Q->p_last->p_next = p_hdr; 427 else 428 Q->p_first = p_hdr; 429 430 Q->p_last = p_hdr; 431 p_hdr->p_next = NULL; 432 p_hdr->status = BUF_STATUS_FREE; 433 p_hdr->task_id = GKI_INVALID_TASK; 434 if (Q->cur_cnt > 0) Q->cur_cnt--; 435 436 GKI_enable(); 437 438 return; 439 } 440 441 /******************************************************************************* 442 ** 443 ** Function GKI_get_buf_size 444 ** 445 ** Description Called by an application to get the size of a buffer. 446 ** 447 ** Parameters p_buf - (input) address of the beginning of a buffer. 448 ** 449 ** Returns the size of the buffer 450 ** 451 *******************************************************************************/ 452 uint16_t GKI_get_buf_size(void* p_buf) { 453 BUFFER_HDR_T* p_hdr; 454 455 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 456 457 if ((uint32_t)p_hdr & 1) return (0); 458 459 if (p_hdr->q_id < GKI_NUM_TOTAL_BUF_POOLS) { 460 return (gki_cb.com.freeq[p_hdr->q_id].size); 461 } 462 463 return (0); 464 } 465 466 /******************************************************************************* 467 ** 468 ** Function gki_chk_buf_damage 469 ** 470 ** Description Called internally by OSS to check for buffer corruption. 471 ** 472 ** Returns TRUE if there is a problem, else FALSE 473 ** 474 *******************************************************************************/ 475 bool gki_chk_buf_damage(void* p_buf) { 476 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE) 477 478 uint32_t* magic; 479 magic = (uint32_t*)((uint8_t*)p_buf + GKI_get_buf_size(p_buf)); 480 481 if ((uint32_t)magic & 1) return true; 482 483 if (*magic == MAGIC_NO) return false; 484 485 return true; 486 487 #else 488 489 return false; 490 491 #endif 492 } 493 494 /******************************************************************************* 495 ** 496 ** Function GKI_send_msg 497 ** 498 ** Description Called by applications to send a buffer to a task 499 ** 500 ** Returns Nothing 501 ** 502 *******************************************************************************/ 503 void GKI_send_msg(uint8_t task_id, uint8_t mbox, void* msg) { 504 BUFFER_HDR_T* p_hdr; 505 tGKI_COM_CB* p_cb = &gki_cb.com; 506 507 /* If task non-existant or not started, drop buffer */ 508 if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX) || 509 (p_cb->OSRdyTbl[task_id] == TASK_DEAD)) { 510 GKI_exception(GKI_ERROR_SEND_MSG_BAD_DEST, "Sending to unknown dest"); 511 GKI_freebuf(msg); 512 return; 513 } 514 515 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE) 516 if (gki_chk_buf_damage(msg)) { 517 GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Send - Buffer corrupted"); 518 return; 519 } 520 #endif 521 522 p_hdr = (BUFFER_HDR_T*)((uint8_t*)msg - BUFFER_HDR_SIZE); 523 524 if (p_hdr->status != BUF_STATUS_UNLINKED) { 525 GKI_exception(GKI_ERROR_SEND_MSG_BUF_LINKED, "Send - buffer linked"); 526 return; 527 } 528 529 GKI_disable(); 530 531 if (p_cb->OSTaskQFirst[task_id][mbox]) 532 p_cb->OSTaskQLast[task_id][mbox]->p_next = p_hdr; 533 else 534 p_cb->OSTaskQFirst[task_id][mbox] = p_hdr; 535 536 p_cb->OSTaskQLast[task_id][mbox] = p_hdr; 537 538 p_hdr->p_next = NULL; 539 p_hdr->status = BUF_STATUS_QUEUED; 540 p_hdr->task_id = task_id; 541 542 GKI_enable(); 543 544 GKI_send_event(task_id, (uint16_t)EVENT_MASK(mbox)); 545 546 return; 547 } 548 549 /******************************************************************************* 550 ** 551 ** Function GKI_read_mbox 552 ** 553 ** Description Called by applications to read a buffer from one of 554 ** the task mailboxes. A task can only read its own mailbox. 555 ** 556 ** Parameters: mbox - (input) mailbox ID to read (0, 1, 2, or 3) 557 ** 558 ** Returns NULL if the mailbox was empty, else the address of a buffer 559 ** 560 *******************************************************************************/ 561 void* GKI_read_mbox(uint8_t mbox) { 562 uint8_t task_id = GKI_get_taskid(); 563 void* p_buf = NULL; 564 BUFFER_HDR_T* p_hdr; 565 566 if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX)) return (NULL); 567 568 GKI_disable(); 569 570 if (gki_cb.com.OSTaskQFirst[task_id][mbox]) { 571 p_hdr = gki_cb.com.OSTaskQFirst[task_id][mbox]; 572 gki_cb.com.OSTaskQFirst[task_id][mbox] = p_hdr->p_next; 573 574 p_hdr->p_next = NULL; 575 p_hdr->status = BUF_STATUS_UNLINKED; 576 577 p_buf = (uint8_t*)p_hdr + BUFFER_HDR_SIZE; 578 } 579 580 GKI_enable(); 581 582 return (p_buf); 583 } 584 585 /******************************************************************************* 586 ** 587 ** Function GKI_enqueue 588 ** 589 ** Description Enqueue a buffer at the tail of the queue 590 ** 591 ** Parameters: p_q - (input) pointer to a queue. 592 ** p_buf - (input) address of the buffer to enqueue 593 ** 594 ** Returns void 595 ** 596 *******************************************************************************/ 597 void GKI_enqueue(BUFFER_Q* p_q, void* p_buf) { 598 BUFFER_HDR_T* p_hdr; 599 600 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE) 601 if (gki_chk_buf_damage(p_buf)) { 602 GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Enqueue - Buffer corrupted"); 603 return; 604 } 605 #endif 606 607 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 608 609 if (p_hdr->status != BUF_STATUS_UNLINKED) { 610 GKI_exception(GKI_ERROR_ENQUEUE_BUF_LINKED, "Eneueue - buf already linked"); 611 return; 612 } 613 614 GKI_disable(); 615 616 /* Since the queue is exposed (C vs C++), keep the pointers in exposed format 617 */ 618 if (p_q->p_first) { 619 BUFFER_HDR_T* p_last_hdr = 620 (BUFFER_HDR_T*)((uint8_t*)p_q->p_last - BUFFER_HDR_SIZE); 621 p_last_hdr->p_next = p_hdr; 622 } else 623 p_q->p_first = p_buf; 624 625 p_q->p_last = p_buf; 626 p_q->count++; 627 628 p_hdr->p_next = NULL; 629 p_hdr->status = BUF_STATUS_QUEUED; 630 631 GKI_enable(); 632 633 return; 634 } 635 636 /******************************************************************************* 637 ** 638 ** Function GKI_enqueue_head 639 ** 640 ** Description Enqueue a buffer at the head of the queue 641 ** 642 ** Parameters: p_q - (input) pointer to a queue. 643 ** p_buf - (input) address of the buffer to enqueue 644 ** 645 ** Returns void 646 ** 647 *******************************************************************************/ 648 void GKI_enqueue_head(BUFFER_Q* p_q, void* p_buf) { 649 BUFFER_HDR_T* p_hdr; 650 651 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE) 652 if (gki_chk_buf_damage(p_buf)) { 653 GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Enqueue - Buffer corrupted"); 654 return; 655 } 656 #endif 657 658 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 659 660 if (p_hdr->status != BUF_STATUS_UNLINKED) { 661 GKI_exception(GKI_ERROR_ENQUEUE_BUF_LINKED, 662 "Eneueue head - buf already linked"); 663 return; 664 } 665 666 GKI_disable(); 667 668 if (p_q->p_first) { 669 p_hdr->p_next = (BUFFER_HDR_T*)((uint8_t*)p_q->p_first - BUFFER_HDR_SIZE); 670 p_q->p_first = p_buf; 671 } else { 672 p_q->p_first = p_buf; 673 p_q->p_last = p_buf; 674 p_hdr->p_next = NULL; 675 } 676 p_q->count++; 677 678 p_hdr->status = BUF_STATUS_QUEUED; 679 680 GKI_enable(); 681 682 return; 683 } 684 685 /******************************************************************************* 686 ** 687 ** Function GKI_dequeue 688 ** 689 ** Description Dequeues a buffer from the head of a queue 690 ** 691 ** Parameters: p_q - (input) pointer to a queue. 692 ** 693 ** Returns NULL if queue is empty, else buffer 694 ** 695 *******************************************************************************/ 696 void* GKI_dequeue(BUFFER_Q* p_q) { 697 BUFFER_HDR_T* p_hdr; 698 699 GKI_disable(); 700 701 if (!p_q || !p_q->count) { 702 GKI_enable(); 703 return (NULL); 704 } 705 706 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_q->p_first - BUFFER_HDR_SIZE); 707 708 /* Keep buffers such that GKI header is invisible 709 */ 710 if (p_hdr->p_next) 711 p_q->p_first = ((uint8_t*)p_hdr->p_next + BUFFER_HDR_SIZE); 712 else { 713 p_q->p_first = NULL; 714 p_q->p_last = NULL; 715 } 716 717 p_q->count--; 718 719 p_hdr->p_next = NULL; 720 p_hdr->status = BUF_STATUS_UNLINKED; 721 722 GKI_enable(); 723 724 return ((uint8_t*)p_hdr + BUFFER_HDR_SIZE); 725 } 726 727 /******************************************************************************* 728 ** 729 ** Function GKI_remove_from_queue 730 ** 731 ** Description Dequeue a buffer from the middle of the queue 732 ** 733 ** Parameters: p_q - (input) pointer to a queue. 734 ** p_buf - (input) address of the buffer to enqueue 735 ** 736 ** Returns NULL if queue is empty, else buffer 737 ** 738 *******************************************************************************/ 739 void* GKI_remove_from_queue(BUFFER_Q* p_q, void* p_buf) { 740 BUFFER_HDR_T* p_prev; 741 BUFFER_HDR_T* p_buf_hdr; 742 743 GKI_disable(); 744 745 if (p_buf == p_q->p_first) { 746 GKI_enable(); 747 return (GKI_dequeue(p_q)); 748 } 749 750 p_buf_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 751 p_prev = (BUFFER_HDR_T*)((uint8_t*)p_q->p_first - BUFFER_HDR_SIZE); 752 753 for (; p_prev; p_prev = p_prev->p_next) { 754 /* If the previous points to this one, move the pointers around */ 755 if (p_prev->p_next == p_buf_hdr) { 756 p_prev->p_next = p_buf_hdr->p_next; 757 758 /* If we are removing the last guy in the queue, update p_last */ 759 if (p_buf == p_q->p_last) p_q->p_last = p_prev + 1; 760 761 /* One less in the queue */ 762 p_q->count--; 763 764 /* The buffer is now unlinked */ 765 p_buf_hdr->p_next = NULL; 766 p_buf_hdr->status = BUF_STATUS_UNLINKED; 767 768 GKI_enable(); 769 return (p_buf); 770 } 771 } 772 773 GKI_enable(); 774 return (NULL); 775 } 776 777 /******************************************************************************* 778 ** 779 ** Function GKI_getfirst 780 ** 781 ** Description Return a pointer to the first buffer in a queue 782 ** 783 ** Parameters: p_q - (input) pointer to a queue. 784 ** 785 ** Returns NULL if queue is empty, else buffer address 786 ** 787 *******************************************************************************/ 788 void* GKI_getfirst(BUFFER_Q* p_q) { return (p_q->p_first); } 789 790 /******************************************************************************* 791 ** 792 ** Function GKI_getlast 793 ** 794 ** Description Return a pointer to the last buffer in a queue 795 ** 796 ** Parameters: p_q - (input) pointer to a queue. 797 ** 798 ** Returns NULL if queue is empty, else buffer address 799 ** 800 *******************************************************************************/ 801 void* GKI_getlast(BUFFER_Q* p_q) { return (p_q->p_last); } 802 803 /******************************************************************************* 804 ** 805 ** Function GKI_getnext 806 ** 807 ** Description Return a pointer to the next buffer in a queue 808 ** 809 ** Parameters: p_buf - (input) pointer to the buffer to find the next one 810 ** from. 811 ** 812 ** Returns NULL if no more buffers in the queue, else next buffer 813 ** address 814 ** 815 *******************************************************************************/ 816 void* GKI_getnext(void* p_buf) { 817 BUFFER_HDR_T* p_hdr; 818 819 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 820 821 if (p_hdr->p_next) 822 return ((uint8_t*)p_hdr->p_next + BUFFER_HDR_SIZE); 823 else 824 return (NULL); 825 } 826 827 /******************************************************************************* 828 ** 829 ** Function GKI_queue_is_empty 830 ** 831 ** Description Check the status of a queue. 832 ** 833 ** Parameters: p_q - (input) pointer to a queue. 834 ** 835 ** Returns TRUE if queue is empty, else FALSE 836 ** 837 *******************************************************************************/ 838 bool GKI_queue_is_empty(BUFFER_Q* p_q) { return ((bool)(p_q->count == 0)); } 839 840 /******************************************************************************* 841 ** 842 ** Function GKI_find_buf_start 843 ** 844 ** Description This function is called with an address inside a buffer, 845 ** and returns the start address ofthe buffer. 846 ** 847 ** The buffer should be one allocated from one of GKI's pools. 848 ** 849 ** Parameters: p_user_area - (input) address of anywhere in a GKI buffer. 850 ** 851 ** Returns void * - Address of the beginning of the specified buffer if 852 ** successful, otherwise NULL if unsuccessful 853 ** 854 *******************************************************************************/ 855 void* GKI_find_buf_start(void* p_user_area) { 856 uint16_t xx, size; 857 uint32_t yy; 858 tGKI_COM_CB* p_cb = &gki_cb.com; 859 uint8_t* p_ua = (uint8_t*)p_user_area; 860 861 for (xx = 0; xx < GKI_NUM_TOTAL_BUF_POOLS; xx++) { 862 if ((p_ua > p_cb->pool_start[xx]) && (p_ua < p_cb->pool_end[xx])) { 863 yy = (uint32_t)(p_ua - p_cb->pool_start[xx]); 864 865 size = p_cb->pool_size[xx]; 866 867 yy = (yy / size) * size; 868 869 return ((void*)(p_cb->pool_start[xx] + yy + sizeof(BUFFER_HDR_T))); 870 } 871 } 872 873 /* If here, invalid address - not in one of our buffers */ 874 GKI_exception(GKI_ERROR_BUF_SIZE_ZERO, "GKI_get_buf_start:: bad addr"); 875 876 return (NULL); 877 } 878 879 /******************************************************** 880 * The following functions are not needed for light stack 881 *********************************************************/ 882 #ifndef BTU_STACK_LITE_ENABLED 883 #define BTU_STACK_LITE_ENABLED FALSE 884 #endif 885 886 #if (BTU_STACK_LITE_ENABLED == FALSE) 887 888 /******************************************************************************* 889 ** 890 ** Function GKI_set_pool_permission 891 ** 892 ** Description This function is called to set or change the permissions for 893 ** the specified pool ID. 894 ** 895 ** Parameters pool_id - (input) pool ID to be set or changed 896 ** permission - (input) GKI_PUBLIC_POOL or GKI_RESTRICTED_POOL 897 ** 898 ** Returns GKI_SUCCESS if successful 899 ** GKI_INVALID_POOL if unsuccessful 900 ** 901 *******************************************************************************/ 902 uint8_t GKI_set_pool_permission(uint8_t pool_id, uint8_t permission) { 903 tGKI_COM_CB* p_cb = &gki_cb.com; 904 905 if (pool_id < GKI_NUM_TOTAL_BUF_POOLS) { 906 if (permission == GKI_RESTRICTED_POOL) 907 p_cb->pool_access_mask = 908 (uint16_t)(p_cb->pool_access_mask | (1 << pool_id)); 909 910 else /* mark the pool as public */ 911 p_cb->pool_access_mask = 912 (uint16_t)(p_cb->pool_access_mask & ~(1 << pool_id)); 913 914 return (GKI_SUCCESS); 915 } else 916 return (GKI_INVALID_POOL); 917 } 918 919 /******************************************************************************* 920 ** 921 ** Function gki_add_to_pool_list 922 ** 923 ** Description Adds pool to the pool list which is arranged in the 924 ** order of size 925 ** 926 ** Returns void 927 ** 928 *******************************************************************************/ 929 static void gki_add_to_pool_list(uint8_t pool_id) { 930 int32_t i, j; 931 tGKI_COM_CB* p_cb = &gki_cb.com; 932 933 /* Find the position where the specified pool should be inserted into the list 934 */ 935 for (i = 0; i < p_cb->curr_total_no_of_pools; i++) { 936 if (p_cb->freeq[pool_id].size <= p_cb->freeq[p_cb->pool_list[i]].size) 937 break; 938 } 939 940 /* Insert the new buffer pool ID into the list of pools */ 941 for (j = p_cb->curr_total_no_of_pools; j > i; j--) { 942 p_cb->pool_list[j] = p_cb->pool_list[j - 1]; 943 } 944 945 p_cb->pool_list[i] = pool_id; 946 947 return; 948 } 949 950 /******************************************************************************* 951 ** 952 ** Function gki_remove_from_pool_list 953 ** 954 ** Description Removes pool from the pool list. Called when a pool is 955 ** deleted 956 ** 957 ** Returns void 958 ** 959 *******************************************************************************/ 960 static void gki_remove_from_pool_list(uint8_t pool_id) { 961 tGKI_COM_CB* p_cb = &gki_cb.com; 962 uint8_t i; 963 964 for (i = 0; i < p_cb->curr_total_no_of_pools; i++) { 965 if (pool_id == p_cb->pool_list[i]) break; 966 } 967 968 while (i < (p_cb->curr_total_no_of_pools - 1)) { 969 p_cb->pool_list[i] = p_cb->pool_list[i + 1]; 970 i++; 971 } 972 973 return; 974 } 975 976 /******************************************************************************* 977 ** 978 ** Function GKI_igetpoolbuf 979 ** 980 ** Description Called by an interrupt service routine to get a free buffer 981 ** from a specific buffer pool. 982 ** 983 ** Parameters pool_id - (input) pool ID to get a buffer out of. 984 ** 985 ** Returns A pointer to the buffer, or NULL if none available 986 ** 987 *******************************************************************************/ 988 void* GKI_igetpoolbuf(uint8_t pool_id) { 989 FREE_QUEUE_T* Q; 990 BUFFER_HDR_T* p_hdr; 991 992 if (pool_id >= GKI_NUM_TOTAL_BUF_POOLS) return (NULL); 993 994 Q = &gki_cb.com.freeq[pool_id]; 995 if (Q->cur_cnt < Q->total) { 996 p_hdr = Q->p_first; 997 Q->p_first = p_hdr->p_next; 998 999 if (!Q->p_first) Q->p_last = NULL; 1000 1001 if (++Q->cur_cnt > Q->max_cnt) Q->max_cnt = Q->cur_cnt; 1002 1003 p_hdr->task_id = GKI_get_taskid(); 1004 1005 p_hdr->status = BUF_STATUS_UNLINKED; 1006 p_hdr->p_next = NULL; 1007 p_hdr->Type = 0; 1008 1009 return ((void*)((uint8_t*)p_hdr + BUFFER_HDR_SIZE)); 1010 } 1011 1012 return (NULL); 1013 } 1014 1015 /******************************************************************************* 1016 ** 1017 ** Function GKI_poolcount 1018 ** 1019 ** Description Called by an application to get the total number of buffers 1020 ** in the specified buffer pool. 1021 ** 1022 ** Parameters pool_id - (input) pool ID to get the free count of. 1023 ** 1024 ** Returns the total number of buffers in the pool 1025 ** 1026 *******************************************************************************/ 1027 uint16_t GKI_poolcount(uint8_t pool_id) { 1028 if (pool_id >= GKI_NUM_TOTAL_BUF_POOLS) return (0); 1029 1030 return (gki_cb.com.freeq[pool_id].total); 1031 } 1032 1033 /******************************************************************************* 1034 ** 1035 ** Function GKI_poolfreecount 1036 ** 1037 ** Description Called by an application to get the number of free buffers 1038 ** in the specified buffer pool. 1039 ** 1040 ** Parameters pool_id - (input) pool ID to get the free count of. 1041 ** 1042 ** Returns the number of free buffers in the pool 1043 ** 1044 *******************************************************************************/ 1045 uint16_t GKI_poolfreecount(uint8_t pool_id) { 1046 FREE_QUEUE_T* Q; 1047 1048 if (pool_id >= GKI_NUM_TOTAL_BUF_POOLS) return (0); 1049 1050 Q = &gki_cb.com.freeq[pool_id]; 1051 1052 return ((uint16_t)(Q->total - Q->cur_cnt)); 1053 } 1054 1055 /******************************************************************************* 1056 ** 1057 ** Function GKI_change_buf_owner 1058 ** 1059 ** Description Called to change the task ownership of a buffer. 1060 ** 1061 ** Parameters: p_buf - (input) pointer to the buffer 1062 ** task_id - (input) task id to change ownership to 1063 ** 1064 ** Returns void 1065 ** 1066 *******************************************************************************/ 1067 void GKI_change_buf_owner(void* p_buf, uint8_t task_id) { 1068 BUFFER_HDR_T* p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE); 1069 1070 p_hdr->task_id = task_id; 1071 1072 return; 1073 } 1074 1075 #if (GKI_SEND_MSG_FROM_ISR == TRUE) 1076 /******************************************************************************* 1077 ** 1078 ** Function GKI_isend_msg 1079 ** 1080 ** Description Called from interrupt context to send a buffer to a task 1081 ** 1082 ** Returns Nothing 1083 ** 1084 *******************************************************************************/ 1085 void GKI_isend_msg(uint8_t task_id, uint8_t mbox, void* msg) { 1086 BUFFER_HDR_T* p_hdr; 1087 tGKI_COM_CB* p_cb = &gki_cb.com; 1088 1089 /* If task non-existant or not started, drop buffer */ 1090 if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX) || 1091 (p_cb->OSRdyTbl[task_id] == TASK_DEAD)) { 1092 GKI_exception(GKI_ERROR_SEND_MSG_BAD_DEST, "Sending to unknown dest"); 1093 GKI_freebuf(msg); 1094 return; 1095 } 1096 1097 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE) 1098 if (gki_chk_buf_damage(msg)) { 1099 GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Send - Buffer corrupted"); 1100 return; 1101 } 1102 #endif 1103 1104 #if (GKI_ENABLE_OWNER_CHECK == TRUE) 1105 if (gki_chk_buf_owner(msg)) { 1106 GKI_exception(GKI_ERROR_NOT_BUF_OWNER, "Send by non-owner"); 1107 return; 1108 } 1109 #endif 1110 1111 p_hdr = (BUFFER_HDR_T*)((uint8_t*)msg - BUFFER_HDR_SIZE); 1112 1113 if (p_hdr->status != BUF_STATUS_UNLINKED) { 1114 GKI_exception(GKI_ERROR_SEND_MSG_BUF_LINKED, "Send - buffer linked"); 1115 return; 1116 } 1117 1118 if (p_cb->OSTaskQFirst[task_id][mbox]) 1119 p_cb->OSTaskQLast[task_id][mbox]->p_next = p_hdr; 1120 else 1121 p_cb->OSTaskQFirst[task_id][mbox] = p_hdr; 1122 1123 p_cb->OSTaskQLast[task_id][mbox] = p_hdr; 1124 1125 p_hdr->p_next = NULL; 1126 p_hdr->status = BUF_STATUS_QUEUED; 1127 p_hdr->task_id = task_id; 1128 1129 GKI_isend_event(task_id, (uint16_t)EVENT_MASK(mbox)); 1130 1131 return; 1132 } 1133 #endif 1134 1135 /******************************************************************************* 1136 ** 1137 ** Function GKI_create_pool 1138 ** 1139 ** Description Called by applications to create a buffer pool. 1140 ** 1141 ** Parameters: size - (input) length (in bytes) of each buffer in the pool 1142 ** count - (input) number of buffers to allocate for the pool 1143 ** permission - (input) restricted or public access? 1144 ** (GKI_PUBLIC_POOL or GKI_RESTRICTED_POOL) 1145 ** p_mem_pool - (input) pointer to an OS memory pool, NULL if 1146 ** not provided 1147 ** 1148 ** Returns the buffer pool ID, which should be used in calls to 1149 ** GKI_getpoolbuf(). If a pool could not be created, this 1150 ** function returns 0xff. 1151 ** 1152 *******************************************************************************/ 1153 uint8_t GKI_create_pool(uint16_t size, uint16_t count, uint8_t permission, 1154 void* p_mem_pool) { 1155 uint8_t xx; 1156 uint32_t mem_needed; 1157 int32_t tempsize = size; 1158 tGKI_COM_CB* p_cb = &gki_cb.com; 1159 1160 /* First make sure the size of each pool has a valid size with room for the 1161 * header info */ 1162 if (size > MAX_USER_BUF_SIZE) return (GKI_INVALID_POOL); 1163 1164 /* First, look for an unused pool */ 1165 for (xx = 0; xx < GKI_NUM_TOTAL_BUF_POOLS; xx++) { 1166 if (!p_cb->pool_start[xx]) break; 1167 } 1168 1169 if (xx == GKI_NUM_TOTAL_BUF_POOLS) return (GKI_INVALID_POOL); 1170 1171 /* Ensure an even number of longwords */ 1172 tempsize = (int32_t)ALIGN_POOL(size); 1173 1174 mem_needed = (tempsize + BUFFER_PADDING_SIZE) * count; 1175 1176 if (!p_mem_pool) p_mem_pool = GKI_os_malloc(mem_needed); 1177 1178 if (p_mem_pool) { 1179 /* Initialize the new pool */ 1180 gki_init_free_queue(xx, size, count, p_mem_pool); 1181 gki_add_to_pool_list(xx); 1182 (void)GKI_set_pool_permission(xx, permission); 1183 p_cb->curr_total_no_of_pools++; 1184 1185 return (xx); 1186 } else 1187 return (GKI_INVALID_POOL); 1188 } 1189 1190 /******************************************************************************* 1191 ** 1192 ** Function GKI_delete_pool 1193 ** 1194 ** Description Called by applications to delete a buffer pool. The 1195 ** function calls the operating specific function to free the 1196 ** actual memory. An exception is generated if an error is 1197 ** detected. 1198 ** 1199 ** Parameters: pool_id - (input) Id of the poll being deleted. 1200 ** 1201 ** Returns void 1202 ** 1203 *******************************************************************************/ 1204 void GKI_delete_pool(uint8_t pool_id) { 1205 FREE_QUEUE_T* Q; 1206 tGKI_COM_CB* p_cb = &gki_cb.com; 1207 1208 if ((pool_id >= GKI_NUM_TOTAL_BUF_POOLS) || (!p_cb->pool_start[pool_id])) 1209 return; 1210 1211 GKI_disable(); 1212 Q = &p_cb->freeq[pool_id]; 1213 1214 if (!Q->cur_cnt) { 1215 Q->size = 0; 1216 Q->total = 0; 1217 Q->cur_cnt = 0; 1218 Q->max_cnt = 0; 1219 Q->p_first = NULL; 1220 Q->p_last = NULL; 1221 1222 GKI_os_free(p_cb->pool_start[pool_id]); 1223 1224 p_cb->pool_start[pool_id] = NULL; 1225 p_cb->pool_end[pool_id] = NULL; 1226 p_cb->pool_size[pool_id] = 0; 1227 1228 gki_remove_from_pool_list(pool_id); 1229 p_cb->curr_total_no_of_pools--; 1230 } else 1231 GKI_exception(GKI_ERROR_DELETE_POOL_BAD_QID, "Deleting bad pool"); 1232 1233 GKI_enable(); 1234 1235 return; 1236 } 1237 1238 #endif /* BTU_STACK_LITE_ENABLED == FALSE */ 1239 1240 /******************************************************************************* 1241 ** 1242 ** Function GKI_get_pool_bufsize 1243 ** 1244 ** Description Called by an application to get the size of buffers in a 1245 ** pool 1246 ** 1247 ** Parameters Pool ID. 1248 ** 1249 ** Returns the size of buffers in the pool 1250 ** 1251 *******************************************************************************/ 1252 uint16_t GKI_get_pool_bufsize(uint8_t pool_id) { 1253 if (pool_id < GKI_NUM_TOTAL_BUF_POOLS) 1254 return (gki_cb.com.freeq[pool_id].size); 1255 1256 return (0); 1257 } 1258 1259 /******************************************************************************* 1260 ** 1261 ** Function GKI_poolutilization 1262 ** 1263 ** Description Called by an application to get the buffer utilization 1264 ** in the specified buffer pool. 1265 ** 1266 ** Parameters pool_id - (input) pool ID to get the free count of. 1267 ** 1268 ** Returns % of buffers used from 0 to 100 1269 ** 1270 *******************************************************************************/ 1271 uint16_t GKI_poolutilization(uint8_t pool_id) { 1272 FREE_QUEUE_T* Q; 1273 1274 if (pool_id >= GKI_NUM_TOTAL_BUF_POOLS) return (100); 1275 1276 Q = &gki_cb.com.freeq[pool_id]; 1277 1278 if (Q->total == 0) return (100); 1279 1280 return ((Q->cur_cnt * 100) / Q->total); 1281 } 1282