1 /******************************************************************************
2 * Copyright (c) 2022 Telink Semiconductor (Shanghai) Co., Ltd. ("TELINK")
3 * All rights reserved.
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 *****************************************************************************/
18 #include "utility.h"
19 #include "drivers.h"
20 #include "tl_common.h"
21
22 // general swap/endianess utils
23
swapN(unsigned char * p,int n)24 void swapN(unsigned char *p, int n)
25 {
26 int i, c;
27 for (i = 0; i < n / 2; i++) {
28 c = p[i];
29 p[i] = p[n - 1 - i];
30 p[n - 1 - i] = c;
31 }
32 }
33
swapX(const u8 * src,u8 * dst,int len)34 void swapX(const u8 *src, u8 *dst, int len)
35 {
36 int i;
37 for (i = 0; i < len; i++) {
38 dst[len - 1 - i] = src[i];
39 }
40 }
41
swap24(u8 dst[3],const u8 src[3])42 void swap24(u8 dst[3], const u8 src[3])
43 {
44 swapX(src, dst, 3);
45 }
46
swap32(u8 dst[4],const u8 src[4])47 void swap32(u8 dst[4], const u8 src[4])
48 {
49 swapX(src, dst, 4);
50 }
51
swap48(u8 dst[7],const u8 src[7])52 void swap48(u8 dst[7], const u8 src[7])
53 {
54 swapX(src, dst, 6);
55 }
56
swap56(u8 dst[7],const u8 src[7])57 void swap56(u8 dst[7], const u8 src[7])
58 {
59 swapX(src, dst, 7);
60 }
61
swap64(u8 dst[8],const u8 src[8])62 void swap64(u8 dst[8], const u8 src[8])
63 {
64 swapX(src, dst, 8);
65 }
66
swap128(u8 dst[16],const u8 src[16])67 void swap128(u8 dst[16], const u8 src[16])
68 {
69 swapX(src, dst, 16);
70 }
71
flip_addr(u8 * dest,u8 * src)72 void flip_addr(u8 *dest, u8 *src)
73 {
74 dest[0] = src[5];
75 dest[1] = src[4];
76 dest[2] = src[3];
77 dest[3] = src[2];
78 dest[4] = src[1];
79 dest[5] = src[0];
80 }
81
my_fifo_init(my_fifo_t * f,int s,u8 n,u8 * p)82 void my_fifo_init(my_fifo_t *f, int s, u8 n, u8 *p)
83 {
84 f->size = s;
85 f->num = n;
86 f->wptr = 0;
87 f->rptr = 0;
88 f->p = p;
89 }
90
my_fifo_wptr(my_fifo_t * f)91 u8 *my_fifo_wptr(my_fifo_t *f)
92 {
93 if (((f->wptr - f->rptr) & 255) < f->num) {
94 return f->p + (f->wptr & (f->num - 1)) * f->size;
95 }
96 return 0;
97 }
98
my_fifo_wptr_v2(my_fifo_t * f)99 u8 *my_fifo_wptr_v2(my_fifo_t *f)
100 {
101 if (((f->wptr - f->rptr) & 255) < f->num - 3) { // keep 3 fifo left for others evt
102 return f->p + (f->wptr & (f->num - 1)) * f->size;
103 }
104 return 0;
105 }
106
my_fifo_next(my_fifo_t * f)107 void my_fifo_next(my_fifo_t *f)
108 {
109 f->wptr++;
110 }
111
my_fifo_push(my_fifo_t * f,u8 * p,int n)112 int my_fifo_push(my_fifo_t *f, u8 *p, int n)
113 {
114 if (((f->wptr - f->rptr) & 255) >= f->num) {
115 return -1;
116 }
117
118 if (n >= f->size) {
119 return -1;
120 }
121 u8 *pd = f->p + (f->wptr++ & (f->num - 1)) * f->size;
122 *pd++ = n & 0xff;
123 *pd++ = (n >> 8) & 0xff;
124 memcpy(pd, p, n);
125 return 0;
126 }
127
my_fifo_pop(my_fifo_t * f)128 void my_fifo_pop(my_fifo_t *f)
129 {
130 f->rptr++;
131 }
132
my_fifo_get(my_fifo_t * f)133 u8 *my_fifo_get(my_fifo_t *f)
134 {
135 if (f->rptr != f->wptr) {
136 u8 *p = f->p + (f->rptr & (f->num - 1)) * f->size;
137 return p;
138 }
139 return 0;
140 }
141