1 /****************************************************************************** 2 * 3 * Copyright (C) 2009-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 19 /****************************************************************************** 20 * 21 * Filename: utils.c 22 * 23 * Description: Contains helper functions 24 * 25 ******************************************************************************/ 26 27 #include <errno.h> 28 #include <pthread.h> 29 #include <time.h> 30 #include "bt_hci_bdroid.h" 31 #include "utils.h" 32 33 /****************************************************************************** 34 ** Static variables 35 ******************************************************************************/ 36 37 static pthread_mutex_t utils_mutex; 38 39 /***************************************************************************** 40 ** UTILS INTERFACE FUNCTIONS 41 *****************************************************************************/ 42 43 /******************************************************************************* 44 ** 45 ** Function utils_init 46 ** 47 ** Description Utils initialization 48 ** 49 ** Returns None 50 ** 51 *******************************************************************************/ 52 void utils_init (void) 53 { 54 pthread_mutex_init(&utils_mutex, NULL); 55 } 56 57 /******************************************************************************* 58 ** 59 ** Function utils_cleanup 60 ** 61 ** Description Utils cleanup 62 ** 63 ** Returns None 64 ** 65 *******************************************************************************/ 66 void utils_cleanup (void) 67 { 68 } 69 70 /******************************************************************************* 71 ** 72 ** Function utils_queue_init 73 ** 74 ** Description Initialize the given buffer queue 75 ** 76 ** Returns None 77 ** 78 *******************************************************************************/ 79 void utils_queue_init (BUFFER_Q *p_q) 80 { 81 p_q->p_first = p_q->p_last = NULL; 82 p_q->count = 0; 83 } 84 85 /******************************************************************************* 86 ** 87 ** Function utils_enqueue 88 ** 89 ** Description Enqueue a buffer at the tail of the given queue 90 ** 91 ** Returns None 92 ** 93 *******************************************************************************/ 94 void utils_enqueue (BUFFER_Q *p_q, void *p_buf) 95 { 96 HC_BUFFER_HDR_T *p_hdr; 97 98 p_hdr = (HC_BUFFER_HDR_T *) ((uint8_t *) p_buf - BT_HC_BUFFER_HDR_SIZE); 99 100 pthread_mutex_lock(&utils_mutex); 101 102 if (p_q->p_last) 103 { 104 HC_BUFFER_HDR_T *p_last_hdr = \ 105 (HC_BUFFER_HDR_T *)((uint8_t *)p_q->p_last - BT_HC_BUFFER_HDR_SIZE); 106 107 p_last_hdr->p_next = p_hdr; 108 } 109 else 110 p_q->p_first = p_buf; 111 112 p_q->p_last = p_buf; 113 p_q->count++; 114 115 p_hdr->p_next = NULL; 116 117 pthread_mutex_unlock(&utils_mutex); 118 } 119 /******************************************************************************* 120 ** 121 ** Function utils_dequeue 122 ** 123 ** Description Dequeues a buffer from the head of the given queue 124 ** 125 ** Returns NULL if queue is empty, else buffer 126 ** 127 *******************************************************************************/ 128 void *utils_dequeue (BUFFER_Q *p_q) 129 { 130 pthread_mutex_lock(&utils_mutex); 131 void* p_buf = utils_dequeue_unlocked(p_q); 132 pthread_mutex_unlock(&utils_mutex); 133 return p_buf; 134 } 135 136 /******************************************************************************* 137 ** 138 ** Function utils_dequeue_unlocked 139 ** 140 ** Description Dequeues a buffer from the head of the given queue without lock 141 ** 142 ** Returns NULL if queue is empty, else buffer 143 ** 144 *******************************************************************************/ 145 void *utils_dequeue_unlocked (BUFFER_Q *p_q) 146 { 147 HC_BUFFER_HDR_T *p_hdr; 148 149 150 if (!p_q || !p_q->count) 151 { 152 return (NULL); 153 } 154 155 p_hdr=(HC_BUFFER_HDR_T *)((uint8_t *)p_q->p_first-BT_HC_BUFFER_HDR_SIZE); 156 157 if (p_hdr->p_next) 158 p_q->p_first = ((uint8_t *)p_hdr->p_next + BT_HC_BUFFER_HDR_SIZE); 159 else 160 { 161 p_q->p_first = NULL; 162 p_q->p_last = NULL; 163 } 164 165 p_q->count--; 166 167 p_hdr->p_next = NULL; 168 return ((uint8_t *)p_hdr + BT_HC_BUFFER_HDR_SIZE); 169 } 170 171 /******************************************************************************* 172 ** 173 ** Function utils_getnext 174 ** 175 ** Description Return a pointer to the next buffer linked to the given 176 ** buffer 177 ** 178 ** Returns NULL if the given buffer does not point to any next buffer, 179 ** else next buffer address 180 ** 181 *******************************************************************************/ 182 void *utils_getnext (void *p_buf) 183 { 184 HC_BUFFER_HDR_T *p_hdr; 185 186 p_hdr = (HC_BUFFER_HDR_T *) ((uint8_t *) p_buf - BT_HC_BUFFER_HDR_SIZE); 187 188 if (p_hdr->p_next) 189 return ((uint8_t *)p_hdr->p_next + BT_HC_BUFFER_HDR_SIZE); 190 else 191 return (NULL); 192 } 193 194 /******************************************************************************* 195 ** 196 ** Function utils_remove_from_queue 197 ** 198 ** Description Dequeue the given buffer from the middle of the given queue 199 ** 200 ** Returns NULL if the given queue is empty, else the given buffer 201 ** 202 *******************************************************************************/ 203 void *utils_remove_from_queue (BUFFER_Q *p_q, void *p_buf) 204 { 205 pthread_mutex_lock(&utils_mutex); 206 p_buf = utils_remove_from_queue_unlocked(p_q, p_buf); 207 pthread_mutex_unlock(&utils_mutex); 208 return p_buf; 209 } 210 /******************************************************************************* 211 ** 212 ** Function utils_remove_from_queue_unlocked 213 ** 214 ** Description Dequeue the given buffer from the middle of the given queue 215 ** 216 ** Returns NULL if the given queue is empty, else the given buffer 217 ** 218 *******************************************************************************/ 219 void *utils_remove_from_queue_unlocked (BUFFER_Q *p_q, void *p_buf) 220 { 221 HC_BUFFER_HDR_T *p_prev; 222 HC_BUFFER_HDR_T *p_buf_hdr; 223 224 225 if (p_buf == p_q->p_first) 226 { 227 return (utils_dequeue_unlocked (p_q)); 228 } 229 230 p_buf_hdr = (HC_BUFFER_HDR_T *)((uint8_t *)p_buf - BT_HC_BUFFER_HDR_SIZE); 231 p_prev=(HC_BUFFER_HDR_T *)((uint8_t *)p_q->p_first-BT_HC_BUFFER_HDR_SIZE); 232 233 for ( ; p_prev; p_prev = p_prev->p_next) 234 { 235 /* If the previous points to this one, move the pointers around */ 236 if (p_prev->p_next == p_buf_hdr) 237 { 238 p_prev->p_next = p_buf_hdr->p_next; 239 240 /* If we are removing the last guy in the queue, update p_last */ 241 if (p_buf == p_q->p_last) 242 p_q->p_last = p_prev + 1; 243 244 /* One less in the queue */ 245 p_q->count--; 246 247 /* The buffer is now unlinked */ 248 p_buf_hdr->p_next = NULL; 249 250 return (p_buf); 251 } 252 } 253 return (NULL); 254 } 255 256 /******************************************************************************* 257 ** 258 ** Function utils_delay 259 ** 260 ** Description sleep unconditionally for timeout milliseconds 261 ** 262 ** Returns None 263 ** 264 *******************************************************************************/ 265 void utils_delay (uint32_t timeout) 266 { 267 struct timespec delay; 268 int err; 269 270 delay.tv_sec = timeout / 1000; 271 delay.tv_nsec = 1000 * 1000 * (timeout%1000); 272 273 /* [u]sleep can't be used because it uses SIGALRM */ 274 do { 275 err = nanosleep(&delay, &delay); 276 } while (err < 0 && errno ==EINTR); 277 } 278 279 /******************************************************************************* 280 ** 281 ** Function utils_lock 282 ** 283 ** Description application calls this function before entering critical 284 ** section 285 ** 286 ** Returns None 287 ** 288 *******************************************************************************/ 289 void utils_lock (void) 290 { 291 pthread_mutex_lock(&utils_mutex); 292 } 293 294 /******************************************************************************* 295 ** 296 ** Function utils_unlock 297 ** 298 ** Description application calls this function when leaving critical 299 ** section 300 ** 301 ** Returns None 302 ** 303 *******************************************************************************/ 304 void utils_unlock (void) 305 { 306 pthread_mutex_unlock(&utils_mutex); 307 } 308 309
