1/* Copyright (c) 2015, The Linux Foundation. All rights reserved. 2 * 3 * Redistribution and use in source and binary forms, with or without 4 * modification, are permitted provided that the following conditions are 5 * met: 6 * * Redistributions of source code must retain the above copyright 7 * notice, this list of conditions and the following disclaimer. 8 * * Redistributions in binary form must reproduce the above 9 * copyright notice, this list of conditions and the following 10 * disclaimer in the documentation and/or other materials provided 11 * with the distribution. 12 * * Neither the name of The Linux Foundation nor the names of its 13 * contributors may be used to endorse or promote products derived 14 * from this software without specific prior written permission. 15 * 16 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 17 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 20 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 23 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 24 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 25 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 26 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 */ 28 29#include <stdint.h> 30#include <stdlib.h> 31#include <string.h> 32#include <pthread.h> 33 34#define LOG_TAG "WifiHAL" 35 36#include <utils/Log.h> 37 38typedef unsigned char u8; 39typedef uint16_t u16; 40typedef uint32_t u32; 41typedef uint64_t u64; 42 43#include "ring_buffer.h" 44 45enum rb_bool { 46 RB_TRUE = 0, 47 RB_FALSE = 1 48}; 49 50typedef struct rb_entry_s { 51 u8 *data; 52 unsigned int last_wr_index; 53 u8 full; 54} rb_entry_t; 55 56typedef struct ring_buf_cb { 57 unsigned int rd_buf_no; // Current buffer number to be read from 58 unsigned int wr_buf_no; // Current buffer number to be written into 59 unsigned int cur_rd_buf_idx; // Read index within the current read buffer 60 unsigned int cur_wr_buf_idx; // Write index within the current write buffer 61 rb_entry_t *bufs; // Array of buffer pointers 62 63 unsigned int max_num_bufs; // Maximum number of buffers that should be used 64 size_t each_buf_size; // Size of each buffer in bytes 65 66 pthread_mutex_t rb_rw_lock; 67 68 /* Threshold vars */ 69 unsigned int num_min_bytes; 70 void (*threshold_cb)(void *); 71 void *cb_ctx; 72 73 u32 total_bytes_written; 74 u32 total_bytes_read; 75 u32 total_bytes_overwritten; 76 u32 cur_valid_bytes; 77 enum rb_bool threshold_reached; 78} rbc_t; 79 80 81#define RB_MIN(x, y) ((x) < (y)?(x):(y)) 82inline void rb_lock(pthread_mutex_t *lock) 83{ 84 int error = pthread_mutex_lock(lock); 85 86 if (error) 87 ALOGE("Failed to acquire lock with err %d", error); 88 // TODO Handle the lock failure 89} 90 91inline void rb_unlock(pthread_mutex_t *lock) 92{ 93 int error = pthread_mutex_unlock(lock); 94 95 if (error) 96 ALOGE("Failed to release lock with err %d", error); 97 // TODO Handle the unlock failure 98} 99 100void * ring_buffer_init(size_t size_of_buf, int num_bufs) 101{ 102 struct ring_buf_cb *rbc; 103 int status; 104 105 rbc = (struct ring_buf_cb *)malloc(sizeof(struct ring_buf_cb)); 106 if (rbc == NULL) { 107 ALOGE("Failed to alloc rbc"); 108 return NULL; 109 } 110 memset(rbc, 0, sizeof(struct ring_buf_cb)); 111 112 rbc->bufs = (rb_entry_t *)malloc(num_bufs * sizeof(rb_entry_t)); 113 if (rbc->bufs == NULL) { 114 free(rbc); 115 ALOGE("Failed to alloc rbc->bufs"); 116 return NULL; 117 } 118 memset(rbc->bufs, 0, (num_bufs * sizeof(rb_entry_t))); 119 120 rbc->each_buf_size = size_of_buf; 121 rbc->max_num_bufs = num_bufs; 122 123 status = pthread_mutex_init(&rbc->rb_rw_lock, NULL); 124 if (status != 0) { 125 ALOGE("Failed to initialize rb_rw_lock"); 126 // TODO handle lock initialization failure 127 } 128 rbc->threshold_reached = RB_FALSE; 129 return rbc; 130} 131 132void ring_buffer_deinit(void *ctx) 133{ 134 rbc_t *rbc = (rbc_t *)ctx; 135 int status; 136 unsigned int buf_no; 137 138 status = pthread_mutex_destroy(&rbc->rb_rw_lock); 139 if (status != 0) { 140 ALOGE("Failed to destroy rb_rw_lock"); 141 // TODO handle the lock destroy failure 142 } 143 for (buf_no = 0; buf_no < rbc->max_num_bufs; buf_no++) { 144 free(rbc->bufs[buf_no].data); 145 } 146 free(rbc->bufs); 147 free(rbc); 148} 149 150/* 151 * record_length : 0 - byte boundary 152 * : >0 - Ensures to write record_length no.of bytes to the same buffer. 153 */ 154enum rb_status rb_write (void *ctx, u8 *buf, size_t length, int overwrite, 155 size_t record_length) 156{ 157 rbc_t *rbc = (rbc_t *)ctx; 158 unsigned int bytes_written = 0; // bytes written into rb so far 159 unsigned int push_in_rd_ptr = 0; // push required in read pointer because of 160 // write in current buffer 161 unsigned int total_push_in_rd_ptr = 0; // Total amount of push in read pointer in this write 162 163 if (record_length > rbc->each_buf_size) { 164 return RB_FAILURE; 165 } 166 167 if (overwrite == 0) { 168 /* Check if the complete RB is full. If the current wr_buf is also 169 * full, it indicates that the complete RB is full 170 */ 171 if (rbc->bufs[rbc->wr_buf_no].full == 1) 172 return RB_FULL; 173 /* Check whether record fits in current buffer */ 174 if (rbc->wr_buf_no == rbc->rd_buf_no) { 175 if ((rbc->cur_wr_buf_idx == rbc->cur_rd_buf_idx) && 176 rbc->cur_valid_bytes) { 177 return RB_FULL; 178 } else if (rbc->cur_wr_buf_idx < rbc->cur_rd_buf_idx) { 179 if (record_length > 180 (rbc->cur_rd_buf_idx - rbc->cur_wr_buf_idx)) { 181 return RB_FULL; 182 } 183 } else { 184 if (record_length > (rbc->each_buf_size - rbc->cur_wr_buf_idx)) { 185 /* Check if the next buffer is not full to write this record into 186 * next buffer 187 */ 188 unsigned int next_buf_no = rbc->wr_buf_no + 1; 189 190 if (next_buf_no >= rbc->max_num_bufs) { 191 next_buf_no = 0; 192 } 193 if (rbc->bufs[next_buf_no].full == 1) { 194 return RB_FULL; 195 } 196 } 197 } 198 } else if (record_length > (rbc->each_buf_size - rbc->cur_wr_buf_idx)) { 199 /* Check if the next buffer is not full to write this record into 200 * next buffer 201 */ 202 unsigned int next_buf_no = rbc->wr_buf_no + 1; 203 204 if (next_buf_no >= rbc->max_num_bufs) { 205 next_buf_no = 0; 206 } 207 if (rbc->bufs[next_buf_no].full == 1) { 208 return RB_FULL; 209 } 210 } 211 } 212 213 /* Go to next buffer if the current buffer is not enough to write the 214 * complete record 215 */ 216 if (record_length > (rbc->each_buf_size - rbc->cur_wr_buf_idx)) { 217 rbc->bufs[rbc->wr_buf_no].full = 1; 218 rbc->bufs[rbc->wr_buf_no].last_wr_index = rbc->cur_wr_buf_idx; 219 rbc->wr_buf_no++; 220 if (rbc->wr_buf_no == rbc->max_num_bufs) { 221 rbc->wr_buf_no = 0; 222 } 223 rbc->cur_wr_buf_idx = 0; 224 } 225 226 227 /* In each iteration of below loop, the data that can be fit into 228 * buffer @wr_buf_no will be copied from input buf */ 229 while (bytes_written < length) { 230 unsigned int cur_copy_len; 231 232 /* Allocate a buffer if no buf available @ wr_buf_no */ 233 if (rbc->bufs[rbc->wr_buf_no].data == NULL) { 234 rbc->bufs[rbc->wr_buf_no].data = (u8 *)malloc(rbc->each_buf_size); 235 if (rbc->bufs[rbc->wr_buf_no].data == NULL) { 236 ALOGE("Failed to alloc write buffer"); 237 return RB_RETRY; 238 } 239 } 240 241 /* Take the minimum of the remaining length that needs to be written 242 * from buf and the maximum length that can be written into current 243 * buffer in ring buffer 244 */ 245 cur_copy_len = RB_MIN((rbc->each_buf_size - rbc->cur_wr_buf_idx), 246 (length - bytes_written)); 247 248 rb_lock(&rbc->rb_rw_lock); 249 250 /* Push the read pointer in case of overrun */ 251 if (rbc->rd_buf_no == rbc->wr_buf_no) { 252 if ((rbc->cur_rd_buf_idx > rbc->cur_wr_buf_idx) || 253 ((rbc->cur_rd_buf_idx == rbc->cur_wr_buf_idx) && 254 rbc->cur_valid_bytes)) { 255 /* If read ptr is ahead of write pointer and if the 256 * gap is not enough to fit the cur_copy_len bytes then 257 * push the read pointer so that points to the start of 258 * old bytes after this write 259 */ 260 if ((rbc->cur_rd_buf_idx - rbc->cur_wr_buf_idx) < 261 cur_copy_len) { 262 push_in_rd_ptr += cur_copy_len - 263 (rbc->cur_rd_buf_idx - rbc->cur_wr_buf_idx); 264 rbc->cur_rd_buf_idx = rbc->cur_wr_buf_idx + cur_copy_len; 265 if (rbc->cur_rd_buf_idx >= 266 rbc->bufs[rbc->rd_buf_no].last_wr_index) { 267 rbc->cur_rd_buf_idx = 0; 268 rbc->rd_buf_no++; 269 if (rbc->rd_buf_no == rbc->max_num_bufs) { 270 rbc->rd_buf_no = 0; 271 ALOGV("Pushing read to the start of ring buffer"); 272 } 273 /* the previous buffer might have little more empty room 274 * after overwriting the remaining bytes 275 */ 276 rbc->bufs[rbc->wr_buf_no].full = 0; 277 } 278 } 279 } 280 } 281 rb_unlock(&rbc->rb_rw_lock); 282 283 /* don't use lock while doing memcpy, so that we don't block the read 284 * context for too long. There is no harm while writing the memory if 285 * locking is properly done while upgrading the pointers */ 286 memcpy((rbc->bufs[rbc->wr_buf_no].data + rbc->cur_wr_buf_idx), 287 (buf + bytes_written), 288 cur_copy_len); 289 290 rb_lock(&rbc->rb_rw_lock); 291 /* Update the write idx by the amount of write done in this iteration */ 292 rbc->cur_wr_buf_idx += cur_copy_len; 293 if (rbc->cur_wr_buf_idx == rbc->each_buf_size) { 294 /* Increment the wr_buf_no as the current buffer is full */ 295 rbc->bufs[rbc->wr_buf_no].full = 1; 296 rbc->bufs[rbc->wr_buf_no].last_wr_index = rbc->cur_wr_buf_idx; 297 rbc->wr_buf_no++; 298 if (rbc->wr_buf_no == rbc->max_num_bufs) { 299 ALOGV("Write rolling over to the start of ring buffer"); 300 rbc->wr_buf_no = 0; 301 } 302 /* Reset the write index to zero as this is a new buffer */ 303 rbc->cur_wr_buf_idx = 0; 304 } 305 306 if ((rbc->cur_valid_bytes + (cur_copy_len - push_in_rd_ptr)) > 307 (rbc->max_num_bufs * rbc->each_buf_size)) { 308 /* The below is only a precautionary print and ideally should never 309 * come */ 310 ALOGE("Something going wrong in ring buffer"); 311 } else { 312 /* Increase the valid bytes count by number of bytes written without 313 * overwriting the old bytes */ 314 rbc->cur_valid_bytes += cur_copy_len - push_in_rd_ptr; 315 } 316 total_push_in_rd_ptr += push_in_rd_ptr; 317 push_in_rd_ptr = 0; 318 rb_unlock(&rbc->rb_rw_lock); 319 bytes_written += cur_copy_len; 320 } 321 322 rb_lock(&rbc->rb_rw_lock); 323 rbc->total_bytes_written += bytes_written - total_push_in_rd_ptr; 324 rbc->total_bytes_overwritten += total_push_in_rd_ptr; 325 326 /* check if valid bytes is going more than threshold */ 327 if ((rbc->threshold_reached == RB_FALSE) && 328 (rbc->cur_valid_bytes >= rbc->num_min_bytes) && 329 ((length == record_length) || !record_length) && 330 rbc->threshold_cb) { 331 /* Release the lock before calling threshold_cb as it might call rb_read 332 * in this same context in order to avoid dead lock 333 */ 334 rbc->threshold_reached = RB_TRUE; 335 rb_unlock(&rbc->rb_rw_lock); 336 rbc->threshold_cb(rbc->cb_ctx); 337 } else { 338 rb_unlock(&rbc->rb_rw_lock); 339 } 340 return RB_SUCCESS; 341} 342 343size_t rb_read (void *ctx, u8 *buf, size_t max_length) 344{ 345 rbc_t *rbc = (rbc_t *)ctx; 346 unsigned int bytes_read = 0; 347 unsigned int no_more_bytes_available = 0; 348 349 rb_lock(&rbc->rb_rw_lock); 350 while (bytes_read < max_length) { 351 unsigned int cur_cpy_len; 352 353 if (rbc->bufs[rbc->rd_buf_no].data == NULL) { 354 break; 355 } 356 357 /* if read and write are on same buffer, work with rd, wr indices */ 358 if (rbc->rd_buf_no == rbc->wr_buf_no) { 359 if (rbc->cur_rd_buf_idx < rbc->cur_wr_buf_idx) { 360 /* Check if all the required bytes are available, if not 361 * read only the available bytes in the current buffer and 362 * break out after reading current buffer 363 */ 364 if ((rbc->cur_wr_buf_idx - rbc->cur_rd_buf_idx) < 365 (max_length - bytes_read)) { 366 cur_cpy_len = rbc->cur_wr_buf_idx - rbc->cur_rd_buf_idx; 367 no_more_bytes_available = 1; 368 } else { 369 cur_cpy_len = max_length - bytes_read; 370 } 371 } else { 372 /* When there are no bytes available to read cur_rd_buf_idx 373 * will be euqal to cur_wr_buf_idx. Handle this scenario using 374 * cur_valid_bytes */ 375 if (rbc->cur_valid_bytes <= bytes_read) { 376 /* Suppress possible static analyzer's warning */ 377 cur_cpy_len = 0; 378 break; 379 } 380 cur_cpy_len = RB_MIN((rbc->each_buf_size - rbc->cur_rd_buf_idx), 381 (max_length - bytes_read)); 382 } 383 } else { 384 /* Check if all remaining_length bytes can be read from this 385 * buffer, if not read only the available bytes in the current 386 * buffer and go to next buffer using the while loop. 387 */ 388 cur_cpy_len = RB_MIN((rbc->each_buf_size - rbc->cur_rd_buf_idx), 389 (max_length - bytes_read)); 390 } 391 392 memcpy((buf + bytes_read), 393 (rbc->bufs[rbc->rd_buf_no].data + rbc->cur_rd_buf_idx), 394 cur_cpy_len); 395 396 /* Update the read index */ 397 rbc->cur_rd_buf_idx += cur_cpy_len; 398 if (rbc->cur_rd_buf_idx == rbc->each_buf_size) { 399 /* Increment rd_buf_no as the current buffer is completely read */ 400 if (rbc->rd_buf_no != rbc->wr_buf_no) { 401 free(rbc->bufs[rbc->rd_buf_no].data); 402 rbc->bufs[rbc->rd_buf_no].data = NULL; 403 } 404 rbc->rd_buf_no++; 405 if (rbc->rd_buf_no == rbc->max_num_bufs) { 406 ALOGV("Read rolling over to the start of ring buffer"); 407 rbc->rd_buf_no = 0; 408 } 409 /* Reset the read index as this is a new buffer */ 410 rbc->cur_rd_buf_idx = 0; 411 } 412 413 bytes_read += cur_cpy_len; 414 if (no_more_bytes_available) { 415 break; 416 } 417 } 418 419 rbc->total_bytes_read += bytes_read; 420 if (rbc->cur_valid_bytes < bytes_read) { 421 /* The below is only a precautionary print and ideally should never 422 * come */ 423 ALOGE("Something going wrong in ring buffer"); 424 } else { 425 rbc->cur_valid_bytes -= bytes_read; 426 } 427 428 /* check if valid bytes is going less than threshold */ 429 if (rbc->threshold_reached == RB_TRUE) { 430 if (rbc->cur_valid_bytes < rbc->num_min_bytes) { 431 rbc->threshold_reached = RB_FALSE; 432 } 433 } 434 rb_unlock(&rbc->rb_rw_lock); 435 return bytes_read; 436} 437 438u8 *rb_get_read_buf(void *ctx, size_t *length) 439{ 440 rbc_t *rbc = (rbc_t *)ctx; 441 unsigned int cur_read_len = 0; 442 u8 *buf; 443 444 /* If no buffer is available for reading */ 445 if (rbc->bufs[rbc->rd_buf_no].data == NULL) { 446 *length = 0; 447 return NULL; 448 } 449 450 rb_lock(&rbc->rb_rw_lock); 451 if ((rbc->bufs[rbc->rd_buf_no].full == 1) && 452 (rbc->cur_rd_buf_idx == rbc->bufs[rbc->rd_buf_no].last_wr_index)) { 453 if (rbc->wr_buf_no != rbc->rd_buf_no) { 454 free(rbc->bufs[rbc->rd_buf_no].data); 455 rbc->bufs[rbc->rd_buf_no].data = NULL; 456 } 457 rbc->bufs[rbc->rd_buf_no].full = 0; 458 rbc->rd_buf_no++; 459 if (rbc->rd_buf_no == rbc->max_num_bufs) { 460 rbc->rd_buf_no = 0; 461 } 462 rbc->cur_rd_buf_idx = 0; 463 } 464 465 if (rbc->wr_buf_no == rbc->rd_buf_no) { 466 /* If read and write are happening on the same buffer currently, use 467 * rd and wr indices within the buffer */ 468 if ((rbc->cur_rd_buf_idx == rbc->cur_wr_buf_idx) && 469 (rbc->cur_valid_bytes == 0)) { 470 /* No bytes available for reading */ 471 *length = 0; 472 rb_unlock(&rbc->rb_rw_lock); 473 return NULL; 474 } else if (rbc->cur_rd_buf_idx < rbc->cur_wr_buf_idx) { 475 /* write is just ahead of read in this buffer */ 476 cur_read_len = rbc->cur_wr_buf_idx - rbc->cur_rd_buf_idx; 477 } else { 478 /* write is rolled over and just behind the read */ 479 cur_read_len = rbc->bufs[rbc->rd_buf_no].last_wr_index - rbc->cur_rd_buf_idx; 480 } 481 } else { 482 if (rbc->cur_rd_buf_idx == 0) { 483 /* The complete buffer can be read out */ 484 cur_read_len = rbc->bufs[rbc->rd_buf_no].last_wr_index; 485 } else { 486 /* Read the remaining bytes in this buffer */ 487 cur_read_len = rbc->bufs[rbc->rd_buf_no].last_wr_index - rbc->cur_rd_buf_idx; 488 } 489 } 490 491 if ((rbc->bufs[rbc->rd_buf_no].full == 1) && 492 (rbc->cur_rd_buf_idx == 0)) { 493 /* Pluck out the complete buffer and send it out */ 494 buf = rbc->bufs[rbc->rd_buf_no].data; 495 rbc->bufs[rbc->rd_buf_no].data = NULL; 496 497 /* Move to the next buffer */ 498 rbc->bufs[rbc->rd_buf_no].full = 0; 499 rbc->rd_buf_no++; 500 if (rbc->rd_buf_no == rbc->max_num_bufs) { 501 ALOGV("Read rolling over to the start of ring buffer"); 502 rbc->rd_buf_no = 0; 503 } 504 } else { 505 /* We cannot give out the complete buffer, so allocate a new memory and 506 * and copy the data into it. 507 */ 508 buf = (u8 *)malloc(cur_read_len); 509 if (buf == NULL) { 510 ALOGE("Failed to alloc buffer for partial buf read"); 511 *length = 0; 512 rb_unlock(&rbc->rb_rw_lock); 513 return NULL; 514 } 515 memcpy(buf, 516 (rbc->bufs[rbc->rd_buf_no].data + rbc->cur_rd_buf_idx), 517 cur_read_len); 518 519 /* Update the read index */ 520 if (rbc->bufs[rbc->rd_buf_no].full == 1) { 521 if (rbc->wr_buf_no != rbc->rd_buf_no) { 522 free(rbc->bufs[rbc->rd_buf_no].data); 523 rbc->bufs[rbc->rd_buf_no].data = NULL; 524 } 525 rbc->bufs[rbc->rd_buf_no].full = 0; 526 rbc->rd_buf_no++; 527 if (rbc->rd_buf_no == rbc->max_num_bufs) { 528 rbc->rd_buf_no = 0; 529 } 530 rbc->cur_rd_buf_idx = 0; 531 } else { 532 rbc->cur_rd_buf_idx += cur_read_len; 533 } 534 } 535 536 rbc->total_bytes_read += cur_read_len; 537 if (rbc->cur_valid_bytes < cur_read_len) { 538 /* The below is only a precautionary print and ideally should never 539 * come */ 540 ALOGE("Something going wrong in ring buffer"); 541 } else { 542 rbc->cur_valid_bytes -= cur_read_len; 543 } 544 545 /* check if valid bytes is going less than threshold */ 546 if (rbc->threshold_reached == RB_TRUE) { 547 if (rbc->cur_valid_bytes < rbc->num_min_bytes) { 548 rbc->threshold_reached = RB_FALSE; 549 } 550 } 551 rb_unlock(&rbc->rb_rw_lock); 552 553 *length = cur_read_len; 554 return buf; 555} 556 557void rb_config_threshold(void *ctx, 558 unsigned int num_min_bytes, 559 threshold_call_back callback, 560 void *cb_ctx) 561{ 562 rbc_t *rbc = (rbc_t *)ctx; 563 564 rbc->num_min_bytes = num_min_bytes; 565 rbc->threshold_cb = callback; 566 rbc->cb_ctx = cb_ctx; 567} 568 569void rb_get_stats(void *ctx, struct rb_stats *rbs) 570{ 571 rbc_t *rbc = (rbc_t *)ctx; 572 573 rbs->total_bytes_written = rbc->total_bytes_written; 574 rbs->total_bytes_read = rbc->total_bytes_read; 575 rbs->cur_valid_bytes = rbc->cur_valid_bytes; 576 rbs->each_buf_size = rbc->each_buf_size; 577 rbs->max_num_bufs = rbc->max_num_bufs; 578} 579