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 */
35FMSTR_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 */
56FMSTR_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 */
77FMSTR_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 */
94FMSTR_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 */
111FMSTR_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 */
128FMSTR_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 */
146FMSTR_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 */
161FMSTR_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 */
176FMSTR_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 */
194static 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 */
261static 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 */
328static 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
412void _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
427FMSTR_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
480FMSTR_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 ******************************************************************************/
533FMSTR_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
596FMSTR_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
614FMSTR_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
632FMSTR_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
643FMSTR_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
654FMSTR_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
665FMSTR_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
676FMSTR_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
687FMSTR_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
698FMSTR_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
709FMSTR_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
720FMSTR_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
732FMSTR_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
747FMSTR_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
774static 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
838FMSTR_BPTR FMSTR_UlebDecode(FMSTR_BPTR in, void *result, FMSTR_SIZE size)
839{
840 return FMSTR_LebDecode(in, result, size, FMSTR_FALSE);
841}
842
843FMSTR_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
854FMSTR_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
960FMSTR_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
977FMSTR_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
1001FMSTR_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
1018void 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
1029void 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
1053void 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
1064void 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
1088FMSTR_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
1136void 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
1162void _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
1181void _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
1209FMSTR_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
1238FMSTR_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
1261FMSTR_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
1280FMSTR_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
1299FMSTR_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
1329FMSTR_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
1358FMSTR_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
1371void _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
1380FMSTR_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