1 /******************************************************************************
2 *
3 * Copyright (C) 2009-2018 Realtek 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 /******************************************************************************
19 *
20 * Module Name:
21 * bt_skbuff.c
22 *
23 * Abstract:
24 * Data buffer managerment through whole bluetooth stack.
25 *
26 * Major Change History:
27 * When Who What
28 * --------------------------------------------------------------
29 * 2010-06-11 W.Bi Created.
30 *
31 * Notes:
32 * To reduce memory copy when pass data buffer to other layers,
33 * RTK_BUFFER is designed referring to linux socket buffer.
34 * But I still wonder its effect, since RTK_BUFFER is much bigger
35 * than original data buffer.RTK_BUFFER will reduce its member if
36 * it would not reach what i had expected.
37 *
38 ******************************************************************************/
39
40 #define LOG_TAG "bt_h5"
41 #undef NDEBUG
42 #include <utils/Log.h>
43 #include <stdlib.h>
44 #include <fcntl.h>
45
46 #include <termios.h>
47 #include <errno.h>
48 #include <pthread.h>
49
50 #include "bt_list.h"
51 #include "bt_skbuff.h"
52 #include "string.h"
53 #include "hci_h5_int.h"
54 #define IN
55 #define OUT
56
57 //****************************************************************************
58 // CONSTANT DEFINITION
59 //****************************************************************************
60 /// default header size
61 /// l2cap header(8)+hci acl(4)
62 #define DEFAULT_HEADER_SIZE (8 + 4)
63
64 // RTK_BUFFER data buffer alignment
65 #define RTB_ALIGN 4
66
67 // do alignment with RTB_ALIGN
68 #define RTB_DATA_ALIGN(_Length) ((_Length + (RTB_ALIGN - 1)) & (~(RTB_ALIGN - 1)))
69
70 //****************************************************************************
71 // STRUCTURE DEFINITION
72 //****************************************************************************
73 typedef struct _RTB_QUEUE_HEAD
74 {
75 RT_LIST_HEAD List;
76 uint32_t QueueLen;
77 pthread_mutex_t Lock;
78 uint8_t Id[RTB_QUEUE_ID_LENGTH];
79 } * PRTB_QUEUE_HEAD;
80
81 //****************************************************************************
82 // FUNCTION
83 //****************************************************************************
84 /**
85 check whether queue is empty
86 \return : FALSE Queue is not empty
87 TRU Queue is empty
88 */
RtbQueueIsEmpty(IN RTB_QUEUE_HEAD * RtkQueueHead)89 bool RtbQueueIsEmpty(IN RTB_QUEUE_HEAD *RtkQueueHead)
90 {
91 // return ListIsEmpty(&RtkQueueHead->List);
92 return RtkQueueHead->QueueLen > 0 ? false : true;
93 }
94
95 /**
96 Allocate a RTK_BUFFER with specified data length and reserved headroom.
97 If caller does not know actual headroom to reserve for further usage, specify it to zero to use default value.
98 \param [IN] Length <uint32_t> : current data buffer length to allcated
99 \param [IN] HeadRoom <uint32_t> : if caller knows reserved head space, set it; otherwise set 0 to use default value
100 \return pointer to RTK_BUFFER if succeed, null otherwise
101 */
RtbAllocate(uint32_t Length,uint32_t HeadRoom)102 RTK_BUFFER *RtbAllocate(uint32_t Length,uint32_t HeadRoom)
103 {
104 RTK_BUFFER *Rtb = NULL;
105 /// Rtb buffer length:
106 /// RTK_BUFFER 48
107 /// HeadRoom HeadRomm or 12
108 /// Length
109 /// memory size: 48 + Length + 12(default) + 8*2(header for each memory) ---> a multiple of 8
110 /// example: (48 + 8)+ (300 + 12 + 8) = 372
111 Rtb = malloc(sizeof(RTK_BUFFER));
112 if (Rtb)
113 {
114 uint32_t BufferLen = HeadRoom ? (Length + HeadRoom) : (Length + DEFAULT_HEADER_SIZE);
115 BufferLen = RTB_DATA_ALIGN(BufferLen);
116 Rtb->Head = malloc(BufferLen);
117 if (Rtb->Head)
118 {
119 Rtb->HeadRoom = HeadRoom ? HeadRoom : DEFAULT_HEADER_SIZE;
120 Rtb->Data = Rtb->Head + Rtb->HeadRoom;
121 Rtb->End = Rtb->Data;
122 Rtb->Tail = Rtb->End + Length;
123 Rtb->Length = 0;
124 ListInitializeHeader(&Rtb->List);
125 Rtb->RefCount = 1;
126 return Rtb;
127 }
128 }
129
130 if (Rtb)
131 {
132 if (Rtb->Head)
133 {
134 free(Rtb->Head);
135 }
136
137 free(Rtb);
138 }
139 return NULL;
140 }
141
142 /**
143 Free specified Rtk_buffer
144 \param [IN] RtkBuffer <RTK_BUFFER*> : buffer to free
145 */
RtbFree(RTK_BUFFER * RtkBuffer)146 void RtbFree(RTK_BUFFER *RtkBuffer)
147 {
148 if (RtkBuffer)
149 {
150 free(RtkBuffer->Head);
151 free(RtkBuffer);
152 }
153 return;
154 }
155
156 /**
157 Add a specified length protocol header to the start of data buffer hold by specified rtk_buffer.
158 This function extends used data area of the buffer at the buffer start.
159 \param [IN OUT] RtkBuffer <RTK_BUFFER*> : data buffer to add
160 \param [IN] Length <uint32_t> : header length
161 \return Pointer to the first byte of the extra data is returned
162 */
RtbAddHead(RTK_BUFFER * RtkBuffer,uint32_t Length)163 uint8_t *RtbAddHead(RTK_BUFFER *RtkBuffer,uint32_t Length)
164 {
165
166 if ((uint32_t)(RtkBuffer->Data - RtkBuffer->Head) >= Length)
167 {
168 RtkBuffer->Data -= Length;
169 RtkBuffer->Length += Length;
170 RtkBuffer->HeadRoom -= Length;
171 return RtkBuffer->Data;
172 }
173
174 return NULL;
175 }
176 /**
177 Remove a specified length data from the start of data buffer hold by specified rtk_buffer.
178 This function returns the memory to the headroom.
179 \param [IN OUT] RtkBuffer <RTK_BUFFER*> : data buffer to remove
180 \param [IN] Length <uint32_t> : header length
181 \return Pointer to the next data in the buffer is returned, usually useless
182 */
RtbRemoveHead(RTK_BUFFER * RtkBuffer,uint32_t Length)183 unsigned char RtbRemoveHead(RTK_BUFFER *RtkBuffer,uint32_t Length)
184 {
185
186 if (RtkBuffer->Length >= Length)
187 {
188 RtkBuffer->Data += Length;
189 RtkBuffer->Length -= Length;
190 RtkBuffer->HeadRoom += Length;
191 return true;
192 }
193
194 return false;
195 }
196
197 /**
198 Add a specified length protocol header to the end of data buffer hold by specified rtk_buffer.
199 This function extends used data area of the buffer at the buffer end.
200 \param [IN OUT] RtkBuffer <RTK_BUFFER*> : data buffer to add
201 \param [IN] Length <uint32_t> : header length
202 \return Pointer to the first byte of the extra data is returned
203 */
RtbAddTail(RTK_BUFFER * RtkBuffer,uint32_t Length)204 uint8_t *RtbAddTail(RTK_BUFFER *RtkBuffer,uint32_t Length)
205 {
206
207 if ((uint32_t)(RtkBuffer->Tail - RtkBuffer->End) >= Length)
208 {
209 uint8_t *Tmp = RtkBuffer->End;
210 RtkBuffer->End += Length;
211 RtkBuffer->Length += Length;
212 return Tmp;
213 }
214
215 return NULL;
216 }
217
RtbRemoveTail(IN OUT RTK_BUFFER * RtkBuffer,IN uint32_t Length)218 unsigned char RtbRemoveTail(IN OUT RTK_BUFFER *RtkBuffer,IN uint32_t Length)
219 {
220
221 if ((uint32_t)(RtkBuffer->End - RtkBuffer->Data) >= Length)
222 {
223 RtkBuffer->End -= Length;
224 RtkBuffer->Length -= Length;
225 return true;
226 }
227
228 return false;
229 }
230 //****************************************************************************
231 // RTB list manipulation
232 //****************************************************************************
233 /**
234 Initialize a rtb queue.
235 \return Initialized rtb queue if succeed, otherwise NULL
236 */
RtbQueueInit()237 RTB_QUEUE_HEAD *RtbQueueInit()
238 {
239 RTB_QUEUE_HEAD *RtbQueue = NULL;
240 int ret = 0;
241 RtbQueue = malloc(sizeof(RTB_QUEUE_HEAD));
242 if (RtbQueue)
243 {
244 ret = pthread_mutex_init(&RtbQueue->Lock, NULL);
245 if (!ret)
246 {
247 ListInitializeHeader(&RtbQueue->List);
248 RtbQueue->QueueLen = 0;
249 return RtbQueue;
250 }
251 }
252
253 // error code comes here
254 if (RtbQueue)
255 {
256 free(RtbQueue);
257 }
258 return NULL;
259 }
260
261 /**
262 Free a rtb queue.
263 \param [IN] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
264 */
RtbQueueFree(RTB_QUEUE_HEAD * RtkQueueHead)265 void RtbQueueFree(RTB_QUEUE_HEAD *RtkQueueHead)
266 {
267 if (RtkQueueHead)
268 {
269
270 RtbEmptyQueue(RtkQueueHead);
271 pthread_mutex_destroy(&RtkQueueHead->Lock);
272 free(RtkQueueHead);
273 }
274 }
275
276 /**
277 Queue specified RtkBuffer into a RtkQueue at list tail.
278 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
279 \param [IN] RtkBuffer <RTK_BUFFER*> : Rtk buffer to add
280 */
RtbQueueTail(IN OUT RTB_QUEUE_HEAD * RtkQueueHead,IN RTK_BUFFER * RtkBuffer)281 void RtbQueueTail(IN OUT RTB_QUEUE_HEAD *RtkQueueHead,IN RTK_BUFFER *RtkBuffer)
282 {
283 pthread_mutex_lock(&RtkQueueHead->Lock);
284 ListAddToTail(&RtkBuffer->List, &RtkQueueHead->List);
285 RtkQueueHead->QueueLen++;
286 pthread_mutex_unlock(&RtkQueueHead->Lock);
287 }
288
289 /**
290 Queue specified RtkBuffer into a RtkQueue at list Head.
291 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
292 \param [IN] RtkBuffer <RTK_BUFFER*> : Rtk buffer to add
293 */
RtbQueueHead(IN OUT RTB_QUEUE_HEAD * RtkQueueHead,IN RTK_BUFFER * RtkBuffer)294 void RtbQueueHead(IN OUT RTB_QUEUE_HEAD *RtkQueueHead,IN RTK_BUFFER *RtkBuffer)
295 {
296 pthread_mutex_lock(&RtkQueueHead->Lock);
297 ListAddToHead(&RtkBuffer->List, &RtkQueueHead->List);
298 RtkQueueHead->QueueLen++;
299 pthread_mutex_unlock(&RtkQueueHead->Lock);
300 }
301
302 /**
303 Insert new Rtkbuffer in the old buffer
304 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
305 \param [IN] OldRtkBuffer <RTK_BUFFER*> : old rtk buffer
306 \param [IN] NewRtkBuffer <RTK_BUFFER*> : Rtk buffer to add
307 */
RtbInsertBefore(IN OUT RTB_QUEUE_HEAD * RtkQueueHead,IN RTK_BUFFER * pOldRtkBuffer,IN RTK_BUFFER * pNewRtkBuffer)308 void RtbInsertBefore(IN OUT RTB_QUEUE_HEAD *RtkQueueHead,IN RTK_BUFFER *pOldRtkBuffer,IN RTK_BUFFER *pNewRtkBuffer)
309 {
310 pthread_mutex_lock(&RtkQueueHead->Lock);
311 ListAdd(&pNewRtkBuffer->List, pOldRtkBuffer->List.Prev, &pOldRtkBuffer->List);
312 RtkQueueHead->QueueLen++;
313 pthread_mutex_unlock(&RtkQueueHead->Lock);
314 }
315
316 /**
317 check whether the buffer is the last node in the queue
318 */
RtbNodeIsLast(IN RTB_QUEUE_HEAD * RtkQueueHead,IN RTK_BUFFER * pRtkBuffer)319 unsigned char RtbNodeIsLast(IN RTB_QUEUE_HEAD *RtkQueueHead,IN RTK_BUFFER *pRtkBuffer)
320 {
321 RTK_BUFFER *pBuf;
322 pthread_mutex_lock(&RtkQueueHead->Lock);
323
324 pBuf = (RTK_BUFFER *)RtkQueueHead->List.Prev;
325 if (pBuf == pRtkBuffer)
326 {
327 pthread_mutex_unlock(&RtkQueueHead->Lock);
328 return true;
329 }
330 pthread_mutex_unlock(&RtkQueueHead->Lock);
331 return false;
332 }
333
334 /**
335 get the next buffer node after the specified buffer in the queue
336 if the specified buffer is the last node in the queue , return NULL
337 \param [IN] RtkBuffer <RTK_BUFFER*> : Rtk Queue
338 \param [IN] RtkBuffer <RTK_BUFFER*> : Rtk buffer
339 \return node after the specified buffer
340 */
RtbQueueNextNode(IN RTB_QUEUE_HEAD * RtkQueueHead,IN RTK_BUFFER * pRtkBuffer)341 RTK_BUFFER *RtbQueueNextNode(IN RTB_QUEUE_HEAD *RtkQueueHead,IN RTK_BUFFER *pRtkBuffer)
342 {
343 RTK_BUFFER *pBuf;
344 pthread_mutex_lock(&RtkQueueHead->Lock);
345 pBuf = (RTK_BUFFER *)RtkQueueHead->List.Prev;
346 if (pBuf == pRtkBuffer)
347 {
348 pthread_mutex_unlock(&RtkQueueHead->Lock);
349 return NULL; ///< if it is already the last node in the queue , return NULL
350 }
351 pBuf = (RTK_BUFFER *)pRtkBuffer->List.Next;
352 pthread_mutex_unlock(&RtkQueueHead->Lock);
353 return pBuf; ///< return next node after this node
354 }
355
356 /**
357 Delete specified RtkBuffer from a RtkQueue.
358 It don't hold spinlock itself, so caller must hold it at someplace.
359 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
360 \param [IN] RtkBuffer <RTK_BUFFER*> : Rtk buffer to Remove
361 */
RtbRemoveNode(IN OUT RTB_QUEUE_HEAD * RtkQueueHead,IN RTK_BUFFER * RtkBuffer)362 void RtbRemoveNode(IN OUT RTB_QUEUE_HEAD *RtkQueueHead,IN RTK_BUFFER *RtkBuffer)
363 {
364 RtkQueueHead->QueueLen--;
365 ListDeleteNode(&RtkBuffer->List);
366 }
367
368 /**
369 Get the RtkBuffer which is the head of a RtkQueue
370 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
371 \return head of the RtkQueue , otherwise NULL
372 */
RtbTopQueue(IN RTB_QUEUE_HEAD * RtkQueueHead)373 RTK_BUFFER *RtbTopQueue(IN RTB_QUEUE_HEAD *RtkQueueHead)
374 {
375 RTK_BUFFER *Rtb = NULL;
376 pthread_mutex_lock(&RtkQueueHead->Lock);
377
378 if (RtbQueueIsEmpty(RtkQueueHead))
379 {
380 pthread_mutex_unlock(&RtkQueueHead->Lock);
381 return NULL;
382 }
383
384 Rtb = (RTK_BUFFER *)RtkQueueHead->List.Next;
385 pthread_mutex_unlock(&RtkQueueHead->Lock);
386
387 return Rtb;
388 }
389
390 /**
391 Remove a RtkBuffer from specified rtkqueue at list tail.
392 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
393 \return removed rtkbuffer if succeed, otherwise NULL
394 */
RtbDequeueTail(IN OUT RTB_QUEUE_HEAD * RtkQueueHead)395 RTK_BUFFER *RtbDequeueTail(IN OUT RTB_QUEUE_HEAD *RtkQueueHead)
396 {
397 RTK_BUFFER *Rtb = NULL;
398
399 pthread_mutex_lock(&RtkQueueHead->Lock);
400 if (RtbQueueIsEmpty(RtkQueueHead))
401 {
402 pthread_mutex_unlock(&RtkQueueHead->Lock);
403 return NULL;
404 }
405 Rtb = (RTK_BUFFER *)RtkQueueHead->List.Prev;
406 RtbRemoveNode(RtkQueueHead, Rtb);
407 pthread_mutex_unlock(&RtkQueueHead->Lock);
408
409 return Rtb;
410 }
411
412 /**
413 Remove a RtkBuffer from specified rtkqueue at list head.
414 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
415 \return removed rtkbuffer if succeed, otherwise NULL
416 */
RtbDequeueHead(IN OUT RTB_QUEUE_HEAD * RtkQueueHead)417 RTK_BUFFER *RtbDequeueHead(IN OUT RTB_QUEUE_HEAD *RtkQueueHead)
418 {
419 RTK_BUFFER *Rtb = NULL;
420 pthread_mutex_lock(&RtkQueueHead->Lock);
421
422 if (RtbQueueIsEmpty(RtkQueueHead))
423 {
424 pthread_mutex_unlock(&RtkQueueHead->Lock);
425 return NULL;
426 }
427 Rtb = (RTK_BUFFER *)RtkQueueHead->List.Next;
428 RtbRemoveNode(RtkQueueHead, Rtb);
429 pthread_mutex_unlock(&RtkQueueHead->Lock);
430 return Rtb;
431 }
432
433 /**
434 Get current rtb queue's length.
435 \param [IN] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
436 \return current queue's length
437 */
RtbGetQueueLen(IN RTB_QUEUE_HEAD * RtkQueueHead)438 signed long RtbGetQueueLen(IN RTB_QUEUE_HEAD *RtkQueueHead)
439 {
440 return RtkQueueHead->QueueLen;
441 }
442
443 /**
444 Empty the rtkqueue.
445 \param [IN OUT] RtkQueueHead <RTB_QUEUE_HEAD*> : Rtk Queue
446 */
RtbEmptyQueue(IN OUT RTB_QUEUE_HEAD * RtkQueueHead)447 void RtbEmptyQueue(IN OUT RTB_QUEUE_HEAD *RtkQueueHead)
448 {
449 RTK_BUFFER *Rtb = NULL;
450 pthread_mutex_lock(&RtkQueueHead->Lock);
451
452 while (!RtbQueueIsEmpty(RtkQueueHead))
453 {
454 Rtb = (RTK_BUFFER *)RtkQueueHead->List.Next;
455 RtbRemoveNode(RtkQueueHead, Rtb);
456 RtbFree(Rtb);
457 }
458
459 pthread_mutex_unlock(&RtkQueueHead->Lock);
460 return;
461 }
462
463 /// Annie_tmp
RtbCheckQueueLen(IN RTB_QUEUE_HEAD * RtkQueueHead,IN uint8_t Len)464 unsigned char RtbCheckQueueLen(IN RTB_QUEUE_HEAD *RtkQueueHead, IN uint8_t Len)
465 {
466 return RtkQueueHead->QueueLen < Len ? true : false;
467 }
468
469 /**
470 clone buffer for upper or lower layer, because original buffer should be stored in l2cap
471 \param <RTK_BUFFER* pDataBuffer: original buffer
472 \return cloned buffer
473 */
RtbCloneBuffer(IN RTK_BUFFER * pDataBuffer)474 RTK_BUFFER *RtbCloneBuffer(IN RTK_BUFFER *pDataBuffer)
475 {
476 RTK_BUFFER *pNewBuffer = NULL;
477 if (pDataBuffer)
478 {
479 pNewBuffer = RtbAllocate(pDataBuffer->Length, 0);
480 if (!pNewBuffer)
481 {
482 return NULL;
483 }
484 if (pDataBuffer && pDataBuffer->Data)
485 memcpy(pNewBuffer->Data, pDataBuffer->Data, pDataBuffer->Length);
486 else
487 {
488 RtbFree(pNewBuffer);
489 return NULL;
490 }
491
492 pNewBuffer->Length = pDataBuffer->Length;
493 }
494 return pNewBuffer;
495 }
496