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@14:e75a59c1123d, 2017-05-05 (annotated)

Committer:
andrewboyson
Date:
Fri May 05 17:44:16 2017 +0000
Revision:
14:e75a59c1123d
Parent:
13:9cd54f7db57a
Child:
15:6ca6778168b1
Made IP addresses and ports available to debug messages

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andrewboyson 11:c051adb70c5a 1 #include "mbed.h"
andrewboyson 11:c051adb70c5a 2 #include "log.h"
andrewboyson 11:c051adb70c5a 3 #include "net.h"
andrewboyson 11:c051adb70c5a 4 #include "icmp6.h"
andrewboyson 11:c051adb70c5a 5 #include "udptcp6.h"
andrewboyson 11:c051adb70c5a 6 #include "ar.h"
andrewboyson 11:c051adb70c5a 7 #include "slaac.h"
andrewboyson 11:c051adb70c5a 8 #include "eth.h"
andrewboyson 14:e75a59c1123d 9 #include "ip.h"
andrewboyson 11:c051adb70c5a 10 #include "ip6.h"
andrewboyson 11:c051adb70c5a 11 #include "ndp.h"
andrewboyson 13:9cd54f7db57a 12 #include "io.h"
andrewboyson 11:c051adb70c5a 13
andrewboyson 11:c051adb70c5a 14 #define DEBUG false
andrewboyson 11:c051adb70c5a 15
andrewboyson 14:e75a59c1123d 16
andrewboyson 14:e75a59c1123d 17 static void addHexNibble(bool* pAdded, int number, int index, char** pp)
andrewboyson 14:e75a59c1123d 18 {
andrewboyson 14:e75a59c1123d 19 int nibble = number;
andrewboyson 14:e75a59c1123d 20 if (index) nibble >>= 4;
andrewboyson 14:e75a59c1123d 21 nibble &= 0xF;
andrewboyson 14:e75a59c1123d 22
andrewboyson 14:e75a59c1123d 23 if (nibble || *pAdded)
andrewboyson 14:e75a59c1123d 24 {
andrewboyson 14:e75a59c1123d 25 **pp = nibble < 10 ? nibble + '0' : nibble - 10 + 'a';
andrewboyson 14:e75a59c1123d 26 *pp += 1;
andrewboyson 14:e75a59c1123d 27 *pAdded = true;
andrewboyson 14:e75a59c1123d 28 }
andrewboyson 14:e75a59c1123d 29 }
andrewboyson 14:e75a59c1123d 30 int Ip6AddressToString(char* pIp, int size, char* pText)
andrewboyson 14:e75a59c1123d 31 {
andrewboyson 14:e75a59c1123d 32 char* pIpE = pIp + 16;
andrewboyson 14:e75a59c1123d 33 char* p = pText;
andrewboyson 14:e75a59c1123d 34 while (true)
andrewboyson 14:e75a59c1123d 35 {
andrewboyson 14:e75a59c1123d 36 bool added = false;
andrewboyson 14:e75a59c1123d 37 if (*pIp || *(pIp + 1))
andrewboyson 14:e75a59c1123d 38 {
andrewboyson 14:e75a59c1123d 39 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 0), 1, &p);
andrewboyson 14:e75a59c1123d 40 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 0), 0, &p);
andrewboyson 14:e75a59c1123d 41 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 1), 1, &p);
andrewboyson 14:e75a59c1123d 42 if (p > pText + size - 2) break; addHexNibble(&added, *(pIp + 1), 0, &p);
andrewboyson 14:e75a59c1123d 43 }
andrewboyson 14:e75a59c1123d 44
andrewboyson 14:e75a59c1123d 45 pIp += 2;
andrewboyson 14:e75a59c1123d 46 if (pIp >= pIpE) break;
andrewboyson 14:e75a59c1123d 47
andrewboyson 14:e75a59c1123d 48 if (p > pText + size - 2) break; *p++ = ':';
andrewboyson 14:e75a59c1123d 49 }
andrewboyson 14:e75a59c1123d 50 *p = 0;
andrewboyson 14:e75a59c1123d 51 return p - pText;
andrewboyson 14:e75a59c1123d 52 }
andrewboyson 14:e75a59c1123d 53
andrewboyson 11:c051adb70c5a 54 char Ip6AllNodes [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01};
andrewboyson 11:c051adb70c5a 55 char Ip6AllRouters[] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02};
andrewboyson 11:c051adb70c5a 56 char Ip6Mdns [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb};
andrewboyson 11:c051adb70c5a 57 char Ip6Llmnr [] = {0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x03};
andrewboyson 11:c051adb70c5a 58
andrewboyson 11:c051adb70c5a 59 void Ip6DestIpFromAction(int action, char* pDstIp)
andrewboyson 11:c051adb70c5a 60 {
andrewboyson 11:c051adb70c5a 61 switch (action)
andrewboyson 11:c051adb70c5a 62 {
andrewboyson 11:c051adb70c5a 63 case UNICAST: break;
andrewboyson 11:c051adb70c5a 64 case UNICAST_DNS: memcpy(pDstIp, NdpDnsServer, 16); break;
andrewboyson 11:c051adb70c5a 65 case MULTICAST_NODE: memcpy(pDstIp, Ip6AllNodes, 16); break;
andrewboyson 11:c051adb70c5a 66 case MULTICAST_ROUTER: memcpy(pDstIp, Ip6AllRouters, 16); break;
andrewboyson 11:c051adb70c5a 67 case MULTICAST_MDNS: memcpy(pDstIp, Ip6Mdns, 16); break;
andrewboyson 11:c051adb70c5a 68 case MULTICAST_LLMNR: memcpy(pDstIp, Ip6Llmnr, 16); break;
andrewboyson 11:c051adb70c5a 69 default:
andrewboyson 11:c051adb70c5a 70 LogTimeF("Ip6 DestIpFromAction unknown action %d\r\n", action);
andrewboyson 11:c051adb70c5a 71 break;
andrewboyson 11:c051adb70c5a 72 }
andrewboyson 11:c051adb70c5a 73 }
andrewboyson 10:f0854784e960 74
andrewboyson 10:f0854784e960 75 #define HEADER_LENGTH 40
andrewboyson 10:f0854784e960 76 __packed struct header
andrewboyson 10:f0854784e960 77 {
andrewboyson 10:f0854784e960 78 uint32_t versionTrafficFlow;
andrewboyson 10:f0854784e960 79 uint16_t dataLength;
andrewboyson 10:f0854784e960 80 uint8_t protocol;
andrewboyson 10:f0854784e960 81 uint8_t hoplimit;
andrewboyson 10:f0854784e960 82 char src[16];
andrewboyson 10:f0854784e960 83 char dst[16];
andrewboyson 10:f0854784e960 84 };
andrewboyson 11:c051adb70c5a 85
andrewboyson 11:c051adb70c5a 86 static uint8_t version;
andrewboyson 11:c051adb70c5a 87 static int dataLength;
andrewboyson 11:c051adb70c5a 88 static uint8_t protocol;
andrewboyson 11:c051adb70c5a 89 static uint8_t hoplimit;
andrewboyson 14:e75a59c1123d 90 char Ip6Src[16];
andrewboyson 14:e75a59c1123d 91 char Ip6Dst[16];
andrewboyson 11:c051adb70c5a 92 static void* pData;
andrewboyson 11:c051adb70c5a 93
andrewboyson 11:c051adb70c5a 94 static void readHeader(struct header * pHeader)
andrewboyson 11:c051adb70c5a 95 {
andrewboyson 11:c051adb70c5a 96 version = (pHeader->versionTrafficFlow >> 4) & 0xF;
andrewboyson 11:c051adb70c5a 97 dataLength = NetToHost16(pHeader->dataLength);
andrewboyson 11:c051adb70c5a 98 protocol = pHeader->protocol;
andrewboyson 11:c051adb70c5a 99 hoplimit = pHeader->hoplimit;
andrewboyson 14:e75a59c1123d 100 memcpy(Ip6Src, pHeader->src, 16);
andrewboyson 14:e75a59c1123d 101 memcpy(Ip6Dst, pHeader->dst, 16);
andrewboyson 11:c051adb70c5a 102 pData = (char*)pHeader + HEADER_LENGTH;
andrewboyson 11:c051adb70c5a 103 }
andrewboyson 11:c051adb70c5a 104 static void writeHeader(struct header * pHeader)
andrewboyson 11:c051adb70c5a 105 {
andrewboyson 11:c051adb70c5a 106 pHeader->versionTrafficFlow = version << 4;
andrewboyson 11:c051adb70c5a 107 pHeader->protocol = protocol;
andrewboyson 11:c051adb70c5a 108 pHeader->hoplimit = 255;
andrewboyson 14:e75a59c1123d 109 memcpy(pHeader->dst, Ip6Dst, 16);
andrewboyson 14:e75a59c1123d 110 memcpy(pHeader->src, Ip6Src, 16);
andrewboyson 11:c051adb70c5a 111 pHeader->dataLength = NetToHost16(dataLength);
andrewboyson 11:c051adb70c5a 112 }
andrewboyson 11:c051adb70c5a 113
andrewboyson 11:c051adb70c5a 114 static void logHeader(char* title)
andrewboyson 11:c051adb70c5a 115 {
andrewboyson 11:c051adb70c5a 116 char text[100];
andrewboyson 11:c051adb70c5a 117 LogTimeF("%s\r\n", title);
andrewboyson 11:c051adb70c5a 118 LogF(" Version %d\r\n", version);
andrewboyson 11:c051adb70c5a 119 LogF(" Payload length %d\r\n", dataLength);
andrewboyson 11:c051adb70c5a 120 LogF(" Hop limit %d\r\n", hoplimit);
andrewboyson 14:e75a59c1123d 121 IpProtocolToString(protocol, sizeof(text), text);
andrewboyson 11:c051adb70c5a 122 LogF(" Protocol %s\r\n", text);
andrewboyson 14:e75a59c1123d 123 Ip6AddressToString(Ip6Src, sizeof(text), text);
andrewboyson 11:c051adb70c5a 124 LogF(" Source IP %s\r\n", text);
andrewboyson 14:e75a59c1123d 125 Ip6AddressToString(Ip6Dst, sizeof(text), text);
andrewboyson 11:c051adb70c5a 126 LogF(" Destination IP %s\r\n", text);
andrewboyson 11:c051adb70c5a 127 }
andrewboyson 11:c051adb70c5a 128
andrewboyson 10:f0854784e960 129 static bool getIsSolicited(char* p)
andrewboyson 10:f0854784e960 130 {
andrewboyson 10:f0854784e960 131 if (*p++ != 0xff) return false;
andrewboyson 10:f0854784e960 132 if (*p++ != 0x02) return false;
andrewboyson 10:f0854784e960 133
andrewboyson 10:f0854784e960 134 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 135 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 136
andrewboyson 10:f0854784e960 137 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 138 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 139
andrewboyson 10:f0854784e960 140 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 141 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 142
andrewboyson 10:f0854784e960 143 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 144 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 145
andrewboyson 10:f0854784e960 146 if (*p++ != 0x00) return false;
andrewboyson 10:f0854784e960 147 if (*p++ != 0x01) return false;
andrewboyson 10:f0854784e960 148
andrewboyson 10:f0854784e960 149 if (*p++ != 0xff) return false;
andrewboyson 10:f0854784e960 150
andrewboyson 10:f0854784e960 151 return true;
andrewboyson 10:f0854784e960 152 }
andrewboyson 10:f0854784e960 153 static bool getIsSame(char* pA, char* pB)
andrewboyson 10:f0854784e960 154 {
andrewboyson 10:f0854784e960 155 return memcmp(pA, pB, 16) == 0;
andrewboyson 10:f0854784e960 156 }
andrewboyson 10:f0854784e960 157 static bool getIsSameGroup(char* pA, char* pB)
andrewboyson 10:f0854784e960 158 {
andrewboyson 10:f0854784e960 159 pA += 13;
andrewboyson 10:f0854784e960 160 pB += 13;
andrewboyson 10:f0854784e960 161 if (*pA++ != *pB++) return false;
andrewboyson 10:f0854784e960 162 if (*pA++ != *pB++) return false;
andrewboyson 10:f0854784e960 163 return *pA == *pB;
andrewboyson 10:f0854784e960 164 }
andrewboyson 10:f0854784e960 165 int Ip6HandleReceivedPacket(char* pSrcMac, void* pPacket, int* pSize, char* pDstMac)
andrewboyson 10:f0854784e960 166 {
andrewboyson 10:f0854784e960 167 struct header * pHeader = (header*)pPacket;
andrewboyson 11:c051adb70c5a 168 readHeader(pHeader);
andrewboyson 10:f0854784e960 169
andrewboyson 14:e75a59c1123d 170 bool isMe = getIsSame(Ip6Dst, SlaacLinkLocalIp);
andrewboyson 14:e75a59c1123d 171 bool isMulticast = Ip6Dst[0] == 0xFF;
andrewboyson 14:e75a59c1123d 172 bool isSolicited = getIsSolicited(Ip6Dst);
andrewboyson 14:e75a59c1123d 173 bool isGroup = isSolicited && getIsSameGroup(Ip6Dst, SlaacLinkLocalIp);
andrewboyson 10:f0854784e960 174
andrewboyson 10:f0854784e960 175 bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
andrewboyson 10:f0854784e960 176
andrewboyson 14:e75a59c1123d 177 if (!doIt)
andrewboyson 14:e75a59c1123d 178 {
andrewboyson 14:e75a59c1123d 179 char text[100];
andrewboyson 14:e75a59c1123d 180 Ip6AddressToString(Ip6Dst, sizeof(text), text);
andrewboyson 14:e75a59c1123d 181 LogTimeF("IP6 filtered out ip %s \r\n", text);
andrewboyson 14:e75a59c1123d 182 return DO_NOTHING;
andrewboyson 14:e75a59c1123d 183 }
andrewboyson 10:f0854784e960 184
andrewboyson 14:e75a59c1123d 185 ArAdd6(pSrcMac, Ip6Src);
andrewboyson 11:c051adb70c5a 186
andrewboyson 11:c051adb70c5a 187 if (DEBUG) logHeader("IP6 packet received");
andrewboyson 11:c051adb70c5a 188
andrewboyson 10:f0854784e960 189 int action = DO_NOTHING;
andrewboyson 10:f0854784e960 190 switch (protocol)
andrewboyson 10:f0854784e960 191 {
andrewboyson 14:e75a59c1123d 192 case HOPOPT: action = DO_NOTHING; break;
andrewboyson 14:e75a59c1123d 193 case ICMP6: action = Icmp6HandleReceivedPacket(Ip6Src, Ip6Dst, &dataLength, pData); break;
andrewboyson 14:e75a59c1123d 194 case UDP: action = Udp6HandleReceivedPacket(Ip6Src, Ip6Dst, &dataLength, pData); break;
andrewboyson 14:e75a59c1123d 195 case TCP: action = Tcp6HandleReceivedPacket(Ip6Src, Ip6Dst, &dataLength, pData); break;
andrewboyson 10:f0854784e960 196 default:
andrewboyson 11:c051adb70c5a 197 logHeader("IP6 packet unhandled");
andrewboyson 10:f0854784e960 198 return DO_NOTHING;
andrewboyson 10:f0854784e960 199 }
andrewboyson 11:c051adb70c5a 200 if (!action) return DO_NOTHING;
andrewboyson 11:c051adb70c5a 201
andrewboyson 11:c051adb70c5a 202 memcpy(pDstMac, pSrcMac, 6);
andrewboyson 11:c051adb70c5a 203
andrewboyson 11:c051adb70c5a 204 if (DEBUG) logHeader("IP6 packet replied to");
andrewboyson 10:f0854784e960 205
andrewboyson 11:c051adb70c5a 206 writeHeader(pHeader);
andrewboyson 11:c051adb70c5a 207
andrewboyson 10:f0854784e960 208 *pSize = HEADER_LENGTH + dataLength;
andrewboyson 10:f0854784e960 209
andrewboyson 10:f0854784e960 210 return action;
andrewboyson 10:f0854784e960 211 }
andrewboyson 10:f0854784e960 212 int Ip6PollForPacketToSend(void* pPacket, int* pSize, char* pDstMac)
andrewboyson 10:f0854784e960 213 {
andrewboyson 11:c051adb70c5a 214 pData = (char*)pPacket + HEADER_LENGTH;
andrewboyson 11:c051adb70c5a 215 dataLength = 0;
andrewboyson 11:c051adb70c5a 216 version = 6;
andrewboyson 11:c051adb70c5a 217 hoplimit = 255;
andrewboyson 10:f0854784e960 218
andrewboyson 10:f0854784e960 219 int action = DO_NOTHING;
andrewboyson 10:f0854784e960 220 if (action == DO_NOTHING)
andrewboyson 10:f0854784e960 221 {
andrewboyson 14:e75a59c1123d 222 action = Icmp6PollForPacketToSend(pData, &dataLength, Ip6Src, Ip6Dst);
andrewboyson 10:f0854784e960 223 protocol = ICMP6;
andrewboyson 10:f0854784e960 224 }
andrewboyson 10:f0854784e960 225
andrewboyson 10:f0854784e960 226 if (action == DO_NOTHING)
andrewboyson 10:f0854784e960 227 {
andrewboyson 14:e75a59c1123d 228 action = Udp6PollForPacketToSend(pData, &dataLength, Ip6Src, Ip6Dst);
andrewboyson 10:f0854784e960 229 protocol = UDP;
andrewboyson 10:f0854784e960 230 }
andrewboyson 11:c051adb70c5a 231 if (!action) return DO_NOTHING;
andrewboyson 11:c051adb70c5a 232 switch (action)
andrewboyson 11:c051adb70c5a 233 {
andrewboyson 11:c051adb70c5a 234 case UNICAST:
andrewboyson 11:c051adb70c5a 235 case UNICAST_DNS:
andrewboyson 11:c051adb70c5a 236 case UNICAST_DHCP:
andrewboyson 14:e75a59c1123d 237 ArRev6(Ip6Dst, pDstMac); //Make the remote MAC from NP
andrewboyson 11:c051adb70c5a 238 break;
andrewboyson 11:c051adb70c5a 239 }
andrewboyson 11:c051adb70c5a 240
andrewboyson 11:c051adb70c5a 241 if (DEBUG) logHeader("IP6 polled packet sent");
andrewboyson 10:f0854784e960 242
andrewboyson 11:c051adb70c5a 243 writeHeader((header*)pPacket);
andrewboyson 11:c051adb70c5a 244
andrewboyson 10:f0854784e960 245 *pSize = HEADER_LENGTH + dataLength;
andrewboyson 10:f0854784e960 246
andrewboyson 10:f0854784e960 247 return action;
andrewboyson 10:f0854784e960 248 }