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 "tl_common.h"
19 #include "drivers.h"
20 #include "utility.h"
21
22
23 // general swap/endianess utils
24
swapN(unsigned char * p,int n)25 void swapN(unsigned char *p, int n)
26 {
27 int i, c;
28 for (i=0; i<n/2; i++)
29 {
30 c = p[i];
31 p[i] = p[n - 1 - i];
32 p[n - 1 - i] = c;
33 }
34 }
35
swapX(const u8 * src,u8 * dst,int len)36 void swapX(const u8 *src, u8 *dst, int len)
37 {
38 int i;
39 for (i = 0; i < len; i++)
40 dst[len - 1 - i] = src[i];
41 }
42
swap24(u8 dst[3],const u8 src[3])43 void swap24(u8 dst[3], const u8 src[3])
44 {
45 swapX(src, dst, 3);
46 }
47
swap32(u8 dst[4],const u8 src[4])48 void swap32(u8 dst[4], const u8 src[4])
49 {
50 swapX(src, dst, 4);
51 }
52
swap48(u8 dst[7],const u8 src[7])53 void swap48(u8 dst[7], const u8 src[7])
54 {
55 swapX(src, dst, 6);
56 }
57
swap56(u8 dst[7],const u8 src[7])58 void swap56(u8 dst[7], const u8 src[7])
59 {
60 swapX(src, dst, 7);
61 }
62
swap64(u8 dst[8],const u8 src[8])63 void swap64(u8 dst[8], const u8 src[8])
64 {
65 swapX(src, dst, 8);
66 }
67
swap128(u8 dst[16],const u8 src[16])68 void swap128(u8 dst[16], const u8 src[16])
69 {
70 swapX(src, dst, 16);
71 }
72
73
74
flip_addr(u8 * dest,u8 * src)75 void flip_addr(u8 *dest, u8 *src){
76 dest[0] = src[5];
77 dest[1] = src[4];
78 dest[2] = src[3];
79 dest[3] = src[2];
80 dest[4] = src[1];
81 dest[5] = src[0];
82 }
83
84
85
my_fifo_init(my_fifo_t * f,int s,u8 n,u8 * p)86 void my_fifo_init (my_fifo_t *f, int s, u8 n, u8 *p)
87 {
88 f->size = s;
89 f->num = n;
90 f->wptr = 0;
91 f->rptr = 0;
92 f->p = p;
93 }
94
my_fifo_wptr(my_fifo_t * f)95 u8* my_fifo_wptr (my_fifo_t *f)
96 {
97 if (((f->wptr - f->rptr) & 255) < f->num)
98 {
99 return f->p + (f->wptr & (f->num-1)) * f->size;
100 }
101 return 0;
102 }
103
my_fifo_wptr_v2(my_fifo_t * f)104 u8* my_fifo_wptr_v2 (my_fifo_t *f)
105 {
106 if (((f->wptr - f->rptr) & 255) < f->num - 3) //keep 3 fifo left for others evt
107 {
108 return f->p + (f->wptr & (f->num-1)) * f->size;
109 }
110 return 0;
111 }
112
my_fifo_next(my_fifo_t * f)113 void my_fifo_next (my_fifo_t *f)
114 {
115 f->wptr++;
116 }
117
my_fifo_push(my_fifo_t * f,u8 * p,int n)118 int my_fifo_push (my_fifo_t *f, u8 *p, int n)
119 {
120 if (((f->wptr - f->rptr) & 255) >= f->num)
121 {
122 return -1;
123 }
124
125 if (n >= f->size)
126 {
127 return -1;
128 }
129 u8 *pd = f->p + (f->wptr++ & (f->num-1)) * f->size;
130 *pd++ = n & 0xff;
131 *pd++ = (n >> 8) & 0xff;
132 memcpy (pd, p, n);
133 return 0;
134 }
135
my_fifo_pop(my_fifo_t * f)136 void my_fifo_pop (my_fifo_t *f)
137 {
138 f->rptr++;
139 }
140
my_fifo_get(my_fifo_t * f)141 u8 * my_fifo_get (my_fifo_t *f)
142 {
143 if (f->rptr != f->wptr)
144 {
145 u8 *p = f->p + (f->rptr & (f->num-1)) * f->size;
146 return p;
147 }
148 return 0;
149 }
150
151
