1 #include <common/kfifo.h>
2 #include <common/glib.h>
3 #include <common/errno.h>
4 #include <common/compiler.h>
5 #include <mm/slab.h>
6
7 /**
8 * @brief 通过动态方式初始化kfifo缓冲队列
9 *
10 * @param fifo 队列结构体
11 * @param size 缓冲区大小
12 * @param reserved 暂时保留，请置为0
13 * @return int 错误码: NOMEM; 成功->0
14 */
kfifo_alloc(struct kfifo_t * fifo,uint32_t size,uint64_t reserved)15 int kfifo_alloc(struct kfifo_t *fifo, uint32_t size, uint64_t reserved)
16 {
17 memset(fifo, 0, sizeof(struct kfifo_t));
18 fifo->buffer = kmalloc(size, 0);
19 if (fifo->buffer == NULL)
20 goto failed;
21
22 fifo->total_size = size;
23 return 0;
24 failed:;
25 return -ENOMEM;
26 }
27
28 /**
29 * @brief 使用指定的缓冲区来初始化kfifo缓冲队列
30 *
31 * @param fifo 队列结构体
32 * @param buffer 缓冲区
33 * @param size 缓冲区大小
34 */
kfifo_init(struct kfifo_t * fifo,void * buffer,uint32_t size)35 void kfifo_init(struct kfifo_t *fifo, void *buffer, uint32_t size)
36 {
37 memset(fifo, 0, sizeof(struct kfifo_t));
38
39 fifo->buffer = buffer;
40 fifo->total_size = size;
41 }
42
43 /**
44 * @brief 向kfifo缓冲区推入数据
45 *
46 * @param fifo 队列结构体
47 * @param from 来源数据地址
48 * @param size 数据大小（字节数）
49 * @return uint32_t 推入的数据大小
50 */
kfifo_in(struct kfifo_t * fifo,const void * from,uint32_t size)51 uint32_t kfifo_in(struct kfifo_t *fifo, const void *from, uint32_t size)
52 {
53 // 判断空间是否够
54 if (unlikely(fifo->size + size > fifo->total_size))
55 return 0;
56 if (unlikely(from == NULL))
57 return 0;
58
59 // 分两种情况，一种是要发生回环，另一种不发生回环
60 if (fifo->in_offset + size > fifo->total_size) // 发生回环
61 {
62 uint32_t tmp = fifo->total_size - fifo->in_offset;
63 memcpy(fifo->buffer + fifo->in_offset, from, tmp);
64 memcpy(fifo->buffer, from + tmp, size - tmp);
65 fifo->in_offset = size - tmp;
66 }
67 else // 不发生回环
68 {
69 memcpy(fifo->buffer + fifo->in_offset, from, size);
70 fifo->in_offset += size;
71 }
72
73 fifo->size += size;
74
75 return size;
76 }
77
78 /**
79 * @brief 从kfifo缓冲区取出数据，并从队列中删除数据
80 *
81 * @param fifo 队列结构体
82 * @param to 拷贝目标地址
83 * @param size 数据大小（字节数）
84 * @return uint32_t 取出的数据大小
85 */
kfifo_out(struct kfifo_t * fifo,void * to,uint32_t size)86 uint32_t kfifo_out(struct kfifo_t *fifo, void *to, uint32_t size)
87 {
88 if (unlikely(to == NULL)) // 判断目标地址是否为空
89 return 0;
90 if (unlikely(size > fifo->size)) // 判断队列中是否有这么多数据
91 return 0;
92
93 // 判断是否会发生回环
94 if (fifo->out_offset + size > fifo->total_size) // 发生回环
95 {
96 uint32_t tmp = fifo->total_size - fifo->out_offset;
97 memcpy(to, fifo->buffer + fifo->out_offset, tmp);
98 memcpy(to + tmp, fifo->buffer, size - tmp);
99 fifo->out_offset = size - tmp;
100 }
101 else // 未发生回环
102 {
103 memcpy(to, fifo->buffer + fifo->out_offset, size);
104 fifo->out_offset += size;
105 }
106
107 fifo->size -= size;
108
109 return size;
110 }
111
112 /**
113 * @brief 从kfifo缓冲区取出数据，但是不从队列中删除数据
114 *
115 * @param fifo 队列结构体
116 * @param to 拷贝目标地址
117 * @param size 数据大小（字节数）
118 * @return uint32_t 取出的数据大小
119 */
kfifo_out_peek(struct kfifo_t * fifo,void * to,uint32_t size)120 uint32_t kfifo_out_peek(struct kfifo_t *fifo, void *to, uint32_t size)
121 {
122 if (unlikely(to == NULL)) // 判断目标地址是否为空
123 return 0;
124 if (unlikely(size > fifo->size)) // 判断队列中是否有这么多数据
125 return 0;
126
127 // 判断是否会发生回环
128 if (fifo->out_offset + size > fifo->total_size) // 发生回环
129 {
130 uint32_t tmp = fifo->total_size - fifo->out_offset;
131 memcpy(to, fifo->buffer + fifo->out_offset, tmp);
132 memcpy(to + tmp, fifo->buffer, size - tmp);
133 }
134 else // 未发生回环
135 {
136 memcpy(to, fifo->buffer + fifo->out_offset, size);
137 }
138
139 return size;
140 }
141
142 /**
143 * @brief 释放通过kfifo_alloc创建的fifo缓冲区
144 *
145 * @param fifo fifo队列结构体
146 */
kfifo_free_alloc(struct kfifo_t * fifo)147 void kfifo_free_alloc(struct kfifo_t *fifo)
148 {
149 kfree(fifo->buffer);
150 memset(fifo, 0, sizeof(struct kfifo_t));
151 }