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 - Pipe I/O |
20 | */ |
21 | |
22 | #include "freemaster.h" |
23 | #include "freemaster_private.h" |
24 | #include "freemaster_protocol.h" |
25 | #include "freemaster_utils.h" |
26 | |
27 | #if FMSTR_USE_PIPES > 0 && FMSTR_DISABLE == 0 |
28 | |
29 | #if FMSTR_USE_PIPE_PRINTF_VARG > 0 |
30 | #include <stdarg.h> |
31 | #endif |
32 | |
33 | /*********************************** |
34 | * local types |
35 | ***********************************/ |
36 | |
37 | /* Flags for command FMSTR_CMD_GETPIPE */ |
38 | #define FMSTR_PIPE_GETPIPE_FLAG_PORT 0x01U |
39 | |
40 | /* Flags for get pipe info in command FMSTR_CMD_GETPIPE */ |
41 | #define FMSTR_PIPE_GETINFO_FLAG_ISOPEN 0x01U |
42 | |
43 | #define FMSTR_PIPE_CFGCODE_NAME 0x81U |
44 | #define FMSTR_PIPE_CFGCODE_INFO 0x82U |
45 | |
46 | /* runtime flags */ |
47 | typedef union |
48 | { |
49 | FMSTR_FLAGS all; |
50 | |
51 | struct |
52 | { |
53 | unsigned bExpectOdd : 1; /* now expecting even round */ |
54 | unsigned bInComm : 1; /* in protocol handler now */ |
55 | unsigned bLocked : 1; /* data buffer access is locked */ |
56 | } flg; |
57 | |
58 | } FMSTR_PIPE_FLAGS; |
59 | |
60 | typedef union |
61 | { |
62 | FMSTR_FLAGS all; |
63 | |
64 | struct |
65 | { |
66 | unsigned bIsFull : 1; /* buffer is full (wp==rp, but not empty) */ |
67 | } flg; |
68 | |
69 | } FMSTR_PIPE_BUFF_FLAGS; |
70 | |
71 | typedef struct |
72 | { |
73 | FMSTR_U8 *buff; |
74 | FMSTR_PIPE_SIZE size; |
75 | FMSTR_PIPE_SIZE wp; |
76 | FMSTR_PIPE_SIZE rp; |
77 | FMSTR_PIPE_BUFF_FLAGS flags; |
78 | } FMSTR_PIPE_BUFF; |
79 | |
80 | typedef struct |
81 | { |
82 | /* pipe information */ |
83 | const FMSTR_CHAR *name; /* String description of the pipe. */ |
84 | FMSTR_U8 type; /* Type of the usage of pipe */ |
85 | |
86 | /* pipe configuration */ |
87 | FMSTR_PIPE_BUFF rx; |
88 | FMSTR_PIPE_BUFF tx; |
89 | FMSTR_PIPE_PORT pipePort; |
90 | FMSTR_PPIPEFUNC pCallback; |
91 | |
92 | /* runtime information */ |
93 | FMSTR_PIPE_FLAGS flags; |
94 | FMSTR_U8 nLastBytesReceived; |
95 | |
96 | /* residue buffer for 8-bit handling on DSP56F8xx platform */ |
97 | #if FMSTR_CFG_BUS_WIDTH > 1 |
98 | FMSTR_U16 rd8Resid; |
99 | FMSTR_U16 wr8Resid; |
100 | #endif |
101 | |
102 | #if FMSTR_USE_PIPE_PRINTF > 0 |
103 | FMSTR_CHAR printfBuff[FMSTR_PIPES_PRINTF_BUFF_SIZE]; |
104 | FMSTR_SIZE printfBPtr; |
105 | #endif |
106 | |
107 | } FMSTR_PIPE; |
108 | |
109 | /* Pipe ITOA formatting flags */ |
110 | typedef union |
111 | { |
112 | FMSTR_FLAGS all; |
113 | |
114 | struct |
115 | { |
116 | unsigned upperc : 1; /* uppercase HEX letters */ |
117 | unsigned zeroes : 1; /* prepend with zeroes rather than spaces */ |
118 | unsigned showsign : 1; /* show sign always (even if plus) */ |
119 | unsigned negative : 1; /* sign determined during itoa */ |
120 | unsigned signedtype : 1; /* type is signed (used with va_list only) */ |
121 | unsigned isstring : 1; /* formatting %s */ |
122 | |
123 | } flg; |
124 | |
125 | } FMSTR_PIPE_ITOA_FLAGS; |
126 | |
127 | /* Pipe printf context */ |
128 | typedef struct |
129 | { |
130 | FMSTR_PIPE_ITOA_FLAGS flags; |
131 | FMSTR_U8 radix; |
132 | FMSTR_U8 dtsize; |
133 | FMSTR_SIZE8 alen; |
134 | |
135 | } FMSTR_PIPE_PRINTF_CTX; |
136 | |
137 | typedef FMSTR_BOOL (*FMSTR_PIPE_ITOA_FUNC)(FMSTR_HPIPE pipeHandle, const void *parg, FMSTR_PIPE_PRINTF_CTX *ctx); |
138 | |
139 | /*********************************** |
140 | * local variables |
141 | ***********************************/ |
142 | |
143 | static FMSTR_PIPE pcm_pipes[FMSTR_USE_PIPES]; |
144 | |
145 | /********************************************************************** |
146 | * local macros |
147 | **********************************************************************/ |
148 | |
149 | #define FMSTR_PIPE_ITOAFMT_BIN 0U |
150 | #define FMSTR_PIPE_ITOAFMT_OCT 1U |
151 | #define FMSTR_PIPE_ITOAFMT_DEC 2U |
152 | #define FMSTR_PIPE_ITOAFMT_HEX 3U |
153 | #define FMSTR_PIPE_ITOAFMT_CHAR 4U |
154 | |
155 | #define FMSTR_IS_DIGIT(x) (((x) >= '0') && ((x) <= '9')) |
156 | |
157 | /********************************************************************** |
158 | * local functions |
159 | **********************************************************************/ |
160 | |
161 | static FMSTR_PIPE *_FMSTR_FindPipe(FMSTR_PIPE_PORT pipePort); |
162 | static FMSTR_PIPE_SIZE _FMSTR_PipeGetBytesFree(FMSTR_PIPE_BUFF *pipeBuff); |
163 | static FMSTR_PIPE_SIZE _FMSTR_PipeGetBytesReady(FMSTR_PIPE_BUFF *pipeBuff); |
164 | static void _FMSTR_PipeDiscardBytes(FMSTR_PIPE_BUFF *pipeBuff, FMSTR_SIZE8 countBytes); |
165 | static FMSTR_BPTR _FMSTR_PipeReceive(FMSTR_BPTR msgBuffIO, FMSTR_PIPE *pp, FMSTR_SIZE8 msgBuffSize); |
166 | static FMSTR_BPTR _FMSTR_PipeTransmit(FMSTR_BPTR msgBuffIO, FMSTR_PIPE *pp, FMSTR_SIZE8 msgBuffSize); |
167 | static FMSTR_BOOL _FMSTR_PipeIToAFinalize(FMSTR_HPIPE pipeHandle, FMSTR_PIPE_PRINTF_CTX *pctx); |
168 | |
169 | static FMSTR_BOOL _FMSTR_PipePrintfOne(FMSTR_HPIPE pipeHandle, |
170 | const char *format, |
171 | void *parg, |
172 | FMSTR_PIPE_ITOA_FUNC pItoaFunc); |
173 | |
174 | static FMSTR_BOOL _FMSTR_PipePrintfAny(FMSTR_HPIPE pipeHandle, va_list *parg, FMSTR_PIPE_PRINTF_CTX *pctx); |
175 | static FMSTR_BOOL _FMSTR_PipePrintfV(FMSTR_HPIPE pipeHandle, const char *format, va_list *parg); |
176 | static FMSTR_CHAR _FMSTR_XDigit(FMSTR_U8 digit, FMSTR_BOOL uppercase); |
177 | |
178 | /********************************************************************** |
179 | * local "inline" functions (may also be static on some platforms) |
180 | **********************************************************************/ |
181 | |
182 | FMSTR_INLINE FMSTR_BOOL _FMSTR_PipePrintfFlush(FMSTR_HPIPE pipeHandle); |
183 | FMSTR_INLINE FMSTR_BOOL _FMSTR_PipePrintfPutc(FMSTR_HPIPE pipeHandle, char c); |
184 | |
185 | /****************************************************************************** |
186 | * |
187 | * @brief Initialization of pipes layer |
188 | * |
189 | ******************************************************************************/ |
190 | |
191 | FMSTR_BOOL FMSTR_InitPipes(void) |
192 | { |
193 | FMSTR_INDEX i; |
194 | |
195 | for (i = 0; i < (FMSTR_INDEX)FMSTR_USE_PIPES; i++) |
196 | { |
197 | pcm_pipes[i].pipePort = 0; |
198 | } |
199 | |
200 | return FMSTR_TRUE; |
201 | } |
202 | |
203 | /****************************************************************************** |
204 | * |
205 | * @brief API: Opening the pipe |
206 | * |
207 | ******************************************************************************/ |
208 | |
209 | FMSTR_HPIPE FMSTR_PipeOpen(FMSTR_PIPE_PORT pipePort, |
210 | FMSTR_PPIPEFUNC pipeCallback, |
211 | FMSTR_ADDR pipeRxBuff, |
212 | FMSTR_PIPE_SIZE pipeRxSize, |
213 | FMSTR_ADDR pipeTxBuff, |
214 | FMSTR_PIPE_SIZE pipeTxSize, |
215 | FMSTR_U8 type, |
216 | const FMSTR_CHAR *name) |
217 | { |
218 | FMSTR_PIPE *pp = NULL; |
219 | FMSTR_INDEX ifree = -1; |
220 | FMSTR_INDEX i; |
221 | |
222 | for (i = 0; i < (FMSTR_INDEX)FMSTR_USE_PIPES; i++) |
223 | { |
224 | pp = &pcm_pipes[i]; |
225 | |
226 | /* find first free pipe */ |
227 | if (pp->pipePort == 0U && ifree < 0) |
228 | { |
229 | ifree = i; |
230 | } |
231 | /* look for existing pipe with the same port */ |
232 | if (pp->pipePort == pipePort) |
233 | { |
234 | break; |
235 | } |
236 | } |
237 | |
238 | /* pipe not found */ |
239 | if (i >= (FMSTR_INDEX)FMSTR_USE_PIPES) |
240 | { |
241 | /* create new pipe */ |
242 | if (ifree >= 0) |
243 | { |
244 | pp = &pcm_pipes[ifree]; |
245 | } |
246 | /* no slot for creating pipe */ |
247 | else |
248 | { |
249 | return NULL; |
250 | } |
251 | } |
252 | |
253 | FMSTR_MemSet(pp, 0, sizeof(FMSTR_PIPE)); |
254 | |
255 | /* disable pipe (just in case the interrupt would come now) */ |
256 | pp->pipePort = 0; |
257 | |
258 | /* initialize pipe */ |
259 | pp->rx.buff = pipeRxBuff; |
260 | pp->rx.size = pipeRxSize; |
261 | pp->tx.buff = pipeTxBuff; |
262 | pp->tx.size = pipeTxSize; |
263 | pp->pCallback = pipeCallback; |
264 | pp->name = name; |
265 | pp->type = type; |
266 | |
267 | #if FMSTR_USE_PIPE_PRINTF > 0 |
268 | pp->printfBPtr = 0; |
269 | #endif |
270 | |
271 | /* activate pipe for the new port */ |
272 | pp->pipePort = pipePort; |
273 | |
274 | return (FMSTR_HPIPE)pp; |
275 | } |
276 | |
277 | /****************************************************************************** |
278 | * |
279 | * @brief PIPE API: Close pipe |
280 | * |
281 | ******************************************************************************/ |
282 | |
283 | void FMSTR_PipeClose(FMSTR_HPIPE pipeHandle) |
284 | { |
285 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
286 | |
287 | /* un-initialize pipe */ |
288 | if (pp != NULL) |
289 | { |
290 | pp->pipePort = 0; |
291 | } |
292 | } |
293 | |
294 | /****************************************************************************** |
295 | * |
296 | * @brief PIPE API: Write data to a pipe |
297 | * |
298 | ******************************************************************************/ |
299 | |
300 | FMSTR_PIPE_SIZE FMSTR_PipeWrite(FMSTR_HPIPE pipeHandle, |
301 | FMSTR_ADDR pipeData, |
302 | FMSTR_PIPE_SIZE pipeDataLen, |
303 | FMSTR_PIPE_SIZE writeGranularity) |
304 | { |
305 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
306 | FMSTR_PIPE_BUFF *pbuff = &pp->tx; |
307 | FMSTR_PIPE_SIZE total = _FMSTR_PipeGetBytesFree(pbuff); |
308 | FMSTR_PIPE_SIZE s; |
309 | |
310 | /* when invalid address is given, just return number of bytes free */ |
311 | if (pipeData != NULL) |
312 | { |
313 | /* only fill the free space */ |
314 | if (pipeDataLen > total) |
315 | { |
316 | pipeDataLen = total; |
317 | } |
318 | |
319 | /* obey granularity */ |
320 | if (writeGranularity > 1U) |
321 | { |
322 | pipeDataLen /= writeGranularity; |
323 | pipeDataLen *= writeGranularity; |
324 | } |
325 | |
326 | /* return value */ |
327 | total = pipeDataLen; |
328 | |
329 | /* valid length only */ |
330 | if (pipeDataLen > 0U) |
331 | { |
332 | /* total bytes available in the rest of buffer */ |
333 | s = (FMSTR_PIPE_SIZE)((pbuff->size - pbuff->wp) * FMSTR_CFG_BUS_WIDTH); |
334 | if (s > pipeDataLen) |
335 | { |
336 | s = pipeDataLen; |
337 | } |
338 | |
339 | /* get the bytes */ |
340 | FMSTR_MemCpyFrom(pbuff->buff + pbuff->wp, pipeData, (FMSTR_SIZE8)s); |
341 | pipeData += s / FMSTR_CFG_BUS_WIDTH; |
342 | |
343 | /* advance & wrap pointer */ |
344 | pbuff->wp += s / FMSTR_CFG_BUS_WIDTH; |
345 | if (pbuff->wp >= pbuff->size) |
346 | { |
347 | pbuff->wp = 0; |
348 | } |
349 | |
350 | /* rest of frame to a (wrapped) beggining of buffer */ |
351 | pipeDataLen -= (FMSTR_SIZE8)s; |
352 | if (pipeDataLen > 0U) |
353 | { |
354 | FMSTR_MemCpyFrom(pbuff->buff + pbuff->wp, pipeData, (FMSTR_SIZE8)pipeDataLen); |
355 | pbuff->wp += pipeDataLen / FMSTR_CFG_BUS_WIDTH; |
356 | } |
357 | |
358 | /* buffer got full? */ |
359 | if (pbuff->wp == pbuff->rp) |
360 | { |
361 | pbuff->flags.flg.bIsFull = 1; |
362 | } |
363 | } |
364 | } |
365 | |
366 | return total; |
367 | } |
368 | |
369 | /****************************************************************************** |
370 | * |
371 | * @brief PIPE API: Put zero-terminated string into pipe. Succeedes only |
372 | * if full string fits into the output buffer and return TRUE if so. |
373 | * |
374 | ******************************************************************************/ |
375 | |
376 | FMSTR_BOOL FMSTR_PipePuts(FMSTR_HPIPE pipeHandle, const char *text) |
377 | { |
378 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
379 | FMSTR_PIPE_BUFF *pbuff = &pp->tx; |
380 | FMSTR_PIPE_SIZE bytesFree = _FMSTR_PipeGetBytesFree(pbuff); |
381 | FMSTR_PIPE_SIZE strLen = (FMSTR_PIPE_SIZE)FMSTR_StrLen(text); |
382 | |
383 | if (strLen > bytesFree) |
384 | { |
385 | return FMSTR_FALSE; |
386 | } |
387 | |
388 | return FMSTR_PipeWrite(pipeHandle, (FMSTR_ADDR)text, strLen, 0) >= strLen ? FMSTR_TRUE : FMSTR_FALSE; |
389 | } |
390 | |
391 | /****************************************************************************** |
392 | * |
393 | * @brief PRINTF-like formatting functions |
394 | * |
395 | *****************************************************************************/ |
396 | |
397 | #if FMSTR_USE_PIPE_PRINTF > 0 |
398 | |
399 | static FMSTR_CHAR _FMSTR_XDigit(FMSTR_U8 digit, FMSTR_BOOL uppercase) |
400 | { |
401 | FMSTR_CHAR c; |
402 | |
403 | if (digit < 10U) |
404 | { |
405 | c = '0'; |
406 | c += (FMSTR_CHAR)digit; |
407 | } |
408 | else |
409 | { |
410 | if (uppercase != FMSTR_FALSE) |
411 | { |
412 | c = 'A' - 10; |
413 | } |
414 | else |
415 | { |
416 | c = 'a' - 10; |
417 | } |
418 | |
419 | c += (FMSTR_CHAR)digit; |
420 | } |
421 | |
422 | return c; |
423 | } |
424 | |
425 | /****************************************************************************** |
426 | * |
427 | * @brief Flush pipe's printf formatting buffer into pipe output buffer |
428 | * |
429 | *****************************************************************************/ |
430 | |
431 | FMSTR_INLINE FMSTR_BOOL _FMSTR_PipePrintfFlush(FMSTR_HPIPE pipeHandle) |
432 | { |
433 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
434 | FMSTR_BOOL ok = FMSTR_TRUE; |
435 | FMSTR_SIZE i; |
436 | |
437 | if (pp->printfBPtr != 0U) |
438 | { |
439 | FMSTR_SIZE sz = FMSTR_PipeWrite(pipeHandle, (FMSTR_ADDR)pp->printfBuff, (FMSTR_PIPE_SIZE)pp->printfBPtr, 0); |
440 | |
441 | /* all characters could NOT be printed */ |
442 | if (sz < pp->printfBPtr) |
443 | { |
444 | /* Move not sent characters */ |
445 | for (i = 0; i < (pp->printfBPtr - sz); i++) |
446 | { |
447 | pp->printfBuff[i] = pp->printfBuff[sz + i]; |
448 | } |
449 | |
450 | pp->printfBPtr -= sz; |
451 | |
452 | ok = FMSTR_FALSE; |
453 | } |
454 | else |
455 | { |
456 | /* not a cyclic buffer, must start over anyway (also if error) */ |
457 | pp->printfBPtr = 0; |
458 | } |
459 | } |
460 | |
461 | return ok; |
462 | } |
463 | |
464 | /****************************************************************************** |
465 | * |
466 | * @brief Put one character into pipe's printf formating buffer |
467 | * |
468 | *****************************************************************************/ |
469 | |
470 | FMSTR_INLINE FMSTR_BOOL _FMSTR_PipePrintfPutc(FMSTR_HPIPE pipeHandle, char c) |
471 | { |
472 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
473 | |
474 | /* when buffer is full */ |
475 | if (pp->printfBPtr >= FMSTR_PIPES_PRINTF_BUFF_SIZE) |
476 | { |
477 | /* try to flush some bytes */ |
478 | if (_FMSTR_PipePrintfFlush(pipeHandle) == FMSTR_FALSE) |
479 | { |
480 | return FMSTR_FALSE; |
481 | } |
482 | } |
483 | |
484 | /* abort if still full */ |
485 | if (pp->printfBPtr >= FMSTR_PIPES_PRINTF_BUFF_SIZE) |
486 | { |
487 | return FMSTR_FALSE; |
488 | } |
489 | |
490 | pp->printfBuff[pp->printfBPtr++] = c; |
491 | return FMSTR_TRUE; |
492 | } |
493 | |
494 | /****************************************************************************** |
495 | * |
496 | * @brief This function finishes the number formatting, adds spacing, signs |
497 | * and reverses the string (the input is comming in reversed order) |
498 | * |
499 | *****************************************************************************/ |
500 | |
501 | static FMSTR_BOOL _FMSTR_PipeIToAFinalize(FMSTR_HPIPE pipeHandle, FMSTR_PIPE_PRINTF_CTX *pctx) |
502 | { |
503 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
504 | FMSTR_SIZE bptr, minlen, i, bhalf; |
505 | FMSTR_CHAR z, sgn; |
506 | |
507 | /* buffer pointer into local variable */ |
508 | bptr = pp->printfBPtr; |
509 | |
510 | /* if anything goes wrong, throw our prepared string away */ |
511 | pp->printfBPtr = 0; |
512 | |
513 | /* zero may come as an empty string from itoa procedures */ |
514 | if (bptr == 0U) |
515 | { |
516 | pp->printfBuff[0] = '0'; |
517 | bptr = 1; |
518 | } |
519 | |
520 | /* first strip the zeroes put by itoa */ |
521 | while (bptr > 1U && pp->printfBuff[bptr - 1U] == '0') |
522 | { |
523 | bptr--; |
524 | } |
525 | |
526 | /* determine sign to print */ |
527 | if (pctx->flags.flg.negative != 0U) |
528 | { |
529 | sgn = '-'; |
530 | |
531 | /* minus need to be shown always */ |
532 | pctx->flags.flg.showsign = 1U; |
533 | } |
534 | else |
535 | { |
536 | /* plus will be shown if flg.showsign was set by caller */ |
537 | sgn = '+'; |
538 | } |
539 | |
540 | /* unsigned types can not print sign */ |
541 | if (pctx->flags.flg.signedtype == 0U) |
542 | { |
543 | pctx->flags.flg.showsign = 0U; |
544 | } |
545 | |
546 | /* calculate minimum buffer length needed */ |
547 | minlen = bptr; |
548 | if (pctx->flags.flg.showsign != 0U) |
549 | { |
550 | minlen++; |
551 | } |
552 | |
553 | /* will it fit? */ |
554 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < minlen) |
555 | { |
556 | return FMSTR_FALSE; |
557 | } |
558 | |
559 | /* required length should never exceed the buffer length */ |
560 | if (pctx->alen > FMSTR_PIPES_PRINTF_BUFF_SIZE) |
561 | { |
562 | pctx->alen = FMSTR_PIPES_PRINTF_BUFF_SIZE; |
563 | } |
564 | |
565 | /* choose prefix character (zero, space or sign-extension OCT/HEX/BIN) */ |
566 | if (pctx->flags.flg.zeroes != 0U) |
567 | { |
568 | z = '0'; |
569 | |
570 | /* sign extend? */ |
571 | if (pctx->flags.flg.negative != 0U) |
572 | { |
573 | switch (pctx->radix) |
574 | { |
575 | case FMSTR_PIPE_ITOAFMT_BIN: |
576 | z = '1'; |
577 | break; |
578 | case FMSTR_PIPE_ITOAFMT_OCT: |
579 | z = '7'; |
580 | break; |
581 | case FMSTR_PIPE_ITOAFMT_HEX: |
582 | z = (FMSTR_CHAR)(pctx->flags.flg.upperc != 0U ? 'F' : 'f'); |
583 | break; |
584 | default: |
585 | z = '0'; |
586 | break; |
587 | } |
588 | } |
589 | |
590 | /* the sign will be in front of added zeroes */ |
591 | if (pctx->flags.flg.showsign != 0U) |
592 | { |
593 | pctx->alen--; |
594 | } |
595 | } |
596 | else |
597 | { |
598 | z = ' '; |
599 | |
600 | /* sign should be in front of the number */ |
601 | if (pctx->flags.flg.showsign != 0U) |
602 | { |
603 | pp->printfBuff[bptr++] = sgn; |
604 | pctx->flags.flg.showsign = 0; /* prevent it to be added again below */ |
605 | } |
606 | } |
607 | |
608 | /* now fill to required len */ |
609 | while (bptr < pctx->alen) |
610 | { |
611 | pp->printfBuff[bptr++] = z; |
612 | } |
613 | |
614 | /* add the sign if needed */ |
615 | if (pctx->flags.flg.showsign != 0U) |
616 | { |
617 | pp->printfBuff[bptr++] = sgn; |
618 | } |
619 | |
620 | /* buffer contains this number of characters */ |
621 | pp->printfBPtr = bptr; |
622 | |
623 | /* now reverse the string and feed it to pipe */ |
624 | bhalf = bptr / 2U; |
625 | bptr--; |
626 | for (i = 0; i < bhalf; i++) |
627 | { |
628 | z = pp->printfBuff[i]; |
629 | pp->printfBuff[i] = pp->printfBuff[bptr - i]; |
630 | pp->printfBuff[bptr - i] = z; |
631 | } |
632 | |
633 | return FMSTR_TRUE; |
634 | } |
635 | |
636 | /****************************************************************************** |
637 | * |
638 | * @brief This function formats the argument into the temporary printf buffer |
639 | * It is granted by the caller that the buffer is empty before calling. |
640 | * |
641 | *****************************************************************************/ |
642 | |
643 | static FMSTR_BOOL FMSTR_PipeU8ToA(FMSTR_HPIPE pipeHandle, const FMSTR_U8 *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
644 | { |
645 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
646 | FMSTR_U8 arg = *parg; |
647 | FMSTR_U8 dig; |
648 | FMSTR_INDEX i; |
649 | |
650 | switch (pctx->radix) |
651 | { |
652 | case FMSTR_PIPE_ITOAFMT_CHAR: |
653 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)arg; |
654 | break; |
655 | |
656 | case FMSTR_PIPE_ITOAFMT_BIN: |
657 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 8U) |
658 | { |
659 | return FMSTR_FALSE; |
660 | } |
661 | |
662 | for (i = 0; i < 8; i++) |
663 | { |
664 | if (arg == 0U) |
665 | { |
666 | break; |
667 | } |
668 | |
669 | dig = (FMSTR_U8)'0'; |
670 | dig += (FMSTR_U8)(arg & 1U); |
671 | |
672 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
673 | arg >>= 1; |
674 | } |
675 | break; |
676 | |
677 | case FMSTR_PIPE_ITOAFMT_OCT: |
678 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 3U) |
679 | { |
680 | return FMSTR_FALSE; |
681 | } |
682 | |
683 | for (i = 0; i < 3; i++) |
684 | { |
685 | if (arg == 0U) |
686 | { |
687 | break; |
688 | } |
689 | |
690 | dig = (FMSTR_U8)'0'; |
691 | dig += (FMSTR_U8)(arg & 7U); |
692 | |
693 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
694 | arg >>= 3; |
695 | } |
696 | break; |
697 | |
698 | case FMSTR_PIPE_ITOAFMT_DEC: |
699 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 3U) |
700 | { |
701 | return FMSTR_FALSE; |
702 | } |
703 | |
704 | for (i = 0; i < 3; i++) |
705 | { |
706 | if (arg == 0U) |
707 | { |
708 | break; |
709 | } |
710 | |
711 | dig = (FMSTR_U8)'0'; |
712 | dig += (FMSTR_U8)(arg % 10U); |
713 | |
714 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
715 | arg /= 10U; |
716 | } |
717 | break; |
718 | |
719 | case FMSTR_PIPE_ITOAFMT_HEX: |
720 | default: |
721 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 2U) |
722 | { |
723 | return FMSTR_FALSE; |
724 | } |
725 | |
726 | for (i = 0; i < 2; i++) |
727 | { |
728 | if (arg == 0U) |
729 | { |
730 | break; |
731 | } |
732 | |
733 | dig = (FMSTR_U8)(arg & 15U); |
734 | |
735 | pp->printfBuff[pp->printfBPtr++] = |
736 | _FMSTR_XDigit((FMSTR_U8)dig, (FMSTR_BOOL)(pctx->flags.flg.upperc != 0U)); |
737 | arg >>= 4; |
738 | } |
739 | break; |
740 | } |
741 | |
742 | return _FMSTR_PipeIToAFinalize(pipeHandle, pctx); |
743 | } |
744 | |
745 | /****************************************************************************** |
746 | * |
747 | * @brief This function formats the argument into the temporary printf buffer |
748 | * It is granted by the caller that the buffer is empty before calling. |
749 | * |
750 | *****************************************************************************/ |
751 | |
752 | static FMSTR_BOOL FMSTR_PipeS8ToA(FMSTR_HPIPE pipeHandle, const FMSTR_S8 *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
753 | { |
754 | FMSTR_S8 arg = *parg; |
755 | |
756 | if (arg < 0) |
757 | { |
758 | pctx->flags.flg.negative = 1U; |
759 | |
760 | /* if sign will be shown, then negate the number */ |
761 | if (pctx->flags.flg.signedtype != 0U) |
762 | { |
763 | arg *= -1; |
764 | } |
765 | } |
766 | |
767 | return FMSTR_PipeU8ToA(pipeHandle, (const FMSTR_U8 *)&arg, pctx); |
768 | } |
769 | |
770 | /****************************************************************************** |
771 | * |
772 | * @brief This function formats the argument into the temporary printf buffer |
773 | * It is granted by the caller that the buffer is empty before calling. |
774 | * |
775 | *****************************************************************************/ |
776 | |
777 | static FMSTR_BOOL FMSTR_PipeU16ToA(FMSTR_HPIPE pipeHandle, const FMSTR_U16 *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
778 | { |
779 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
780 | FMSTR_U16 arg = *parg; |
781 | FMSTR_U8 dig; |
782 | FMSTR_INDEX i; |
783 | |
784 | switch (pctx->radix) |
785 | { |
786 | case FMSTR_PIPE_ITOAFMT_CHAR: |
787 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)arg; |
788 | break; |
789 | |
790 | case FMSTR_PIPE_ITOAFMT_BIN: |
791 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 16U) |
792 | { |
793 | return FMSTR_FALSE; |
794 | } |
795 | |
796 | for (i = 0; i < 16; i++) |
797 | { |
798 | if (arg == 0U) |
799 | { |
800 | break; |
801 | } |
802 | |
803 | dig = (FMSTR_U8)'0'; |
804 | dig += (FMSTR_U8)(arg & 1U); |
805 | |
806 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
807 | arg >>= 1; |
808 | } |
809 | break; |
810 | |
811 | case FMSTR_PIPE_ITOAFMT_OCT: |
812 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 6U) |
813 | { |
814 | return FMSTR_FALSE; |
815 | } |
816 | |
817 | for (i = 0; i < 6; i++) |
818 | { |
819 | if (arg == 0U) |
820 | { |
821 | break; |
822 | } |
823 | |
824 | dig = (FMSTR_U8)'0'; |
825 | dig += (FMSTR_U8)(arg & 7U); |
826 | |
827 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
828 | arg >>= 3; |
829 | } |
830 | break; |
831 | |
832 | case FMSTR_PIPE_ITOAFMT_DEC: |
833 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 5U) |
834 | { |
835 | return FMSTR_FALSE; |
836 | } |
837 | |
838 | for (i = 0; i < 5; i++) |
839 | { |
840 | if (arg == 0U) |
841 | { |
842 | break; |
843 | } |
844 | |
845 | dig = (FMSTR_U8)'0'; |
846 | dig += (FMSTR_U8)(arg % 10U); |
847 | |
848 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
849 | arg /= 10U; |
850 | } |
851 | break; |
852 | |
853 | case FMSTR_PIPE_ITOAFMT_HEX: |
854 | default: |
855 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 4U) |
856 | { |
857 | return FMSTR_FALSE; |
858 | } |
859 | |
860 | for (i = 0; i < 4; i++) |
861 | { |
862 | if (arg == 0U) |
863 | { |
864 | break; |
865 | } |
866 | |
867 | dig = (FMSTR_U8)(arg & 15U); |
868 | |
869 | pp->printfBuff[pp->printfBPtr++] = |
870 | _FMSTR_XDigit((FMSTR_U8)dig, (FMSTR_BOOL)(pctx->flags.flg.upperc != 0U)); |
871 | arg >>= 4; |
872 | } |
873 | break; |
874 | } |
875 | |
876 | return _FMSTR_PipeIToAFinalize(pipeHandle, pctx); |
877 | } |
878 | |
879 | /****************************************************************************** |
880 | * |
881 | * @brief This function formats the argument into the temporary printf buffer |
882 | * It is granted by the caller that the buffer is empty before calling. |
883 | * |
884 | *****************************************************************************/ |
885 | |
886 | static FMSTR_BOOL FMSTR_PipeS16ToA(FMSTR_HPIPE pipeHandle, const FMSTR_S16 *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
887 | { |
888 | FMSTR_S16 arg = *parg; |
889 | |
890 | if (arg < 0) |
891 | { |
892 | pctx->flags.flg.negative = 1U; |
893 | |
894 | /* if sign will be shown, then negate the number */ |
895 | if (pctx->flags.flg.signedtype != 0U) |
896 | { |
897 | arg *= -1; |
898 | } |
899 | } |
900 | |
901 | return FMSTR_PipeU16ToA(pipeHandle, (const FMSTR_U16 *)&arg, pctx); |
902 | } |
903 | |
904 | /****************************************************************************** |
905 | * |
906 | * @brief This function formats the argument into the temporary printf buffer |
907 | * It is granted by the caller that the buffer is empty before calling. |
908 | * |
909 | *****************************************************************************/ |
910 | |
911 | static FMSTR_BOOL FMSTR_PipeU32ToA(FMSTR_HPIPE pipeHandle, const FMSTR_U32 *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
912 | { |
913 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
914 | FMSTR_U32 arg = *parg; |
915 | FMSTR_U8 dig; |
916 | FMSTR_INDEX i; |
917 | |
918 | switch (pctx->radix) |
919 | { |
920 | case FMSTR_PIPE_ITOAFMT_CHAR: |
921 | pp->printfBuff[pp->printfBPtr++] = (char)arg; |
922 | break; |
923 | |
924 | case FMSTR_PIPE_ITOAFMT_BIN: |
925 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 32U) |
926 | { |
927 | return FMSTR_FALSE; |
928 | } |
929 | |
930 | for (i = 0; i < 32; i++) |
931 | { |
932 | if (arg == 0U) |
933 | { |
934 | break; |
935 | } |
936 | |
937 | dig = (FMSTR_U8)'0'; |
938 | dig += (FMSTR_U8)(arg & 1U); |
939 | |
940 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
941 | arg >>= 1; |
942 | } |
943 | break; |
944 | |
945 | case FMSTR_PIPE_ITOAFMT_OCT: |
946 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 11U) |
947 | { |
948 | return FMSTR_FALSE; |
949 | } |
950 | |
951 | for (i = 0; i < 11; i++) |
952 | { |
953 | if (arg == 0U) |
954 | { |
955 | break; |
956 | } |
957 | |
958 | dig = (FMSTR_U8)'0'; |
959 | dig += (FMSTR_U8)(arg & 7U); |
960 | |
961 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
962 | arg >>= 3; |
963 | } |
964 | break; |
965 | |
966 | case FMSTR_PIPE_ITOAFMT_DEC: |
967 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 10U) |
968 | { |
969 | return FMSTR_FALSE; |
970 | } |
971 | |
972 | for (i = 0; i < 10; i++) |
973 | { |
974 | if (arg == 0U) |
975 | { |
976 | break; |
977 | } |
978 | |
979 | dig = (FMSTR_U8)'0'; |
980 | dig += (FMSTR_U8)(arg % 10U); |
981 | |
982 | pp->printfBuff[pp->printfBPtr++] = (FMSTR_CHAR)dig; |
983 | arg /= 10U; |
984 | } |
985 | break; |
986 | |
987 | case FMSTR_PIPE_ITOAFMT_HEX: |
988 | default: |
989 | if (FMSTR_PIPES_PRINTF_BUFF_SIZE < 8U) |
990 | { |
991 | return FMSTR_FALSE; |
992 | } |
993 | |
994 | for (i = 0; i < 8; i++) |
995 | { |
996 | if (arg == 0U) |
997 | { |
998 | break; |
999 | } |
1000 | |
1001 | dig = (FMSTR_U8)(arg & 15U); |
1002 | |
1003 | pp->printfBuff[pp->printfBPtr++] = |
1004 | _FMSTR_XDigit((FMSTR_U8)dig, (FMSTR_BOOL)(pctx->flags.flg.upperc != 0U)); |
1005 | arg >>= 4; |
1006 | } |
1007 | break; |
1008 | } |
1009 | |
1010 | return _FMSTR_PipeIToAFinalize(pipeHandle, pctx); |
1011 | } |
1012 | |
1013 | /****************************************************************************** |
1014 | * |
1015 | * @brief This function formats the argument into the temporary printf buffer |
1016 | * It is granted by the caller that the buffer is empty before calling. |
1017 | * |
1018 | *****************************************************************************/ |
1019 | |
1020 | static FMSTR_BOOL FMSTR_PipeS32ToA(FMSTR_HPIPE pipeHandle, const FMSTR_S32 *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
1021 | { |
1022 | FMSTR_S32 arg = *parg; |
1023 | |
1024 | if (arg < 0) |
1025 | { |
1026 | pctx->flags.flg.negative = 1U; |
1027 | |
1028 | /* if sign will be shown, then negate the number */ |
1029 | if (pctx->flags.flg.signedtype != 0U) |
1030 | { |
1031 | arg *= -1; |
1032 | } |
1033 | } |
1034 | |
1035 | return FMSTR_PipeU32ToA(pipeHandle, (const FMSTR_U32 *)&arg, pctx); |
1036 | } |
1037 | |
1038 | /****************************************************************************** |
1039 | * |
1040 | * @brief This function parses the printf format and sets the context |
1041 | * structure properly. |
1042 | * |
1043 | * @return The function returns the pointer to end of format string handled |
1044 | * |
1045 | *****************************************************************************/ |
1046 | |
1047 | static const char *FMSTR_PipeParseFormat(const char *format, FMSTR_PIPE_PRINTF_CTX *pctx) |
1048 | { |
1049 | FMSTR_CHAR c; |
1050 | |
1051 | pctx->flags.all = 0; |
1052 | |
1053 | /* skip percent sign */ |
1054 | if (*format == '%') |
1055 | { |
1056 | format++; |
1057 | } |
1058 | |
1059 | /* show sign always? */ |
1060 | if (*format == '+') |
1061 | { |
1062 | pctx->flags.flg.showsign = 1U; |
1063 | format++; |
1064 | } |
1065 | |
1066 | /* prefix with zeroes? */ |
1067 | if (*format == '0') |
1068 | { |
1069 | pctx->flags.flg.zeroes = 1U; |
1070 | format++; |
1071 | } |
1072 | |
1073 | /* parse length */ |
1074 | pctx->alen = 0; |
1075 | while (FMSTR_IS_DIGIT(*format)) |
1076 | { |
1077 | c = *format++; |
1078 | c -= '0'; |
1079 | pctx->alen *= 10U; |
1080 | pctx->alen += (FMSTR_SIZE8)c; |
1081 | } |
1082 | |
1083 | /* parse dtsize modifier */ |
1084 | switch (*format) |
1085 | { |
1086 | /* short modifier (char for hh)*/ |
1087 | case 'h': |
1088 | pctx->dtsize = (FMSTR_U8)sizeof(short); |
1089 | format++; |
1090 | |
1091 | /* one more 'h' means 'char' */ |
1092 | if (*format == 'h') |
1093 | { |
1094 | pctx->dtsize = (FMSTR_U8)sizeof(char); |
1095 | format++; |
1096 | } |
1097 | break; |
1098 | |
1099 | case 'l': |
1100 | pctx->dtsize = (FMSTR_U8)sizeof(long); |
1101 | format++; |
1102 | break; |
1103 | |
1104 | default: |
1105 | /* default data type is 'int' */ |
1106 | pctx->dtsize = (FMSTR_U8)sizeof(int); |
1107 | break; |
1108 | } |
1109 | |
1110 | /* now finally concluding to format letter */ |
1111 | switch (*format++) |
1112 | { |
1113 | /* HEXADECIMAL */ |
1114 | case 'X': |
1115 | pctx->flags.flg.upperc = 1U; |
1116 | pctx->radix = FMSTR_PIPE_ITOAFMT_HEX; |
1117 | break; |
1118 | |
1119 | /* hexadecimal */ |
1120 | case 'x': |
1121 | pctx->radix = FMSTR_PIPE_ITOAFMT_HEX; |
1122 | break; |
1123 | |
1124 | /* octal */ |
1125 | case 'o': |
1126 | pctx->radix = FMSTR_PIPE_ITOAFMT_OCT; |
1127 | break; |
1128 | |
1129 | /* binary */ |
1130 | case 'b': |
1131 | pctx->radix = FMSTR_PIPE_ITOAFMT_BIN; |
1132 | break; |
1133 | |
1134 | /* decimal signed */ |
1135 | case 'd': |
1136 | case 'i': |
1137 | pctx->flags.flg.signedtype = 1U; |
1138 | pctx->radix = FMSTR_PIPE_ITOAFMT_DEC; |
1139 | break; |
1140 | |
1141 | /* decimal unsigned */ |
1142 | case 'u': |
1143 | pctx->radix = FMSTR_PIPE_ITOAFMT_DEC; |
1144 | break; |
1145 | |
1146 | /* character */ |
1147 | case 'c': |
1148 | pctx->radix = FMSTR_PIPE_ITOAFMT_CHAR; |
1149 | pctx->dtsize = (FMSTR_U8)sizeof(char); |
1150 | break; |
1151 | |
1152 | /* string */ |
1153 | case 's': |
1154 | pctx->flags.flg.isstring = 1U; |
1155 | pctx->dtsize = (FMSTR_U8)sizeof(void *); |
1156 | break; |
1157 | |
1158 | /* unknown */ |
1159 | default: |
1160 | FMSTR_ASSERT(0 == 1); |
1161 | pctx->radix = FMSTR_PIPE_ITOAFMT_HEX; |
1162 | break; |
1163 | } |
1164 | |
1165 | return format; |
1166 | } |
1167 | |
1168 | /****************************************************************************** |
1169 | * |
1170 | * @brief Printf with one argument passed by pointer. |
1171 | * |
1172 | ******************************************************************************/ |
1173 | |
1174 | static FMSTR_BOOL _FMSTR_PipePrintfOne(FMSTR_HPIPE pipeHandle, |
1175 | const char *format, |
1176 | void *parg, |
1177 | FMSTR_PIPE_ITOA_FUNC pItoaFunc) |
1178 | { |
1179 | FMSTR_BOOL ok = FMSTR_TRUE; |
1180 | FMSTR_PIPE_PRINTF_CTX ctx; |
1181 | |
1182 | while (*format != (FMSTR_CHAR)0 && ok != FMSTR_FALSE) |
1183 | { |
1184 | if (*format == '%') |
1185 | { |
1186 | format++; |
1187 | |
1188 | if (*format == '%') |
1189 | { |
1190 | ok = _FMSTR_PipePrintfPutc(pipeHandle, '%'); |
1191 | format++; |
1192 | continue; |
1193 | } |
1194 | |
1195 | /* empty the pipe's temporary buffer */ |
1196 | ok = _FMSTR_PipePrintfFlush(pipeHandle); |
1197 | |
1198 | if (ok != FMSTR_FALSE) |
1199 | { |
1200 | format = FMSTR_PipeParseFormat(format, &ctx); |
1201 | |
1202 | if (ctx.flags.flg.isstring != 0U) |
1203 | { |
1204 | const char *psz = (const char *)parg; |
1205 | ok = FMSTR_PipePuts(pipeHandle, psz != NULL ? psz : "NULL"); |
1206 | } |
1207 | else |
1208 | { |
1209 | ok = pItoaFunc(pipeHandle, parg, &ctx); |
1210 | } |
1211 | } |
1212 | } |
1213 | else |
1214 | { |
1215 | ok = _FMSTR_PipePrintfPutc(pipeHandle, *format++); |
1216 | } |
1217 | } |
1218 | |
1219 | if (ok != FMSTR_FALSE) |
1220 | { |
1221 | ok = _FMSTR_PipePrintfFlush(pipeHandle); |
1222 | } |
1223 | |
1224 | return (FMSTR_BOOL)(ok != FMSTR_FALSE); |
1225 | } |
1226 | |
1227 | /****************************************************************************** |
1228 | * |
1229 | * @brief PIPE API: Format argument into the pipe output stream. The format |
1230 | * follows the standard printf format. The leading '%' is optional. |
1231 | * |
1232 | ******************************************************************************/ |
1233 | |
1234 | FMSTR_BOOL FMSTR_PipePrintfU8(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_U8 arg) |
1235 | { |
1236 | return _FMSTR_PipePrintfOne(pipeHandle, format, &arg, (FMSTR_PIPE_ITOA_FUNC)FMSTR_PipeU8ToA); |
1237 | } |
1238 | |
1239 | /****************************************************************************** |
1240 | * |
1241 | * @brief PIPE API: Format argument into the pipe output stream. The format |
1242 | * follows the standard printf format. The leading '%' is optional. |
1243 | * |
1244 | ******************************************************************************/ |
1245 | |
1246 | FMSTR_BOOL FMSTR_PipePrintfS8(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_S8 arg) |
1247 | { |
1248 | return _FMSTR_PipePrintfOne(pipeHandle, format, &arg, (FMSTR_PIPE_ITOA_FUNC)FMSTR_PipeS8ToA); |
1249 | } |
1250 | |
1251 | /****************************************************************************** |
1252 | * |
1253 | * @brief PIPE API: Format argument into the pipe output stream. The format |
1254 | * follows the standard printf format. The leading '%' is optional. |
1255 | * |
1256 | ******************************************************************************/ |
1257 | |
1258 | FMSTR_BOOL FMSTR_PipePrintfU16(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_U16 arg) |
1259 | { |
1260 | return _FMSTR_PipePrintfOne(pipeHandle, format, &arg, (FMSTR_PIPE_ITOA_FUNC)FMSTR_PipeU16ToA); |
1261 | } |
1262 | |
1263 | /****************************************************************************** |
1264 | * |
1265 | * @brief PIPE API: Format argument into the pipe output stream. The format |
1266 | * follows the standard printf format. The leading '%' is optional. |
1267 | * |
1268 | ******************************************************************************/ |
1269 | |
1270 | FMSTR_BOOL FMSTR_PipePrintfS16(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_S16 arg) |
1271 | { |
1272 | return _FMSTR_PipePrintfOne(pipeHandle, format, &arg, (FMSTR_PIPE_ITOA_FUNC)FMSTR_PipeS16ToA); |
1273 | } |
1274 | |
1275 | /****************************************************************************** |
1276 | * |
1277 | * @brief PIPE API: Format argument into the pipe output stream. The format |
1278 | * follows the standard printf format. The leading '%' is optional. |
1279 | * |
1280 | ******************************************************************************/ |
1281 | |
1282 | FMSTR_BOOL FMSTR_PipePrintfU32(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_U32 arg) |
1283 | { |
1284 | return _FMSTR_PipePrintfOne(pipeHandle, format, &arg, (FMSTR_PIPE_ITOA_FUNC)FMSTR_PipeU32ToA); |
1285 | } |
1286 | |
1287 | /****************************************************************************** |
1288 | * |
1289 | * @brief PIPE API: Format argument into the pipe output stream. The format |
1290 | * follows the standard printf format. The leading '%' is optional. |
1291 | * |
1292 | ******************************************************************************/ |
1293 | |
1294 | FMSTR_BOOL FMSTR_PipePrintfS32(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_S32 arg) |
1295 | { |
1296 | return _FMSTR_PipePrintfOne(pipeHandle, format, &arg, (FMSTR_PIPE_ITOA_FUNC)FMSTR_PipeS32ToA); |
1297 | } |
1298 | |
1299 | /****************************************************************************** |
1300 | * |
1301 | * @brief Format va_list argument into the pipe output stream. This function |
1302 | * is called as a part of our printf routine. |
1303 | * |
1304 | ******************************************************************************/ |
1305 | static FMSTR_BOOL _FMSTR_PipePrintfAny(FMSTR_HPIPE pipeHandle, va_list *parg, FMSTR_PIPE_PRINTF_CTX *pctx) |
1306 | { |
1307 | FMSTR_BOOL ok = FMSTR_FALSE; |
1308 | |
1309 | switch (pctx->dtsize) |
1310 | { |
1311 | case 1: |
1312 | if (pctx->flags.flg.signedtype != 0U) |
1313 | { |
1314 | FMSTR_S8 arg = (FMSTR_S8)va_arg(*parg, int); |
1315 | ok = FMSTR_PipeS8ToA(pipeHandle, &arg, pctx); |
1316 | } |
1317 | else |
1318 | { |
1319 | FMSTR_U8 arg = (FMSTR_U8)va_arg(*parg, unsigned); |
1320 | ok = FMSTR_PipeU8ToA(pipeHandle, &arg, pctx); |
1321 | } |
1322 | break; |
1323 | |
1324 | case 2: |
1325 | if (pctx->flags.flg.signedtype != 0U) |
1326 | { |
1327 | FMSTR_S16 arg = (FMSTR_S16)va_arg(*parg, int); |
1328 | ok = FMSTR_PipeS16ToA(pipeHandle, &arg, pctx); |
1329 | } |
1330 | else |
1331 | { |
1332 | FMSTR_U16 arg = (FMSTR_U16)va_arg(*parg, unsigned); |
1333 | ok = FMSTR_PipeU16ToA(pipeHandle, &arg, pctx); |
1334 | } |
1335 | break; |
1336 | |
1337 | case 4: |
1338 | if (pctx->flags.flg.signedtype != 0U) |
1339 | { |
1340 | FMSTR_S32 arg = (FMSTR_S32)va_arg(*parg, long); |
1341 | ok = FMSTR_PipeS32ToA(pipeHandle, &arg, pctx); |
1342 | } |
1343 | else |
1344 | { |
1345 | FMSTR_U32 arg = (FMSTR_U32)va_arg(*parg, unsigned long); |
1346 | ok = FMSTR_PipeU32ToA(pipeHandle, &arg, pctx); |
1347 | } |
1348 | break; |
1349 | |
1350 | default: |
1351 | ok = FMSTR_FALSE; |
1352 | break; |
1353 | } |
1354 | |
1355 | return ok; |
1356 | } |
1357 | |
1358 | /****************************************************************************** |
1359 | * |
1360 | * @brief Printf with va_list arguments prepared. |
1361 | * |
1362 | * This function is not declared static (may be reused as global), |
1363 | * but public prototype is not available (not to force user to |
1364 | * have va_list defined. |
1365 | * |
1366 | ******************************************************************************/ |
1367 | |
1368 | static FMSTR_BOOL _FMSTR_PipePrintfV(FMSTR_HPIPE pipeHandle, const char *format, va_list *parg) |
1369 | { |
1370 | FMSTR_BOOL ok = FMSTR_TRUE; |
1371 | FMSTR_PIPE_PRINTF_CTX ctx; |
1372 | |
1373 | while (*format != (FMSTR_CHAR)0 && ok != FMSTR_FALSE) |
1374 | { |
1375 | if (*format == '%') |
1376 | { |
1377 | format++; |
1378 | |
1379 | if (*format == '%') |
1380 | { |
1381 | ok = _FMSTR_PipePrintfPutc(pipeHandle, '%'); |
1382 | format++; |
1383 | } |
1384 | else |
1385 | { |
1386 | ok = _FMSTR_PipePrintfFlush(pipeHandle); |
1387 | |
1388 | if (ok != FMSTR_FALSE) |
1389 | { |
1390 | format = FMSTR_PipeParseFormat(format, &ctx); |
1391 | |
1392 | if (ctx.flags.flg.isstring != 0U) |
1393 | { |
1394 | const char *psz = va_arg(*parg, char *); |
1395 | ok = FMSTR_PipePuts(pipeHandle, psz != NULL ? psz : "NULL"); |
1396 | } |
1397 | else |
1398 | { |
1399 | ok = _FMSTR_PipePrintfAny(pipeHandle, parg, &ctx); |
1400 | } |
1401 | } |
1402 | } |
1403 | } |
1404 | else |
1405 | { |
1406 | ok = _FMSTR_PipePrintfPutc(pipeHandle, *format++); |
1407 | } |
1408 | } |
1409 | |
1410 | if (ok != FMSTR_FALSE) |
1411 | { |
1412 | ok = _FMSTR_PipePrintfFlush(pipeHandle); |
1413 | } |
1414 | |
1415 | return (FMSTR_BOOL)(ok != FMSTR_FALSE); |
1416 | } |
1417 | |
1418 | #if FMSTR_USE_PIPE_PRINTF_VARG > 0 |
1419 | |
1420 | /****************************************************************************** |
1421 | * |
1422 | * @brief PIPE API: The printf into the pipe |
1423 | * |
1424 | ******************************************************************************/ |
1425 | |
1426 | FMSTR_BOOL FMSTR_PipePrintf(FMSTR_HPIPE pipeHandle, const char *format, ...) |
1427 | { |
1428 | FMSTR_BOOL ok; |
1429 | |
1430 | va_list args; |
1431 | va_start(args, format); |
1432 | ok = _FMSTR_PipePrintfV(pipeHandle, format, &args); |
1433 | va_end(args); |
1434 | |
1435 | return ok; |
1436 | } |
1437 | |
1438 | #endif /* FMSTR_USE_PIPE_PRINTF_VARG */ |
1439 | #endif /* FMSTR_USE_PIPE_PRINTF */ |
1440 | |
1441 | /****************************************************************************** |
1442 | * |
1443 | * @brief PIPE API: Read data from a pipe |
1444 | * |
1445 | ******************************************************************************/ |
1446 | |
1447 | FMSTR_PIPE_SIZE FMSTR_PipeRead(FMSTR_HPIPE pipeHandle, |
1448 | FMSTR_ADDR pipeData, |
1449 | FMSTR_PIPE_SIZE pipeDataLen, |
1450 | FMSTR_PIPE_SIZE readGranularity) |
1451 | { |
1452 | FMSTR_PIPE *pp = (FMSTR_PIPE *)pipeHandle; |
1453 | FMSTR_PIPE_BUFF *pbuff = &pp->rx; |
1454 | FMSTR_PIPE_SIZE total = _FMSTR_PipeGetBytesReady(pbuff); |
1455 | FMSTR_PIPE_SIZE s; |
1456 | |
1457 | /* when invalid address is given, only return number of bytes available */ |
1458 | if (FMSTR_ADDR_VALID(pipeData) != FMSTR_FALSE) |
1459 | { |
1460 | /* round length to bus width */ |
1461 | pipeDataLen /= FMSTR_CFG_BUS_WIDTH; |
1462 | pipeDataLen *= FMSTR_CFG_BUS_WIDTH; |
1463 | |
1464 | /* only fetch what we have cached */ |
1465 | if (pipeDataLen > total) |
1466 | { |
1467 | pipeDataLen = total; |
1468 | } |
1469 | |
1470 | /* obey granularity */ |
1471 | if (readGranularity > 1U) |
1472 | { |
1473 | pipeDataLen /= readGranularity; |
1474 | pipeDataLen *= readGranularity; |
1475 | } |
1476 | |
1477 | /* return value */ |
1478 | total = pipeDataLen; |
1479 | |
1480 | /* rest of cyclic buffer */ |
1481 | if (pipeDataLen > 0U) |
1482 | { |
1483 | /* total bytes available in the rest of buffer */ |
1484 | s = (FMSTR_PIPE_SIZE)((pbuff->size - pbuff->rp) * FMSTR_CFG_BUS_WIDTH); |
1485 | if (s > pipeDataLen) |
1486 | { |
1487 | s = pipeDataLen; |
1488 | } |
1489 | |
1490 | /* put bytes */ |
1491 | FMSTR_MemCpyTo(pipeData, pbuff->buff + pbuff->rp, (FMSTR_SIZE8)s); |
1492 | pipeData += s / FMSTR_CFG_BUS_WIDTH; |
1493 | |
1494 | /* advance & wrap pointer */ |
1495 | pbuff->rp += s / FMSTR_CFG_BUS_WIDTH; |
1496 | if (pbuff->rp >= pbuff->size) |
1497 | { |
1498 | pbuff->rp = 0; |
1499 | } |
1500 | |
1501 | /* rest of frame to a (wrapped) beggining of buffer */ |
1502 | pipeDataLen -= (FMSTR_SIZE8)s; |
1503 | if (pipeDataLen > 0U) |
1504 | { |
1505 | FMSTR_MemCpyTo(pipeData, pbuff->buff + pbuff->rp, (FMSTR_SIZE8)pipeDataLen); |
1506 | pbuff->rp += pipeDataLen / FMSTR_CFG_BUS_WIDTH; |
1507 | } |
1508 | |
1509 | /* buffer for sure not full now */ |
1510 | pbuff->flags.flg.bIsFull = 0; |
1511 | } |
1512 | } |
1513 | |
1514 | return total; |
1515 | } |
1516 | |
1517 | /****************************************************************************** |
1518 | * |
1519 | * @brief Find pipe by port number |
1520 | * |
1521 | ******************************************************************************/ |
1522 | |
1523 | static FMSTR_PIPE *_FMSTR_FindPipe(FMSTR_PIPE_PORT pipePort) |
1524 | { |
1525 | FMSTR_PIPE *pp; |
1526 | FMSTR_INDEX i; |
1527 | |
1528 | for (i = 0; i < (FMSTR_INDEX)FMSTR_USE_PIPES; i++) |
1529 | { |
1530 | pp = &pcm_pipes[i]; |
1531 | |
1532 | /* look for existing pipe with the same port */ |
1533 | if (pp->pipePort == pipePort) |
1534 | { |
1535 | return pp; |
1536 | } |
1537 | } |
1538 | |
1539 | return NULL; |
1540 | } |
1541 | |
1542 | /****************************************************************************** |
1543 | * |
1544 | * @brief Find pipe index by port number |
1545 | * |
1546 | ******************************************************************************/ |
1547 | |
1548 | FMSTR_INDEX FMSTR_FindPipeIndex(FMSTR_PIPE_PORT pipePort) |
1549 | { |
1550 | FMSTR_PIPE *pp; |
1551 | FMSTR_INDEX i; |
1552 | |
1553 | for (i = 0; i < (FMSTR_INDEX)FMSTR_USE_PIPES; i++) |
1554 | { |
1555 | pp = &pcm_pipes[i]; |
1556 | |
1557 | /* look for existing pipe with the same port */ |
1558 | if (pp->pipePort == pipePort) |
1559 | { |
1560 | return i; |
1561 | } |
1562 | } |
1563 | |
1564 | return -1; |
1565 | } |
1566 | |
1567 | /****************************************************************************** |
1568 | * |
1569 | * @brief Get number of bytes free in the buffer |
1570 | * |
1571 | ******************************************************************************/ |
1572 | |
1573 | static FMSTR_PIPE_SIZE _FMSTR_PipeGetBytesFree(FMSTR_PIPE_BUFF *pipeBuff) |
1574 | { |
1575 | FMSTR_PIPE_SIZE szFree; |
1576 | |
1577 | if (pipeBuff->flags.flg.bIsFull != 0U) |
1578 | { |
1579 | szFree = 0; |
1580 | } |
1581 | else if (pipeBuff->wp < pipeBuff->rp) |
1582 | { |
1583 | szFree = (FMSTR_PIPE_SIZE)(pipeBuff->rp - pipeBuff->wp); |
1584 | } |
1585 | else |
1586 | { |
1587 | szFree = (FMSTR_PIPE_SIZE)(pipeBuff->size - pipeBuff->wp + pipeBuff->rp); |
1588 | } |
1589 | |
1590 | return (FMSTR_PIPE_SIZE)(szFree * FMSTR_CFG_BUS_WIDTH); |
1591 | } |
1592 | |
1593 | static FMSTR_PIPE_SIZE _FMSTR_PipeGetBytesReady(FMSTR_PIPE_BUFF *pipeBuff) |
1594 | { |
1595 | FMSTR_PIPE_SIZE szFull; |
1596 | |
1597 | if (pipeBuff->flags.flg.bIsFull != 0U) |
1598 | { |
1599 | szFull = (FMSTR_PIPE_SIZE)pipeBuff->size; |
1600 | } |
1601 | else if (pipeBuff->wp >= pipeBuff->rp) |
1602 | { |
1603 | szFull = (FMSTR_PIPE_SIZE)(pipeBuff->wp - pipeBuff->rp); |
1604 | } |
1605 | else |
1606 | { |
1607 | szFull = (FMSTR_PIPE_SIZE)(pipeBuff->size - pipeBuff->rp + pipeBuff->wp); |
1608 | } |
1609 | |
1610 | return (FMSTR_PIPE_SIZE)(szFull * FMSTR_CFG_BUS_WIDTH); |
1611 | } |
1612 | |
1613 | static void _FMSTR_PipeDiscardBytes(FMSTR_PIPE_BUFF *pipeBuff, FMSTR_SIZE8 countBytes) |
1614 | { |
1615 | FMSTR_PIPE_SIZE total; |
1616 | FMSTR_PIPE_SIZE discard; |
1617 | |
1618 | total = _FMSTR_PipeGetBytesReady(pipeBuff); |
1619 | discard = (FMSTR_PIPE_SIZE)(countBytes > total ? total : countBytes); |
1620 | discard /= FMSTR_CFG_BUS_WIDTH; |
1621 | |
1622 | if (discard > 0U) |
1623 | { |
1624 | FMSTR_PIPE_SIZE rest = (FMSTR_PIPE_SIZE)(pipeBuff->size - pipeBuff->rp); |
1625 | FMSTR_PIPE_SIZE rp; |
1626 | |
1627 | /* will RP wrap? */ |
1628 | if (rest <= discard) |
1629 | { |
1630 | rp = (FMSTR_PIPE_SIZE)(discard - rest); |
1631 | } |
1632 | else |
1633 | { |
1634 | rp = (FMSTR_PIPE_SIZE)(pipeBuff->rp + discard); |
1635 | } |
1636 | |
1637 | /* buffer is for sure not full */ |
1638 | pipeBuff->flags.flg.bIsFull = 0; |
1639 | pipeBuff->rp = rp; |
1640 | } |
1641 | } |
1642 | |
1643 | /* get data from frame into our Rx buffer, we are already sure it fits */ |
1644 | |
1645 | static FMSTR_BPTR _FMSTR_PipeReceive(FMSTR_BPTR msgBuffIO, FMSTR_PIPE *pp, FMSTR_SIZE8 msgBuffSize) |
1646 | { |
1647 | FMSTR_PIPE_BUFF *pbuff = &pp->rx; |
1648 | FMSTR_PIPE_SIZE s; |
1649 | |
1650 | if (msgBuffSize > 0U) |
1651 | { |
1652 | /* total bytes available in the rest of buffer */ |
1653 | s = (FMSTR_PIPE_SIZE)((pbuff->size - pbuff->wp) * FMSTR_CFG_BUS_WIDTH); |
1654 | if (s > (FMSTR_PIPE_SIZE)msgBuffSize) |
1655 | { |
1656 | s = (FMSTR_PIPE_SIZE)msgBuffSize; |
1657 | } |
1658 | |
1659 | /* get the bytes */ |
1660 | msgBuffIO = FMSTR_CopyFromBuffer(pbuff->buff + pbuff->wp, msgBuffIO, (FMSTR_SIZE8)s); |
1661 | |
1662 | /* advance & wrap pointer */ |
1663 | pbuff->wp += s / FMSTR_CFG_BUS_WIDTH; |
1664 | if (pbuff->wp >= pbuff->size) |
1665 | { |
1666 | pbuff->wp = 0; |
1667 | } |
1668 | |
1669 | /* rest of frame to a (wrapped) beginning of buffer */ |
1670 | msgBuffSize -= (FMSTR_SIZE8)s; |
1671 | if (msgBuffSize > 0U) |
1672 | { |
1673 | msgBuffIO = FMSTR_CopyFromBuffer(pbuff->buff + pbuff->wp, msgBuffIO, msgBuffSize); |
1674 | pbuff->wp += ((FMSTR_PIPE_SIZE)msgBuffSize) / FMSTR_CFG_BUS_WIDTH; |
1675 | } |
1676 | |
1677 | /* buffer got full? */ |
1678 | if (pbuff->wp == pbuff->rp) |
1679 | { |
1680 | pbuff->flags.flg.bIsFull = 1; |
1681 | } |
1682 | } |
1683 | |
1684 | return msgBuffIO; |
1685 | } |
1686 | |
1687 | /* put data into the comm buffer, we are already sure it fits, buffer's RP is not modified */ |
1688 | |
1689 | static FMSTR_BPTR _FMSTR_PipeTransmit(FMSTR_BPTR msgBuffIO, FMSTR_PIPE *pp, FMSTR_SIZE8 msgBuffSize) |
1690 | { |
1691 | FMSTR_PIPE_BUFF *pbuff = &pp->tx; |
1692 | FMSTR_PIPE_SIZE s, rp = pbuff->rp; |
1693 | |
1694 | if (msgBuffSize > 0U) |
1695 | { |
1696 | /* total bytes available in the rest of buffer */ |
1697 | s = (FMSTR_PIPE_SIZE)((pbuff->size - rp) * FMSTR_CFG_BUS_WIDTH); |
1698 | if (s > (FMSTR_PIPE_SIZE)msgBuffSize) |
1699 | { |
1700 | s = (FMSTR_PIPE_SIZE)msgBuffSize; |
1701 | } |
1702 | |
1703 | /* put bytes */ |
1704 | msgBuffIO = FMSTR_CopyToBuffer(msgBuffIO, pbuff->buff + rp, (FMSTR_SIZE8)s); |
1705 | |
1706 | /* advance & wrap pointer */ |
1707 | rp += s / FMSTR_CFG_BUS_WIDTH; |
1708 | if (rp >= pbuff->size) |
1709 | { |
1710 | rp = 0; |
1711 | } |
1712 | |
1713 | /* rest of frame to a (wrapped) beggining of buffer */ |
1714 | msgBuffSize -= (FMSTR_SIZE8)s; |
1715 | if (msgBuffSize > 0U) |
1716 | { |
1717 | msgBuffIO = FMSTR_CopyToBuffer(msgBuffIO, pbuff->buff + rp, msgBuffSize); |
1718 | } |
1719 | } |
1720 | |
1721 | return msgBuffIO; |
1722 | } |
1723 | |
1724 | /****************************************************************************** |
1725 | * |
1726 | * @brief Get PIPE info |
1727 | * |
1728 | * @param session - transport session |
1729 | * @param msgBuffIO - original command (in) and response buffer (out) |
1730 | * @param msgSize - size of data in buffer |
1731 | * @param retStatus - response status |
1732 | * |
1733 | * @param msgBuffIO - original command (in) and response buffer (out) |
1734 | * |
1735 | ******************************************************************************/ |
1736 | |
1737 | FMSTR_BPTR FMSTR_GetPipe(FMSTR_SESSION *session, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_U8 *retStatus) |
1738 | { |
1739 | FMSTR_BPTR response = msgBuffIO; |
1740 | FMSTR_U8 cfgCode, pipeIndex, pipeFlags; |
1741 | FMSTR_PIPE *pp; |
1742 | |
1743 | FMSTR_ASSERT(msgBuffIO != NULL); |
1744 | FMSTR_ASSERT(retStatus != NULL); |
1745 | |
1746 | /* need at least pipe index and cfgCode */ |
1747 | if (msgSize < 2U) |
1748 | { |
1749 | /* return status */ |
1750 | *retStatus = FMSTR_STC_PIPEERR; |
1751 | return response; |
1752 | } |
1753 | |
1754 | /* get Pipe flags */ |
1755 | msgBuffIO = FMSTR_ValueFromBuffer8(&pipeFlags, msgBuffIO); |
1756 | |
1757 | /* get Pipe index */ |
1758 | msgBuffIO = FMSTR_ValueFromBuffer8(&pipeIndex, msgBuffIO); |
1759 | |
1760 | /* get Pipe cfgCode */ |
1761 | msgBuffIO = FMSTR_ValueFromBuffer8(&cfgCode, msgBuffIO); |
1762 | |
1763 | /* Pipe index is a port number of a pipe */ |
1764 | if ((pipeFlags & FMSTR_PIPE_GETPIPE_FLAG_PORT) != 0U) |
1765 | { |
1766 | /* Find pipe by port number */ |
1767 | pp = _FMSTR_FindPipe(pipeIndex); |
1768 | if (pp == NULL) |
1769 | { |
1770 | /* return status */ |
1771 | *retStatus = FMSTR_STC_PIPEERR; |
1772 | return response; |
1773 | } |
1774 | } |
1775 | else |
1776 | { |
1777 | /* Check maximum pipe index */ |
1778 | if (pipeIndex >= (FMSTR_U8)FMSTR_USE_PIPES) |
1779 | { |
1780 | /* return status */ |
1781 | *retStatus = FMSTR_STC_INSTERR; |
1782 | return response; |
1783 | } |
1784 | |
1785 | /* Get pipe from the list */ |
1786 | pp = &pcm_pipes[pipeIndex]; |
1787 | } |
1788 | |
1789 | switch (cfgCode) |
1790 | { |
1791 | case FMSTR_PIPE_CFGCODE_NAME: |
1792 | /* Put pipe name */ |
1793 | if (pp->name != NULL) |
1794 | { |
1795 | response = FMSTR_CopyToBuffer(response, (FMSTR_ADDR)pp->name, FMSTR_StrLen(pp->name)); |
1796 | } |
1797 | break; |
1798 | |
1799 | case FMSTR_PIPE_CFGCODE_INFO: |
1800 | default: |
1801 | pipeFlags = 0; |
1802 | |
1803 | /* Add flag isOpen, when pipe is opened (has port number) */ |
1804 | if (pp->pipePort != 0U) |
1805 | { |
1806 | pipeFlags |= FMSTR_PIPE_GETINFO_FLAG_ISOPEN; |
1807 | } |
1808 | |
1809 | /* Put pipe port */ |
1810 | response = FMSTR_ValueToBuffer8(response, pp->pipePort); |
1811 | /* Put pipe type */ |
1812 | response = FMSTR_ValueToBuffer8(response, pp->type); |
1813 | /* Put pipe flags */ |
1814 | response = FMSTR_ValueToBuffer8(response, pipeFlags); |
1815 | break; |
1816 | } |
1817 | |
1818 | /* success */ |
1819 | *retStatus = FMSTR_STS_OK | FMSTR_STSF_VARLEN; |
1820 | return response; |
1821 | } |
1822 | |
1823 | /****************************************************************************** |
1824 | * |
1825 | * @brief Handling PIPE commands |
1826 | * |
1827 | * @param session - transport session |
1828 | * @param msgBuffIO - original command (in) and response buffer (out) |
1829 | * @param msgSize - size of data in buffer |
1830 | * @param retStatus - response status |
1831 | * |
1832 | * @param msgBuffIO - original command (in) and response buffer (out) |
1833 | * |
1834 | ******************************************************************************/ |
1835 | |
1836 | FMSTR_BPTR FMSTR_PipeFrame(FMSTR_SESSION *session, FMSTR_BPTR msgBuffIO, FMSTR_SIZE msgSize, FMSTR_U8 *retStatus) |
1837 | { |
1838 | FMSTR_BPTR response = msgBuffIO; |
1839 | FMSTR_U8 skipLen, pipePort; |
1840 | FMSTR_PIPE *pp; |
1841 | |
1842 | FMSTR_ASSERT(msgBuffIO != NULL); |
1843 | FMSTR_ASSERT(retStatus != NULL); |
1844 | |
1845 | /* need at least port number and tx-discard bytes */ |
1846 | if (msgSize < 1U) |
1847 | { |
1848 | /* return status */ |
1849 | *retStatus = FMSTR_STC_PIPEERR; |
1850 | return response; |
1851 | } |
1852 | |
1853 | /* get port number and even/odd flag */ |
1854 | msgBuffIO = FMSTR_ValueFromBuffer8(&pipePort, msgBuffIO); |
1855 | |
1856 | #if FMSTR_SESSION_COUNT > 1 |
1857 | /* Is feature locked by me */ |
1858 | if (FMSTR_IsFeatureOwned(session, FMSTR_FEATURE_PIPE, (FMSTR_PIPE_PORT)(pipePort & 0x7fU)) == FMSTR_FALSE) |
1859 | { |
1860 | *retStatus = FMSTR_STC_SERVBUSY; |
1861 | return response; |
1862 | } |
1863 | #endif |
1864 | |
1865 | /* get pipe by port */ |
1866 | pp = _FMSTR_FindPipe((FMSTR_PIPE_PORT)(pipePort & 0x7fU)); |
1867 | |
1868 | /* pipe port must exist (i.e. be open) */ |
1869 | if (pp == NULL) |
1870 | { |
1871 | /* return status */ |
1872 | *retStatus = FMSTR_STC_PIPEERR; |
1873 | return response; |
1874 | } |
1875 | |
1876 | /* data-in are valid only in "matching" request (even to even, odd to odd) */ |
1877 | if ((pipePort & 0x80U) != 0U) |
1878 | { |
1879 | if (pp->flags.flg.bExpectOdd == 0U) |
1880 | { |
1881 | msgSize = 0U; |
1882 | } |
1883 | else |
1884 | { |
1885 | pp->flags.flg.bExpectOdd = pp->flags.flg.bExpectOdd != 0U ? 0U : 1U; |
1886 | } |
1887 | } |
1888 | else |
1889 | { |
1890 | if (pp->flags.flg.bExpectOdd != 0U) |
1891 | { |
1892 | msgSize = 0U; |
1893 | } |
1894 | else |
1895 | { |
1896 | pp->flags.flg.bExpectOdd = pp->flags.flg.bExpectOdd != 0U ? 0U : 1U; |
1897 | } |
1898 | } |
1899 | |
1900 | /* process received data */ |
1901 | if (msgSize > 0U) |
1902 | { |
1903 | /* first byte tells me how many output bytes can be discarded from my |
1904 | pipe-transmit buffer (this is how PC acknowledges how many bytes it |
1905 | received and saved from the last response) */ |
1906 | msgBuffIO = FMSTR_ValueFromBuffer8(&skipLen, msgBuffIO); |
1907 | |
1908 | /* discard bytes from pipe's transmit buffer */ |
1909 | if (skipLen > 0U) |
1910 | { |
1911 | _FMSTR_PipeDiscardBytes(&pp->tx, skipLen); |
1912 | } |
1913 | |
1914 | /* next come (msgSize-2) bytes to be received */ |
1915 | if (msgSize > 2U) |
1916 | { |
1917 | /* how many bytes may I accept? */ |
1918 | FMSTR_PIPE_SIZE rxFree = _FMSTR_PipeGetBytesFree(&pp->rx); |
1919 | /* how many bytes PC want to push? */ |
1920 | FMSTR_U8 rxToRead = (FMSTR_U8)(msgSize - 2U); |
1921 | |
1922 | /* round to bus width */ |
1923 | rxToRead /= FMSTR_CFG_BUS_WIDTH; |
1924 | rxToRead *= FMSTR_CFG_BUS_WIDTH; |
1925 | |
1926 | /* get the lower of the two numbers */ |
1927 | if (rxFree < (FMSTR_PIPE_SIZE)rxToRead) |
1928 | { |
1929 | rxToRead = (FMSTR_U8)rxFree; |
1930 | } |
1931 | |
1932 | /* get frame data */ |
1933 | msgBuffIO = _FMSTR_PipeReceive(msgBuffIO, pp, rxToRead); |
1934 | FMSTR_UNUSED(msgBuffIO); |
1935 | |
1936 | /* this is the number to be returned to PC to inform it how |
1937 | many bytes it may discard in his transmit buffer */ |
1938 | pp->nLastBytesReceived = rxToRead; |
1939 | } |
1940 | else |
1941 | { |
1942 | /* no bytes received */ |
1943 | pp->nLastBytesReceived = 0; |
1944 | } |
1945 | } |
1946 | |
1947 | /* now call the pipe's handler, it may read or write data */ |
1948 | if (pp->pCallback != NULL) |
1949 | { |
1950 | pp->flags.flg.bInComm = 1; |
1951 | pp->pCallback((FMSTR_HPIPE)pp); |
1952 | pp->flags.flg.bInComm = 0; |
1953 | } |
1954 | |
1955 | /* now put our output data */ |
1956 | { |
1957 | /* how many bytes are waiting to be sent? */ |
1958 | FMSTR_PIPE_SIZE txAvail = _FMSTR_PipeGetBytesReady(&pp->tx); |
1959 | /* how many bytes I can safely put? */ |
1960 | FMSTR_U8 txToSend = (FMSTR_U8)FMSTR_COMM_BUFFER_SIZE - 3U; |
1961 | |
1962 | /* round to bus width */ |
1963 | txToSend /= FMSTR_CFG_BUS_WIDTH; |
1964 | txToSend *= FMSTR_CFG_BUS_WIDTH; |
1965 | |
1966 | /* get the lower of two values */ |
1967 | if (txAvail < (FMSTR_PIPE_SIZE)txToSend) |
1968 | { |
1969 | txToSend = (FMSTR_U8)txAvail; |
1970 | } |
1971 | |
1972 | /* send pipe's transmit data back */ |
1973 | response = FMSTR_ValueToBuffer8(response, pipePort); |
1974 | |
1975 | /* inform PC how many bytes it may discard from its pipe's transmit buffer */ |
1976 | skipLen = pp->nLastBytesReceived; |
1977 | response = FMSTR_ValueToBuffer8(response, skipLen); |
1978 | |
1979 | /* put data */ |
1980 | if (txToSend != 0U) |
1981 | { |
1982 | response = _FMSTR_PipeTransmit(response, pp, txToSend); |
1983 | } |
1984 | } |
1985 | |
1986 | /* success */ |
1987 | *retStatus = FMSTR_STS_OK | FMSTR_STSF_VARLEN; |
1988 | return response; |
1989 | } |
1990 | |
1991 | #else /* FMSTR_USE_PIPES && (!FMSTR_DISABLE) */ |
1992 | |
1993 | /* implement void pipe-API functions */ |
1994 | |
1995 | FMSTR_HPIPE FMSTR_PipeOpen(FMSTR_PIPE_PORT pipePort, |
1996 | FMSTR_PPIPEFUNC pipeCallback, |
1997 | FMSTR_ADDR pipeRxBuff, |
1998 | FMSTR_PIPE_SIZE pipeRxSize, |
1999 | FMSTR_ADDR pipeTxBuff, |
2000 | FMSTR_PIPE_SIZE pipeTxSize, |
2001 | FMSTR_U8 type, |
2002 | const FMSTR_CHAR *name) |
2003 | { |
2004 | FMSTR_UNUSED(pipePort); |
2005 | FMSTR_UNUSED(pipeCallback); |
2006 | FMSTR_UNUSED(pipeRxBuff); |
2007 | FMSTR_UNUSED(pipeRxSize); |
2008 | FMSTR_UNUSED(pipeTxBuff); |
2009 | FMSTR_UNUSED(pipeTxSize); |
2010 | FMSTR_UNUSED(type); |
2011 | FMSTR_UNUSED(name); |
2012 | |
2013 | return NULL; |
2014 | } |
2015 | |
2016 | void FMSTR_PipeClose(FMSTR_HPIPE pipeHandle) |
2017 | { |
2018 | FMSTR_UNUSED(pipeHandle); |
2019 | } |
2020 | |
2021 | FMSTR_PIPE_SIZE FMSTR_PipeWrite(FMSTR_HPIPE pipeHandle, |
2022 | FMSTR_ADDR pipeData, |
2023 | FMSTR_PIPE_SIZE pipeDataLen, |
2024 | FMSTR_PIPE_SIZE writeGranularity) |
2025 | { |
2026 | FMSTR_UNUSED(pipeHandle); |
2027 | FMSTR_UNUSED(pipeData); |
2028 | FMSTR_UNUSED(pipeDataLen); |
2029 | FMSTR_UNUSED(writeGranularity); |
2030 | |
2031 | return 0U; |
2032 | } |
2033 | |
2034 | FMSTR_PIPE_SIZE FMSTR_PipeRead(FMSTR_HPIPE pipeHandle, |
2035 | FMSTR_ADDR pipeData, |
2036 | FMSTR_PIPE_SIZE pipeDataLen, |
2037 | FMSTR_PIPE_SIZE readGranularity) |
2038 | { |
2039 | FMSTR_UNUSED(pipeHandle); |
2040 | FMSTR_UNUSED(pipeData); |
2041 | FMSTR_UNUSED(pipeDataLen); |
2042 | FMSTR_UNUSED(readGranularity); |
2043 | |
2044 | return 0U; |
2045 | } |
2046 | |
2047 | /*lint -efile(766, freemaster_protocol.h) include file is not used in this case */ |
2048 | |
2049 | #endif /* FMSTR_USE_PIPES && (!FMSTR_DISABLE) */ |
2050 | |
2051 | #if FMSTR_DISABLE > 0 || FMSTR_USE_PIPES == 0 || FMSTR_USE_PIPE_PRINTF == 0 |
2052 | |
2053 | FMSTR_BOOL FMSTR_PipePrintfU8(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_U8 arg) |
2054 | { |
2055 | FMSTR_UNUSED(pipeHandle); |
2056 | FMSTR_UNUSED(format); |
2057 | FMSTR_UNUSED(arg); |
2058 | |
2059 | return FMSTR_FALSE; |
2060 | } |
2061 | |
2062 | FMSTR_BOOL FMSTR_PipePrintfS8(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_S8 arg) |
2063 | { |
2064 | FMSTR_UNUSED(pipeHandle); |
2065 | FMSTR_UNUSED(format); |
2066 | FMSTR_UNUSED(arg); |
2067 | |
2068 | return FMSTR_FALSE; |
2069 | } |
2070 | |
2071 | FMSTR_BOOL FMSTR_PipePrintfU16(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_U16 arg) |
2072 | { |
2073 | FMSTR_UNUSED(pipeHandle); |
2074 | FMSTR_UNUSED(format); |
2075 | FMSTR_UNUSED(arg); |
2076 | |
2077 | return FMSTR_FALSE; |
2078 | } |
2079 | |
2080 | FMSTR_BOOL FMSTR_PipePrintfS16(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_S16 arg) |
2081 | { |
2082 | FMSTR_UNUSED(pipeHandle); |
2083 | FMSTR_UNUSED(format); |
2084 | FMSTR_UNUSED(arg); |
2085 | |
2086 | return FMSTR_FALSE; |
2087 | } |
2088 | |
2089 | FMSTR_BOOL FMSTR_PipePrintfU32(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_U32 arg) |
2090 | { |
2091 | FMSTR_UNUSED(pipeHandle); |
2092 | FMSTR_UNUSED(format); |
2093 | FMSTR_UNUSED(arg); |
2094 | |
2095 | return FMSTR_FALSE; |
2096 | } |
2097 | |
2098 | FMSTR_BOOL FMSTR_PipePrintfS32(FMSTR_HPIPE pipeHandle, const char *format, FMSTR_S32 arg) |
2099 | { |
2100 | FMSTR_UNUSED(pipeHandle); |
2101 | FMSTR_UNUSED(format); |
2102 | FMSTR_UNUSED(arg); |
2103 | |
2104 | return FMSTR_FALSE; |
2105 | } |
2106 | |
2107 | #endif /* (!(FMSTR_USE_PIPES)) || (!(FMSTR_USE_PIPE_PRINTF)) */ |
2108 | |
2109 | #if FMSTR_DISABLE > 0 || FMSTR_USE_PIPES == 0 || FMSTR_USE_PIPE_PRINTF_VARG == 0 |
2110 | |
2111 | FMSTR_BOOL FMSTR_PipePrintf(FMSTR_HPIPE pipeHandle, const char *format, ...) |
2112 | { |
2113 | FMSTR_UNUSED(pipeHandle); |
2114 | FMSTR_UNUSED(format); |
2115 | |
2116 | return FMSTR_FALSE; |
2117 | } |
2118 | |
2119 | #endif /* (!(FMSTR_USE_PIPES)) || (!(FMSTR_USE_PIPE_PRINTF_VARG)) */ |
2120 | |