1 /******************************************************************************
2 *
3 * Copyright (C) 1999-2012 Broadcom Corporation
4 * Copyright 2019 NXP
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at:
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 ******************************************************************************/
19 #include "uwb_gki.h"
20 #include "uwb_gki_common.h"
21 #include "uwb_gki_int.h"
22
23 #if (GKI_NUM_TOTAL_BUF_POOLS > 16)
24 #error Number of pools out of range (16 Max)!
25 #endif
26
27 #include "uci_log.h"
28
29 /*******************************************************************************
30 **
31 ** Function phUwb_phUwb_gki_init_free_queue
32 **
33 ** Description Internal function called at startup to initialize a free
34 ** queue. It is called once for each free queue.
35 **
36 ** Returns void
37 **
38 *******************************************************************************/
phUwb_gki_init_free_queue(uint8_t id,uint16_t size,uint16_t total,void * p_mem)39 static void phUwb_gki_init_free_queue(uint8_t id, uint16_t size, uint16_t total,
40 void* p_mem) {
41 uint16_t i;
42 uint16_t act_size;
43 BUFFER_HDR_T* hdr;
44 BUFFER_HDR_T* hdr1 = NULL;
45 uint32_t* magic;
46 int32_t tempsize = size;
47 tGKI_COM_CB* p_cb = &gki_cb.com;
48
49 /* Ensure an even number of longwords */
50 tempsize = (int32_t)ALIGN_POOL(size);
51 act_size = (uint16_t)(tempsize + BUFFER_PADDING_SIZE);
52
53 /* Remember pool start and end addresses */
54 if (p_mem) {
55 p_cb->pool_start[id] = (uint8_t*)p_mem;
56 p_cb->pool_end[id] = (uint8_t*)p_mem + (act_size * total);
57 }
58
59 p_cb->pool_size[id] = act_size;
60
61 p_cb->freeq[id].size = (uint16_t)tempsize;
62 p_cb->freeq[id].total = total;
63 p_cb->freeq[id].cur_cnt = 0;
64 p_cb->freeq[id].max_cnt = 0;
65
66 /* Initialize index table */
67 if (p_mem) {
68 hdr = (BUFFER_HDR_T*)p_mem;
69 p_cb->freeq[id].p_first = hdr;
70 for (i = 0; i < total; i++) {
71 hdr->task_id = GKI_INVALID_TASK;
72 hdr->q_id = id;
73 hdr->status = BUF_STATUS_FREE;
74 magic = (uint32_t*)((uint8_t*)hdr + BUFFER_HDR_SIZE + tempsize);
75 *magic = MAGIC_NO;
76 hdr1 = hdr;
77 hdr = (BUFFER_HDR_T*)((uint8_t*)hdr + act_size);
78 hdr1->p_next = hdr;
79 }
80 if (hdr1 != NULL) hdr = hdr1;
81 hdr->p_next = NULL;
82 p_cb->freeq[id].p_last = hdr;
83 }
84 return;
85 }
86
phUwb_gki_alloc_free_queue(uint8_t id)87 static bool phUwb_gki_alloc_free_queue(uint8_t id) {
88 FREE_QUEUE_T* Q;
89 tGKI_COM_CB* p_cb = &gki_cb.com;
90
91 Q = &p_cb->freeq[p_cb->pool_list[id]];
92
93 if (Q->p_first == 0) {
94 void* p_mem =
95 phUwb_GKI_os_malloc((Q->size + BUFFER_PADDING_SIZE) * Q->total);
96 if (p_mem) {
97 // re-initialize the queue with allocated memory
98 phUwb_gki_init_free_queue(id, Q->size, Q->total, p_mem);
99 return true;
100 }
101 phUwb_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 phUwb_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 *******************************************************************************/
phUwb_gki_buffer_init(void)117 void phUwb_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 phUwb_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 phUwb_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 phUwb_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 phUwb_gki_init_free_queue(3, GKI_BUF3_SIZE, GKI_BUF3_MAX, p_cb->bufpool3);
159 #endif
160
161 /* add pools to the pool_list which is arranged in the order of size */
162 for (i = 0; i < GKI_NUM_FIXED_BUF_POOLS; i++) {
163 p_cb->pool_list[i] = i;
164 }
165
166 p_cb->curr_total_no_of_pools = GKI_NUM_FIXED_BUF_POOLS;
167
168 return;
169 }
170
171 /*******************************************************************************
172 **
173 ** Function phUwb_GKI_init_q
174 **
175 ** Description Called by an application to initialize a buffer queue.
176 **
177 ** Returns void
178 **
179 *******************************************************************************/
phUwb_GKI_init_q(BUFFER_Q * p_q)180 void phUwb_GKI_init_q(BUFFER_Q* p_q) {
181 p_q->p_first = p_q->p_last = NULL;
182 p_q->count = 0;
183
184 return;
185 }
186
187 /*******************************************************************************
188 **
189 ** Function phUwb_GKI_getbuf
190 **
191 ** Description Called by an application to get a free buffer which
192 ** is of size greater or equal to the requested size.
193 **
194 ** Note: This routine only takes buffers from public pools.
195 ** It will not use any buffers from pools
196 ** marked GKI_RESTRICTED_POOL.
197 **
198 ** Parameters size - (input) number of bytes needed.
199 **
200 ** Returns A pointer to the buffer, or NULL if none available
201 **
202 *******************************************************************************/
phUwb_GKI_getbuf(uint16_t size)203 void* phUwb_GKI_getbuf(uint16_t size) {
204 uint8_t i;
205 FREE_QUEUE_T* Q;
206 BUFFER_HDR_T* p_hdr;
207 tGKI_COM_CB* p_cb = &gki_cb.com;
208
209 if (size == 0) {
210 phUwb_GKI_exception(GKI_ERROR_BUF_SIZE_ZERO, "getbuf: Size is zero");
211 return (NULL);
212 }
213
214 /* Find the first buffer pool that is public that can hold the desired size */
215 for (i = 0; i < p_cb->curr_total_no_of_pools; i++) {
216 if (size <= p_cb->freeq[p_cb->pool_list[i]].size) break;
217 }
218
219 if (i == p_cb->curr_total_no_of_pools) {
220 phUwb_GKI_exception(GKI_ERROR_BUF_SIZE_TOOBIG, "getbuf: Size is too big");
221 return (NULL);
222 }
223
224 /* Make sure the buffers aren't disturbed til finished with allocation */
225 phUwb_GKI_disable();
226
227 /* search the public buffer pools that are big enough to hold the size
228 * until a free buffer is found */
229 for (; i < p_cb->curr_total_no_of_pools; i++) {
230 /* Only look at PUBLIC buffer pools (bypass RESTRICTED pools) */
231 if (((uint16_t)1 << p_cb->pool_list[i]) & p_cb->pool_access_mask) continue;
232
233 Q = &p_cb->freeq[p_cb->pool_list[i]];
234 if (Q->cur_cnt < Q->total) {
235 if (Q->p_first == 0 && phUwb_gki_alloc_free_queue(i) != true) {
236 UCI_TRACE_E("out of buffer");
237 phUwb_GKI_enable();
238 return NULL;
239 }
240
241 if (Q->p_first == 0) {
242 /* phUwb_gki_alloc_free_queue() failed to alloc memory */
243 UCI_TRACE_E("fail alloc free queue");
244 phUwb_GKI_enable();
245 return NULL;
246 }
247
248 p_hdr = Q->p_first;
249 Q->p_first = p_hdr->p_next;
250
251 if (!Q->p_first) Q->p_last = NULL;
252
253 if (++Q->cur_cnt > Q->max_cnt) Q->max_cnt = Q->cur_cnt;
254
255 phUwb_GKI_enable();
256
257 p_hdr->task_id = phUwb_GKI_get_taskid();
258
259 p_hdr->status = BUF_STATUS_UNLINKED;
260 p_hdr->p_next = NULL;
261 p_hdr->Type = 0;
262 return ((void*)((uint8_t*)p_hdr + BUFFER_HDR_SIZE));
263 }
264 }
265
266 UCI_TRACE_E("unable to allocate buffer!!!!!");
267
268 phUwb_GKI_enable();
269
270 return (NULL);
271 }
272
273 /*******************************************************************************
274 **
275 ** Function phUwb_GKI_getpoolbuf
276 **
277 ** Description Called by an application to get a free buffer from
278 ** a specific buffer pool.
279 **
280 ** Note: If there are no more buffers available from the pool,
281 ** the public buffers are searched for an available
282 ** buffer.
283 **
284 ** Parameters pool_id - (input) pool ID to get a buffer out of.
285 **
286 ** Returns A pointer to the buffer, or NULL if none available
287 **
288 *******************************************************************************/
phUwb_GKI_getpoolbuf(uint8_t pool_id)289 void* phUwb_GKI_getpoolbuf(uint8_t pool_id) {
290 FREE_QUEUE_T* Q;
291 BUFFER_HDR_T* p_hdr;
292 tGKI_COM_CB* p_cb = &gki_cb.com;
293
294 if (pool_id >= GKI_NUM_TOTAL_BUF_POOLS) return (NULL);
295
296 /* Make sure the buffers aren't disturbed til finished with allocation */
297 phUwb_GKI_disable();
298
299 Q = &p_cb->freeq[pool_id];
300 if (Q->cur_cnt < Q->total) {
301 if (Q->p_first == 0 && phUwb_gki_alloc_free_queue(pool_id) != true)
302 return NULL;
303
304 if (Q->p_first == 0) {
305 /* gki_alloc_free_queue() failed to alloc memory */
306 UCI_TRACE_E("fail alloc free queue");
307 return NULL;
308 }
309
310 p_hdr = Q->p_first;
311 Q->p_first = p_hdr->p_next;
312
313 if (!Q->p_first) Q->p_last = NULL;
314
315 if (++Q->cur_cnt > Q->max_cnt) Q->max_cnt = Q->cur_cnt;
316
317 phUwb_GKI_enable();
318
319 p_hdr->task_id = phUwb_GKI_get_taskid();
320
321 p_hdr->status = BUF_STATUS_UNLINKED;
322 p_hdr->p_next = NULL;
323 p_hdr->Type = 0;
324
325 return ((void*)((uint8_t*)p_hdr + BUFFER_HDR_SIZE));
326 }
327
328 /* If here, no buffers in the specified pool */
329 phUwb_GKI_enable();
330
331 /* try for free buffers in public pools */
332 return (phUwb_GKI_getbuf(p_cb->freeq[pool_id].size));
333 }
334
335 /*******************************************************************************
336 **
337 ** Function phUwb_GKI_freebuf
338 **
339 ** Description Called by an application to return a buffer to the free
340 ** pool.
341 **
342 ** Parameters p_buf - (input) address of the beginning of a buffer.
343 **
344 ** Returns void
345 **
346 *******************************************************************************/
phUwb_GKI_freebuf(void * p_buf)347 void phUwb_GKI_freebuf(void* p_buf) {
348 FREE_QUEUE_T* Q;
349 BUFFER_HDR_T* p_hdr;
350
351 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE)
352 if (!p_buf || phUwb_gki_chk_buf_damage(p_buf)) {
353 phUwb_GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Free - Buf Corrupted");
354 return;
355 }
356 #endif
357
358 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE);
359
360 if (p_hdr->status != BUF_STATUS_UNLINKED) {
361 phUwb_GKI_exception(GKI_ERROR_FREEBUF_BUF_LINKED, "Freeing Linked Buf");
362 return;
363 }
364
365 if (p_hdr->q_id >= GKI_NUM_TOTAL_BUF_POOLS) {
366 phUwb_GKI_exception(GKI_ERROR_FREEBUF_BAD_QID, "Bad Buf QId");
367 return;
368 }
369
370 phUwb_GKI_disable();
371
372 /*
373 ** Release the buffer
374 */
375 Q = &gki_cb.com.freeq[p_hdr->q_id];
376 if (Q->p_last)
377 Q->p_last->p_next = p_hdr;
378 else
379 Q->p_first = p_hdr;
380
381 Q->p_last = p_hdr;
382 p_hdr->p_next = NULL;
383 p_hdr->status = BUF_STATUS_FREE;
384 p_hdr->task_id = GKI_INVALID_TASK;
385 if (Q->cur_cnt > 0) Q->cur_cnt--;
386
387 phUwb_GKI_enable();
388
389 return;
390 }
391
392 /*******************************************************************************
393 **
394 ** Function phUwb_GKI_get_buf_size
395 **
396 ** Description Called by an application to get the size of a buffer.
397 **
398 ** Parameters p_buf - (input) address of the beginning of a buffer.
399 **
400 ** Returns the size of the buffer
401 **
402 *******************************************************************************/
phUwb_GKI_get_buf_size(void * p_buf)403 uint16_t phUwb_GKI_get_buf_size(void* p_buf) {
404 BUFFER_HDR_T* p_hdr;
405
406 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE);
407
408 if ((uintptr_t)p_hdr & 1) return (0);
409
410 if (p_hdr->q_id < GKI_NUM_TOTAL_BUF_POOLS) {
411 return (gki_cb.com.freeq[p_hdr->q_id].size);
412 }
413
414 return (0);
415 }
416
417 /*******************************************************************************
418 **
419 ** Function phUwb_gki_chk_buf_damage
420 **
421 ** Description Called internally by OSS to check for buffer corruption.
422 **
423 ** Returns TRUE if there is a problem, else FALSE
424 **
425 *******************************************************************************/
phUwb_gki_chk_buf_damage(void * p_buf)426 bool phUwb_gki_chk_buf_damage(void* p_buf) {
427 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE)
428
429 uint32_t* magic;
430 magic = (uint32_t*)((uint8_t*)p_buf + phUwb_GKI_get_buf_size(p_buf));
431
432 if ((uintptr_t)magic & 1) return true;
433
434 if (*magic == MAGIC_NO) return false;
435
436 return true;
437
438 #else
439
440 return false;
441
442 #endif
443 }
444
445 /*******************************************************************************
446 **
447 ** Function phUwb_GKI_send_msg
448 **
449 ** Description Called by applications to send a buffer to a task
450 **
451 ** Returns Nothing
452 **
453 *******************************************************************************/
phUwb_GKI_send_msg(uint8_t task_id,uint8_t mbox,void * msg)454 void phUwb_GKI_send_msg(uint8_t task_id, uint8_t mbox, void* msg) {
455 BUFFER_HDR_T* p_hdr;
456 tGKI_COM_CB* p_cb = &gki_cb.com;
457
458 /* If task non-existant or not started, drop buffer */
459 if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX) ||
460 (p_cb->OSRdyTbl[task_id] == TASK_DEAD)) {
461 phUwb_GKI_exception(GKI_ERROR_SEND_MSG_BAD_DEST, "Sending to unknown dest");
462 phUwb_GKI_freebuf(msg);
463 return;
464 }
465
466 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE)
467 if (phUwb_gki_chk_buf_damage(msg)) {
468 phUwb_GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Send - Buffer corrupted");
469 return;
470 }
471 #endif
472
473 p_hdr = (BUFFER_HDR_T*)((uint8_t*)msg - BUFFER_HDR_SIZE);
474
475 if (p_hdr->status != BUF_STATUS_UNLINKED) {
476 phUwb_GKI_exception(GKI_ERROR_SEND_MSG_BUF_LINKED, "Send - buffer linked");
477 return;
478 }
479
480 phUwb_GKI_disable();
481
482 if (p_cb->OSTaskQFirst[task_id][mbox])
483 p_cb->OSTaskQLast[task_id][mbox]->p_next = p_hdr;
484 else
485 p_cb->OSTaskQFirst[task_id][mbox] = p_hdr;
486
487 p_cb->OSTaskQLast[task_id][mbox] = p_hdr;
488
489 p_hdr->p_next = NULL;
490 p_hdr->status = BUF_STATUS_QUEUED;
491 p_hdr->task_id = task_id;
492
493 phUwb_GKI_enable();
494
495 phUwb_GKI_send_event(task_id, (uint16_t)EVENT_MASK(mbox));
496
497 return;
498 }
499
500 /*******************************************************************************
501 **
502 ** Function phUwb_GKI_read_mbox
503 **
504 ** Description Called by applications to read a buffer from one of
505 ** the task mailboxes. A task can only read its own mailbox.
506 **
507 ** Parameters: mbox - (input) mailbox ID to read (0, 1, 2, or 3)
508 **
509 ** Returns NULL if the mailbox was empty, else the address of a buffer
510 **
511 *******************************************************************************/
phUwb_GKI_read_mbox(uint8_t mbox)512 void* phUwb_GKI_read_mbox(uint8_t mbox) {
513 uint8_t task_id = phUwb_GKI_get_taskid();
514 void* p_buf = NULL;
515 BUFFER_HDR_T* p_hdr;
516
517 if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX)) return (NULL);
518
519 phUwb_GKI_disable();
520
521 if (gki_cb.com.OSTaskQFirst[task_id][mbox]) {
522 p_hdr = gki_cb.com.OSTaskQFirst[task_id][mbox];
523 gki_cb.com.OSTaskQFirst[task_id][mbox] = p_hdr->p_next;
524
525 p_hdr->p_next = NULL;
526 p_hdr->status = BUF_STATUS_UNLINKED;
527
528 p_buf = (uint8_t*)p_hdr + BUFFER_HDR_SIZE;
529 }
530
531 phUwb_GKI_enable();
532
533 return (p_buf);
534 }
535
536 /*******************************************************************************
537 **
538 ** Function phUwb_GKI_enqueue
539 **
540 ** Description Enqueue a buffer at the tail of the queue
541 **
542 ** Parameters: p_q - (input) pointer to a queue.
543 ** p_buf - (input) address of the buffer to enqueue
544 **
545 ** Returns void
546 **
547 *******************************************************************************/
phUwb_GKI_enqueue(BUFFER_Q * p_q,void * p_buf)548 void phUwb_GKI_enqueue(BUFFER_Q* p_q, void* p_buf) {
549 BUFFER_HDR_T* p_hdr;
550
551 #if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE)
552 if (phUwb_gki_chk_buf_damage(p_buf)) {
553 phUwb_GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Enqueue - Buffer corrupted");
554 return;
555 }
556 #endif
557
558 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_buf - BUFFER_HDR_SIZE);
559
560 if (p_hdr->status != BUF_STATUS_UNLINKED) {
561 phUwb_GKI_exception(GKI_ERROR_ENQUEUE_BUF_LINKED,
562 "Eneueue - buf already linked");
563 return;
564 }
565
566 phUwb_GKI_disable();
567
568 /* Since the queue is exposed (C vs C++), keep the pointers in exposed format
569 */
570 if (p_q->p_first) {
571 BUFFER_HDR_T* p_last_hdr =
572 (BUFFER_HDR_T*)((uint8_t*)p_q->p_last - BUFFER_HDR_SIZE);
573 p_last_hdr->p_next = p_hdr;
574 } else
575 p_q->p_first = p_buf;
576
577 p_q->p_last = p_buf;
578 p_q->count++;
579
580 p_hdr->p_next = NULL;
581 p_hdr->status = BUF_STATUS_QUEUED;
582
583 phUwb_GKI_enable();
584
585 return;
586 }
587
588 /*******************************************************************************
589 **
590 ** Function phUwb_GKI_dequeue
591 **
592 ** Description Dequeues a buffer from the head of a queue
593 **
594 ** Parameters: p_q - (input) pointer to a queue.
595 **
596 ** Returns NULL if queue is empty, else buffer
597 **
598 *******************************************************************************/
phUwb_GKI_dequeue(BUFFER_Q * p_q)599 void* phUwb_GKI_dequeue(BUFFER_Q* p_q) {
600 BUFFER_HDR_T* p_hdr;
601
602 phUwb_GKI_disable();
603
604 if (!p_q || !p_q->count) {
605 phUwb_GKI_enable();
606 return (NULL);
607 }
608
609 p_hdr = (BUFFER_HDR_T*)((uint8_t*)p_q->p_first - BUFFER_HDR_SIZE);
610
611 /* Keep buffers such that GKI header is invisible
612 */
613 if (p_hdr->p_next)
614 p_q->p_first = ((uint8_t*)p_hdr->p_next + BUFFER_HDR_SIZE);
615 else {
616 p_q->p_first = NULL;
617 p_q->p_last = NULL;
618 }
619
620 p_q->count--;
621
622 p_hdr->p_next = NULL;
623 p_hdr->status = BUF_STATUS_UNLINKED;
624
625 phUwb_GKI_enable();
626
627 return ((uint8_t*)p_hdr + BUFFER_HDR_SIZE);
628 }
629
630 /********************************************************
631 * The following functions are not needed for light stack
632 *********************************************************/
633 #ifndef BTU_STACK_LITE_ENABLED
634 #define BTU_STACK_LITE_ENABLED FALSE
635 #endif
636