1 /*
2 * Copyright (c) 2022-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 "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 fail!");
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 fail!");
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 HDF_LOGE("CanRxBoxAddMsg: rbox is null!");
65 return HDF_ERR_INVALID_OBJECT;
66 }
67
68 if (!CanRxBoxMsgMatch(rbox, cmsg)) {
69 HDF_LOGE("CanRxBoxAddMsg: rbox is null!");
70 return HDF_ERR_NOT_SUPPORT;
71 }
72
73 pmsg = (struct PlatformMsg *)OsalMemCalloc(sizeof(*pmsg));
74 if (pmsg == NULL) {
75 return HDF_ERR_MALLOC_FAIL;
76 }
77 pmsg->data = cmsg;
78 CanMsgGet(cmsg); // increase ref count before enqueue
79 ret = PlatformQueueAddMsg(rbox->queue, pmsg);
80 if (ret != HDF_SUCCESS) {
81 OsalMemFree(pmsg);
82 CanMsgPut(cmsg); // decrase ref count if enqueue failed
83 }
84 return ret;
85 }
86
CanRxBoxGetMsg(struct CanRxBox * rbox,struct CanMsg ** cmsg,uint32_t tms)87 int32_t CanRxBoxGetMsg(struct CanRxBox *rbox, struct CanMsg **cmsg, uint32_t tms)
88 {
89 int32_t ret;
90 struct PlatformMsg *pmsg = NULL;
91
92 if (rbox == NULL) {
93 return HDF_ERR_INVALID_OBJECT;
94 }
95
96 ret = PlatformQueueGetMsg(rbox->queue, &pmsg, tms);
97 if (ret != HDF_SUCCESS) {
98 HDF_LOGE("CanRxBoxGetMsg: get platform msg fail, ret: %d!", ret);
99 return ret;
100 }
101
102 *cmsg = (struct CanMsg *)pmsg->data;
103 OsalMemFree(pmsg);
104 return HDF_SUCCESS;
105 }
106
CanFilterMatch(const struct CanFilter * filter,const struct CanMsg * cmsg)107 static bool CanFilterMatch(const struct CanFilter *filter, const struct CanMsg *cmsg)
108 {
109 (void)filter;
110 (void)cmsg;
111 uint32_t mask;
112
113 if (filter->rtrMask == 1 && filter->rtr != cmsg->rtr) {
114 return false;
115 }
116
117 if (filter->ideMask == 1 && filter->ide != cmsg->ide) {
118 return false;
119 }
120
121 mask = (cmsg->ide == 1) ? 0x7FF : 0x1FFFFFFF; // 11bits or 29bits ?
122 mask &= filter->idMask;
123 return ((cmsg->id & mask) == (filter->id & mask));
124 }
125
CanRxBoxMsgMatch(struct CanRxBox * rbox,const struct CanMsg * cmsg)126 static bool CanRxBoxMsgMatch(struct CanRxBox *rbox, const struct CanMsg *cmsg)
127 {
128 bool match = true;
129 struct CanFilterNode *cfNode = NULL;
130
131 if (rbox == NULL || cmsg == NULL) {
132 HDF_LOGE("CanRxBoxMsgMatch: rbox or cmsg is null!");
133 return false;
134 }
135
136 CanRxBoxLock(rbox);
137 DLIST_FOR_EACH_ENTRY(cfNode, &rbox->filters, struct CanFilterNode, node) {
138 if (CanFilterMatch(cfNode->filter, cmsg)) {
139 match = true;
140 break;
141 }
142 match = false;
143 }
144 CanRxBoxUnlock(rbox);
145 return match;
146 }
147
CanRxBoxAddFilter(struct CanRxBox * rbox,const struct CanFilter * filter)148 int32_t CanRxBoxAddFilter(struct CanRxBox *rbox, const struct CanFilter *filter)
149 {
150 struct CanFilterNode *cfNode = NULL;
151
152 if (rbox == NULL) {
153 HDF_LOGE("CanRxBoxAddFilter: rbox is null!");
154 return HDF_ERR_INVALID_OBJECT;
155 }
156 if (filter == NULL) {
157 HDF_LOGE("CanRxBoxAddFilter: filter is null!");
158 return HDF_ERR_INVALID_PARAM;
159 }
160 cfNode = (struct CanFilterNode *)OsalMemCalloc(sizeof(*cfNode));
161 if (cfNode == NULL) {
162 HDF_LOGE("CanRxBoxAddFilter: memcalloc cfNode fail!");
163 return HDF_ERR_MALLOC_FAIL;
164 }
165 cfNode->filter = filter;
166 cfNode->active = true;
167 CanRxBoxLock(rbox);
168 DListInsertTail(&cfNode->node, &rbox->filters);
169 CanRxBoxUnlock(rbox);
170 return HDF_SUCCESS;
171 }
172
CanRxBoxDelFilter(struct CanRxBox * rbox,const struct CanFilter * filter)173 int32_t CanRxBoxDelFilter(struct CanRxBox *rbox, const struct CanFilter *filter)
174 {
175 struct CanFilterNode *cfNode = NULL;
176 struct CanFilterNode *tmp = NULL;
177
178 if (rbox == NULL) {
179 HDF_LOGE("CanRxBoxDelFilter: rbox is null!");
180 return HDF_ERR_INVALID_OBJECT;
181 }
182 if (filter == NULL) {
183 HDF_LOGE("CanRxBoxDelFilter: filter is null!");
184 return HDF_ERR_INVALID_PARAM;
185 }
186 CanRxBoxLock(rbox);
187 DLIST_FOR_EACH_ENTRY_SAFE(cfNode, tmp, &rbox->filters, struct CanFilterNode, node) {
188 if (cfNode->filter == filter) {
189 DListRemove(&cfNode->node);
190 CanRxBoxUnlock(rbox);
191 OsalMemFree(cfNode);
192 return HDF_SUCCESS;
193 }
194 }
195 CanRxBoxUnlock(rbox);
196 HDF_LOGE("CanRxBoxDelFilter: not support!");
197 return HDF_ERR_NOT_SUPPORT;
198 }
199