1 | /* |
2 | * Copyright (c) 2007-2015 Freescale Semiconductor, Inc. |
3 | * Copyright 2018-2020 NXP |
4 | * |
5 | * License: NXP LA_OPT_NXP_Software_License |
6 | * |
7 | * NXP Confidential. This software is owned or controlled by NXP and may |
8 | * only be used strictly in accordance with the applicable license terms. |
9 | * By expressly accepting such terms or by downloading, installing, |
10 | * activating and/or otherwise using the software, you are agreeing that |
11 | * you have read, and that you agree to comply with and are bound by, |
12 | * such license terms. If you do not agree to be bound by the applicable |
13 | * license terms, then you may not retain, install, activate or otherwise |
14 | * use the software. This code may only be used in a microprocessor, |
15 | * microcontroller, sensor or digital signal processor ("NXP Product") |
16 | * supplied directly or indirectly from NXP. See the full NXP Software |
17 | * License Agreement in license/LA_OPT_NXP_Software_License.pdf |
18 | * |
19 | * FreeMASTER Communication Driver - utility code |
20 | */ |
21 | |
22 | #include "freemaster.h" |
23 | #include "freemaster_private.h" |
24 | #include "freemaster_utils.h" |
25 | |
26 | #if FMSTR_DISABLE == 0 |
27 | |
28 | /******************************************************** |
29 | * optimized memory copy helper macros |
30 | ********************************************************/ |
31 | |
32 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
33 | |
34 | /* Copy variable to destination by bytes from an aligned source address */ |
35 | FMSTR_INLINE void _FMSTR_CopySrcAligned_U64(FMSTR_U8 *dest, FMSTR_U64 *src) |
36 | { |
37 | union |
38 | { |
39 | FMSTR_U64 n; |
40 | FMSTR_U8 raw[8]; |
41 | } value; |
42 | |
43 | FMSTR_U8 *raw = value.raw; |
44 | value.n = *src; /* read aligned source value with a single operation */ |
45 | *dest++ = *raw++; /* copy all bytes to generally unaligned destination */ |
46 | *dest++ = *raw++; |
47 | *dest++ = *raw++; |
48 | *dest++ = *raw++; |
49 | *dest++ = *raw++; |
50 | *dest++ = *raw++; |
51 | *dest++ = *raw++; |
52 | *dest++ = *raw++; |
53 | } |
54 | |
55 | /* Copy variable from source by bytes to an aligned destination address */ |
56 | FMSTR_INLINE void _FMSTR_CopyDstAligned_U64(FMSTR_U64 *dest, FMSTR_U8 *src) |
57 | { |
58 | union |
59 | { |
60 | FMSTR_U64 n; |
61 | FMSTR_U8 raw[8]; |
62 | } value; |
63 | |
64 | FMSTR_U8 *raw = value.raw; |
65 | *raw++ = *src++; /* copy all bytes from generally unaligned source */ |
66 | *raw++ = *src++; |
67 | *raw++ = *src++; |
68 | *raw++ = *src++; |
69 | *raw++ = *src++; |
70 | *raw++ = *src++; |
71 | *raw++ = *src++; |
72 | *raw++ = *src++; |
73 | *dest = value.n; /* write aligned destination with a single operation */ |
74 | } |
75 | |
76 | /* Masked copy variable from source by bytes to an aligned destination address */ |
77 | FMSTR_INLINE void _FMSTR_CopyMaskedDstAligned_U64(FMSTR_U64 *dest, FMSTR_U8 *src, FMSTR_U8 *mask) |
78 | { |
79 | FMSTR_U64 v, m, x; |
80 | |
81 | _FMSTR_CopyDstAligned_U64(&v, src); |
82 | _FMSTR_CopyDstAligned_U64(&m, mask); |
83 | |
84 | x = *dest; |
85 | x = (x & ~m) | (v & m); |
86 | |
87 | *dest = x; |
88 | } |
89 | #endif |
90 | |
91 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
92 | |
93 | /* Copy variable to destination by bytes from an aligned source address */ |
94 | FMSTR_INLINE void _FMSTR_CopySrcAligned_U32(FMSTR_U8 *dest, FMSTR_U32 *src) |
95 | { |
96 | union |
97 | { |
98 | FMSTR_U32 n; |
99 | FMSTR_U8 raw[4]; |
100 | } value; |
101 | |
102 | FMSTR_U8 *raw = value.raw; |
103 | value.n = *src; /* read aligned source value with a single operation */ |
104 | *dest++ = *raw++; /* copy all bytes to generally unaligned destination */ |
105 | *dest++ = *raw++; |
106 | *dest++ = *raw++; |
107 | *dest++ = *raw++; |
108 | } |
109 | |
110 | /* Copy variable from source by bytes to an aligned destination address */ |
111 | FMSTR_INLINE void _FMSTR_CopyDstAligned_U32(FMSTR_U32 *dest, FMSTR_U8 *src) |
112 | { |
113 | union |
114 | { |
115 | FMSTR_U32 n; |
116 | FMSTR_U8 raw[4]; |
117 | } value; |
118 | |
119 | FMSTR_U8 *raw = value.raw; |
120 | *raw++ = *src++; /* copy all bytes from generally unaligned source */ |
121 | *raw++ = *src++; |
122 | *raw++ = *src++; |
123 | *raw++ = *src++; |
124 | *dest = value.n; /* write aligned destination with a single operation */ |
125 | } |
126 | |
127 | /* Masked copy variable from source by bytes to an aligned destination address */ |
128 | FMSTR_INLINE void _FMSTR_CopyMaskedDstAligned_U32(FMSTR_U32 *dest, FMSTR_U8 *src, FMSTR_U8 *mask) |
129 | { |
130 | FMSTR_U32 v, m, x; |
131 | |
132 | _FMSTR_CopyDstAligned_U32(&v, src); |
133 | _FMSTR_CopyDstAligned_U32(&m, mask); |
134 | |
135 | x = *dest; |
136 | x = (x & ~m) | (v & m); |
137 | |
138 | *dest = x; |
139 | } |
140 | |
141 | #endif |
142 | |
143 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
144 | |
145 | /* Copy variable to destination by bytes from an aligned source address */ |
146 | FMSTR_INLINE void _FMSTR_CopySrcAligned_U16(FMSTR_U8 *dest, FMSTR_U16 *src) |
147 | { |
148 | union |
149 | { |
150 | FMSTR_U16 n; |
151 | FMSTR_U8 raw[2]; |
152 | } value; |
153 | |
154 | FMSTR_U8 *raw = value.raw; |
155 | value.n = *src; /* read aligned source value with a single operation */ |
156 | *dest++ = *raw++; /* copy all bytes to generally unaligned destination */ |
157 | *dest++ = *raw++; |
158 | } |
159 | |
160 | /* Copy variable from source by bytes to an aligned destination address */ |
161 | FMSTR_INLINE void _FMSTR_CopyDstAligned_U16(FMSTR_U16 *dest, FMSTR_U8 *src) |
162 | { |
163 | union |
164 | { |
165 | FMSTR_U16 n; |
166 | FMSTR_U8 raw[2]; |
167 | } value; |
168 | |
169 | FMSTR_U8 *raw = value.raw; |
170 | *raw++ = *src++; /* copy all bytes from generally unaligned source */ |
171 | *raw++ = *src++; |
172 | *dest = value.n; /* write aligned destination with a single operation */ |
173 | } |
174 | |
175 | /* Masked copy variable from source by bytes to an aligned destination address */ |
176 | FMSTR_INLINE void _FMSTR_CopyMaskedDstAligned_U16(FMSTR_U16 *dest, FMSTR_U8 *src, FMSTR_U8 *mask) |
177 | { |
178 | FMSTR_U16 v, m, x; |
179 | _FMSTR_CopyDstAligned_U16(&v, src); |
180 | _FMSTR_CopyDstAligned_U16(&m, mask); |
181 | |
182 | x = *dest; |
183 | x = (x & ~m) | (v & m); |
184 | |
185 | *dest = x; |
186 | } |
187 | |
188 | #endif |
189 | |
190 | /* Test if FMSTR_ADDR address is mis-aligned for given number of bits */ |
191 | #define TEST_MISALIGNED(addr, bits) ((((FMSTR_U32)(addr)) & ((1U << (bits)) - 1U)) != 0U) |
192 | |
193 | /* in this helper call, we are already sure that the destination pointer is 64-bit aligned */ |
194 | static void _FMSTR_MemCpyDstAligned(FMSTR_ADDR dest, FMSTR_ADDR src, FMSTR_SIZE size) |
195 | { |
196 | FMSTR_U8 *src8 = (FMSTR_U8 *)src; |
197 | |
198 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
199 | { |
200 | /* 64-bit aligned part */ |
201 | FMSTR_U64 *d64 = (FMSTR_U64 *)dest; |
202 | |
203 | while (size >= sizeof(FMSTR_U64)) |
204 | { |
205 | _FMSTR_CopyDstAligned_U64(d64, src8); |
206 | size -= sizeof(FMSTR_U64); |
207 | src8 += sizeof(FMSTR_U64); |
208 | d64++; |
209 | } |
210 | |
211 | dest = (FMSTR_ADDR)(d64); |
212 | } |
213 | #endif |
214 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
215 | { |
216 | /* remaining word(s) */ |
217 | FMSTR_U32 *d32 = (FMSTR_U32 *)dest; |
218 | |
219 | while (size >= sizeof(FMSTR_U32)) |
220 | { |
221 | _FMSTR_CopyDstAligned_U32(d32, src8); |
222 | size -= sizeof(FMSTR_U32); |
223 | src8 += sizeof(FMSTR_U32); |
224 | d32++; |
225 | } |
226 | |
227 | dest = (FMSTR_ADDR)(d32); |
228 | } |
229 | #endif |
230 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
231 | { |
232 | /* remaining halfword(s) */ |
233 | FMSTR_U16 *d16 = (FMSTR_U16 *)dest; |
234 | |
235 | while (size >= sizeof(FMSTR_U16)) |
236 | { |
237 | _FMSTR_CopyDstAligned_U16(d16, src8); |
238 | size -= sizeof(FMSTR_U16); |
239 | src8 += sizeof(FMSTR_U16); |
240 | d16++; |
241 | } |
242 | |
243 | dest = (FMSTR_ADDR)(d16); |
244 | } |
245 | #endif |
246 | { |
247 | volatile FMSTR_U8 *d8 = (FMSTR_U8 *)dest; |
248 | |
249 | /* remaining byte(s) */ |
250 | while (size >= 1U) |
251 | { |
252 | *d8++ = *src8++; |
253 | size--; |
254 | } |
255 | } |
256 | |
257 | FMSTR_ASSERT(size == 0U); |
258 | } |
259 | |
260 | /* in this helper call, we are already sure that the source pointer is 64-bit aligned */ |
261 | static void _FMSTR_MemCpySrcAligned(FMSTR_ADDR dest, FMSTR_ADDR src, FMSTR_SIZE size) |
262 | { |
263 | FMSTR_U8 *dest8 = (FMSTR_U8 *)dest; |
264 | |
265 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
266 | { |
267 | /* 64-bit aligned part */ |
268 | FMSTR_U64 *s64 = (FMSTR_U64 *)src; |
269 | |
270 | while (size >= sizeof(FMSTR_U64)) |
271 | { |
272 | _FMSTR_CopySrcAligned_U64(dest8, s64); |
273 | size -= sizeof(FMSTR_U64); |
274 | dest8 += sizeof(FMSTR_U64); |
275 | s64++; |
276 | } |
277 | |
278 | src = (FMSTR_ADDR)(s64); |
279 | } |
280 | #endif |
281 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
282 | { |
283 | /* remaining word(s) */ |
284 | FMSTR_U32 *s32 = (FMSTR_U32 *)src; |
285 | |
286 | while (size >= sizeof(FMSTR_U32)) |
287 | { |
288 | _FMSTR_CopySrcAligned_U32(dest8, s32); |
289 | size -= sizeof(FMSTR_U32); |
290 | dest8 += sizeof(FMSTR_U32); |
291 | s32++; |
292 | } |
293 | |
294 | src = (FMSTR_ADDR)(s32); |
295 | } |
296 | #endif |
297 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
298 | { |
299 | /* remaining halfword(s) */ |
300 | FMSTR_U16 *s16 = (FMSTR_U16 *)src; |
301 | |
302 | while (size >= sizeof(FMSTR_U16)) |
303 | { |
304 | _FMSTR_CopySrcAligned_U16(dest8, s16); |
305 | size -= sizeof(FMSTR_U16); |
306 | dest8 += sizeof(FMSTR_U16); |
307 | s16++; |
308 | } |
309 | |
310 | src = (FMSTR_ADDR)(s16); |
311 | } |
312 | #endif |
313 | { |
314 | volatile FMSTR_U8 *s8 = (FMSTR_U8 *)src; |
315 | |
316 | /* remaining byte(s) */ |
317 | while (size >= 1U) |
318 | { |
319 | *dest8++ = *s8++; |
320 | size--; |
321 | } |
322 | } |
323 | |
324 | FMSTR_ASSERT(size == 0U); |
325 | } |
326 | |
327 | /* in this helper call, we are already sure that the required pointer is aligned */ |
328 | static void _FMSTR_MemCpyMaskedDstAligned(FMSTR_ADDR dest, FMSTR_ADDR src, FMSTR_ADDR mask, FMSTR_SIZE size) |
329 | { |
330 | FMSTR_U8 *src8 = (FMSTR_U8 *)src; |
331 | FMSTR_U8 *mask8 = (FMSTR_U8 *)mask; |
332 | |
333 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
334 | { |
335 | /* 64-bit aligned part */ |
336 | FMSTR_U64 *d64 = (FMSTR_U64 *)dest; |
337 | |
338 | while (size >= sizeof(FMSTR_U64)) |
339 | { |
340 | _FMSTR_CopyMaskedDstAligned_U64(d64, src8, mask8); |
341 | size -= sizeof(FMSTR_U64); |
342 | src8 += sizeof(FMSTR_U64); |
343 | mask8 += sizeof(FMSTR_U64); |
344 | d64++; |
345 | } |
346 | |
347 | dest = (FMSTR_ADDR)(d64); |
348 | } |
349 | #endif |
350 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
351 | { |
352 | /* remaining word(s) */ |
353 | FMSTR_U32 *d32 = (FMSTR_U32 *)dest; |
354 | |
355 | while (size >= sizeof(FMSTR_U32)) |
356 | { |
357 | _FMSTR_CopyMaskedDstAligned_U32(d32, src8, mask8); |
358 | size -= sizeof(FMSTR_U32); |
359 | src8 += sizeof(FMSTR_U32); |
360 | mask8 += sizeof(FMSTR_U32); |
361 | d32++; |
362 | } |
363 | |
364 | dest = (FMSTR_ADDR)(d32); |
365 | } |
366 | #endif |
367 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
368 | { |
369 | /* remaining halfword(s) */ |
370 | FMSTR_U16 *d16 = (FMSTR_U16 *)dest; |
371 | |
372 | while (size >= sizeof(FMSTR_U16)) |
373 | { |
374 | _FMSTR_CopyMaskedDstAligned_U16(d16, src8, mask8); |
375 | size -= sizeof(FMSTR_U16); |
376 | src8 += sizeof(FMSTR_U16); |
377 | mask8 += sizeof(FMSTR_U16); |
378 | d16++; |
379 | } |
380 | |
381 | dest = (FMSTR_ADDR)(d16); |
382 | } |
383 | #endif |
384 | { |
385 | volatile FMSTR_U8 *d8 = (FMSTR_U8 *)dest; |
386 | FMSTR_U8 m, s; |
387 | |
388 | /* remaining byte(s) */ |
389 | while (size >= 1U) |
390 | { |
391 | m = *mask8++; |
392 | s = (FMSTR_U8)(*src8++ & m); |
393 | s |= (FMSTR_U8)(*d8 & (~m)); |
394 | *d8++ = s; |
395 | size--; |
396 | } |
397 | } |
398 | |
399 | FMSTR_ASSERT(size == 0U); |
400 | } |
401 | |
402 | /****************************************************************************** |
403 | * |
404 | * @brief Generic memory copy routine without alignment and transfer size requirements |
405 | * |
406 | * @param destAddr - destination memory address |
407 | * @param srcBuff - pointer to source memory in communication buffer |
408 | * @param size - buffer size (always in bytes) |
409 | * |
410 | ******************************************************************************/ |
411 | |
412 | void _FMSTR_MemCpy(void *dest, const void *src, FMSTR_SIZE size) |
413 | { |
414 | FMSTR_MemCpyTo(FMSTR_CAST_PTR_TO_ADDR(dest), FMSTR_CAST_PTR_TO_ADDR(src), size); |
415 | } |
416 | |
417 | /****************************************************************************** |
418 | * |
419 | * @brief Copy data. Reading from source memory is as aligned as it can be. |
420 | * |
421 | * @param destAddr - destination memory address |
422 | * @param srcAddr - source memory address |
423 | * @param size - buffer size in bytes |
424 | * |
425 | ******************************************************************************/ |
426 | |
427 | FMSTR_WEAK void FMSTR_MemCpyFrom(FMSTR_ADDR destAddr, FMSTR_ADDR srcAddr, FMSTR_SIZE size) |
428 | { |
429 | FMSTR_U8 *dest8 = (FMSTR_U8 *)destAddr; |
430 | |
431 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
432 | /* misaligned odd byte */ |
433 | if (TEST_MISALIGNED(srcAddr, 1U) && size >= sizeof(FMSTR_U8)) |
434 | { |
435 | FMSTR_U8 *s8 = (FMSTR_U8 *)srcAddr; |
436 | *dest8++ = *s8++; |
437 | size -= sizeof(FMSTR_U8); |
438 | srcAddr = (FMSTR_ADDR)(s8); |
439 | } |
440 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
441 | /* misaligned odd halfword */ |
442 | if (TEST_MISALIGNED(srcAddr, 2U) && size >= sizeof(FMSTR_U16)) |
443 | { |
444 | FMSTR_U16 *s16 = (FMSTR_U16 *)srcAddr; |
445 | _FMSTR_CopySrcAligned_U16(dest8, s16); |
446 | size -= sizeof(FMSTR_U16); |
447 | dest8 += sizeof(FMSTR_U16); |
448 | s16++; |
449 | srcAddr = (FMSTR_ADDR)(s16); |
450 | } |
451 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
452 | /* misaligned odd word */ |
453 | if (TEST_MISALIGNED(srcAddr, 3U) && size >= sizeof(FMSTR_U32)) |
454 | { |
455 | FMSTR_U32 *s32 = (FMSTR_U32 *)srcAddr; |
456 | _FMSTR_CopySrcAligned_U32(dest8, s32); |
457 | size -= sizeof(FMSTR_U32); |
458 | dest8 += sizeof(FMSTR_U32); |
459 | s32++; |
460 | srcAddr = (FMSTR_ADDR)(s32); |
461 | } |
462 | #endif |
463 | #endif |
464 | #endif |
465 | |
466 | /* the rest is already aligned */ |
467 | _FMSTR_MemCpySrcAligned((FMSTR_ADDR)(dest8), srcAddr, size); |
468 | } |
469 | |
470 | /****************************************************************************** |
471 | * |
472 | * @brief Copy data. Writing to destination memory is as aligned as it can be. |
473 | * |
474 | * @param destAddr - destination memory address |
475 | * @param srcAddr - source memory address |
476 | * @param size - buffer size in bytes |
477 | * |
478 | ******************************************************************************/ |
479 | |
480 | FMSTR_WEAK void FMSTR_MemCpyTo(FMSTR_ADDR destAddr, FMSTR_ADDR srcAddr, FMSTR_SIZE size) |
481 | { |
482 | FMSTR_U8 *src8 = (FMSTR_U8 *)srcAddr; |
483 | |
484 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
485 | /* misaligned odd byte */ |
486 | if (TEST_MISALIGNED(destAddr, 1U) && size >= sizeof(FMSTR_U8)) |
487 | { |
488 | FMSTR_U8 *d8 = (FMSTR_U8 *)destAddr; |
489 | *d8++ = *src8++; |
490 | size -= sizeof(FMSTR_U8); |
491 | destAddr = (FMSTR_ADDR)(d8); |
492 | } |
493 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
494 | /* misaligned odd halfword */ |
495 | if (TEST_MISALIGNED(destAddr, 2U) && size >= sizeof(FMSTR_U16)) |
496 | { |
497 | FMSTR_U16 *d16 = (FMSTR_U16 *)destAddr; |
498 | _FMSTR_CopyDstAligned_U16(d16, src8); |
499 | size -= sizeof(FMSTR_U16); |
500 | src8 += sizeof(FMSTR_U16); |
501 | d16++; |
502 | destAddr = (FMSTR_ADDR)(d16); |
503 | } |
504 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
505 | /* misaligned odd word */ |
506 | if (TEST_MISALIGNED(destAddr, 3U) && size >= sizeof(FMSTR_U32)) |
507 | { |
508 | FMSTR_U32 *d32 = (FMSTR_U32 *)destAddr; |
509 | _FMSTR_CopyDstAligned_U32(d32, src8); |
510 | size -= sizeof(FMSTR_U32); |
511 | src8 += sizeof(FMSTR_U32); |
512 | d32++; |
513 | destAddr = (FMSTR_ADDR)(d32); |
514 | } |
515 | #endif |
516 | #endif |
517 | #endif |
518 | |
519 | /* the rest is already aligned */ |
520 | _FMSTR_MemCpyDstAligned(destAddr, (FMSTR_ADDR)(src8), size); |
521 | } |
522 | |
523 | /****************************************************************************** |
524 | * |
525 | * @brief Copy data with mask. Write to destination memory is as aligned as it can be. |
526 | * |
527 | * @param destAddr - destination memory address |
528 | * @param srcAddr - source memory address |
529 | * @param maskAddr - source mask address |
530 | * @param size - buiffer size in bytes |
531 | * |
532 | ******************************************************************************/ |
533 | FMSTR_WEAK void FMSTR_MemCpyToMasked(FMSTR_ADDR destAddr, FMSTR_ADDR srcAddr, FMSTR_ADDR maskAddr, FMSTR_SIZE size) |
534 | { |
535 | FMSTR_U8 *src8 = (FMSTR_U8 *)srcAddr; |
536 | FMSTR_U8 *mask8 = (FMSTR_U8 *)maskAddr; |
537 | |
538 | #if FMSTR_MEMCPY_MAX_SIZE >= 2 |
539 | /* misaligned odd byte */ |
540 | if (TEST_MISALIGNED(destAddr, 1) && size >= sizeof(FMSTR_U8)) |
541 | { |
542 | FMSTR_U8 *d8 = (FMSTR_U8 *)destAddr; |
543 | FMSTR_U8 m, s; |
544 | |
545 | m = *mask8++; |
546 | s = *src8++ & m; |
547 | s |= *d8 & (~m); |
548 | *d8++ = s; |
549 | size -= sizeof(FMSTR_U8); |
550 | destAddr = (FMSTR_ADDR)(d8); |
551 | } |
552 | #endif |
553 | #if FMSTR_MEMCPY_MAX_SIZE >= 4 |
554 | /* misaligned odd halfword */ |
555 | if (TEST_MISALIGNED(destAddr, 2U) && size >= sizeof(FMSTR_U16)) |
556 | { |
557 | FMSTR_U16 *d16 = (FMSTR_U16 *)destAddr; |
558 | _FMSTR_CopyMaskedDstAligned_U16(d16, src8, mask8); |
559 | size -= sizeof(FMSTR_U16); |
560 | src8 += sizeof(FMSTR_U16); |
561 | mask8 += sizeof(FMSTR_U16); |
562 | d16++; |
563 | destAddr = (FMSTR_ADDR)(d16); |
564 | } |
565 | #endif |
566 | #if FMSTR_MEMCPY_MAX_SIZE >= 8 |
567 | /* misaligned odd word */ |
568 | if (TEST_MISALIGNED(destAddr, 3U) && size >= sizeof(FMSTR_U32)) |
569 | { |
570 | FMSTR_U32 *d32 = (FMSTR_U32 *)destAddr; |
571 | _FMSTR_CopyMaskedDstAligned_U32(d32, src8, mask8); |
572 | size -= sizeof(FMSTR_U32); |
573 | src8 += sizeof(FMSTR_U32); |
574 | mask8 += sizeof(FMSTR_U32); |
575 | d32++; |
576 | destAddr = (FMSTR_ADDR)(d32); |
577 | } |
578 | #endif |
579 | |
580 | /* the rest is already aligned */ |
581 | _FMSTR_MemCpyMaskedDstAligned(destAddr, (FMSTR_ADDR)(src8), (FMSTR_ADDR)(mask8), size); |
582 | } |
583 | |
584 | /****************************************************************************** |
585 | * |
586 | * @brief Write to the communication buffer memory |
587 | * |
588 | * @param destBuff - pointer to destination memory in communication buffer |
589 | * @param srcAddr - source memory address |
590 | * @param size - buffer size in bytes |
591 | * |
592 | * @return This function returns a pointer to next byte in comm. buffer |
593 | * |
594 | ******************************************************************************/ |
595 | |
596 | FMSTR_WEAK FMSTR_BPTR FMSTR_CopyToBuffer(FMSTR_BPTR destBuff, FMSTR_ADDR srcAddr, FMSTR_SIZE size) |
597 | { |
598 | FMSTR_MemCpyFrom((FMSTR_ADDR)(destBuff), srcAddr, size); |
599 | return destBuff + size; |
600 | } |
601 | |
602 | /****************************************************************************** |
603 | * |
604 | * @brief Read from communication buffer memory |
605 | * |
606 | * @param destAddr - destination memory address |
607 | * @param srcBuff - pointer to source memory in communication buffer |
608 | * @param size - buffer size in bytes |
609 | * |
610 | * @return This function returns a pointer to next byte in comm. buffer |
611 | * |
612 | ******************************************************************************/ |
613 | |
614 | FMSTR_WEAK FMSTR_BPTR FMSTR_CopyFromBuffer(FMSTR_ADDR destAddr, FMSTR_BPTR srcBuff, FMSTR_SIZE size) |
615 | { |
616 | FMSTR_MemCpyTo(destAddr, (FMSTR_ADDR)(srcBuff), size); |
617 | return srcBuff + size; |
618 | } |
619 | |
620 | /****************************************************************************** |
621 | * |
622 | * @brief Read from communication buffer memory and copy bytes with masking |
623 | * |
624 | * @param destAddr - destination memory address |
625 | * @param srcBuff - pointer to source memory and mask in communication buffer |
626 | * @param size - buffer size in bytes |
627 | * |
628 | * @return This function returns a pointer to next byte in comm. buffer |
629 | * |
630 | ******************************************************************************/ |
631 | |
632 | FMSTR_WEAK void FMSTR_CopyFromBufferWithMask(FMSTR_ADDR destAddr, FMSTR_BPTR srcBuff, FMSTR_SIZE size) |
633 | { |
634 | FMSTR_MemCpyToMasked(destAddr, (FMSTR_ADDR)(srcBuff), (FMSTR_ADDR)(srcBuff + size), size); |
635 | } |
636 | |
637 | /****************************************************************************** |
638 | * |
639 | * @brief Store address in LEB format to communication buffer. |
640 | * |
641 | ******************************************************************************/ |
642 | |
643 | FMSTR_WEAK FMSTR_BPTR FMSTR_AddressToBuffer(FMSTR_BPTR dest, FMSTR_ADDR addr) |
644 | { |
645 | return FMSTR_UlebEncode(dest, &addr, sizeof(addr)); |
646 | } |
647 | |
648 | /****************************************************************************** |
649 | * |
650 | * @brief Fetch address in LEB format from communication buffer |
651 | * |
652 | ******************************************************************************/ |
653 | |
654 | FMSTR_WEAK FMSTR_BPTR FMSTR_AddressFromBuffer(FMSTR_ADDR *paddr, FMSTR_BPTR src) |
655 | { |
656 | return FMSTR_UlebDecode(src, paddr, sizeof(*paddr)); |
657 | } |
658 | |
659 | /****************************************************************************** |
660 | * |
661 | * @brief Store size in LEB format to communication buffer. |
662 | * |
663 | ******************************************************************************/ |
664 | |
665 | FMSTR_WEAK FMSTR_BPTR FMSTR_SizeToBuffer(FMSTR_BPTR dest, FMSTR_SIZE size) |
666 | { |
667 | return FMSTR_UlebEncode(dest, &size, sizeof(size)); |
668 | } |
669 | |
670 | /****************************************************************************** |
671 | * |
672 | * @brief Fetch size in LEB format from communication buffer |
673 | * |
674 | ******************************************************************************/ |
675 | |
676 | FMSTR_WEAK FMSTR_BPTR FMSTR_SizeFromBuffer(FMSTR_SIZE *psize, FMSTR_BPTR src) |
677 | { |
678 | return FMSTR_UlebDecode(src, psize, sizeof(*psize)); |
679 | } |
680 | |
681 | /****************************************************************************** |
682 | * |
683 | * @brief Fetch index in signed LEB format from communication buffer |
684 | * |
685 | ******************************************************************************/ |
686 | |
687 | FMSTR_WEAK FMSTR_BPTR FMSTR_IndexFromBuffer(FMSTR_INDEX *pindex, FMSTR_BPTR src) |
688 | { |
689 | return FMSTR_SlebDecode(src, pindex, sizeof(*pindex)); |
690 | } |
691 | |
692 | /****************************************************************************** |
693 | * |
694 | * @brief Store generic U32 number to communication buffer as ULEB |
695 | * |
696 | ******************************************************************************/ |
697 | |
698 | FMSTR_BPTR FMSTR_ULebToBuffer(FMSTR_BPTR dest, FMSTR_U32 num) |
699 | { |
700 | return FMSTR_UlebEncode(dest, &num, sizeof(num)); |
701 | } |
702 | |
703 | /****************************************************************************** |
704 | * |
705 | * @brief Fetch generic U32 value as ULEB from communication buffer |
706 | * |
707 | ******************************************************************************/ |
708 | |
709 | FMSTR_BPTR FMSTR_ULebFromBuffer(FMSTR_U32 *pnum, FMSTR_BPTR src) |
710 | { |
711 | return FMSTR_UlebDecode(src, pnum, sizeof(*pnum)); |
712 | } |
713 | |
714 | /****************************************************************************** |
715 | * |
716 | * @brief Fetch generic U16 value from communication buffer |
717 | * |
718 | ******************************************************************************/ |
719 | |
720 | FMSTR_BPTR FMSTR_ValueFromBuffer16BE(FMSTR_U16 *pnum, FMSTR_BPTR src) |
721 | { |
722 | *pnum = (FMSTR_U16)((((FMSTR_U16)(src[0])) << 8) | (src[1])); |
723 | return (src + 2); |
724 | } |
725 | |
726 | /****************************************************************************** |
727 | * |
728 | * @brief Store generic U16 number to communication buffer |
729 | * |
730 | ******************************************************************************/ |
731 | |
732 | FMSTR_BPTR FMSTR_ValueToBuffer16BE(FMSTR_BPTR dest, FMSTR_U16 num) |
733 | { |
734 | dest[0] = (FMSTR_BCHR)((num >> 8) & 0xffU); |
735 | dest[1] = (FMSTR_BCHR)(num & 0xffU); |
736 | |
737 | return (dest + 2); |
738 | } |
739 | |
740 | /****************************************************************************** |
741 | * |
742 | * @brief Return number of bytes that given address needs to add in order to |
743 | * get properly aligned. |
744 | * |
745 | ******************************************************************************/ |
746 | |
747 | FMSTR_WEAK FMSTR_SIZE FMSTR_GetAlignmentCorrection(FMSTR_ADDR addr, FMSTR_SIZE size) |
748 | { |
749 | FMSTR_U32 addrn = (FMSTR_U32)addr; |
750 | FMSTR_U32 aligned = addrn; |
751 | |
752 | FMSTR_ASSERT(size == 0U || size == 1U || size == 2U || size == 4U || size == 8U); |
753 | |
754 | if (size > 0U) |
755 | { |
756 | aligned += size - 1U; |
757 | aligned &= ~(size - 1U); |
758 | } |
759 | |
760 | return (FMSTR_SIZE)(aligned - addrn); |
761 | } |
762 | |
763 | /****************************************************************************** |
764 | * |
765 | * @brief Decode LEB number to destination variable |
766 | * |
767 | * @param in Pointer to input data |
768 | * @param result Pointer to destination variable |
769 | * @param size Size of the destination variable |
770 | * @param sleb True when decoding SLEB format |
771 | * |
772 | ******************************************************************************/ |
773 | |
774 | static FMSTR_BPTR FMSTR_LebDecode(FMSTR_BPTR in, void *result, FMSTR_SIZE size, FMSTR_BOOL sleb) |
775 | { |
776 | FMSTR_BCHR b; |
777 | FMSTR_U8 v; |
778 | FMSTR_U8 *dest; |
779 | FMSTR_INDEX dadd; |
780 | FMSTR_SIZE shift = 0U; |
781 | |
782 | /* Initialize result to 0 value */ |
783 | FMSTR_MemSet(result, 0, size); |
784 | |
785 | #if FMSTR_PLATFORM_BIG_ENDIAN > 0 |
786 | dest = ((FMSTR_U8 *)result) + size - 1; |
787 | dadd = -1; |
788 | #else |
789 | dest = (FMSTR_U8 *)result; |
790 | dadd = 1; |
791 | #endif |
792 | |
793 | do |
794 | { |
795 | b = *in++; |
796 | v = (FMSTR_U8)(b & 0x7fU); |
797 | |
798 | if (size > 0U) |
799 | { |
800 | *dest |= (FMSTR_U8)((v << shift)); |
801 | shift += 7U; |
802 | |
803 | if (shift >= 8U) |
804 | { |
805 | shift -= 8U; |
806 | dest += dadd; |
807 | size--; |
808 | |
809 | if (size > 0U && shift > 0U) |
810 | { |
811 | *dest |= (FMSTR_U8)(v >> (7U - shift)); |
812 | } |
813 | } |
814 | } |
815 | } while ((b & 0x80U) != 0U); |
816 | |
817 | /* negative number? */ |
818 | if (sleb != FMSTR_FALSE && (b & 0x40U) != 0U) |
819 | { |
820 | if (size > 0U) |
821 | { |
822 | *dest |= (FMSTR_U8)(0xffU << shift); |
823 | dest += dadd; |
824 | size--; |
825 | } |
826 | |
827 | while (size > 0U) |
828 | { |
829 | *dest = 0xffU; |
830 | dest += dadd; |
831 | size--; |
832 | } |
833 | } |
834 | |
835 | return in; |
836 | } |
837 | |
838 | FMSTR_BPTR FMSTR_UlebDecode(FMSTR_BPTR in, void *result, FMSTR_SIZE size) |
839 | { |
840 | return FMSTR_LebDecode(in, result, size, FMSTR_FALSE); |
841 | } |
842 | |
843 | FMSTR_BPTR FMSTR_SlebDecode(FMSTR_BPTR in, void *result, FMSTR_SIZE size) |
844 | { |
845 | return FMSTR_LebDecode(in, result, size, FMSTR_TRUE); |
846 | } |
847 | |
848 | /****************************************************************************** |
849 | * |
850 | * @brief Encode unsigned variable to ULEB record |
851 | * |
852 | ******************************************************************************/ |
853 | |
854 | FMSTR_BPTR FMSTR_UlebEncode(FMSTR_BPTR out, void *source, FMSTR_SIZE size) |
855 | { |
856 | FMSTR_BCHR b; |
857 | FMSTR_U8 v; |
858 | FMSTR_U8 *src; |
859 | FMSTR_INDEX sadd; |
860 | FMSTR_SIZE shift = 0; |
861 | FMSTR_SIZE zeroes = 0; |
862 | |
863 | #if FMSTR_PLATFORM_BIG_ENDIAN > 0 |
864 | src = (FMSTR_U8 *)source; |
865 | while (zeroes < size) |
866 | { |
867 | if (*src++ == 0U) |
868 | { |
869 | zeroes++; |
870 | } |
871 | else |
872 | { |
873 | break; |
874 | } |
875 | } |
876 | src = ((FMSTR_U8 *)source) + size - 1; |
877 | sadd = -1; |
878 | #else |
879 | src = ((FMSTR_U8 *)source) + size - 1; |
880 | while (zeroes < size) |
881 | { |
882 | if (*src-- == 0U) |
883 | { |
884 | zeroes++; |
885 | } |
886 | else |
887 | { |
888 | break; |
889 | } |
890 | } |
891 | src = (FMSTR_U8 *)source; |
892 | sadd = 1; |
893 | #endif |
894 | |
895 | /* now: 'zeroes' is number of useless most-significant zero bytes |
896 | * 'src' points to least-significant byte and 'sadd' is a direction */ |
897 | if (zeroes < size) |
898 | { |
899 | /* we will not encode the zero bytes */ |
900 | size -= zeroes; |
901 | |
902 | /* start with zero (will be or-ing to it) */ |
903 | *out = 0; |
904 | |
905 | while (size-- > 0U) |
906 | { |
907 | v = *src; |
908 | src += sadd; |
909 | |
910 | b = ((FMSTR_BCHR)(v << shift)) & 0x7FU; |
911 | *out |= b; |
912 | |
913 | // shift is number of bits remaining in v |
914 | v = (FMSTR_U8)(v >> (7U - shift)); |
915 | |
916 | // other bits to the next out byte |
917 | if (size > 0U || v != 0U) |
918 | { |
919 | *out++ |= 0x80U; |
920 | *out = (FMSTR_BCHR)(v & 0x7FU); |
921 | } |
922 | |
923 | shift++; |
924 | |
925 | if (shift >= 8U) |
926 | { |
927 | v >>= 7; |
928 | |
929 | if (size > 0U || v != 0U) |
930 | { |
931 | *out++ |= 0x80U; |
932 | *out = (FMSTR_BCHR)(v & 0x7FU); |
933 | } |
934 | |
935 | shift = 1; |
936 | } |
937 | } |
938 | |
939 | out++; |
940 | } |
941 | else |
942 | { |
943 | /* variable is equal to 0, this encodes as a single 0 byte */ |
944 | *out++ = 0; |
945 | } |
946 | |
947 | return out; |
948 | } |
949 | |
950 | /****************************************************************************** |
951 | * |
952 | * @brief Skip the LEB field in buffer (it doesn't matter if signed or unsigned) |
953 | * |
954 | * @param dest - Pointer to LEB field |
955 | * |
956 | * @return pointer to buffer just behind LEB field |
957 | * |
958 | ******************************************************************************/ |
959 | |
960 | FMSTR_BPTR FMSTR_SkipInBufferLeb(FMSTR_BPTR dest) |
961 | { |
962 | FMSTR_BCHR b; |
963 | do |
964 | { |
965 | b = *(dest++); |
966 | } while ((b & 0x80U) != 0U); |
967 | |
968 | return dest; |
969 | } |
970 | |
971 | /****************************************************************************** |
972 | * |
973 | * @brief Get string from incomming buffer |
974 | * |
975 | ******************************************************************************/ |
976 | |
977 | FMSTR_BPTR FMSTR_StringFromBuffer(FMSTR_BPTR in, FMSTR_CHAR *pStr, FMSTR_SIZE maxSize) |
978 | { |
979 | FMSTR_BCHR b; |
980 | |
981 | do |
982 | { |
983 | in = FMSTR_ValueFromBuffer8(&b, in); |
984 | |
985 | if (maxSize > 0U) |
986 | { |
987 | maxSize--; |
988 | *pStr++ = (FMSTR_CHAR)(maxSize > 0U ? b : 0U); |
989 | } |
990 | } while (b != 0U); |
991 | |
992 | return in; |
993 | } |
994 | |
995 | /****************************************************************************** |
996 | * |
997 | * @brief Copy string from memory to outcomming buffer |
998 | * |
999 | ******************************************************************************/ |
1000 | |
1001 | FMSTR_BPTR FMSTR_StringCopyToBuffer(FMSTR_BPTR out, const FMSTR_CHAR *pStr) |
1002 | { |
1003 | while (*pStr != (FMSTR_CHAR)0) |
1004 | { |
1005 | out = FMSTR_ValueToBuffer8(out, (FMSTR_U8)(*pStr++)); |
1006 | } |
1007 | |
1008 | out = FMSTR_ValueToBuffer8(out, (FMSTR_U8)0); |
1009 | return out; |
1010 | } |
1011 | |
1012 | /****************************************************************************** |
1013 | * |
1014 | * @brief Initialize CRC16 calculation |
1015 | * |
1016 | ******************************************************************************/ |
1017 | |
1018 | void FMSTR_Crc16Init(FMSTR_U16 *crc) |
1019 | { |
1020 | *crc = FMSTR_CRC16_CCITT_SEED; |
1021 | } |
1022 | |
1023 | /****************************************************************************** |
1024 | * |
1025 | * @brief Add new byte to CRC16 calculation |
1026 | * |
1027 | ******************************************************************************/ |
1028 | |
1029 | void FMSTR_Crc16AddByte(FMSTR_U16 *crc, FMSTR_U8 data) |
1030 | { |
1031 | FMSTR_INDEX x; |
1032 | |
1033 | *crc ^= ((FMSTR_U16)data) << 8; /* XOR hi-byte of CRC w/dat */ |
1034 | for (x = 8; x != 0; x--) /* Then, for 8 bit shifts... */ |
1035 | { |
1036 | if ((*crc & 0x8000U) != 0U) /* Test hi order bit of CRC */ |
1037 | { |
1038 | *crc = *crc << 1 ^ 0x1021U; /* if set, shift & XOR w/$1021 */ |
1039 | } |
1040 | else |
1041 | { |
1042 | *crc <<= 1; /* Else, just shift left once. */ |
1043 | } |
1044 | } |
1045 | } |
1046 | |
1047 | /****************************************************************************** |
1048 | * |
1049 | * @brief Initialize CRC8 calculation |
1050 | * |
1051 | ******************************************************************************/ |
1052 | |
1053 | void FMSTR_Crc8Init(FMSTR_U8 *crc) |
1054 | { |
1055 | *crc = FMSTR_CRC8_CCITT_SEED; |
1056 | } |
1057 | |
1058 | /****************************************************************************** |
1059 | * |
1060 | * @brief Add new byte to CRC8 calculation |
1061 | * |
1062 | ******************************************************************************/ |
1063 | |
1064 | void FMSTR_Crc8AddByte(FMSTR_U8 *crc, FMSTR_U8 data) |
1065 | { |
1066 | FMSTR_INDEX x; |
1067 | |
1068 | *crc ^= data; /* XOR hi-byte of CRC w/dat */ |
1069 | for (x = 8; x != 0; x--) /* Then, for 8 bit shifts... */ |
1070 | { |
1071 | if ((*crc & 0x80U) != 0U) /* Test hi order bit of CRC */ |
1072 | { |
1073 | *crc = (FMSTR_U8)((*crc << 1) ^ 0x07U); /* if set, shift & XOR w/$07 */ |
1074 | } |
1075 | else |
1076 | { |
1077 | *crc <<= 1; /* Else, just shift left once. */ |
1078 | } |
1079 | } |
1080 | } |
1081 | |
1082 | /****************************************************************************** |
1083 | * |
1084 | * @brief Get array of random numbers |
1085 | * |
1086 | ******************************************************************************/ |
1087 | |
1088 | FMSTR_BPTR FMSTR_RandomNumbersToBuffer(FMSTR_BPTR out, FMSTR_SIZE length) |
1089 | { |
1090 | FMSTR_BPTR ret = out; |
1091 | |
1092 | #if FMSTR_CFG_F1_RESTRICTED_ACCESS != 0 |
1093 | FMSTR_SIZE i; |
1094 | FMSTR_SIZE sz; |
1095 | FMSTR_U32 r; |
1096 | FMSTR_U8 *dest = out; |
1097 | FMSTR_U8 div; |
1098 | |
1099 | for (i = 0U; i < length; i += 4U) |
1100 | { |
1101 | /* TODO: this generator uses stdlib rand implemntation by default, which is weak. |
1102 | * replace this by defining your own FMSTR_Rand() macro and replace it by |
1103 | * a true random number engine. */ |
1104 | |
1105 | /* Achieve somewhat better entropy by skipping random number of sequenced numbers */ |
1106 | div = (FMSTR_U8)FMSTR_Rand(); |
1107 | while (div-- > 0U) |
1108 | { |
1109 | r = (FMSTR_U32)FMSTR_Rand(); |
1110 | } |
1111 | |
1112 | sz = length - i; |
1113 | if (sz > 4U) |
1114 | { |
1115 | sz = 4U; |
1116 | } |
1117 | |
1118 | ret = FMSTR_CopyToBuffer(&dest[i], (FMSTR_ADDR)&r, sz); |
1119 | } |
1120 | #else |
1121 | /* this function should not be called when restricted access is not required */ |
1122 | FMSTR_ASSERT(0 == 1); |
1123 | FMSTR_UNUSED(out); |
1124 | FMSTR_UNUSED(length); |
1125 | #endif /* FMSTR_CFG_F1_RESTRICTED_ACCESS */ |
1126 | |
1127 | return ret; |
1128 | } |
1129 | |
1130 | /****************************************************************************** |
1131 | * |
1132 | * @brief Get array of random numbers |
1133 | * |
1134 | ******************************************************************************/ |
1135 | |
1136 | void FMSTR_Randomize(FMSTR_U32 entropy) |
1137 | { |
1138 | #if FMSTR_CFG_F1_RESTRICTED_ACCESS != 0 |
1139 | FMSTR_U32 r; |
1140 | |
1141 | /* skip next few numbers in rand sequence to achieve a better behavior */ |
1142 | entropy &= 15U; |
1143 | |
1144 | do |
1145 | { |
1146 | r = (FMSTR_U32)FMSTR_Rand(); |
1147 | FMSTR_UNUSED(r); |
1148 | } while (entropy-- > 0U); |
1149 | #else |
1150 | /* this function should not be called when restricted access is not required */ |
1151 | FMSTR_ASSERT(0 == 1); |
1152 | FMSTR_UNUSED(entropy); |
1153 | #endif /* FMSTR_CFG_F1_RESTRICTED_ACCESS */ |
1154 | } |
1155 | |
1156 | /****************************************************************************** |
1157 | * |
1158 | * @brief The function prepares ring buffer |
1159 | * |
1160 | ******************************************************************************/ |
1161 | |
1162 | void _FMSTR_RingBuffCreate(FMSTR_RING_BUFFER *ringBuff, FMSTR_BPTR buffer, FMSTR_U32 size) |
1163 | { |
1164 | FMSTR_ASSERT(ringBuff != NULL); |
1165 | FMSTR_ASSERT(buffer != NULL); |
1166 | |
1167 | FMSTR_MemSet(ringBuff, 0, sizeof(FMSTR_RING_BUFFER)); |
1168 | |
1169 | ringBuff->buffer = buffer; |
1170 | ringBuff->size = size; |
1171 | ringBuff->rp = buffer; |
1172 | ringBuff->wp = buffer; |
1173 | } |
1174 | |
1175 | /****************************************************************************** |
1176 | * |
1177 | * @brief The function adds character into ring buffer |
1178 | * |
1179 | ******************************************************************************/ |
1180 | |
1181 | void _FMSTR_RingBuffPut(FMSTR_RING_BUFFER *ringBuff, FMSTR_BCHR nRxChar) |
1182 | { |
1183 | FMSTR_BPTR wpnext; |
1184 | |
1185 | FMSTR_ASSERT(ringBuff != NULL); |
1186 | |
1187 | /* future value of write pointer */ |
1188 | wpnext = ringBuff->wp + 1; |
1189 | |
1190 | if (wpnext >= (ringBuff->buffer + ringBuff->size)) |
1191 | { |
1192 | wpnext = ringBuff->buffer; |
1193 | } |
1194 | |
1195 | /* any space in queue? */ |
1196 | if (wpnext != ringBuff->rp) |
1197 | { |
1198 | *ringBuff->wp = (FMSTR_U8)nRxChar; |
1199 | ringBuff->wp = wpnext; |
1200 | } |
1201 | } |
1202 | |
1203 | /****************************************************************************** |
1204 | * |
1205 | * @brief The function gets character from ring buffer |
1206 | * |
1207 | ******************************************************************************/ |
1208 | |
1209 | FMSTR_BCHR _FMSTR_RingBuffGet(FMSTR_RING_BUFFER *ringBuff) |
1210 | { |
1211 | FMSTR_BCHR nChar = 0U; |
1212 | |
1213 | FMSTR_ASSERT(ringBuff != NULL); |
1214 | |
1215 | /* get all queued characters */ |
1216 | if (ringBuff->rp != ringBuff->wp) |
1217 | { |
1218 | FMSTR_BPTR rpnext = ringBuff->rp; |
1219 | nChar = *rpnext++; |
1220 | |
1221 | if (rpnext >= (ringBuff->buffer + ringBuff->size)) |
1222 | { |
1223 | rpnext = ringBuff->buffer; |
1224 | } |
1225 | |
1226 | ringBuff->rp = rpnext; |
1227 | } |
1228 | |
1229 | return nChar; |
1230 | } |
1231 | |
1232 | /****************************************************************************** |
1233 | * |
1234 | * @brief The function returns true, when is space in ring buffer |
1235 | * |
1236 | ******************************************************************************/ |
1237 | |
1238 | FMSTR_BOOL _FMSTR_RingBuffIsSpace(FMSTR_RING_BUFFER *ringBuff) |
1239 | { |
1240 | FMSTR_BPTR wpnext; |
1241 | |
1242 | FMSTR_ASSERT(ringBuff != NULL); |
1243 | |
1244 | wpnext = ringBuff->wp + 1; |
1245 | |
1246 | /* Is any space in buffer? */ |
1247 | if (wpnext != ringBuff->rp) |
1248 | { |
1249 | return FMSTR_TRUE; |
1250 | } |
1251 | |
1252 | return FMSTR_FALSE; |
1253 | } |
1254 | |
1255 | /****************************************************************************** |
1256 | * |
1257 | * @brief The function returns true, when some data in ring buffer |
1258 | * |
1259 | ******************************************************************************/ |
1260 | |
1261 | FMSTR_BOOL _FMSTR_RingBuffHasData(FMSTR_RING_BUFFER *ringBuff) |
1262 | { |
1263 | FMSTR_ASSERT(ringBuff != NULL); |
1264 | |
1265 | /* Is any data available to get from buffer? */ |
1266 | if (ringBuff->rp != ringBuff->wp) |
1267 | { |
1268 | return FMSTR_TRUE; |
1269 | } |
1270 | |
1271 | return FMSTR_FALSE; |
1272 | } |
1273 | |
1274 | /****************************************************************************** |
1275 | * |
1276 | * @brief Compare helper |
1277 | * |
1278 | ******************************************************************************/ |
1279 | |
1280 | FMSTR_INLINE FMSTR_INDEX _FMSTR_Compare(FMSTR_U8 c1, FMSTR_U8 c2) |
1281 | { |
1282 | if (c1 < c2) |
1283 | { |
1284 | return -1; |
1285 | } |
1286 | if (c1 > c2) |
1287 | { |
1288 | return +1; |
1289 | } |
1290 | return 0; |
1291 | } |
1292 | |
1293 | /****************************************************************************** |
1294 | * |
1295 | * @brief Standard strcmp library function |
1296 | * |
1297 | ******************************************************************************/ |
1298 | |
1299 | FMSTR_INDEX _FMSTR_StrCmp(const FMSTR_CHAR *str1, const FMSTR_CHAR *str2) |
1300 | { |
1301 | FMSTR_INDEX cmp = 0; |
1302 | FMSTR_U8 c1, c2; |
1303 | |
1304 | const FMSTR_U8 *s1 = (const FMSTR_U8 *)str1; |
1305 | const FMSTR_U8 *s2 = (const FMSTR_U8 *)str2; |
1306 | |
1307 | if (str1 == str2) |
1308 | { |
1309 | return 0; |
1310 | } |
1311 | |
1312 | FMSTR_ASSERT_RETURN(str1 != NULL, 1); |
1313 | FMSTR_ASSERT_RETURN(str2 != NULL, -1); |
1314 | |
1315 | do |
1316 | { |
1317 | c1 = *s1++; |
1318 | c2 = *s2++; |
1319 | cmp = _FMSTR_Compare(c1, c2); |
1320 | if (cmp != 0) |
1321 | { |
1322 | return cmp; |
1323 | } |
1324 | } while (c1 != 0U && c2 != 0U); |
1325 | |
1326 | return 0; |
1327 | } |
1328 | |
1329 | FMSTR_INDEX _FMSTR_MemCmp(const void *b1, const void *b2, FMSTR_SIZE size) |
1330 | { |
1331 | const FMSTR_U8 *p1 = (const FMSTR_U8 *)b1; |
1332 | const FMSTR_U8 *p2 = (const FMSTR_U8 *)b2; |
1333 | FMSTR_INDEX cmp; |
1334 | FMSTR_U8 c1, c2; |
1335 | |
1336 | if (p1 == p2) |
1337 | { |
1338 | return 0; |
1339 | } |
1340 | |
1341 | FMSTR_ASSERT_RETURN(p1 != NULL, 1); |
1342 | FMSTR_ASSERT_RETURN(p2 != NULL, -1); |
1343 | |
1344 | while (size-- > 0U) |
1345 | { |
1346 | c1 = *p1++; |
1347 | c2 = *p2++; |
1348 | cmp = _FMSTR_Compare(c1, c2); |
1349 | if (cmp != 0) |
1350 | { |
1351 | return cmp; |
1352 | } |
1353 | } |
1354 | |
1355 | return 0; |
1356 | } |
1357 | |
1358 | FMSTR_SIZE _FMSTR_StrLen(const FMSTR_CHAR *str) |
1359 | { |
1360 | const FMSTR_CHAR *s = str; |
1361 | FMSTR_SIZE len = 0; |
1362 | |
1363 | while (*s++ != (FMSTR_CHAR)0) |
1364 | { |
1365 | len++; |
1366 | } |
1367 | |
1368 | return len; |
1369 | } |
1370 | |
1371 | void _FMSTR_MemSet(void *dest, FMSTR_U8 fill, FMSTR_SIZE size) |
1372 | { |
1373 | FMSTR_U8 *d = dest; |
1374 | while (size-- > 0U) |
1375 | { |
1376 | *d++ = fill; |
1377 | } |
1378 | } |
1379 | |
1380 | FMSTR_U32 _FMSTR_Rand(void) |
1381 | { |
1382 | /* Random number generation not yet implemented, use stdlib function */ |
1383 | FMSTR_ASSERT(0 == 1); |
1384 | return 0; |
1385 | } |
1386 | |
1387 | #endif /* !FMSTR_DISABLE */ |
1388 | |