Andrew Boyson / net

A stack which works with or without an Mbed os library. Provides IPv4 or IPv6 with a full 1500 byte buffer.

Dependents:   oldheating gps motorhome heating

ip6/ip6.cpp@43:bc028d5a6424, 2017-10-19 (annotated)

Committer:
andrewboyson
Date:
Thu Oct 19 20:56:58 2017 +0000
Revision:
43:bc028d5a6424
Parent:
42:222a4f45f916
Child:
44:83ce5ace337b
Added verbose option to trace

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andrewboyson 30:e34173b7585c 1 #include "mbed.h"
andrewboyson 30:e34173b7585c 2 #include "log.h"
andrewboyson 30:e34173b7585c 3 #include "net.h"
andrewboyson 37:793b39683406 4 #include "action.h"
andrewboyson 30:e34173b7585c 5 #include "icmp6.h"
andrewboyson 30:e34173b7585c 6 #include "udptcp6.h"
andrewboyson 30:e34173b7585c 7 #include "ar.h"
andrewboyson 35:93c39d260a83 8 #include "nr.h"
andrewboyson 30:e34173b7585c 9 #include "slaac.h"
andrewboyson 30:e34173b7585c 10 #include "eth.h"
andrewboyson 30:e34173b7585c 11 #include "ip.h"
andrewboyson 30:e34173b7585c 12 #include "ip6.h"
andrewboyson 30:e34173b7585c 13 #include "ra.h"
andrewboyson 30:e34173b7585c 14 #include "io.h"
andrewboyson 35:93c39d260a83 15 #include "ntp.h"
andrewboyson 36:900e24b27bfb 16 #include "mac.h"
andrewboyson 11:c051adb70c5a 17
andrewboyson 34:e3a7bff69bfc 18 #define SHOW_FILTERED true
andrewboyson 11:c051adb70c5a 19
andrewboyson 36:900e24b27bfb 20 void Ip6Clear(char* ip)
andrewboyson 36:900e24b27bfb 21 {
andrewboyson 36:900e24b27bfb 22 *ip = 0;
andrewboyson 36:900e24b27bfb 23 }
andrewboyson 36:900e24b27bfb 24 bool Ip6IsEmpty(char* ip)
andrewboyson 36:900e24b27bfb 25 {
andrewboyson 36:900e24b27bfb 26 return !*ip; //Just check for the first byte being non zero
andrewboyson 36:900e24b27bfb 27 }
andrewboyson 14:e75a59c1123d 28 static void addHexNibble(bool* pAdded, int number, int index, char** pp)
andrewboyson 14:e75a59c1123d 29 {
andrewboyson 14:e75a59c1123d 30 int nibble = number;
andrewboyson 14:e75a59c1123d 31 if (index) nibble >>= 4;
andrewboyson 14:e75a59c1123d 32 nibble &= 0xF;
andrewboyson 14:e75a59c1123d 33
andrewboyson 14:e75a59c1123d 34 if (nibble || *pAdded)
andrewboyson 14:e75a59c1123d 35 {
andrewboyson 14:e75a59c1123d 36 **pp = nibble < 10 ? nibble + '0' : nibble - 10 + 'a';
andrewboyson 14:e75a59c1123d 37 *pp += 1;
andrewboyson 14:e75a59c1123d 38 *pAdded = true;
andrewboyson 14:e75a59c1123d 39 }
andrewboyson 14:e75a59c1123d 40 }
andrewboyson 14:e75a59c1123d 41 int Ip6AddressToString(char* pIp, int size, char* pText)
andrewboyson 14:e75a59c1123d 42 {
andrewboyson 14:e75a59c1123d 43 char* pIpE = pIp + 16;
andrewboyson 14:e75a59c1123d 44 char* p = pText;
andrewboyson 14:e75a59c1123d 45 while (true)
andrewboyson 14:e75a59c1123d 46 {
andrewboyson 14:e75a59c1123d 47 bool added = false;
andrewboyson 14:e75a59c1123d 48 if (*pIp || *(pIp + 1))
andrewboyson 14:e75a59c1123d 49 {
andrewboyson 14:e75a59c1123d 50 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 0), 1, &p);
andrewboyson 14:e75a59c1123d 51 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 0), 0, &p);
andrewboyson 14:e75a59c1123d 52 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 1), 1, &p);
andrewboyson 14:e75a59c1123d 53 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 1), 0, &p);
andrewboyson 14:e75a59c1123d 54 }
andrewboyson 14:e75a59c1123d 55
andrewboyson 14:e75a59c1123d 56 pIp += 2;
andrewboyson 14:e75a59c1123d 57 if (pIp >= pIpE) break;
andrewboyson 14:e75a59c1123d 58
andrewboyson 14:e75a59c1123d 59 if (p > pText + size - 2) break; *p++ = ':';
andrewboyson 14:e75a59c1123d 60 }
andrewboyson 14:e75a59c1123d 61 *p = 0;
andrewboyson 14:e75a59c1123d 62 return p - pText;
andrewboyson 14:e75a59c1123d 63 }
andrewboyson 35:93c39d260a83 64 bool Ip6IsSame(char* ipA, char* ipB)
andrewboyson 35:93c39d260a83 65 {
andrewboyson 35:93c39d260a83 66 return memcmp(ipA, ipB, 16) == 0;
andrewboyson 35:93c39d260a83 67 }
andrewboyson 36:900e24b27bfb 68 void Ip6Copy(char* ipTo, char* ipFrom)
andrewboyson 35:93c39d260a83 69 {
andrewboyson 35:93c39d260a83 70 memcpy(ipTo, ipFrom, 16);
andrewboyson 35:93c39d260a83 71 }
andrewboyson 14:e75a59c1123d 72
andrewboyson 11:c051adb70c5a 73 char Ip6AllNodes [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01};
andrewboyson 11:c051adb70c5a 74 char Ip6AllRouters[] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02};
andrewboyson 11:c051adb70c5a 75 char Ip6Mdns [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb};
andrewboyson 11:c051adb70c5a 76 char Ip6Llmnr [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x03};
andrewboyson 11:c051adb70c5a 77
andrewboyson 37:793b39683406 78 void Ip6DestIpFromDest(int dest, char* pDstIp)
andrewboyson 11:c051adb70c5a 79 {
andrewboyson 37:793b39683406 80 switch (dest)
andrewboyson 11:c051adb70c5a 81 {
andrewboyson 36:900e24b27bfb 82 case UNICAST: break;
andrewboyson 36:900e24b27bfb 83 case UNICAST_DNS: Ip6Copy(pDstIp, RaDnsServer ); break;
andrewboyson 36:900e24b27bfb 84 case UNICAST_NTP: Ip6Copy(pDstIp, NtpServerIp6 ); break;
andrewboyson 36:900e24b27bfb 85 case MULTICAST_NODE: Ip6Copy(pDstIp, Ip6AllNodes ); break;
andrewboyson 36:900e24b27bfb 86 case MULTICAST_ROUTER: Ip6Copy(pDstIp, Ip6AllRouters); break;
andrewboyson 36:900e24b27bfb 87 case MULTICAST_MDNS: Ip6Copy(pDstIp, Ip6Mdns ); break;
andrewboyson 36:900e24b27bfb 88 case MULTICAST_LLMNR: Ip6Copy(pDstIp, Ip6Llmnr ); break;
andrewboyson 11:c051adb70c5a 89 default:
andrewboyson 37:793b39683406 90 LogTimeF("Ip6DestIpFromDest unknown destination %d\r\n", dest);
andrewboyson 11:c051adb70c5a 91 break;
andrewboyson 11:c051adb70c5a 92 }
andrewboyson 11:c051adb70c5a 93 }
andrewboyson 10:f0854784e960 94
andrewboyson 10:f0854784e960 95 #define HEADER_LENGTH 40
andrewboyson 10:f0854784e960 96 __packed struct header
andrewboyson 10:f0854784e960 97 {
andrewboyson 10:f0854784e960 98 uint32_t versionTrafficFlow;
andrewboyson 10:f0854784e960 99 uint16_t dataLength;
andrewboyson 10:f0854784e960 100 uint8_t protocol;
andrewboyson 10:f0854784e960 101 uint8_t hoplimit;
andrewboyson 10:f0854784e960 102 char src[16];
andrewboyson 10:f0854784e960 103 char dst[16];
andrewboyson 10:f0854784e960 104 };
andrewboyson 11:c051adb70c5a 105
andrewboyson 11:c051adb70c5a 106 static uint8_t version;
andrewboyson 11:c051adb70c5a 107 static int dataLength;
andrewboyson 11:c051adb70c5a 108 static uint8_t protocol;
andrewboyson 11:c051adb70c5a 109 static uint8_t hoplimit;
andrewboyson 35:93c39d260a83 110 static char srcIp[16];
andrewboyson 35:93c39d260a83 111 static char dstIp[16];
andrewboyson 11:c051adb70c5a 112 static void* pData;
andrewboyson 11:c051adb70c5a 113
andrewboyson 11:c051adb70c5a 114 static void readHeader(struct header * pHeader)
andrewboyson 11:c051adb70c5a 115 {
andrewboyson 11:c051adb70c5a 116 version = (pHeader->versionTrafficFlow >> 4) & 0xF;
andrewboyson 11:c051adb70c5a 117 dataLength = NetToHost16(pHeader->dataLength);
andrewboyson 11:c051adb70c5a 118 protocol = pHeader->protocol;
andrewboyson 11:c051adb70c5a 119 hoplimit = pHeader->hoplimit;
andrewboyson 36:900e24b27bfb 120 Ip6Copy(srcIp, pHeader->src);
andrewboyson 36:900e24b27bfb 121 Ip6Copy(dstIp, pHeader->dst);
andrewboyson 11:c051adb70c5a 122 pData = (char*)pHeader + HEADER_LENGTH;
andrewboyson 11:c051adb70c5a 123 }
andrewboyson 11:c051adb70c5a 124 static void writeHeader(struct header * pHeader)
andrewboyson 11:c051adb70c5a 125 {
andrewboyson 36:900e24b27bfb 126 pHeader->versionTrafficFlow = version << 4;
andrewboyson 36:900e24b27bfb 127 pHeader->protocol = protocol;
andrewboyson 36:900e24b27bfb 128 pHeader->hoplimit = 255;
andrewboyson 36:900e24b27bfb 129 Ip6Copy(pHeader->dst, dstIp);
andrewboyson 36:900e24b27bfb 130 Ip6Copy(pHeader->src, srcIp);
andrewboyson 36:900e24b27bfb 131 pHeader->dataLength = NetToHost16(dataLength);
andrewboyson 11:c051adb70c5a 132 }
andrewboyson 11:c051adb70c5a 133
andrewboyson 37:793b39683406 134 static void logHeader()
andrewboyson 11:c051adb70c5a 135 {
andrewboyson 11:c051adb70c5a 136 char text[100];
andrewboyson 43:bc028d5a6424 137 if (NetTraceVerbose)
andrewboyson 43:bc028d5a6424 138 {
andrewboyson 43:bc028d5a6424 139 Log("IP6 header\r\n");
andrewboyson 43:bc028d5a6424 140 LogF(" Version %d\r\n", version);
andrewboyson 43:bc028d5a6424 141 LogF(" Payload length %d\r\n", dataLength);
andrewboyson 43:bc028d5a6424 142 LogF(" Hop limit %d\r\n", hoplimit);
andrewboyson 43:bc028d5a6424 143 IpProtocolToString(protocol, sizeof(text), text);
andrewboyson 43:bc028d5a6424 144 LogF(" Protocol %s\r\n", text);
andrewboyson 43:bc028d5a6424 145 Ip6AddressToString(srcIp, sizeof(text), text);
andrewboyson 43:bc028d5a6424 146 LogF(" Source IP %s\r\n", text);
andrewboyson 43:bc028d5a6424 147 Ip6AddressToString(dstIp, sizeof(text), text);
andrewboyson 43:bc028d5a6424 148 LogF(" Destination IP %s\r\n", text);
andrewboyson 43:bc028d5a6424 149 }
andrewboyson 43:bc028d5a6424 150 else
andrewboyson 43:bc028d5a6424 151 {
andrewboyson 43:bc028d5a6424 152 Log("IP6 header ");
andrewboyson 43:bc028d5a6424 153 IpProtocolToString(protocol, sizeof(text), text);
andrewboyson 43:bc028d5a6424 154 Log(text);
andrewboyson 43:bc028d5a6424 155 Log(" ");
andrewboyson 43:bc028d5a6424 156 Ip6AddressToString(srcIp, sizeof(text), text);
andrewboyson 43:bc028d5a6424 157 Log(text);
andrewboyson 43:bc028d5a6424 158 Log(" >>> ");
andrewboyson 43:bc028d5a6424 159 Ip6AddressToString(dstIp, sizeof(text), text);
andrewboyson 43:bc028d5a6424 160 Log(text);
andrewboyson 43:bc028d5a6424 161 Log("\r\n");
andrewboyson 43:bc028d5a6424 162 }
andrewboyson 11:c051adb70c5a 163 }
andrewboyson 11:c051adb70c5a 164
andrewboyson 10:f0854784e960 165 static bool getIsSolicited(char* p)
andrewboyson 10:f0854784e960 166 {
andrewboyson 10:f0854784e960 167 if (*p++ != 0xff) return false;
andrewboyson 10:f0854784e960 168 if (*p++ != 0x02) return false;
andrewboyson 10:f0854784e960 169
andrewboyson 10:f0854784e960 170 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 171 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 172
andrewboyson 10:f0854784e960 173 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 174 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 175
andrewboyson 10:f0854784e960 176 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 177 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 178
andrewboyson 10:f0854784e960 179 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 180 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 181
andrewboyson 10:f0854784e960 182 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 183 if (*p++ != 0x01) return false;
andrewboyson 10:f0854784e960 184
andrewboyson 10:f0854784e960 185 if (*p++ != 0xff) return false;
andrewboyson 10:f0854784e960 186
andrewboyson 10:f0854784e960 187 return true;
andrewboyson 10:f0854784e960 188 }
andrewboyson 10:f0854784e960 189 static bool getIsSameGroup(char* pA, char* pB)
andrewboyson 10:f0854784e960 190 {
andrewboyson 10:f0854784e960 191 pA += 13;
andrewboyson 10:f0854784e960 192 pB += 13;
andrewboyson 10:f0854784e960 193 if (*pA++ != *pB++) return false;
andrewboyson 10:f0854784e960 194 if (*pA++ != *pB++) return false;
andrewboyson 10:f0854784e960 195 return *pA == *pB;
andrewboyson 10:f0854784e960 196 }
andrewboyson 37:793b39683406 197 static void (*pTraceBack)(void);
andrewboyson 37:793b39683406 198 static void trace()
andrewboyson 10:f0854784e960 199 {
andrewboyson 37:793b39683406 200 pTraceBack();
andrewboyson 37:793b39683406 201 logHeader();
andrewboyson 37:793b39683406 202 }
andrewboyson 37:793b39683406 203 int Ip6HandleReceivedPacket(void (*traceback)(void), char* pSrcMac, void* pPacket, int* pSize, char* pDstMac)
andrewboyson 37:793b39683406 204 {
andrewboyson 37:793b39683406 205 pTraceBack = traceback;
andrewboyson 37:793b39683406 206
andrewboyson 10:f0854784e960 207 struct header * pHeader = (header*)pPacket;
andrewboyson 11:c051adb70c5a 208 readHeader(pHeader);
andrewboyson 10:f0854784e960 209
andrewboyson 35:93c39d260a83 210 bool isMe = SlaacIsMe(dstIp);
andrewboyson 35:93c39d260a83 211 bool isMulticast = dstIp[0] == 0xFF;
andrewboyson 35:93c39d260a83 212 bool isSolicited = getIsSolicited(dstIp);
andrewboyson 35:93c39d260a83 213 bool isGroup = getIsSameGroup(dstIp, SlaacLinkLocalIp);
andrewboyson 10:f0854784e960 214
andrewboyson 34:e3a7bff69bfc 215 bool doIt = isMe || (isMulticast && !isSolicited) || (isGroup && isSolicited);
andrewboyson 10:f0854784e960 216
andrewboyson 14:e75a59c1123d 217 if (!doIt)
andrewboyson 14:e75a59c1123d 218 {
andrewboyson 37:793b39683406 219 if (SHOW_FILTERED)
andrewboyson 15:6ca6778168b1 220 {
andrewboyson 15:6ca6778168b1 221 char text[100];
andrewboyson 35:93c39d260a83 222 Ip6AddressToString(dstIp, sizeof(text), text);
andrewboyson 15:6ca6778168b1 223 LogTimeF("IP6 filtered out ip %s ", text);
andrewboyson 35:93c39d260a83 224 Ip6AddressToString(srcIp, sizeof(text), text);
andrewboyson 15:6ca6778168b1 225 LogF("from %s \r\n", text);
andrewboyson 15:6ca6778168b1 226 }
andrewboyson 14:e75a59c1123d 227 return DO_NOTHING;
andrewboyson 14:e75a59c1123d 228 }
andrewboyson 10:f0854784e960 229
andrewboyson 35:93c39d260a83 230 ArAddIp6Record(pSrcMac, srcIp);
andrewboyson 35:93c39d260a83 231 NrMakeRequestForNameFromIp6(srcIp);
andrewboyson 11:c051adb70c5a 232
andrewboyson 10:f0854784e960 233 int action = DO_NOTHING;
andrewboyson 10:f0854784e960 234 switch (protocol)
andrewboyson 10:f0854784e960 235 {
andrewboyson 42:222a4f45f916 236 case HOPOPT: action = DO_NOTHING; break;
andrewboyson 37:793b39683406 237 case ICMP6: action = Icmp6HandleReceivedPacket(trace, srcIp, dstIp, &dataLength, pData); break;
andrewboyson 37:793b39683406 238 case UDP: action = Udp6HandleReceivedPacket(trace, srcIp, dstIp, &dataLength, pData); break;
andrewboyson 37:793b39683406 239 case TCP: action = Tcp6HandleReceivedPacket(trace, srcIp, dstIp, &dataLength, pData); break;
andrewboyson 10:f0854784e960 240 default:
andrewboyson 37:793b39683406 241 LogTimeF("IP6 protocol %d unhandled\r\n", protocol);
andrewboyson 10:f0854784e960 242 return DO_NOTHING;
andrewboyson 10:f0854784e960 243 }
andrewboyson 11:c051adb70c5a 244 if (!action) return DO_NOTHING;
andrewboyson 11:c051adb70c5a 245
andrewboyson 36:900e24b27bfb 246 MacCopy(pDstMac, pSrcMac);
andrewboyson 10:f0854784e960 247
andrewboyson 11:c051adb70c5a 248 writeHeader(pHeader);
andrewboyson 11:c051adb70c5a 249
andrewboyson 10:f0854784e960 250 *pSize = HEADER_LENGTH + dataLength;
andrewboyson 10:f0854784e960 251
andrewboyson 37:793b39683406 252 if (ActionGetTracePart(action)) logHeader();
andrewboyson 37:793b39683406 253
andrewboyson 10:f0854784e960 254 return action;
andrewboyson 10:f0854784e960 255 }
andrewboyson 10:f0854784e960 256 int Ip6PollForPacketToSend(void* pPacket, int* pSize, char* pDstMac)
andrewboyson 10:f0854784e960 257 {
andrewboyson 11:c051adb70c5a 258 pData = (char*)pPacket + HEADER_LENGTH;
andrewboyson 11:c051adb70c5a 259 dataLength = 0;
andrewboyson 11:c051adb70c5a 260 version = 6;
andrewboyson 11:c051adb70c5a 261 hoplimit = 255;
andrewboyson 10:f0854784e960 262
andrewboyson 10:f0854784e960 263 int action = DO_NOTHING;
andrewboyson 10:f0854784e960 264 if (action == DO_NOTHING)
andrewboyson 10:f0854784e960 265 {
andrewboyson 35:93c39d260a83 266 action = Icmp6PollForPacketToSend(pData, &dataLength, srcIp, dstIp);
andrewboyson 10:f0854784e960 267 protocol = ICMP6;
andrewboyson 10:f0854784e960 268 }
andrewboyson 10:f0854784e960 269
andrewboyson 10:f0854784e960 270 if (action == DO_NOTHING)
andrewboyson 10:f0854784e960 271 {
andrewboyson 35:93c39d260a83 272 action = Udp6PollForPacketToSend(pData, &dataLength, srcIp, dstIp);
andrewboyson 10:f0854784e960 273 protocol = UDP;
andrewboyson 10:f0854784e960 274 }
andrewboyson 11:c051adb70c5a 275 if (!action) return DO_NOTHING;
andrewboyson 37:793b39683406 276
andrewboyson 37:793b39683406 277 int dest = ActionGetDestPart(action);
andrewboyson 37:793b39683406 278 switch (dest)
andrewboyson 11:c051adb70c5a 279 {
andrewboyson 11:c051adb70c5a 280 case UNICAST:
andrewboyson 11:c051adb70c5a 281 case UNICAST_DNS:
andrewboyson 11:c051adb70c5a 282 case UNICAST_DHCP:
andrewboyson 22:914b970356f0 283 case UNICAST_NTP:
andrewboyson 35:93c39d260a83 284 ArIpToMac6(dstIp, pDstMac); //Make the remote MAC from NP
andrewboyson 11:c051adb70c5a 285 break;
andrewboyson 22:914b970356f0 286 case MULTICAST_NODE:
andrewboyson 22:914b970356f0 287 case MULTICAST_ROUTER:
andrewboyson 22:914b970356f0 288 case MULTICAST_MDNS:
andrewboyson 22:914b970356f0 289 case MULTICAST_LLMNR:
andrewboyson 22:914b970356f0 290 case SOLICITED_NODE:
andrewboyson 22:914b970356f0 291 break;
andrewboyson 22:914b970356f0 292 default:
andrewboyson 37:793b39683406 293 LogTimeF("Ip6PollForPacketToSend - undefined destination %d\r\n", dest);
andrewboyson 22:914b970356f0 294 break;
andrewboyson 11:c051adb70c5a 295 }
andrewboyson 10:f0854784e960 296
andrewboyson 11:c051adb70c5a 297 writeHeader((header*)pPacket);
andrewboyson 11:c051adb70c5a 298
andrewboyson 10:f0854784e960 299 *pSize = HEADER_LENGTH + dataLength;
andrewboyson 10:f0854784e960 300
andrewboyson 37:793b39683406 301 if (ActionGetTracePart(action)) logHeader();
andrewboyson 37:793b39683406 302
andrewboyson 10:f0854784e960 303 return action;
andrewboyson 10:f0854784e960 304 }