1 /*
2 * Copyright (c) 2022 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 "can/can_mail.h"
10 #include "can/can_msg.h"
11 #include "hdf_dlist.h"
12 #include "hdf_log.h"
13 #include "osal_mem.h"
14
15 #define HDF_LOG_TAG can_mail
16
17 struct CanFilterNode {
18 const struct CanFilter *filter;
19 struct DListHead node;
20 bool active;
21 };
22
CanRxBoxCreate()23 struct CanRxBox *CanRxBoxCreate()
24 {
25 struct CanRxBox *rbox = NULL;
26
27 rbox = (struct CanRxBox *)OsalMemCalloc(sizeof(*rbox));
28 if (rbox == NULL) {
29 HDF_LOGE("CanRxBoxCreate: malloc failed");
30 return NULL;
31 }
32
33 rbox->queue = PlatformQueueCreate(NULL, "can_rbox", rbox);
34 if (rbox->queue == NULL) {
35 HDF_LOGE("CanRxBoxCreate: create rbox queue failed");
36 OsalMemFree(rbox);
37 return NULL;
38 }
39
40 DListHeadInit(&rbox->filters);
41 (void)OsalSpinInit(&rbox->spin);
42 return rbox;
43 }
44
CanRxBoxDestroy(struct CanRxBox * rbox)45 void CanRxBoxDestroy(struct CanRxBox *rbox)
46 {
47 if (rbox != NULL) {
48 if (rbox->queue != NULL) {
49 PlatformQueueDestroy(rbox->queue);
50 }
51 OsalSpinDestroy(&rbox->spin);
52 OsalMemFree(rbox);
53 }
54 }
55
56 static bool CanRxBoxMsgMatch(struct CanRxBox *, const struct CanMsg *);
57
CanRxBoxAddMsg(struct CanRxBox * rbox,struct CanMsg * cmsg)58 int32_t CanRxBoxAddMsg(struct CanRxBox *rbox, struct CanMsg *cmsg)
59 {
60 int32_t ret;
61 struct PlatformMsg *pmsg = NULL;
62
63 if (rbox == NULL) {
64 return HDF_ERR_INVALID_OBJECT;
65 }
66
67 if (!CanRxBoxMsgMatch(rbox, cmsg)) {
68 return HDF_ERR_NOT_SUPPORT;
69 }
70
71 pmsg = (struct PlatformMsg *)OsalMemCalloc(sizeof(*pmsg));
72 if (pmsg == NULL) {
73 return HDF_ERR_MALLOC_FAIL;
74 }
75 pmsg->data = cmsg;
76 CanMsgGet(cmsg); // increase ref count before enqueue
77 ret = PlatformQueueAddMsg(rbox->queue, pmsg);
78 if (ret != HDF_SUCCESS) {
79 OsalMemFree(pmsg);
80 CanMsgPut(cmsg); // decrase ref count if enqueue failed
81 }
82 return ret;
83 }
84
CanRxBoxGetMsg(struct CanRxBox * rbox,struct CanMsg ** cmsg,uint32_t tms)85 int32_t CanRxBoxGetMsg(struct CanRxBox *rbox, struct CanMsg **cmsg, uint32_t tms)
86 {
87 int32_t ret;
88 struct PlatformMsg *pmsg = NULL;
89
90 if (rbox == NULL) {
91 return HDF_ERR_INVALID_OBJECT;
92 }
93
94 ret = PlatformQueueGetMsg(rbox->queue, &pmsg, tms);
95 if (ret != HDF_SUCCESS) {
96 HDF_LOGE("CanRxBoxGetMsg: get platform msg failed:%d", ret);
97 return ret;
98 }
99
100 *cmsg = (struct CanMsg *)pmsg->data;
101 OsalMemFree(pmsg);
102 return HDF_SUCCESS;
103 }
104
CanFilterMatch(const struct CanFilter * filter,const struct CanMsg * cmsg)105 static bool CanFilterMatch(const struct CanFilter *filter, const struct CanMsg *cmsg)
106 {
107 (void)filter;
108 (void)cmsg;
109 uint32_t mask;
110
111 if (filter->rtrMask == 1 && filter->rtr != cmsg->rtr) {
112 return false;
113 }
114
115 if (filter->ideMask == 1 && filter->ide != cmsg->ide) {
116 return false;
117 }
118
119 mask = (cmsg->ide == 1) ? 0x7FF : 0x1FFFFFFF; // 11bits or 29bits ?
120 mask &= filter->idMask;
121 return ((cmsg->id & mask) == (filter->id & mask));
122 }
123
CanRxBoxMsgMatch(struct CanRxBox * rbox,const struct CanMsg * cmsg)124 static bool CanRxBoxMsgMatch(struct CanRxBox *rbox, const struct CanMsg *cmsg)
125 {
126 bool match = true;
127 struct CanFilterNode *cfNode = NULL;
128
129 if (rbox == NULL || cmsg == NULL) {
130 return false;
131 }
132
133 CanRxBoxLock(rbox);
134 DLIST_FOR_EACH_ENTRY(cfNode, &rbox->filters, struct CanFilterNode, node) {
135 if (CanFilterMatch(cfNode->filter, cmsg)) {
136 match = true;
137 break;
138 }
139 match = false;
140 }
141 CanRxBoxUnlock(rbox);
142 return match;
143 }
144
CanRxBoxAddFilter(struct CanRxBox * rbox,const struct CanFilter * filter)145 int32_t CanRxBoxAddFilter(struct CanRxBox *rbox, const struct CanFilter *filter)
146 {
147 struct CanFilterNode *cfNode = NULL;
148
149 if (rbox == NULL) {
150 return HDF_ERR_INVALID_OBJECT;
151 }
152 if (filter == NULL) {
153 return HDF_ERR_INVALID_PARAM;
154 }
155 cfNode = (struct CanFilterNode *)OsalMemCalloc(sizeof(*cfNode));
156 if (cfNode == NULL) {
157 return HDF_ERR_MALLOC_FAIL;
158 }
159 cfNode->filter = filter;
160 cfNode->active = true;
161 CanRxBoxLock(rbox);
162 DListInsertTail(&cfNode->node, &rbox->filters);
163 CanRxBoxUnlock(rbox);
164 return HDF_SUCCESS;
165 }
166
CanRxBoxDelFilter(struct CanRxBox * rbox,const struct CanFilter * filter)167 int32_t CanRxBoxDelFilter(struct CanRxBox *rbox, const struct CanFilter *filter)
168 {
169 struct CanFilterNode *cfNode = NULL;
170 struct CanFilterNode *tmp = NULL;
171
172 if (rbox == NULL) {
173 return HDF_ERR_INVALID_OBJECT;
174 }
175 if (filter == NULL) {
176 return HDF_ERR_INVALID_PARAM;
177 }
178 CanRxBoxLock(rbox);
179 DLIST_FOR_EACH_ENTRY_SAFE(cfNode, tmp, &rbox->filters, struct CanFilterNode, node) {
180 if (cfNode->filter == filter) {
181 DListRemove(&cfNode->node);
182 CanRxBoxUnlock(rbox);
183 OsalMemFree(cfNode);
184 return HDF_SUCCESS;
185 }
186 }
187 CanRxBoxUnlock(rbox);
188 return HDF_ERR_NOT_SUPPORT;
189 }
190