WizziLab's serial protocol library
Dependents: modem_ref_helper_for_v5_3_217 modem_ref_helper
kal_buf_circ.cpp@17:8ce53c6e0350, 2022-03-10 (annotated)
- Committer:
- Jeej
- Date:
- Thu Mar 10 09:40:23 2022 +0000
- Revision:
- 17:8ce53c6e0350
- Parent:
- 10:c87566aded6e
Increase cb stack size
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jeej | 9:0140247bab90 | 1 | /// @copyright |
Jeej | 9:0140247bab90 | 2 | /// ========================================================================={{{ |
Jeej | 9:0140247bab90 | 3 | /// Copyright (c) 2019 WizziLab / |
Jeej | 9:0140247bab90 | 4 | /// All rights reserved / |
Jeej | 9:0140247bab90 | 5 | /// / |
Jeej | 9:0140247bab90 | 6 | /// IMPORTANT: This Software may not be modified, copied or distributed unless / |
Jeej | 9:0140247bab90 | 7 | /// embedded on a WizziLab product. Other than for the foregoing purpose, this / |
Jeej | 9:0140247bab90 | 8 | /// Software and/or its documentation may not be used, reproduced, copied, / |
Jeej | 9:0140247bab90 | 9 | /// prepared derivative works of, modified, performed, distributed, displayed / |
Jeej | 9:0140247bab90 | 10 | /// or sold for any purpose. For the sole purpose of embedding this Software / |
Jeej | 9:0140247bab90 | 11 | /// on a WizziLab product, copy, modification and distribution of this / |
Jeej | 9:0140247bab90 | 12 | /// Software is granted provided that the following conditions are respected: / |
Jeej | 9:0140247bab90 | 13 | /// / |
Jeej | 9:0140247bab90 | 14 | /// * Redistributions of source code must retain the above copyright notice, / |
Jeej | 9:0140247bab90 | 15 | /// this list of conditions and the following disclaimer / |
Jeej | 9:0140247bab90 | 16 | /// / |
Jeej | 9:0140247bab90 | 17 | /// * Redistributions in binary form must reproduce the above copyright / |
Jeej | 9:0140247bab90 | 18 | /// notice, this list of conditions and the following disclaimer in the / |
Jeej | 9:0140247bab90 | 19 | /// documentation and/or other materials provided with the distribution. / |
Jeej | 9:0140247bab90 | 20 | /// / |
Jeej | 9:0140247bab90 | 21 | /// * The name of WizziLab can not be used to endorse or promote products / |
Jeej | 9:0140247bab90 | 22 | /// derived from this software without specific prior written permission. / |
Jeej | 9:0140247bab90 | 23 | /// / |
Jeej | 9:0140247bab90 | 24 | /// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS / |
Jeej | 9:0140247bab90 | 25 | /// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED / |
Jeej | 9:0140247bab90 | 26 | /// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR / |
Jeej | 9:0140247bab90 | 27 | /// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR / |
Jeej | 9:0140247bab90 | 28 | /// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, / |
Jeej | 9:0140247bab90 | 29 | /// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, / |
Jeej | 9:0140247bab90 | 30 | /// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, / |
Jeej | 9:0140247bab90 | 31 | /// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY / |
Jeej | 9:0140247bab90 | 32 | /// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING / |
Jeej | 9:0140247bab90 | 33 | /// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS / |
Jeej | 9:0140247bab90 | 34 | /// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. / |
Jeej | 9:0140247bab90 | 35 | /// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, / |
Jeej | 9:0140247bab90 | 36 | /// ENHANCEMENTS OR MODIFICATIONS. / |
Jeej | 9:0140247bab90 | 37 | /// / |
Jeej | 9:0140247bab90 | 38 | /// Should you have any questions regarding your right to use this Software, / |
Jeej | 9:0140247bab90 | 39 | /// contact WizziLab at www.wizzilab.com. / |
Jeej | 9:0140247bab90 | 40 | /// / |
Jeej | 9:0140247bab90 | 41 | /// =========================================================================}}} |
Jeej | 9:0140247bab90 | 42 | /// @endcopyright |
Jeej | 9:0140247bab90 | 43 | |
Jeej | 9:0140247bab90 | 44 | /// XXX: New code for reference. Still to be tested. |
Jeej | 9:0140247bab90 | 45 | |
Jeej | 9:0140247bab90 | 46 | #include "kal_buf_circ.h" |
Jeej | 9:0140247bab90 | 47 | |
Jeej | 9:0140247bab90 | 48 | #if defined(KAL_MALLOC) && defined(KAL_FREE) |
Jeej | 9:0140247bab90 | 49 | #ifndef CIRCULAR_BUFFER_MALLOC |
Jeej | 9:0140247bab90 | 50 | #define CIRCULAR_BUFFER_MALLOC(_s) KAL_MALLOC(_s) |
Jeej | 9:0140247bab90 | 51 | #endif |
Jeej | 9:0140247bab90 | 52 | #ifndef CIRCULAR_BUFFER_FREE |
Jeej | 9:0140247bab90 | 53 | #define CIRCULAR_BUFFER_FREE(_p) KAL_FREE(_p) |
Jeej | 9:0140247bab90 | 54 | #endif |
Jeej | 10:c87566aded6e | 55 | #define CIRCULAR_BUFFER_DYNAMIC |
Jeej | 9:0140247bab90 | 56 | #endif |
Jeej | 9:0140247bab90 | 57 | |
Jeej | 9:0140247bab90 | 58 | static __inline uint32_t kal_buf_circ_next_head(kal_buf_circ_static_handle_t* h, uint32_t nb_elements) |
Jeej | 9:0140247bab90 | 59 | { |
Jeej | 9:0140247bab90 | 60 | uint32_t next = h->head + (nb_elements * h->element_size); |
Jeej | 9:0140247bab90 | 61 | return (next >= h->max_length)? next - h->max_length : next; |
Jeej | 9:0140247bab90 | 62 | } |
Jeej | 9:0140247bab90 | 63 | |
Jeej | 9:0140247bab90 | 64 | static __inline uint32_t kal_buf_circ_next_tail(kal_buf_circ_static_handle_t* h, uint32_t nb_elements) |
Jeej | 9:0140247bab90 | 65 | { |
Jeej | 9:0140247bab90 | 66 | uint32_t next = h->tail + (nb_elements * h->element_size); |
Jeej | 9:0140247bab90 | 67 | return (next >= h->max_length)? next - h->max_length : next; |
Jeej | 9:0140247bab90 | 68 | } |
Jeej | 9:0140247bab90 | 69 | |
Jeej | 10:c87566aded6e | 70 | #ifdef CIRCULAR_BUFFER_DYNAMIC |
Jeej | 9:0140247bab90 | 71 | kal_buf_circ_handle_t kal_buf_circ_create(uint32_t nb_elements, uint32_t element_size) |
Jeej | 9:0140247bab90 | 72 | { |
Jeej | 9:0140247bab90 | 73 | // Malloc the h and the buffer |
Jeej | 9:0140247bab90 | 74 | kal_buf_circ_static_handle_t* h = CIRCULAR_BUFFER_MALLOC(sizeof(kal_buf_circ_static_handle_t)); |
Jeej | 9:0140247bab90 | 75 | |
Jeej | 9:0140247bab90 | 76 | h->buffer = (uint8_t*)CIRCULAR_BUFFER_MALLOC((nb_elements + 1) * element_size); |
Jeej | 9:0140247bab90 | 77 | h->head = 0; |
Jeej | 9:0140247bab90 | 78 | h->tail = 0; |
Jeej | 9:0140247bab90 | 79 | h->element_size = element_size; |
Jeej | 9:0140247bab90 | 80 | h->max_length = (nb_elements + 1) * element_size; |
Jeej | 9:0140247bab90 | 81 | h->is_static = false; |
Jeej | 9:0140247bab90 | 82 | |
Jeej | 9:0140247bab90 | 83 | return (kal_buf_circ_handle_t)h; |
Jeej | 9:0140247bab90 | 84 | } |
Jeej | 9:0140247bab90 | 85 | #endif |
Jeej | 9:0140247bab90 | 86 | |
Jeej | 9:0140247bab90 | 87 | int kal_buf_circ_create_static(kal_buf_circ_static_handle_t* static_handle, uint8_t* buffer, uint32_t nb_elements, uint32_t element_size) |
Jeej | 9:0140247bab90 | 88 | { |
Jeej | 9:0140247bab90 | 89 | // h and buffer are passed as parameters. |
Jeej | 9:0140247bab90 | 90 | kal_buf_circ_static_handle_t* h = static_handle; |
Jeej | 9:0140247bab90 | 91 | |
Jeej | 9:0140247bab90 | 92 | h->buffer = buffer; |
Jeej | 9:0140247bab90 | 93 | h->head = 0; |
Jeej | 9:0140247bab90 | 94 | h->tail = 0; |
Jeej | 9:0140247bab90 | 95 | h->element_size = element_size; |
Jeej | 9:0140247bab90 | 96 | h->max_length = (nb_elements + 1) * element_size; |
Jeej | 9:0140247bab90 | 97 | h->is_static = true; |
Jeej | 9:0140247bab90 | 98 | |
Jeej | 9:0140247bab90 | 99 | return 0; |
Jeej | 9:0140247bab90 | 100 | } |
Jeej | 9:0140247bab90 | 101 | |
Jeej | 9:0140247bab90 | 102 | int kal_buf_circ_delete(kal_buf_circ_handle_t handle) |
Jeej | 9:0140247bab90 | 103 | { |
Jeej | 9:0140247bab90 | 104 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 105 | |
Jeej | 9:0140247bab90 | 106 | if (h->is_static) |
Jeej | 9:0140247bab90 | 107 | { |
Jeej | 9:0140247bab90 | 108 | // Set h to 0 |
Jeej | 9:0140247bab90 | 109 | memset(h->buffer, 0, h->max_length); |
Jeej | 9:0140247bab90 | 110 | memset(h, 0, sizeof(kal_buf_circ_static_handle_t)); |
Jeej | 9:0140247bab90 | 111 | } |
Jeej | 10:c87566aded6e | 112 | #ifdef CIRCULAR_BUFFER_DYNAMIC |
Jeej | 9:0140247bab90 | 113 | else |
Jeej | 9:0140247bab90 | 114 | { |
Jeej | 9:0140247bab90 | 115 | CIRCULAR_BUFFER_FREE(h->buffer); |
Jeej | 9:0140247bab90 | 116 | CIRCULAR_BUFFER_FREE(h); |
Jeej | 9:0140247bab90 | 117 | } |
Jeej | 9:0140247bab90 | 118 | #endif |
Jeej | 9:0140247bab90 | 119 | |
Jeej | 9:0140247bab90 | 120 | return 0; |
Jeej | 9:0140247bab90 | 121 | } |
Jeej | 9:0140247bab90 | 122 | |
Jeej | 9:0140247bab90 | 123 | int kal_buf_circ_reset(kal_buf_circ_handle_t handle) |
Jeej | 9:0140247bab90 | 124 | { |
Jeej | 9:0140247bab90 | 125 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 126 | |
Jeej | 9:0140247bab90 | 127 | h->head = 0; |
Jeej | 9:0140247bab90 | 128 | h->tail = 0; |
Jeej | 9:0140247bab90 | 129 | memset(h->buffer, 0, h->max_length); |
Jeej | 9:0140247bab90 | 130 | |
Jeej | 9:0140247bab90 | 131 | return 0; |
Jeej | 9:0140247bab90 | 132 | } |
Jeej | 9:0140247bab90 | 133 | |
Jeej | 10:c87566aded6e | 134 | int kal_buf_circ_full(kal_buf_circ_handle_t handle) |
Jeej | 9:0140247bab90 | 135 | { |
Jeej | 9:0140247bab90 | 136 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 137 | |
Jeej | 9:0140247bab90 | 138 | // if the next head is the tail, circular buffer is full |
Jeej | 9:0140247bab90 | 139 | return (kal_buf_circ_next_head(h, 1) == h->tail)? true : false; |
Jeej | 9:0140247bab90 | 140 | } |
Jeej | 9:0140247bab90 | 141 | |
Jeej | 10:c87566aded6e | 142 | int kal_buf_circ_empty(kal_buf_circ_handle_t handle) |
Jeej | 9:0140247bab90 | 143 | { |
Jeej | 9:0140247bab90 | 144 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 145 | |
Jeej | 9:0140247bab90 | 146 | // if the head isn't ahead of the tail, we don't have any elements |
Jeej | 9:0140247bab90 | 147 | return (h->head == h->tail)? true : false; |
Jeej | 9:0140247bab90 | 148 | } |
Jeej | 9:0140247bab90 | 149 | |
Jeej | 9:0140247bab90 | 150 | uint32_t kal_buf_circ_size(kal_buf_circ_handle_t handle) |
Jeej | 9:0140247bab90 | 151 | { |
Jeej | 9:0140247bab90 | 152 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 153 | |
Jeej | 9:0140247bab90 | 154 | // head is ahead of tail |
Jeej | 9:0140247bab90 | 155 | if (h->head >= h->tail) |
Jeej | 9:0140247bab90 | 156 | { |
Jeej | 9:0140247bab90 | 157 | // return the difference |
Jeej | 9:0140247bab90 | 158 | return (h->head - h->tail) / h->element_size; |
Jeej | 9:0140247bab90 | 159 | } |
Jeej | 9:0140247bab90 | 160 | else |
Jeej | 9:0140247bab90 | 161 | { |
Jeej | 9:0140247bab90 | 162 | // else add max_length |
Jeej | 9:0140247bab90 | 163 | return (h->max_length + h->head - h->tail) / h->element_size; |
Jeej | 9:0140247bab90 | 164 | } |
Jeej | 9:0140247bab90 | 165 | } |
Jeej | 9:0140247bab90 | 166 | |
Jeej | 10:c87566aded6e | 167 | uint32_t kal_buf_circ_space(kal_buf_circ_handle_t handle) |
Jeej | 10:c87566aded6e | 168 | { |
Jeej | 10:c87566aded6e | 169 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 10:c87566aded6e | 170 | |
Jeej | 10:c87566aded6e | 171 | return ((h->max_length / h->element_size) - 1) - kal_buf_circ_size(handle); |
Jeej | 10:c87566aded6e | 172 | } |
Jeej | 10:c87566aded6e | 173 | |
Jeej | 9:0140247bab90 | 174 | int kal_buf_circ_push(kal_buf_circ_handle_t handle, uint8_t* p) |
Jeej | 9:0140247bab90 | 175 | { |
Jeej | 9:0140247bab90 | 176 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 177 | |
Jeej | 9:0140247bab90 | 178 | // check if circular buffer is full |
Jeej | 9:0140247bab90 | 179 | if (kal_buf_circ_full(handle)) |
Jeej | 9:0140247bab90 | 180 | { |
Jeej | 9:0140247bab90 | 181 | return -1; // and return with an error. |
Jeej | 9:0140247bab90 | 182 | } |
Jeej | 9:0140247bab90 | 183 | |
Jeej | 9:0140247bab90 | 184 | // next is where head will point to after this write. |
Jeej | 9:0140247bab90 | 185 | uint32_t next = kal_buf_circ_next_head(h, 1); |
Jeej | 9:0140247bab90 | 186 | |
Jeej | 9:0140247bab90 | 187 | // Load data and then move |
Jeej | 9:0140247bab90 | 188 | memcpy(&(h->buffer[h->head]), p, h->element_size); |
Jeej | 9:0140247bab90 | 189 | |
Jeej | 9:0140247bab90 | 190 | // head to next data offset. |
Jeej | 9:0140247bab90 | 191 | h->head = next; |
Jeej | 9:0140247bab90 | 192 | |
Jeej | 9:0140247bab90 | 193 | // return success to indicate successful push. |
Jeej | 9:0140247bab90 | 194 | return 0; |
Jeej | 9:0140247bab90 | 195 | } |
Jeej | 9:0140247bab90 | 196 | |
Jeej | 9:0140247bab90 | 197 | int kal_buf_circ_pop(kal_buf_circ_handle_t handle, uint8_t* p) |
Jeej | 9:0140247bab90 | 198 | { |
Jeej | 9:0140247bab90 | 199 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 200 | |
Jeej | 9:0140247bab90 | 201 | // check if circular buffer is empty |
Jeej | 9:0140247bab90 | 202 | if (kal_buf_circ_empty(handle)) |
Jeej | 9:0140247bab90 | 203 | { |
Jeej | 9:0140247bab90 | 204 | return -1; // and return with an error |
Jeej | 9:0140247bab90 | 205 | } |
Jeej | 9:0140247bab90 | 206 | |
Jeej | 9:0140247bab90 | 207 | // next is where tail will point to after this read. |
Jeej | 9:0140247bab90 | 208 | uint32_t next = kal_buf_circ_next_tail(h, 1); |
Jeej | 9:0140247bab90 | 209 | |
Jeej | 9:0140247bab90 | 210 | if (p) |
Jeej | 9:0140247bab90 | 211 | { |
Jeej | 9:0140247bab90 | 212 | // Read data and then move |
Jeej | 9:0140247bab90 | 213 | memcpy(p, &(h->buffer[h->tail]), h->element_size); |
Jeej | 9:0140247bab90 | 214 | } |
Jeej | 9:0140247bab90 | 215 | |
Jeej | 9:0140247bab90 | 216 | // tail to next data offset. |
Jeej | 9:0140247bab90 | 217 | h->tail = next; |
Jeej | 9:0140247bab90 | 218 | |
Jeej | 9:0140247bab90 | 219 | // return success to indicate successful pop. |
Jeej | 9:0140247bab90 | 220 | return 0; |
Jeej | 9:0140247bab90 | 221 | } |
Jeej | 9:0140247bab90 | 222 | |
Jeej | 9:0140247bab90 | 223 | int kal_buf_circ_peek(kal_buf_circ_handle_t handle, uint8_t* p) |
Jeej | 9:0140247bab90 | 224 | { |
Jeej | 9:0140247bab90 | 225 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 226 | |
Jeej | 9:0140247bab90 | 227 | // check if circular buffer is empty |
Jeej | 9:0140247bab90 | 228 | if (kal_buf_circ_empty(handle)) |
Jeej | 9:0140247bab90 | 229 | { |
Jeej | 9:0140247bab90 | 230 | return -1; // and return with an error |
Jeej | 9:0140247bab90 | 231 | } |
Jeej | 9:0140247bab90 | 232 | |
Jeej | 9:0140247bab90 | 233 | // Read data |
Jeej | 9:0140247bab90 | 234 | memcpy(p, &(h->buffer[h->tail]), h->element_size); |
Jeej | 9:0140247bab90 | 235 | |
Jeej | 9:0140247bab90 | 236 | // return success to indicate successful peek. |
Jeej | 9:0140247bab90 | 237 | return 0; |
Jeej | 9:0140247bab90 | 238 | } |
Jeej | 9:0140247bab90 | 239 | |
Jeej | 9:0140247bab90 | 240 | int kal_buf_circ_put(kal_buf_circ_handle_t handle, uint8_t* p, uint32_t nb_elements) |
Jeej | 9:0140247bab90 | 241 | { |
Jeej | 9:0140247bab90 | 242 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 243 | |
Jeej | 9:0140247bab90 | 244 | // Check if there is enough available spots in the buffer |
Jeej | 9:0140247bab90 | 245 | if (nb_elements + kal_buf_circ_size(handle) >= h->max_length / h->element_size) |
Jeej | 9:0140247bab90 | 246 | { |
Jeej | 9:0140247bab90 | 247 | return -1; // and return with an error |
Jeej | 9:0140247bab90 | 248 | } |
Jeej | 9:0140247bab90 | 249 | |
Jeej | 9:0140247bab90 | 250 | // check if write should be done in two parts |
Jeej | 9:0140247bab90 | 251 | uint32_t available_now = (h->max_length - h->head) / h->element_size; |
Jeej | 9:0140247bab90 | 252 | if (nb_elements > available_now) |
Jeej | 9:0140247bab90 | 253 | { |
Jeej | 9:0140247bab90 | 254 | // write first part up to max_length |
Jeej | 9:0140247bab90 | 255 | if (kal_buf_circ_put(handle, p, available_now)) |
Jeej | 9:0140247bab90 | 256 | { |
Jeej | 9:0140247bab90 | 257 | return -1; |
Jeej | 9:0140247bab90 | 258 | } |
Jeej | 9:0140247bab90 | 259 | |
Jeej | 9:0140247bab90 | 260 | // increment write pointer |
Jeej | 9:0140247bab90 | 261 | p += available_now * h->element_size; |
Jeej | 9:0140247bab90 | 262 | // subtract what we just wrote |
Jeej | 9:0140247bab90 | 263 | nb_elements -= available_now; |
Jeej | 9:0140247bab90 | 264 | } |
Jeej | 9:0140247bab90 | 265 | |
Jeej | 9:0140247bab90 | 266 | // next is where head will point to after this write. |
Jeej | 9:0140247bab90 | 267 | uint32_t next = kal_buf_circ_next_head(h, nb_elements); |
Jeej | 9:0140247bab90 | 268 | |
Jeej | 9:0140247bab90 | 269 | // write data |
Jeej | 9:0140247bab90 | 270 | memcpy(&(h->buffer[h->head]), p, nb_elements * h->element_size); |
Jeej | 9:0140247bab90 | 271 | |
Jeej | 9:0140247bab90 | 272 | // head to next data offset. |
Jeej | 9:0140247bab90 | 273 | h->head = next; |
Jeej | 9:0140247bab90 | 274 | |
Jeej | 9:0140247bab90 | 275 | // return success to indicate successful get. |
Jeej | 9:0140247bab90 | 276 | return 0; |
Jeej | 9:0140247bab90 | 277 | } |
Jeej | 9:0140247bab90 | 278 | |
Jeej | 9:0140247bab90 | 279 | int kal_buf_circ_get(kal_buf_circ_handle_t handle, uint8_t* p, uint32_t nb_elements) |
Jeej | 9:0140247bab90 | 280 | { |
Jeej | 9:0140247bab90 | 281 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 282 | |
Jeej | 9:0140247bab90 | 283 | // Check if there is enough elements in the buffer |
Jeej | 9:0140247bab90 | 284 | if (nb_elements > kal_buf_circ_size(handle)) |
Jeej | 9:0140247bab90 | 285 | { |
Jeej | 9:0140247bab90 | 286 | return -1; // and return with an error |
Jeej | 9:0140247bab90 | 287 | } |
Jeej | 9:0140247bab90 | 288 | |
Jeej | 9:0140247bab90 | 289 | // check if read should be done in two parts |
Jeej | 9:0140247bab90 | 290 | uint32_t available_now = (h->max_length - h->tail) / h->element_size; |
Jeej | 9:0140247bab90 | 291 | if (nb_elements > available_now) |
Jeej | 9:0140247bab90 | 292 | { |
Jeej | 9:0140247bab90 | 293 | // get first part up to max_length |
Jeej | 9:0140247bab90 | 294 | if (kal_buf_circ_get(handle, p, available_now)) |
Jeej | 9:0140247bab90 | 295 | { |
Jeej | 9:0140247bab90 | 296 | return -1; |
Jeej | 9:0140247bab90 | 297 | } |
Jeej | 9:0140247bab90 | 298 | |
Jeej | 9:0140247bab90 | 299 | if (p) |
Jeej | 9:0140247bab90 | 300 | { |
Jeej | 9:0140247bab90 | 301 | // increment read pointer |
Jeej | 9:0140247bab90 | 302 | p += available_now * h->element_size; |
Jeej | 9:0140247bab90 | 303 | } |
Jeej | 9:0140247bab90 | 304 | // subtract what we just read |
Jeej | 9:0140247bab90 | 305 | nb_elements -= available_now; |
Jeej | 9:0140247bab90 | 306 | } |
Jeej | 9:0140247bab90 | 307 | |
Jeej | 9:0140247bab90 | 308 | // next is where tail will point to after this read. |
Jeej | 9:0140247bab90 | 309 | uint32_t next = kal_buf_circ_next_tail(h, nb_elements); |
Jeej | 9:0140247bab90 | 310 | |
Jeej | 9:0140247bab90 | 311 | if (p) |
Jeej | 9:0140247bab90 | 312 | { |
Jeej | 9:0140247bab90 | 313 | // read data |
Jeej | 9:0140247bab90 | 314 | memcpy(p, &(h->buffer[h->tail]), nb_elements * h->element_size); |
Jeej | 9:0140247bab90 | 315 | } |
Jeej | 9:0140247bab90 | 316 | |
Jeej | 9:0140247bab90 | 317 | // tail to next data offset. |
Jeej | 9:0140247bab90 | 318 | h->tail = next; |
Jeej | 9:0140247bab90 | 319 | |
Jeej | 9:0140247bab90 | 320 | // return success to indicate successful get. |
Jeej | 9:0140247bab90 | 321 | return 0; |
Jeej | 9:0140247bab90 | 322 | } |
Jeej | 9:0140247bab90 | 323 | |
Jeej | 9:0140247bab90 | 324 | int kal_buf_circ_fetch(kal_buf_circ_handle_t handle, uint8_t* p, uint32_t nb_elements) |
Jeej | 9:0140247bab90 | 325 | { |
Jeej | 9:0140247bab90 | 326 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 327 | |
Jeej | 9:0140247bab90 | 328 | // Check if there is enough elements in the buffer |
Jeej | 9:0140247bab90 | 329 | if (nb_elements > kal_buf_circ_size(handle)) |
Jeej | 9:0140247bab90 | 330 | { |
Jeej | 9:0140247bab90 | 331 | return -1; // and return with an error |
Jeej | 9:0140247bab90 | 332 | } |
Jeej | 9:0140247bab90 | 333 | |
Jeej | 9:0140247bab90 | 334 | // check if read should be done in two parts |
Jeej | 9:0140247bab90 | 335 | uint32_t available_now = (h->max_length - h->tail) / h->element_size; |
Jeej | 9:0140247bab90 | 336 | if (nb_elements > available_now) |
Jeej | 9:0140247bab90 | 337 | { |
Jeej | 9:0140247bab90 | 338 | // get first part from tail up to max_length |
Jeej | 9:0140247bab90 | 339 | memcpy(p, &(h->buffer[h->tail]), available_now * h->element_size); |
Jeej | 9:0140247bab90 | 340 | |
Jeej | 9:0140247bab90 | 341 | // increment read pointer |
Jeej | 9:0140247bab90 | 342 | p += available_now * h->element_size; |
Jeej | 9:0140247bab90 | 343 | |
Jeej | 9:0140247bab90 | 344 | // subtract what we just read |
Jeej | 9:0140247bab90 | 345 | nb_elements -= available_now; |
Jeej | 9:0140247bab90 | 346 | |
Jeej | 9:0140247bab90 | 347 | // get second part from 0 up to remaining length |
Jeej | 9:0140247bab90 | 348 | memcpy(p, &(h->buffer[0]), nb_elements * h->element_size); |
Jeej | 9:0140247bab90 | 349 | } |
Jeej | 9:0140247bab90 | 350 | else |
Jeej | 9:0140247bab90 | 351 | { |
Jeej | 9:0140247bab90 | 352 | // get data from tail |
Jeej | 9:0140247bab90 | 353 | memcpy(p, &(h->buffer[h->tail]), nb_elements * h->element_size); |
Jeej | 9:0140247bab90 | 354 | } |
Jeej | 9:0140247bab90 | 355 | |
Jeej | 9:0140247bab90 | 356 | // return success to indicate successful fetch. |
Jeej | 9:0140247bab90 | 357 | return 0; |
Jeej | 9:0140247bab90 | 358 | } |
Jeej | 9:0140247bab90 | 359 | |
Jeej | 9:0140247bab90 | 360 | int kal_buf_circ_erase(kal_buf_circ_handle_t handle) |
Jeej | 9:0140247bab90 | 361 | { |
Jeej | 9:0140247bab90 | 362 | kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle; |
Jeej | 9:0140247bab90 | 363 | |
Jeej | 9:0140247bab90 | 364 | // check if circular buffer is empty |
Jeej | 9:0140247bab90 | 365 | if (kal_buf_circ_empty(handle)) |
Jeej | 9:0140247bab90 | 366 | { |
Jeej | 9:0140247bab90 | 367 | return -1; // and return with an error |
Jeej | 9:0140247bab90 | 368 | } |
Jeej | 9:0140247bab90 | 369 | |
Jeej | 9:0140247bab90 | 370 | // head is at 0, we wrap back |
Jeej | 9:0140247bab90 | 371 | if (h->head == 0) |
Jeej | 9:0140247bab90 | 372 | { |
Jeej | 9:0140247bab90 | 373 | h->head = h->max_length - h->element_size; |
Jeej | 9:0140247bab90 | 374 | } |
Jeej | 9:0140247bab90 | 375 | // else go back one element |
Jeej | 9:0140247bab90 | 376 | else |
Jeej | 9:0140247bab90 | 377 | { |
Jeej | 9:0140247bab90 | 378 | h->head = h->head - h->element_size; |
Jeej | 9:0140247bab90 | 379 | } |
Jeej | 9:0140247bab90 | 380 | |
Jeej | 9:0140247bab90 | 381 | // return success to indicate successful erase. |
Jeej | 9:0140247bab90 | 382 | return 0; |
Jeej | 9:0140247bab90 | 383 | } |