1 /*
2  * Copyright (c) 2020-2023 Huawei Device Co., Ltd.
3  *
4  * HDF is dual licensed: you can use it either under the terms of
5  * the GPL, or the BSD license, at your option.
6  * See the LICENSE file in the root of this repository for complete details.
7  */
8 
9 #include "flow_control.h"
10 
11 #include "flow_control_task.h"
12 #include "securec.h"
13 #include "hdf_log.h"
14 #include "osal_mem.h"
15 #include "net_device.h"
16 #include "net_device_adapter.h"
17 
18 #define HDF_LOG_TAG HDF_WIFI_CORE
19 #define ETHER_TYPE_PROCESS_COUNT 8
20 #define TOS_TO_ID_COUNT 6
21 #define PROTOCOL_STANDARD_SHIFT_COUNT 2
22 static FlowControlQueueID g_tosToIdHash[TOS_TO_ID_COUNT] = {
23     BE_QUEUE_ID, BK_QUEUE_ID, BK_QUEUE_ID, BE_QUEUE_ID, VI_QUEUE_ID, VI_QUEUE_ID
24 };
25 
26 static FlowControlQueueID IpProcessFunc(const void *buff, uint32_t len);
27 static struct EtherProcessMap g_etherTypeProMap[ETHER_TYPE_PROCESS_COUNT] = {
28     {ETHER_TYPE_IP, IpProcessFunc},
29     {ETHER_TYPE_IPV6, NULL},
30     {ETHER_TYPE_PAE, NULL},
31     {ETHER_TYPE_TDLS, NULL},
32     {ETHER_TYPE_PPP_DISC, NULL},
33     {ETHER_TYPE_PPP_SES, NULL},
34     {ETHER_TYPE_WAI, NULL},
35     {ETHER_TYPE_VLAN, NULL},
36 };
37 
TosToFcQueueId(uint8_t priority)38 static FlowControlQueueID TosToFcQueueId(uint8_t priority)
39 {
40     if (priority >= TOS_TO_ID_COUNT) {
41         return VO_QUEUE_ID;
42     }
43     return g_tosToIdHash[priority];
44 }
45 
IsDhcpPort(struct UdpHeader * udpHdr,uint32_t len)46 static bool IsDhcpPort(struct UdpHeader *udpHdr, uint32_t len)
47 {
48     if (udpHdr == NULL) {
49         HDF_LOGE("%s udpHdr:  = null!", __func__);
50         return false;
51     }
52     if (len < sizeof(struct UdpHeader)) {
53         HDF_LOGE("%s len not right!", __func__);
54         return false;
55     }
56     if ((ntohs(udpHdr->dest) == DHCP_UDP_SRC_PORT) || (ntohs(udpHdr->dest) == DHCP_UDP_DES_PORT)) {
57         return true;
58     }
59     return false;
60 }
61 
62 /* TcpKeyFrame:ACK/SYN */
IsTcpKeyFrame(struct IpHeader * ipHeader,uint32_t len)63 static bool IsTcpKeyFrame(struct IpHeader *ipHeader, uint32_t len)
64 {
65     struct TcpHeader *tcpHdr = NULL;
66     uint32_t ipPktLen;
67     uint32_t tcpHdrLen;
68     uint8_t ipHdrLen;
69     uint8_t *ip = NULL;
70     if (ipHeader == NULL) {
71         HDF_LOGE("IsTcpAck Fail ipHeader = null");
72         return false;
73     }
74     if (len < sizeof(struct IpHeader)) {
75         HDF_LOGE("%s len not right!", __func__);
76         return false;
77     }
78     if ((len - sizeof(struct IpHeader)) < sizeof(struct TcpHeader)) {
79         HDF_LOGE("%s tcp hdr len not right!", __func__);
80         return false;
81     }
82 
83     tcpHdr = (struct TcpHeader *)(ipHeader + 1);
84     ip = (uint8_t *)(ipHeader);
85     ipPktLen = ntohs(ipHeader->totLen);
86     ipHdrLen = ((*ip) & 0x0F) << PROTOCOL_STANDARD_SHIFT_COUNT;
87     tcpHdrLen = (tcpHdr->offset & 0xF0) << PROTOCOL_STANDARD_SHIFT_COUNT;
88     if (tcpHdrLen + ipHdrLen == ipPktLen) {
89         return true;
90     }
91     return false;
92 }
93 
IpProcessFunc(const void * buff,uint32_t len)94 static FlowControlQueueID IpProcessFunc(const void *buff, uint32_t len)
95 {
96     struct IpHeader *ipHeader = NULL;
97     struct UdpHeader *udpHdr = NULL;
98     FlowControlQueueID id = NORMAL_QUEUE_ID;
99     if (buff == NULL) {
100         HDF_LOGE("%s fail: buff = null!", __func__);
101         return QUEUE_ID_COUNT;
102     }
103     if (len < sizeof(struct IpHeader)) {
104         HDF_LOGE("%s fail: IpHeader len not right!", __func__);
105         return QUEUE_ID_COUNT;
106     }
107     ipHeader = (struct IpHeader *)buff;
108     if (ipHeader->protocol == UDP_PROTOCOL) {
109         /* from IP TOS find priority */
110         /*
111          * ----------------------------------------------------------------------
112          *     tos define
113          *  ----------------------------------------------------------------------
114          * | bit7~bit5 | bit4  |  bit3      |  bit2       |   bit1            | bit0    |
115          * | priority  | Delay | Throughput | reliability | Transmission cost | reserve |
116          *  ----------------------------------------------------------------------
117          */
118         id = TosToFcQueueId(ipHeader->tos >> IP_PRI_SHIFT);
119         if ((len - sizeof(struct IpHeader)) < sizeof(struct UdpHeader)) {
120             HDF_LOGE("%s fail: UdpHeader len not right!", __func__);
121             return QUEUE_ID_COUNT;
122         }
123         udpHdr = (struct UdpHeader *)(ipHeader + 1);
124         if (((ntohs(ipHeader->fragInfo) & 0x1FFF) == 0) && IsDhcpPort(udpHdr, len - sizeof(struct IpHeader))) {
125             id = VIP_QUEUE_ID;
126         }
127     } else if (ipHeader->protocol == TCP_PROTOCOL) {
128         if (IsTcpKeyFrame(ipHeader, len)) {
129             id = TCP_ACK_QUEUE_ID;
130         } else {
131             id = TCP_DATA_QUEUE_ID;
132         }
133     }
134     return id;
135 }
136 
FlowControlQueueInit(struct FlowControlModule * fcm)137 static void FlowControlQueueInit(struct FlowControlModule *fcm)
138 {
139     int32_t i;
140     int32_t j;
141     if (fcm == NULL) {
142         HDF_LOGE("%s fail: fcm = null!", __func__);
143         return;
144     }
145     for (i = 0; i < FLOW_DIR_COUNT; i++) {
146         for (j = 0; j < QUEUE_ID_COUNT; j++) {
147             NetBufQueueInit(&fcm->fcmQueue[i].queues[j].dataQueue);
148             OsalSpinInit(&fcm->fcmQueue[i].queues[j].lock);
149         }
150     }
151 }
152 
FlowControlQueueDeinit(struct FlowControlModule * fcm)153 static void FlowControlQueueDeinit(struct FlowControlModule *fcm)
154 {
155     int32_t i;
156     int32_t j;
157     if (fcm == NULL) {
158         HDF_LOGE("%s fcm already free!", __func__);
159         return;
160     }
161 
162     for (i = 0; i < FLOW_DIR_COUNT; i++) {
163         for (j = 0; j < QUEUE_ID_COUNT; j++) {
164             NetBufQueueClear(&fcm->fcmQueue[i].queues[j].dataQueue);
165             OsalSpinDestroy(&fcm->fcmQueue[i].queues[j].lock);
166         }
167     }
168     return;
169 }
170 
SchedTransfer(struct FlowControlModule * fcm,FlowDir dir)171 static int32_t SchedTransfer(struct FlowControlModule *fcm, FlowDir dir)
172 {
173     if (fcm == NULL || dir >= FLOW_DIR_COUNT) {
174         HDF_LOGE("%s : fcm = null dir=%d!", __func__, dir);
175         return HDF_ERR_INVALID_PARAM;
176     }
177     OsalSemPost(&fcm->sem[dir]);
178     return HDF_SUCCESS;
179 }
180 
RegisterFlowControlOp(struct FlowControlModule * fcm,struct FlowControlOp * op)181 static int32_t RegisterFlowControlOp(struct FlowControlModule *fcm, struct FlowControlOp *op)
182 {
183     if ((fcm == NULL) || (op == NULL)) {
184         HDF_LOGE("%s fail : fcm = null or op = null!", __func__);
185         return HDF_ERR_INVALID_PARAM;
186     }
187     fcm->op = op;
188     return HDF_SUCCESS;
189 }
190 
IsValidSentToFCMPra(const struct FlowControlModule * fcm,uint32_t id,uint32_t dir)191 static bool IsValidSentToFCMPra(const struct FlowControlModule *fcm, uint32_t id, uint32_t dir)
192 {
193     if (fcm == NULL) {
194         HDF_LOGE("%s fail : fcm = null or buff = null!", __func__);
195         return false;
196     }
197     if (id >= QUEUE_ID_COUNT || dir >= FLOW_DIR_COUNT) {
198         HDF_LOGE("%s fail : id or dir not right!", __func__);
199         return false;
200     }
201     return true;
202 }
203 
SetQueueThreshold(struct FlowControlModule * fcm,uint32_t queueThreshold,uint32_t id,uint32_t dir)204 static int32_t SetQueueThreshold(struct FlowControlModule *fcm, uint32_t queueThreshold, uint32_t id, uint32_t dir)
205 {
206     struct FlowControlQueue *fcmQueue = NULL;
207     if (!IsValidSentToFCMPra(fcm, id, dir)) {
208         HDF_LOGE("%s fail IsValidSentToFCMPra FALSE!", __func__);
209         return HDF_ERR_INVALID_PARAM;
210     }
211     fcmQueue = &fcm->fcmQueue[dir].queues[id];
212     fcmQueue->queueThreshold = queueThreshold;
213     return HDF_SUCCESS;
214 }
215 
FcmQueuePreProcess(struct FlowControlQueue * fcmQueue)216 static void FcmQueuePreProcess(struct FlowControlQueue *fcmQueue)
217 {
218     NetBufQueue *dataQ = &fcmQueue->dataQueue;
219     NetBuf *oldBuff = NULL;
220     uint32_t threshold = fcmQueue->queueThreshold;
221     uint32_t qLen = NetBufQueueSize(dataQ);
222     if (threshold > 0 && threshold < qLen) {
223         HDF_LOGE("%s abandon netbuff!", __func__);
224         oldBuff = NetBufQueueDequeue(dataQ);
225         if (oldBuff != NULL) {
226             NetBufFree(oldBuff);
227         }
228     }
229     return;
230 }
231 
SendBuffToFCM(struct FlowControlModule * fcm,NetBuf * buff,uint32_t id,uint32_t dir)232 static int32_t SendBuffToFCM(struct FlowControlModule *fcm, NetBuf *buff, uint32_t id, uint32_t dir)
233 {
234     struct FlowControlQueue *fcmQueue = NULL;
235     NetBufQueue *dataQ = NULL;
236     if ((buff == NULL) || (NetBufGetDataLen(buff) == 0)) {
237         HDF_LOGE("%s fail : buff=null or len=0!", __func__);
238         return HDF_ERR_INVALID_PARAM;
239     }
240     if (!IsValidSentToFCMPra(fcm, id, dir)) {
241         HDF_LOGE("%s fail : IsValidSentToFCMPra FALSE!", __func__);
242         return HDF_ERR_INVALID_PARAM;
243     }
244 
245     fcmQueue = &fcm->fcmQueue[dir].queues[id];
246     dataQ = &fcmQueue->dataQueue;
247     FcmQueuePreProcess(fcmQueue);
248     if (NetBufQueueIsEmpty(dataQ)) {
249         fcm->fcmQueue[dir].queues[id].pktCount = 0;
250     }
251     NetBufQueueEnqueue(dataQ, buff);
252     fcm->fcmQueue[dir].queues[id].pktCount++;
253     return HDF_SUCCESS;
254 }
255 
GetQueueIdByEtherBuff(const NetBuf * buff)256 static FlowControlQueueID GetQueueIdByEtherBuff(const NetBuf *buff)
257 {
258     uint32_t len;
259     struct EtherHeader *header = NULL;
260     int i;
261     uint16_t etherType = 0;
262     if (buff == NULL) {
263         HDF_LOGE("%s fail : buff = null!", __func__);
264         return QUEUE_ID_COUNT;
265     }
266     len = NetBufGetDataLen(buff);
267     if (len < sizeof(struct EtherHeader)) {
268         HDF_LOGE("%s fail : buff->data_len not right!", __func__);
269         return QUEUE_ID_COUNT;
270     }
271     header = (struct EtherHeader *)NetBufGetAddress(buff, E_DATA_BUF);
272     etherType = ntohs(header->etherType);
273 
274     /* if processFun = null  return VIP_QUEUE_ID */
275     for (i = 0; i < ETHER_TYPE_PROCESS_COUNT; i++) {
276         if (g_etherTypeProMap[i].etherType == etherType) {
277             if (g_etherTypeProMap[i].processFun != NULL) {
278                 return g_etherTypeProMap[i].processFun((void *)(header + 1), len - sizeof(struct EtherHeader));
279             }
280             return VIP_QUEUE_ID;
281         }
282     }
283     return NORMAL_QUEUE_ID;
284 }
285 
286 static struct FlowControlInterface g_fcInterface = {
287     .setQueueThreshold = SetQueueThreshold,
288     .getQueueIdByEtherBuff = GetQueueIdByEtherBuff,
289     .sendBuffToFCM = SendBuffToFCM,
290     .schedFCM = SchedTransfer,
291     .registerFlowControlOp = RegisterFlowControlOp,
292 };
293 
294 static struct FlowControlModule *g_fcm = NULL;
295 
SendFlowControlQueue(struct FlowControlModule * fcm,uint32_t id,uint32_t dir)296 int32_t SendFlowControlQueue(struct FlowControlModule *fcm, uint32_t id, uint32_t dir)
297 {
298     NetBufQueue *q = NULL;
299     int32_t fwPriorityId = 0;
300     int32_t rxPriorityId = 0;
301     if (!IsValidSentToFCMPra(fcm, id, dir)) {
302         HDF_LOGE("%s fail : IsValidSentToFCMPra FALSE!", __func__);
303         return HDF_ERR_INVALID_PARAM;
304     }
305     q = &fcm->fcmQueue[dir].queues[id].dataQueue;
306     if (NetBufQueueIsEmpty(q)) {
307         return HDF_SUCCESS;
308     }
309     if (dir == FLOW_TX) {
310         if (fcm->op != NULL && fcm->op->getTxPriorityId != NULL) {
311             fwPriorityId = fcm->op->getTxPriorityId(id);
312         }
313         if (fcm->op != NULL && fcm->op->txDataPacket != NULL) {
314             fcm->op->txDataPacket(q, fcm->fcmPriv, fwPriorityId);
315         } else {
316             HDF_LOGE("%s fail : fcm->op->txDataPacket = null!", __func__);
317             return HDF_ERR_INVALID_PARAM;
318         }
319     }
320 
321     if (dir == FLOW_RX) {
322         if (fcm->op != NULL && fcm->op->getRxPriorityId != NULL) {
323             rxPriorityId = fcm->op->getRxPriorityId(id);
324         }
325         if (fcm->op != NULL && fcm->op->rxDataPacket != NULL) {
326             fcm->op->rxDataPacket(q, fcm->fcmPriv, rxPriorityId);
327         } else {
328             HDF_LOGE("%s fail : fcm->op->txDataPacket = null!", __func__);
329             return HDF_ERR_INVALID_PARAM;
330         }
331     }
332     return HDF_SUCCESS;
333 }
334 
InitFlowControl(void * fcmPriv)335 struct FlowControlModule *InitFlowControl(void *fcmPriv)
336 {
337     struct FlowControlModule *fcm = NULL;
338     int i;
339     if (g_fcm != NULL) {
340         HDF_LOGE("%s already init", __func__);
341         return g_fcm;
342     }
343     fcm = (struct FlowControlModule *)OsalMemCalloc(sizeof(struct FlowControlModule));
344     if (fcm == NULL) {
345         HDF_LOGE("%s fail: malloc fail!", __func__);
346         return NULL;
347     }
348     (void)memset_s(fcm, sizeof(struct FlowControlModule), 0, sizeof(struct FlowControlModule));
349 
350     /* init queue */
351     FlowControlQueueInit(fcm);
352 
353     /* init wait */
354     for (i = 0; i < FLOW_DIR_COUNT; i++) {
355         if (OsalSemInit(&fcm->sem[i], 0) != HDF_SUCCESS) {
356             HDF_LOGE("%s init osalsem fail!", __func__);
357             if (i == FLOW_RX) {
358                 OsalSemDestroy(&fcm->sem[FLOW_TX]);
359             }
360             OsalMemFree(fcm);
361             return NULL;
362         }
363     }
364 
365     /* init task */
366     if (CreateFlowControlTask(fcm) != HDF_SUCCESS) {
367         HDF_LOGE("%s fail: CreateFlowControlTask fail!", __func__);
368         for (i = 0; i < FLOW_DIR_COUNT; i++) {
369             (void)OsalSemDestroy(&fcm->sem[i]);
370         }
371         OsalMemFree(fcm);
372         return NULL;
373     }
374 
375     /* register interface */
376     fcm->interface = &g_fcInterface;
377     fcm->fcmPriv = fcmPriv;
378     g_fcm = fcm;
379     return fcm;
380 }
381 
DeInitFlowControl(struct FlowControlModule * fcm)382 void DeInitFlowControl(struct FlowControlModule *fcm)
383 {
384     int i;
385     if (fcm == NULL) {
386         HDF_LOGE("%s fail : fcm already been free.", __func__);
387         return;
388     }
389 
390     /* 1:Destroy task. 2:Destroy osalwait. 3:free NetBuff. */
391     DestroyFlowControlTask(fcm);
392     for (i = 0; i < FLOW_DIR_COUNT; i++) {
393         OsalSemDestroy(&fcm->sem[i]);
394     }
395     FlowControlQueueDeinit(fcm);
396     OsalMemFree(fcm);
397     g_fcm = NULL;
398     return;
399 }
400 
GetFlowControlModule(void)401 struct FlowControlModule *GetFlowControlModule(void)
402 {
403     return g_fcm;
404 }
405