1#include "headers.h"
2
3static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
4	struct bcm_ipv6_hdr *pstIpv6Header);
5static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
6	struct bcm_ipv6_hdr *pstIpv6Header);
7static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header);
8
9static UCHAR *GetNextIPV6ChainedHeader(UCHAR **ppucPayload,
10	UCHAR *pucNextHeader, bool *bParseDone, USHORT *pusPayloadLength)
11{
12	UCHAR *pucRetHeaderPtr = NULL;
13	UCHAR *pucPayloadPtr = NULL;
14	USHORT  usNextHeaderOffset = 0;
15	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
16
17	if ((ppucPayload == NULL) || (*pusPayloadLength == 0) ||
18		(*bParseDone)) {
19		*bParseDone = TRUE;
20		return NULL;
21	}
22
23	pucRetHeaderPtr = *ppucPayload;
24	pucPayloadPtr = *ppucPayload;
25
26	if (!pucRetHeaderPtr || !pucPayloadPtr) {
27		*bParseDone = TRUE;
28		return NULL;
29	}
30
31	/* Get the Nextt Header Type */
32	*bParseDone = false;
33
34
35	switch (*pucNextHeader) {
36	case IPV6HDR_TYPE_HOPBYHOP:
37		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
38				DBG_LVL_ALL, "\nIPv6 HopByHop Header");
39		usNextHeaderOffset += sizeof(struct bcm_ipv6_options_hdr);
40		break;
41
42	case IPV6HDR_TYPE_ROUTING:
43		{
44			struct bcm_ipv6_routing_hdr *pstIpv6RoutingHeader;
45
46			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
47					DBG_LVL_ALL, "\nIPv6 Routing Header");
48			pstIpv6RoutingHeader =
49				(struct bcm_ipv6_routing_hdr *)pucPayloadPtr;
50			usNextHeaderOffset += sizeof(struct bcm_ipv6_routing_hdr);
51			usNextHeaderOffset += pstIpv6RoutingHeader->ucNumAddresses *
52					      IPV6_ADDRESS_SIZEINBYTES;
53		}
54		break;
55
56	case IPV6HDR_TYPE_FRAGMENTATION:
57		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
58				DBG_LVL_ALL,
59				"\nIPv6 Fragmentation Header");
60		usNextHeaderOffset += sizeof(struct bcm_ipv6_fragment_hdr);
61		break;
62
63	case IPV6HDR_TYPE_DESTOPTS:
64		{
65			struct bcm_ipv6_dest_options_hdr *pstIpv6DestOptsHdr =
66				(struct bcm_ipv6_dest_options_hdr *)pucPayloadPtr;
67			int nTotalOptions = pstIpv6DestOptsHdr->ucHdrExtLen;
68
69			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
70					DBG_LVL_ALL,
71					"\nIPv6 DestOpts Header Header");
72			usNextHeaderOffset += sizeof(struct bcm_ipv6_dest_options_hdr);
73			usNextHeaderOffset += nTotalOptions *
74					      IPV6_DESTOPTS_HDR_OPTIONSIZE;
75		}
76		break;
77
78
79	case IPV6HDR_TYPE_AUTHENTICATION:
80		{
81			struct bcm_ipv6_authentication_hdr *pstIpv6AuthHdr =
82				(struct bcm_ipv6_authentication_hdr *)pucPayloadPtr;
83			int nHdrLen = pstIpv6AuthHdr->ucLength;
84
85			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
86					DBG_LVL_ALL,
87					"\nIPv6 Authentication Header");
88			usNextHeaderOffset += nHdrLen * 4;
89		}
90		break;
91
92	case IPV6HDR_TYPE_ENCRYPTEDSECURITYPAYLOAD:
93		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
94				DBG_LVL_ALL,
95				"\nIPv6 Encrypted Security Payload Header");
96		*bParseDone = TRUE;
97		break;
98
99	case IPV6_ICMP_HDR_TYPE:
100		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
101				DBG_LVL_ALL, "\nICMP Header");
102		*bParseDone = TRUE;
103		break;
104
105	case TCP_HEADER_TYPE:
106		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
107				DBG_LVL_ALL, "\nTCP Header");
108		*bParseDone = TRUE;
109		break;
110
111	case UDP_HEADER_TYPE:
112		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
113				DBG_LVL_ALL, "\nUDP Header");
114		*bParseDone = TRUE;
115		break;
116
117	default:
118		*bParseDone = TRUE;
119		break;
120	}
121
122	if (*bParseDone == false) {
123		if (*pusPayloadLength <= usNextHeaderOffset) {
124			*bParseDone = TRUE;
125		} else {
126			*pucNextHeader = *pucPayloadPtr;
127			pucPayloadPtr += usNextHeaderOffset;
128			(*pusPayloadLength) -= usNextHeaderOffset;
129		}
130
131	}
132
133	*ppucPayload = pucPayloadPtr;
134	return pucRetHeaderPtr;
135}
136
137
138static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload, USHORT *pusSrcPort,
139	USHORT *pusDestPort, USHORT usPayloadLength, UCHAR ucNextHeader)
140{
141	UCHAR *pIpv6HdrScanContext = pucPayload;
142	bool bDone = false;
143	UCHAR ucHeaderType = 0;
144	UCHAR *pucNextHeader = NULL;
145	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
146
147	if (!pucPayload || (usPayloadLength == 0))
148		return 0;
149
150	*pusSrcPort = *pusDestPort = 0;
151	ucHeaderType = ucNextHeader;
152	while (!bDone) {
153		pucNextHeader = GetNextIPV6ChainedHeader(&pIpv6HdrScanContext,
154							 &ucHeaderType,
155							 &bDone,
156							 &usPayloadLength);
157		if (bDone) {
158			if ((ucHeaderType == TCP_HEADER_TYPE) ||
159				(ucHeaderType == UDP_HEADER_TYPE)) {
160				*pusSrcPort = *((PUSHORT)(pucNextHeader));
161				*pusDestPort = *((PUSHORT)(pucNextHeader+2));
162				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
163						DBG_LVL_ALL,
164						"\nProtocol Ports - Src Port :0x%x Dest Port : 0x%x",
165						ntohs(*pusSrcPort),
166						ntohs(*pusDestPort));
167			}
168			break;
169
170		}
171	}
172	return ucHeaderType;
173}
174
175
176/*
177 * Arg 1 struct bcm_mini_adapter *Adapter is a pointer ot the driver control
178 * structure
179 * Arg 2 PVOID pcIpHeader is a pointer to the IP header of the packet
180 */
181USHORT	IpVersion6(struct bcm_mini_adapter *Adapter, PVOID pcIpHeader,
182		   struct bcm_classifier_rule *pstClassifierRule)
183{
184	USHORT	ushDestPort = 0;
185	USHORT	ushSrcPort = 0;
186	UCHAR   ucNextProtocolAboveIP = 0;
187	struct bcm_ipv6_hdr *pstIpv6Header = NULL;
188	bool bClassificationSucceed = false;
189
190	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
191			DBG_LVL_ALL, "IpVersion6 ==========>\n");
192
193	pstIpv6Header = pcIpHeader;
194
195	DumpIpv6Header(pstIpv6Header);
196
197	/*
198	 * Try to get the next higher layer protocol
199	 * and the Ports Nos if TCP or UDP
200	 */
201	ucNextProtocolAboveIP = GetIpv6ProtocolPorts((UCHAR *)(pcIpHeader +
202						     sizeof(struct bcm_ipv6_hdr)),
203						     &ushSrcPort,
204						     &ushDestPort,
205						     pstIpv6Header->usPayloadLength,
206						     pstIpv6Header->ucNextHeader);
207
208	do {
209		if (pstClassifierRule->ucDirection == 0) {
210			/*
211			 * cannot be processed for classification.
212			 * it is a down link connection
213			 */
214			break;
215		}
216
217		if (!pstClassifierRule->bIpv6Protocol) {
218			/*
219			 * We are looking for Ipv6 Classifiers
220			 * Lets ignore this classifier and try the next one
221			 */
222			break;
223		}
224
225		bClassificationSucceed = MatchSrcIpv6Address(pstClassifierRule,
226							     pstIpv6Header);
227		if (!bClassificationSucceed)
228			break;
229
230		bClassificationSucceed = MatchDestIpv6Address(pstClassifierRule,
231							      pstIpv6Header);
232		if (!bClassificationSucceed)
233			break;
234
235		/*
236		 * Match the protocol type.
237		 * For IPv6 the next protocol at end of
238		 * Chain of IPv6 prot headers
239		 */
240		bClassificationSucceed = MatchProtocol(pstClassifierRule,
241						       ucNextProtocolAboveIP);
242		if (!bClassificationSucceed)
243			break;
244
245		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
246				DBG_LVL_ALL, "\nIPv6 Protocol Matched");
247
248		if ((ucNextProtocolAboveIP == TCP_HEADER_TYPE) ||
249			(ucNextProtocolAboveIP == UDP_HEADER_TYPE)) {
250			/* Match Src Port */
251			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
252					DBG_LVL_ALL, "\nIPv6 Source Port:%x\n",
253					ntohs(ushSrcPort));
254			bClassificationSucceed = MatchSrcPort(pstClassifierRule,
255							      ntohs(ushSrcPort));
256			if (!bClassificationSucceed)
257				break;
258
259			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
260					DBG_LVL_ALL, "\nIPv6 Src Port Matched");
261
262			/* Match Dest Port */
263			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
264					DBG_LVL_ALL,
265					"\nIPv6 Destination Port:%x\n",
266					ntohs(ushDestPort));
267			bClassificationSucceed = MatchDestPort(pstClassifierRule,
268							       ntohs(ushDestPort));
269			if (!bClassificationSucceed)
270				break;
271			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
272					DBG_LVL_ALL,
273					"\nIPv6 Dest Port Matched");
274		}
275	} while (0);
276
277	if (bClassificationSucceed == TRUE) {
278		INT iMatchedSFQueueIndex = 0;
279
280		iMatchedSFQueueIndex = SearchSfid(Adapter,
281						  pstClassifierRule->ulSFID);
282		if ((iMatchedSFQueueIndex >= NO_OF_QUEUES) ||
283		    (Adapter->PackInfo[iMatchedSFQueueIndex].bActive == false))
284			bClassificationSucceed = false;
285	}
286
287	return bClassificationSucceed;
288}
289
290
291static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
292				struct bcm_ipv6_hdr *pstIpv6Header)
293{
294	UINT uiLoopIndex = 0;
295	UINT uiIpv6AddIndex = 0;
296	UINT uiIpv6AddrNoLongWords = 4;
297	ULONG aulSrcIP[4];
298	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
299	union u_ip_address *src_addr = &pstClassifierRule->stSrcIpAddress;
300
301	/*
302	 * This is the no. of Src Addresses ie Range of IP Addresses contained
303	 * in the classifier rule for which we need to match
304	 */
305	UINT  uiCountIPSrcAddresses =
306		(UINT)pstClassifierRule->ucIPSourceAddressLength;
307
308
309	if (uiCountIPSrcAddresses == 0)
310		return TRUE;
311
312
313	/* First Convert the Ip Address in the packet to Host Endian order */
314	for (uiIpv6AddIndex = 0;
315	     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
316	     uiIpv6AddIndex++)
317		aulSrcIP[uiIpv6AddIndex] =
318			ntohl(pstIpv6Header->ulSrcIpAddress[uiIpv6AddIndex]);
319
320	for (uiLoopIndex = 0;
321	     uiLoopIndex < uiCountIPSrcAddresses;
322	     uiLoopIndex += uiIpv6AddrNoLongWords) {
323		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
324				"\n Src Ipv6 Address In Received Packet :\n ");
325		DumpIpv6Address(aulSrcIP);
326		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
327				"\n Src Ipv6 Mask In Classifier Rule:\n");
328		DumpIpv6Address(&src_addr->ulIpv6Mask[uiLoopIndex]);
329		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
330				"\n Src Ipv6 Address In Classifier Rule :\n");
331		DumpIpv6Address(&src_addr->ulIpv6Addr[uiLoopIndex]);
332
333		for (uiIpv6AddIndex = 0;
334		     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
335		     uiIpv6AddIndex++) {
336			if ((src_addr->ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] &
337				aulSrcIP[uiIpv6AddIndex]) !=
338			    src_addr->ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
339				/*
340				 * Match failed for current Ipv6 Address
341				 * Try next Ipv6 Address
342				 */
343				break;
344			}
345
346			if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
347				/* Match Found */
348				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
349						DBG_LVL_ALL,
350						"Ipv6 Src Ip Address Matched\n");
351				return TRUE;
352			}
353		}
354	}
355	return false;
356}
357
358static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
359				 struct bcm_ipv6_hdr *pstIpv6Header)
360{
361	UINT uiLoopIndex = 0;
362	UINT uiIpv6AddIndex = 0;
363	UINT uiIpv6AddrNoLongWords = 4;
364	ULONG aulDestIP[4];
365	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
366	union u_ip_address *dest_addr = &pstClassifierRule->stDestIpAddress;
367
368	/*
369	 * This is the no. of Destination Addresses
370	 * ie Range of IP Addresses contained in the classifier rule
371	 * for which we need to match
372	 */
373	UINT uiCountIPDestinationAddresses =
374		(UINT)pstClassifierRule->ucIPDestinationAddressLength;
375
376	if (uiCountIPDestinationAddresses == 0)
377		return TRUE;
378
379
380	/* First Convert the Ip Address in the packet to Host Endian order */
381	for (uiIpv6AddIndex = 0;
382	     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
383	     uiIpv6AddIndex++)
384		aulDestIP[uiIpv6AddIndex] =
385			ntohl(pstIpv6Header->ulDestIpAddress[uiIpv6AddIndex]);
386
387	for (uiLoopIndex = 0;
388	     uiLoopIndex < uiCountIPDestinationAddresses;
389	     uiLoopIndex += uiIpv6AddrNoLongWords) {
390		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
391				"\n Destination Ipv6 Address In Received Packet :\n ");
392		DumpIpv6Address(aulDestIP);
393		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
394				"\n Destination Ipv6 Mask In Classifier Rule :\n");
395		DumpIpv6Address(&dest_addr->ulIpv6Mask[uiLoopIndex]);
396		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
397				"\n Destination Ipv6 Address In Classifier Rule :\n");
398		DumpIpv6Address(&dest_addr->ulIpv6Addr[uiLoopIndex]);
399
400		for (uiIpv6AddIndex = 0;
401		     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
402		     uiIpv6AddIndex++) {
403			if ((dest_addr->ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] &
404				aulDestIP[uiIpv6AddIndex]) !=
405			    dest_addr->ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
406				/*
407				 * Match failed for current Ipv6 Address.
408				 * Try next Ipv6 Address
409				 */
410				break;
411			}
412
413			if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
414				/* Match Found */
415				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
416						DBG_LVL_ALL,
417						"Ipv6 Destination Ip Address Matched\n");
418				return TRUE;
419			}
420		}
421	}
422	return false;
423
424}
425
426VOID DumpIpv6Address(ULONG *puIpv6Address)
427{
428	UINT uiIpv6AddrNoLongWords = 4;
429	UINT uiIpv6AddIndex = 0;
430	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
431
432	for (uiIpv6AddIndex = 0;
433	     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
434	     uiIpv6AddIndex++) {
435		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
436				":%lx", puIpv6Address[uiIpv6AddIndex]);
437	}
438
439}
440
441static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header)
442{
443	UCHAR ucVersion;
444	UCHAR ucPrio;
445	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
446
447	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
448			"----Ipv6 Header---");
449	ucVersion = pstIpv6Header->ucVersionPrio & 0xf0;
450	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
451			"Version : %x\n", ucVersion);
452	ucPrio = pstIpv6Header->ucVersionPrio & 0x0f;
453	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
454			"Priority : %x\n", ucPrio);
455	/*
456	 * BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
457	 * "Flow Label : %x\n",(pstIpv6Header->ucVersionPrio &0xf0);
458	 */
459	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
460			"Payload Length : %x\n",
461			ntohs(pstIpv6Header->usPayloadLength));
462	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
463			"Next Header : %x\n", pstIpv6Header->ucNextHeader);
464	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
465			"Hop Limit : %x\n", pstIpv6Header->ucHopLimit);
466	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
467			"Src Address :\n");
468	DumpIpv6Address(pstIpv6Header->ulSrcIpAddress);
469	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
470			"Dest Address :\n");
471	DumpIpv6Address(pstIpv6Header->ulDestIpAddress);
472	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
473			"----Ipv6 Header End---");
474
475
476}
477