1 | /* |
2 | * Copyright (c) 2007-2015 Freescale Semiconductor, Inc. |
3 | * Copyright 2018-2020 NXP |
4 | * |
5 | * License: NXP LA_OPT_NXP_Software_License |
6 | * |
7 | * NXP Confidential. This software is owned or controlled by NXP and may |
8 | * only be used strictly in accordance with the applicable license terms. |
9 | * By expressly accepting such terms or by downloading, installing, |
10 | * activating and/or otherwise using the software, you are agreeing that |
11 | * you have read, and that you agree to comply with and are bound by, |
12 | * such license terms. If you do not agree to be bound by the applicable |
13 | * license terms, then you may not retain, install, activate or otherwise |
14 | * use the software. This code may only be used in a microprocessor, |
15 | * microcontroller, sensor or digital signal processor ("NXP Product") |
16 | * supplied directly or indirectly from NXP. See the full NXP Software |
17 | * License Agreement in license/LA_OPT_NXP_Software_License.pdf |
18 | * |
19 | * FreeMASTER Communication Driver - User Resources |
20 | */ |
21 | |
22 | #include "freemaster.h" |
23 | #include "freemaster_private.h" |
24 | #include "freemaster_protocol.h" |
25 | #include "freemaster_ures.h" |
26 | |
27 | #if FMSTR_USE_TSA > 0 && FMSTR_DISABLE == 0 |
28 | |
29 | static FMSTR_BPTR _FMSTR_UresControlRead( |
30 | FMSTR_ADDR resourceId, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_BPTR response, FMSTR_U8 *retStatus); |
31 | static FMSTR_BPTR _FMSTR_UresControlWrite( |
32 | FMSTR_ADDR resourceId, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_BPTR response, FMSTR_U8 *retStatus); |
33 | static FMSTR_BPTR _FMSTR_UresControlIoctl( |
34 | FMSTR_ADDR resourceId, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_BPTR response, FMSTR_U8 *retStatus); |
35 | |
36 | /****************************************************************************** |
37 | * |
38 | * @brief Handling User Resources |
39 | * |
40 | * @param msgBuffIO - original command (in) and response buffer (out) |
41 | * @param msgSize - size of data in buffer |
42 | * @param retStatus - response status |
43 | * |
44 | * @return As all command handlers, the return value should be the buffer |
45 | * pointer where the response output finished (except checksum) |
46 | * |
47 | ******************************************************************************/ |
48 | |
49 | FMSTR_BPTR FMSTR_UresControl(FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_U8 *retStatus) |
50 | { |
51 | volatile FMSTR_BPTR response = msgBuffIO; |
52 | FMSTR_URES_OP_CODE opCode; |
53 | FMSTR_ADDR resourceId; |
54 | FMSTR_INDEX hdrDiff; |
55 | FMSTR_SIZE hdrSize; |
56 | |
57 | FMSTR_ASSERT(msgBuffIO != NULL); |
58 | FMSTR_ASSERT(retStatus != NULL); |
59 | |
60 | /* need at least operation code and resource ID */ |
61 | if (msgSize < 2U) |
62 | { |
63 | /* return status */ |
64 | *retStatus = FMSTR_STC_URESERR; |
65 | return response; |
66 | } |
67 | |
68 | /* Get operation code */ |
69 | msgBuffIO = FMSTR_ValueFromBuffer8(&opCode, msgBuffIO); |
70 | |
71 | /* Get reource ID */ |
72 | msgBuffIO = FMSTR_AddressFromBuffer(&resourceId, msgBuffIO); |
73 | |
74 | /* Where have we stopped while processing? */ |
75 | hdrDiff = (FMSTR_INDEX)(msgBuffIO - response); |
76 | hdrSize = (FMSTR_SIZE)hdrDiff; |
77 | |
78 | /* Handle command by operation code */ |
79 | switch (opCode) |
80 | { |
81 | case FMSTR_URES_OP_READ: |
82 | response = _FMSTR_UresControlRead(resourceId, msgBuffIO, msgSize - hdrSize, response, retStatus); |
83 | break; |
84 | case FMSTR_URES_OP_WRITE: |
85 | response = _FMSTR_UresControlWrite(resourceId, msgBuffIO, msgSize - hdrSize, response, retStatus); |
86 | break; |
87 | case FMSTR_URES_OP_IOCTL: |
88 | response = _FMSTR_UresControlIoctl(resourceId, msgBuffIO, msgSize - hdrSize, response, retStatus); |
89 | break; |
90 | default: |
91 | *retStatus = FMSTR_STC_URESERR; |
92 | break; |
93 | } |
94 | |
95 | return response; |
96 | } |
97 | |
98 | /****************************************************************************** |
99 | * |
100 | * @brief Handling User Resources - read |
101 | * |
102 | * @param resourceId - resource ID in TSA table |
103 | * @param msgBuffIO - message input buffer |
104 | * @param msgSize - size of data in buffer |
105 | * @param response - message output buffer |
106 | * @param retStatus - response status |
107 | * |
108 | * @return As all command handlers, the return value should be the buffer |
109 | * pointer where the response output finished (except checksum) |
110 | * |
111 | ******************************************************************************/ |
112 | |
113 | static FMSTR_BPTR _FMSTR_UresControlRead( |
114 | FMSTR_ADDR resourceId, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_BPTR response, FMSTR_U8 *retStatus) |
115 | { |
116 | FMSTR_BOOL ret; |
117 | FMSTR_ADDR readOffset; |
118 | FMSTR_SIZE readLen = 0; |
119 | const FMSTR_TSA_ENTRY *ures; |
120 | FMSTR_URES_HANDLER_FUNC uresFunc; |
121 | FMSTR_RWI_BUFF uresBuffRWI; |
122 | |
123 | FMSTR_ASSERT(resourceId != NULL); |
124 | FMSTR_ASSERT(msgBuffIO != NULL); |
125 | FMSTR_ASSERT(response != NULL); |
126 | FMSTR_ASSERT(retStatus != NULL); |
127 | |
128 | /* need at least read offset and read len */ |
129 | if (msgSize < 2U) |
130 | { |
131 | /* return status */ |
132 | *retStatus = FMSTR_STC_URESERR; |
133 | return response; |
134 | } |
135 | |
136 | /* Get read offset */ |
137 | msgBuffIO = FMSTR_AddressFromBuffer(&readOffset, msgBuffIO); |
138 | |
139 | /* Get read length */ |
140 | msgBuffIO = FMSTR_SizeFromBuffer(&readLen, msgBuffIO); |
141 | |
142 | /* Find resource in TSA table */ |
143 | ures = FMSTR_FindUresInTsa(resourceId); |
144 | if (ures == NULL) |
145 | { |
146 | /* User resource not found in TSA table */ |
147 | /* return status */ |
148 | *retStatus = FMSTR_STC_URESERR; |
149 | return response; |
150 | } |
151 | |
152 | /* Red/write/ioctl buffer */ |
153 | uresBuffRWI.buff = response; |
154 | uresBuffRWI.sizeIn = 0; |
155 | uresBuffRWI.sizeOut = 0; |
156 | uresBuffRWI.sizeOutMax = readLen; |
157 | uresBuffRWI.offset = readOffset; |
158 | uresBuffRWI.ioctlCode = 0; |
159 | |
160 | /* Check max size */ |
161 | if (uresBuffRWI.sizeOutMax > (FMSTR_U16)FMSTR_COMM_BUFFER_SIZE) |
162 | { |
163 | uresBuffRWI.sizeOutMax = (FMSTR_U16)FMSTR_COMM_BUFFER_SIZE; |
164 | } |
165 | |
166 | /* User function */ |
167 | uresFunc = (FMSTR_URES_HANDLER_FUNC)ures->addr.p; |
168 | if (uresFunc == NULL) |
169 | { |
170 | /* User resource function not exists */ |
171 | /* return status */ |
172 | *retStatus = FMSTR_STC_URESERR; |
173 | return response; |
174 | } |
175 | |
176 | /* Call user function */ |
177 | ret = uresFunc(FMSTR_URES_OP_READ, &uresBuffRWI, (void *)ures->info.p); |
178 | if (ret == FMSTR_FALSE) |
179 | { |
180 | /* User resource function error */ |
181 | /* return status */ |
182 | *retStatus = FMSTR_STC_URESERR; |
183 | return response; |
184 | } |
185 | |
186 | /* Move pointer in response buffer */ |
187 | response += uresBuffRWI.sizeOut; |
188 | |
189 | /* success */ |
190 | *retStatus = FMSTR_STS_OK | FMSTR_STSF_VARLEN; |
191 | return response; |
192 | } |
193 | |
194 | /****************************************************************************** |
195 | * |
196 | * @brief Handling User Resources - write |
197 | * |
198 | * @param resourceId - resource ID in TSA table |
199 | * @param msgBuffIO - message input buffer |
200 | * @param msgSize - size of data in buffer |
201 | * @param response - message output buffer |
202 | * @param retStatus - response status |
203 | * |
204 | * @return As all command handlers, the return value should be the buffer |
205 | * pointer where the response output finished (except checksum) |
206 | * |
207 | ******************************************************************************/ |
208 | |
209 | static FMSTR_BPTR _FMSTR_UresControlWrite( |
210 | FMSTR_ADDR resourceId, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_BPTR response, FMSTR_U8 *retStatus) |
211 | { |
212 | FMSTR_BOOL ret; |
213 | FMSTR_ADDR writeOffset; |
214 | FMSTR_SIZE writeLen = 0; |
215 | const FMSTR_TSA_ENTRY *ures; |
216 | FMSTR_URES_HANDLER_FUNC uresFunc; |
217 | FMSTR_RWI_BUFF uresBuffRWI; |
218 | |
219 | FMSTR_ASSERT(resourceId != NULL); |
220 | FMSTR_ASSERT(msgBuffIO != NULL); |
221 | FMSTR_ASSERT(response != NULL); |
222 | FMSTR_ASSERT(retStatus != NULL); |
223 | |
224 | /* need at least write offset and write len */ |
225 | if (msgSize < 2U) |
226 | { |
227 | /* return status */ |
228 | *retStatus = FMSTR_STC_URESERR; |
229 | return response; |
230 | } |
231 | |
232 | /* Get write offset */ |
233 | msgBuffIO = FMSTR_AddressFromBuffer(&writeOffset, msgBuffIO); |
234 | |
235 | /* Get write length */ |
236 | msgBuffIO = FMSTR_SizeFromBuffer(&writeLen, msgBuffIO); |
237 | |
238 | /* Find resource in TSA table */ |
239 | ures = FMSTR_FindUresInTsa(resourceId); |
240 | if (ures == NULL) |
241 | { |
242 | /* User resource not found in TSA table */ |
243 | /* return status */ |
244 | *retStatus = FMSTR_STC_URESERR; |
245 | return response; |
246 | } |
247 | |
248 | /* Red/write/ioctl buffer */ |
249 | uresBuffRWI.buff = msgBuffIO; |
250 | uresBuffRWI.sizeIn = writeLen; |
251 | uresBuffRWI.sizeOut = 0; |
252 | uresBuffRWI.sizeOutMax = 0; |
253 | uresBuffRWI.offset = writeOffset; |
254 | uresBuffRWI.ioctlCode = 0; |
255 | |
256 | /* User function */ |
257 | uresFunc = (FMSTR_URES_HANDLER_FUNC)ures->addr.p; |
258 | if (uresFunc == NULL) |
259 | { |
260 | /* User resource function not exists */ |
261 | /* return status */ |
262 | *retStatus = FMSTR_STC_URESERR; |
263 | return response; |
264 | } |
265 | |
266 | /* Call user function */ |
267 | ret = uresFunc(FMSTR_URES_OP_WRITE, &uresBuffRWI, (void *)ures->info.p); |
268 | if (ret == FMSTR_FALSE) |
269 | { |
270 | /* User resource function error */ |
271 | /* return status */ |
272 | *retStatus = FMSTR_STC_URESERR; |
273 | return response; |
274 | } |
275 | |
276 | /* Successful wrote data size */ |
277 | response = FMSTR_SizeToBuffer(response, uresBuffRWI.sizeConsumed); |
278 | |
279 | /* success */ |
280 | *retStatus = FMSTR_STS_OK | FMSTR_STSF_VARLEN; |
281 | return response; |
282 | } |
283 | |
284 | static FMSTR_BPTR _FMSTR_UresControlIoctl( |
285 | FMSTR_ADDR resourceId, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_BPTR response, FMSTR_U8 *retStatus) |
286 | { |
287 | FMSTR_BOOL ret; |
288 | FMSTR_URES_IOCTL_CODE ioctlCode; |
289 | const FMSTR_TSA_ENTRY *ures; |
290 | FMSTR_URES_HANDLER_FUNC uresFunc; |
291 | FMSTR_RWI_BUFF uresBuffRWI; |
292 | FMSTR_INDEX hdrSize; |
293 | FMSTR_SIZE sizeIn = 0; |
294 | FMSTR_SIZE i; |
295 | |
296 | FMSTR_ASSERT(resourceId != NULL); |
297 | FMSTR_ASSERT(msgBuffIO != NULL); |
298 | FMSTR_ASSERT(response != NULL); |
299 | FMSTR_ASSERT(retStatus != NULL); |
300 | |
301 | /* need at least write offset and write len */ |
302 | if (msgSize < 2U) |
303 | { |
304 | /* return status */ |
305 | *retStatus = FMSTR_STC_URESERR; |
306 | return response; |
307 | } |
308 | |
309 | /* Get IOCTL code */ |
310 | msgBuffIO = FMSTR_SizeFromBuffer(&ioctlCode, msgBuffIO); |
311 | |
312 | /* Where have we stopped while processing? */ |
313 | hdrSize = msgBuffIO - response; |
314 | |
315 | /* Find resource in TSA table */ |
316 | ures = FMSTR_FindUresInTsa(resourceId); |
317 | if (ures == NULL) |
318 | { |
319 | /* User resource not found in TSA table */ |
320 | /* return status */ |
321 | *retStatus = FMSTR_STC_URESERR; |
322 | return response; |
323 | } |
324 | |
325 | /* move the reminder of the IO buffer to the response */ |
326 | sizeIn = 0U; |
327 | for (i = (FMSTR_SIZE)hdrSize; i < msgSize; i++) |
328 | { |
329 | response[sizeIn++] = *msgBuffIO++; |
330 | } |
331 | |
332 | /* Read/write/ioctl buffer */ |
333 | uresBuffRWI.buff = response; |
334 | uresBuffRWI.sizeIn = sizeIn; |
335 | uresBuffRWI.sizeOut = 0U; |
336 | uresBuffRWI.sizeOutMax = FMSTR_COMM_BUFFER_SIZE; |
337 | uresBuffRWI.offset = NULL; |
338 | uresBuffRWI.ioctlCode = ioctlCode; |
339 | |
340 | /* User function */ |
341 | uresFunc = (FMSTR_URES_HANDLER_FUNC)ures->addr.p; |
342 | if (uresFunc == NULL) |
343 | { |
344 | /* User resource function not exists */ |
345 | /* return status */ |
346 | *retStatus = FMSTR_STC_URESERR; |
347 | return response; |
348 | } |
349 | |
350 | /* Call user function */ |
351 | ret = uresFunc(FMSTR_URES_OP_IOCTL, &uresBuffRWI, (void *)ures->info.p); |
352 | if (ret == FMSTR_FALSE) |
353 | { |
354 | /* User resource function error */ |
355 | /* return status */ |
356 | *retStatus = FMSTR_STC_URESERR; |
357 | return response; |
358 | } |
359 | |
360 | /* Move pointer in response buffer */ |
361 | response += uresBuffRWI.sizeOut; |
362 | |
363 | /* success */ |
364 | *retStatus = FMSTR_STS_OK | FMSTR_STSF_VARLEN; |
365 | return response; |
366 | } |
367 | |
368 | #endif /* (FMSTR_USE_TSA) && (!(FMSTR_DISABLE)) */ |
369 | |