1 /*
2 * Copyright (C) 2010 NXP Semiconductors
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /**
18 * \file phFriNfc_Llcp.c
19 * \brief NFC LLCP core
20 *
21 * Project: NFC-FRI
22 *
23 */
24
25 /*include files*/
26 #include <phNfcTypes.h>
27 #include <phNfcHalTypes.h>
28 #include <phLibNfcStatus.h>
29 #include <phFriNfc_LlcpUtils.h>
30 #include <phFriNfc_Llcp.h>
31
phFriNfc_Llcp_DecodeTLV(phNfc_sData_t * psRawData,uint32_t * pOffset,uint8_t * pType,phNfc_sData_t * psValueBuffer)32 NFCSTATUS phFriNfc_Llcp_DecodeTLV( phNfc_sData_t *psRawData,
33 uint32_t *pOffset,
34 uint8_t *pType,
35 phNfc_sData_t *psValueBuffer )
36 {
37 uint8_t type;
38 uint8_t length;
39 uint32_t offset = *pOffset;
40
41 /* Check for NULL pointers */
42 if ((psRawData == NULL) || (pOffset == NULL) || (pType == NULL) || (psValueBuffer == NULL))
43 {
44 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
45 }
46
47 /* Check offset */
48 if (offset > psRawData->length)
49 {
50 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
51 }
52
53 /* Check if enough room for Type and Length (with overflow check) */
54 if ((offset+2 > psRawData->length) && (offset+2 > offset))
55 {
56 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
57 }
58
59 /* Get Type and Length from current TLV, and update offset */
60 type = psRawData->buffer[offset];
61 length = psRawData->buffer[offset+1];
62 offset += 2;
63
64 /* Check if enough room for Value with announced Length (with overflow check) */
65 if ((offset+length > psRawData->length) && (offset+length > offset))
66 {
67 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
68 }
69
70 /* Save response, and update offset */
71 *pType = type;
72 psValueBuffer->buffer = psRawData->buffer + offset;
73 psValueBuffer->length = length;
74 offset += length;
75
76 /* Save updated offset */
77 *pOffset = offset;
78
79 return NFCSTATUS_SUCCESS;
80 }
81
phFriNfc_Llcp_EncodeTLV(phNfc_sData_t * psValueBuffer,uint32_t * pOffset,uint8_t type,uint8_t length,uint8_t * pValue)82 NFCSTATUS phFriNfc_Llcp_EncodeTLV( phNfc_sData_t *psValueBuffer,
83 uint32_t *pOffset,
84 uint8_t type,
85 uint8_t length,
86 uint8_t *pValue)
87 {
88 uint32_t offset = *pOffset;
89 uint32_t finalOffset = offset + 2 + length; /* 2 stands for Type and Length fields size */
90 uint8_t i;
91
92 /* Check for NULL pointers */
93 if ((psValueBuffer == NULL) || (pOffset == NULL) || (pValue == NULL))
94 {
95 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
96 }
97
98 /* Check offset */
99 if (offset > psValueBuffer->length)
100 {
101 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
102 }
103
104 /* Check if enough room for Type, Length and Value (with overflow check) */
105 if ((finalOffset > psValueBuffer->length) || (finalOffset < offset))
106 {
107 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
108 }
109
110 /* Set the TYPE */
111 psValueBuffer->buffer[offset] = type;
112 offset += 1;
113
114 /* Set the LENGTH */
115 psValueBuffer->buffer[offset] = length;
116 offset += 1;
117
118 /* Set the VALUE */
119 for(i=0;i<length;i++,offset++)
120 {
121 psValueBuffer->buffer[offset] = pValue[i];
122 }
123
124 /* Save updated offset */
125 *pOffset = offset;
126
127 return NFCSTATUS_SUCCESS;
128 }
129
phFriNfc_Llcp_AppendTLV(phNfc_sData_t * psValueBuffer,uint32_t nTlvOffset,uint32_t * pCurrentOffset,uint8_t length,uint8_t * pValue)130 NFCSTATUS phFriNfc_Llcp_AppendTLV( phNfc_sData_t *psValueBuffer,
131 uint32_t nTlvOffset,
132 uint32_t *pCurrentOffset,
133 uint8_t length,
134 uint8_t *pValue)
135 {
136 uint32_t offset = *pCurrentOffset;
137 uint32_t finalOffset = offset + length;
138
139 /* Check for NULL pointers */
140 if ((psValueBuffer == NULL) || (pCurrentOffset == NULL) || (pValue == NULL))
141 {
142 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
143 }
144
145 /* Check offset */
146 if (offset > psValueBuffer->length)
147 {
148 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
149 }
150
151 /* Check if enough room for Type and Length (with overflow check) */
152 if ((finalOffset > psValueBuffer->length) || (finalOffset < offset))
153 {
154 return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
155 }
156
157 /* Update the LENGTH */
158 psValueBuffer->buffer[nTlvOffset+1] += length;
159
160 /* Set the VALUE */
161 memcpy(psValueBuffer->buffer + offset, pValue, length);
162 offset += length;
163
164 /* Save updated offset */
165 *pCurrentOffset = offset;
166
167 return NFCSTATUS_SUCCESS;
168 }
169
170
171 /* TODO: comment function EncodeMIUX */
phFriNfc_Llcp_EncodeMIUX(uint16_t miux,uint8_t * pMiuxEncoded)172 void phFriNfc_Llcp_EncodeMIUX(uint16_t miux,
173 uint8_t* pMiuxEncoded)
174 {
175 /* MASK */
176 miux = miux & PHFRINFC_LLCP_TLV_MIUX_MASK;
177
178 pMiuxEncoded[0] = miux >> 8;
179 pMiuxEncoded[1] = miux & 0xff;
180 }
181
182 /* TODO: comment function EncodeRW */
phFriNfc_Llcp_EncodeRW(uint8_t * pRw)183 void phFriNfc_Llcp_EncodeRW(uint8_t *pRw)
184 {
185 /* MASK */
186 *pRw = *pRw & PHFRINFC_LLCP_TLV_RW_MASK;
187 }
188
189 /**
190 * Initializes a Fifo Cyclic Buffer to point to some allocated memory.
191 */
phFriNfc_Llcp_CyclicFifoInit(P_UTIL_FIFO_BUFFER pUtilFifo,const uint8_t * pBuffStart,uint32_t buffLength)192 void phFriNfc_Llcp_CyclicFifoInit(P_UTIL_FIFO_BUFFER pUtilFifo,
193 const uint8_t *pBuffStart,
194 uint32_t buffLength)
195 {
196 pUtilFifo->pBuffStart = (uint8_t *)pBuffStart;
197 pUtilFifo->pBuffEnd = (uint8_t *)pBuffStart + buffLength-1;
198 pUtilFifo->pIn = (uint8_t *)pBuffStart;
199 pUtilFifo->pOut = (uint8_t *)pBuffStart;
200 pUtilFifo->bFull = FALSE;
201 }
202
203 /**
204 * Clears the Fifo Cyclic Buffer - loosing any data that was in it.
205 */
phFriNfc_Llcp_CyclicFifoClear(P_UTIL_FIFO_BUFFER pUtilFifo)206 void phFriNfc_Llcp_CyclicFifoClear(P_UTIL_FIFO_BUFFER pUtilFifo)
207 {
208 pUtilFifo->pIn = pUtilFifo->pBuffStart;
209 pUtilFifo->pOut = pUtilFifo->pBuffStart;
210 pUtilFifo->bFull = FALSE;
211 }
212
213 /**
214 * Attempts to write dataLength bytes to the specified Fifo Cyclic Buffer.
215 */
phFriNfc_Llcp_CyclicFifoWrite(P_UTIL_FIFO_BUFFER pUtilFifo,uint8_t * pData,uint32_t dataLength)216 uint32_t phFriNfc_Llcp_CyclicFifoWrite(P_UTIL_FIFO_BUFFER pUtilFifo,
217 uint8_t *pData,
218 uint32_t dataLength)
219 {
220 uint32_t dataLengthWritten = 0;
221 uint8_t * pNext;
222
223 while(dataLengthWritten < dataLength)
224 {
225 pNext = (uint8_t*)pUtilFifo->pIn+1;
226
227 if(pNext > pUtilFifo->pBuffEnd)
228 {
229 //Wrap around
230 pNext = pUtilFifo->pBuffStart;
231 }
232
233 if(pUtilFifo->bFull)
234 {
235 //Full
236 break;
237 }
238
239 if(pNext == pUtilFifo->pOut)
240 {
241 // Trigger Full flag
242 pUtilFifo->bFull = TRUE;
243 }
244
245 dataLengthWritten++;
246 *pNext = *pData++;
247 pUtilFifo->pIn = pNext;
248 }
249
250 return dataLengthWritten;
251 }
252
253 /**
254 * Attempts to read dataLength bytes from the specified Fifo Cyclic Buffer.
255 */
phFriNfc_Llcp_CyclicFifoFifoRead(P_UTIL_FIFO_BUFFER pUtilFifo,uint8_t * pBuffer,uint32_t dataLength)256 uint32_t phFriNfc_Llcp_CyclicFifoFifoRead(P_UTIL_FIFO_BUFFER pUtilFifo,
257 uint8_t *pBuffer,
258 uint32_t dataLength)
259 {
260 uint32_t dataLengthRead = 0;
261 uint8_t * pNext;
262
263 while(dataLengthRead < dataLength)
264 {
265 if((pUtilFifo->pOut == pUtilFifo->pIn) && (pUtilFifo->bFull == FALSE))
266 {
267 //No more bytes in buffer
268 break;
269 }
270 else
271 {
272 dataLengthRead++;
273
274 if(pUtilFifo->pOut == pUtilFifo->pBuffEnd)
275 {
276 /* Wrap around */
277 pNext = pUtilFifo->pBuffStart;
278 }
279 else
280 {
281 pNext = (uint8_t*)pUtilFifo->pOut + 1;
282 }
283
284 *pBuffer++ = *pNext;
285
286 pUtilFifo->pOut = pNext;
287
288 pUtilFifo->bFull = FALSE;
289 }
290 }
291
292 return dataLengthRead;
293 }
294
295 /**
296 * Returns the number of bytes currently stored in Fifo Cyclic Buffer.
297 */
phFriNfc_Llcp_CyclicFifoUsage(P_UTIL_FIFO_BUFFER pUtilFifo)298 uint32_t phFriNfc_Llcp_CyclicFifoUsage(P_UTIL_FIFO_BUFFER pUtilFifo)
299 {
300 uint32_t dataLength;
301 uint8_t * pIn = (uint8_t *)pUtilFifo->pIn;
302 uint8_t * pOut = (uint8_t *)pUtilFifo->pOut;
303
304 if (pUtilFifo->bFull)
305 {
306 dataLength = pUtilFifo->pBuffEnd - pUtilFifo->pBuffStart + 1;
307 }
308 else
309 {
310 if(pIn >= pOut)
311 {
312 dataLength = pIn - pOut;
313 }
314 else
315 {
316 dataLength = pUtilFifo->pBuffEnd - pOut;
317 dataLength += (pIn+1) - pUtilFifo->pBuffStart;
318 }
319 }
320
321 return dataLength;
322 }
323
324
325 /**
326 * Returns the available room for writing in Fifo Cyclic Buffer.
327 */
phFriNfc_Llcp_CyclicFifoAvailable(P_UTIL_FIFO_BUFFER pUtilFifo)328 uint32_t phFriNfc_Llcp_CyclicFifoAvailable(P_UTIL_FIFO_BUFFER pUtilFifo)
329 {
330 uint32_t dataLength;
331 uint32_t size;
332 uint8_t * pIn = (uint8_t *)pUtilFifo->pIn;
333 uint8_t * pOut = (uint8_t *)pUtilFifo->pOut;
334
335 if (pUtilFifo->bFull)
336 {
337 dataLength = 0;
338 }
339 else
340 {
341 if(pIn >= pOut)
342 {
343 size = pUtilFifo->pBuffEnd - pUtilFifo->pBuffStart + 1;
344 dataLength = size - (pIn - pOut);
345 }
346 else
347 {
348 dataLength = pOut - pIn;
349 }
350 }
351
352 return dataLength;
353 }
354
355
356
phFriNfc_Llcp_Header2Buffer(phFriNfc_Llcp_sPacketHeader_t * psHeader,uint8_t * pBuffer,uint32_t nOffset)357 uint32_t phFriNfc_Llcp_Header2Buffer( phFriNfc_Llcp_sPacketHeader_t *psHeader, uint8_t *pBuffer, uint32_t nOffset )
358 {
359 uint32_t nOriginalOffset = nOffset;
360 pBuffer[nOffset++] = (uint8_t)((psHeader->dsap << 2) | (psHeader->ptype >> 2));
361 pBuffer[nOffset++] = (uint8_t)((psHeader->ptype << 6) | psHeader->ssap);
362 return nOffset - nOriginalOffset;
363 }
364
phFriNfc_Llcp_Sequence2Buffer(phFriNfc_Llcp_sPacketSequence_t * psSequence,uint8_t * pBuffer,uint32_t nOffset)365 uint32_t phFriNfc_Llcp_Sequence2Buffer( phFriNfc_Llcp_sPacketSequence_t *psSequence, uint8_t *pBuffer, uint32_t nOffset )
366 {
367 uint32_t nOriginalOffset = nOffset;
368 pBuffer[nOffset++] = (uint8_t)((psSequence->ns << 4) | (psSequence->nr));
369 return nOffset - nOriginalOffset;
370 }
371
phFriNfc_Llcp_Buffer2Header(uint8_t * pBuffer,uint32_t nOffset,phFriNfc_Llcp_sPacketHeader_t * psHeader)372 uint32_t phFriNfc_Llcp_Buffer2Header( uint8_t *pBuffer, uint32_t nOffset, phFriNfc_Llcp_sPacketHeader_t *psHeader )
373 {
374 psHeader->dsap = (pBuffer[nOffset] & 0xFC) >> 2;
375 psHeader->ptype = ((pBuffer[nOffset] & 0x03) << 2) | ((pBuffer[nOffset+1] & 0xC0) >> 6);
376 psHeader->ssap = pBuffer[nOffset+1] & 0x3F;
377 return PHFRINFC_LLCP_PACKET_HEADER_SIZE;
378 }
379
phFriNfc_Llcp_Buffer2Sequence(uint8_t * pBuffer,uint32_t nOffset,phFriNfc_Llcp_sPacketSequence_t * psSequence)380 uint32_t phFriNfc_Llcp_Buffer2Sequence( uint8_t *pBuffer, uint32_t nOffset, phFriNfc_Llcp_sPacketSequence_t *psSequence )
381 {
382 psSequence->ns = pBuffer[nOffset] >> 4;
383 psSequence->nr = pBuffer[nOffset] & 0x0F;
384 return PHFRINFC_LLCP_PACKET_SEQUENCE_SIZE;
385 }
386
387
388