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

eth/eth.cpp@10:f0854784e960, 2017-04-16 (annotated)

Committer:
andrewboyson
Date:
Sun Apr 16 14:21:55 2017 +0000
Revision:
10:f0854784e960
Child:
11:c051adb70c5a
MDNS and LLMNR now working.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andrewboyson 10:f0854784e960 1 #include "mbed.h"
andrewboyson 10:f0854784e960 2 #include "io.h"
andrewboyson 10:f0854784e960 3 #include "log.h"
andrewboyson 10:f0854784e960 4 #include "net.h"
andrewboyson 10:f0854784e960 5 #include "arp.h"
andrewboyson 10:f0854784e960 6 #include "ip4.h"
andrewboyson 10:f0854784e960 7 #include "ip6.h"
andrewboyson 10:f0854784e960 8 #include "phy.h"
andrewboyson 10:f0854784e960 9 #include "eth.h"
andrewboyson 10:f0854784e960 10
andrewboyson 10:f0854784e960 11 #define HEADER_SIZE 14
andrewboyson 10:f0854784e960 12 __packed struct header
andrewboyson 10:f0854784e960 13 {
andrewboyson 10:f0854784e960 14 char dst[6];
andrewboyson 10:f0854784e960 15 char src[6];
andrewboyson 10:f0854784e960 16 uint16_t typ;
andrewboyson 10:f0854784e960 17 };
andrewboyson 10:f0854784e960 18
andrewboyson 10:f0854784e960 19 char EthLocalMac[6];
andrewboyson 10:f0854784e960 20
andrewboyson 10:f0854784e960 21 static void typeToString(uint16_t type, int size, char* text)
andrewboyson 10:f0854784e960 22 {
andrewboyson 10:f0854784e960 23 switch (type)
andrewboyson 10:f0854784e960 24 {
andrewboyson 10:f0854784e960 25 case ARP: strncpy (text, "ARP" , size); break;
andrewboyson 10:f0854784e960 26 case IPV4: strncpy (text, "IPV4", size); break;
andrewboyson 10:f0854784e960 27 case IPV6: strncpy (text, "IPV6", size); break;
andrewboyson 10:f0854784e960 28 default: snprintf(text, size, "%04hX", type); break;
andrewboyson 10:f0854784e960 29 }
andrewboyson 10:f0854784e960 30 }
andrewboyson 10:f0854784e960 31 static bool getIsSolicited(char* p)
andrewboyson 10:f0854784e960 32 {
andrewboyson 10:f0854784e960 33 if (*p++ != 0x33) return false;
andrewboyson 10:f0854784e960 34 if (*p++ != 0x33) return false;
andrewboyson 10:f0854784e960 35 if (*p++ != 0xff) return false;
andrewboyson 10:f0854784e960 36 return true;
andrewboyson 10:f0854784e960 37 }
andrewboyson 10:f0854784e960 38 static bool getIsSame(char* pA, char* pB)
andrewboyson 10:f0854784e960 39 {
andrewboyson 10:f0854784e960 40 return memcmp(pA, pB, 6) == 0;
andrewboyson 10:f0854784e960 41 }
andrewboyson 10:f0854784e960 42 static bool getIsSameGroup(char* pA, char* pB)
andrewboyson 10:f0854784e960 43 {
andrewboyson 10:f0854784e960 44 pA += 3;
andrewboyson 10:f0854784e960 45 pB += 3;
andrewboyson 10:f0854784e960 46 if (*pA++ != *pB++) return false;
andrewboyson 10:f0854784e960 47 if (*pA++ != *pB++) return false;
andrewboyson 10:f0854784e960 48 return *pA == *pB;
andrewboyson 10:f0854784e960 49 }
andrewboyson 10:f0854784e960 50
andrewboyson 10:f0854784e960 51 static void finalisePacket(int action, int type, int dataLength, void* pPacket, int* pSize)
andrewboyson 10:f0854784e960 52 {
andrewboyson 10:f0854784e960 53 struct header * pHeader = (header*)pPacket;
andrewboyson 10:f0854784e960 54
andrewboyson 10:f0854784e960 55 switch (action)
andrewboyson 10:f0854784e960 56 {
andrewboyson 10:f0854784e960 57 case DO_NOTHING:
andrewboyson 10:f0854784e960 58 return;
andrewboyson 10:f0854784e960 59 case UNICAST:
andrewboyson 10:f0854784e960 60 case UNICAST_DNS:
andrewboyson 10:f0854784e960 61 case UNICAST_DHCP:
andrewboyson 10:f0854784e960 62 break;
andrewboyson 10:f0854784e960 63 case MULTICAST_NODE:
andrewboyson 10:f0854784e960 64 pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
andrewboyson 10:f0854784e960 65 pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
andrewboyson 10:f0854784e960 66 pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
andrewboyson 10:f0854784e960 67 pHeader->dst[3] = 0x00;
andrewboyson 10:f0854784e960 68 pHeader->dst[4] = 0x00;
andrewboyson 10:f0854784e960 69 pHeader->dst[5] = 0x01;
andrewboyson 10:f0854784e960 70 break;
andrewboyson 10:f0854784e960 71 case MULTICAST_ROUTER:
andrewboyson 10:f0854784e960 72 pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
andrewboyson 10:f0854784e960 73 pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
andrewboyson 10:f0854784e960 74 pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
andrewboyson 10:f0854784e960 75 pHeader->dst[3] = 0x00;
andrewboyson 10:f0854784e960 76 pHeader->dst[4] = 0x00;
andrewboyson 10:f0854784e960 77 pHeader->dst[5] = 0x02;
andrewboyson 10:f0854784e960 78 break;
andrewboyson 10:f0854784e960 79 case MULTICAST_MDNS:
andrewboyson 10:f0854784e960 80 pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
andrewboyson 10:f0854784e960 81 pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
andrewboyson 10:f0854784e960 82 pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
andrewboyson 10:f0854784e960 83 pHeader->dst[3] = 0x00;
andrewboyson 10:f0854784e960 84 pHeader->dst[4] = 0x00;
andrewboyson 10:f0854784e960 85 pHeader->dst[5] = 0xfb;
andrewboyson 10:f0854784e960 86 break;
andrewboyson 10:f0854784e960 87 case MULTICAST_LLMNR:
andrewboyson 10:f0854784e960 88 pHeader->dst[0] = type == IPV6 ? 0x33 : 0x01;
andrewboyson 10:f0854784e960 89 pHeader->dst[1] = type == IPV6 ? 0x33 : 0x00;
andrewboyson 10:f0854784e960 90 pHeader->dst[2] = type == IPV6 ? 0x00 : 0x5e;
andrewboyson 10:f0854784e960 91 pHeader->dst[3] = type == IPV6 ? 0x01 : 0x00;
andrewboyson 10:f0854784e960 92 pHeader->dst[4] = type == IPV6 ? 0x00 : 0x00;
andrewboyson 10:f0854784e960 93 pHeader->dst[5] = type == IPV6 ? 0x03 : 0xfc;
andrewboyson 10:f0854784e960 94 break;
andrewboyson 10:f0854784e960 95 case BROADCAST:
andrewboyson 10:f0854784e960 96 memset(pHeader->dst, 0xFF, 6); //Set to broadcast
andrewboyson 10:f0854784e960 97 break;
andrewboyson 10:f0854784e960 98 default:
andrewboyson 10:f0854784e960 99 LogTimeF("Unknown ETH action %d\r\n", action);
andrewboyson 10:f0854784e960 100 return;
andrewboyson 10:f0854784e960 101 }
andrewboyson 10:f0854784e960 102 memcpy(pHeader->src, EthLocalMac, 6); //Put our MAC into the source
andrewboyson 10:f0854784e960 103 pHeader->typ = NetToHost16(type);
andrewboyson 10:f0854784e960 104
andrewboyson 10:f0854784e960 105 *pSize = HEADER_SIZE + dataLength;
andrewboyson 10:f0854784e960 106 }
andrewboyson 10:f0854784e960 107 int EthHandlePacket(void* pPacket, int* pSize)
andrewboyson 10:f0854784e960 108 {
andrewboyson 10:f0854784e960 109 struct header * pHeader = (header*)pPacket;
andrewboyson 10:f0854784e960 110 int dataLength = *pSize - HEADER_SIZE;
andrewboyson 10:f0854784e960 111 void* pData = (char*)pPacket + HEADER_SIZE;
andrewboyson 10:f0854784e960 112
andrewboyson 10:f0854784e960 113 bool isSpanningTree = pHeader->dst[0] == 0x01 && pHeader->dst[1] == 0x80 && pHeader->dst[2] == 0xC2;
andrewboyson 10:f0854784e960 114
andrewboyson 10:f0854784e960 115 bool isMe = getIsSame(pHeader->dst, EthLocalMac);
andrewboyson 10:f0854784e960 116 bool isMulticast = pHeader->dst[0] & 0x01;
andrewboyson 10:f0854784e960 117 bool isSolicited = getIsSolicited(pHeader->dst);
andrewboyson 10:f0854784e960 118 bool isGroup = isSolicited && getIsSameGroup(pHeader->dst, EthLocalMac);
andrewboyson 10:f0854784e960 119
andrewboyson 10:f0854784e960 120 bool doIt = isMe || (isMulticast && !isSolicited) || isGroup;
andrewboyson 10:f0854784e960 121
andrewboyson 10:f0854784e960 122 if (!doIt) return DO_NOTHING;
andrewboyson 10:f0854784e960 123
andrewboyson 10:f0854784e960 124 if (isSpanningTree) return DO_NOTHING; //Drop - these multicast messages come from Sonos devices to prevent issues between wireless and wired.
andrewboyson 10:f0854784e960 125
andrewboyson 10:f0854784e960 126 uint16_t type = NetToHost16(pHeader->typ);
andrewboyson 10:f0854784e960 127 if (type < 1500) return DO_NOTHING; //drop 802.3 messages
andrewboyson 10:f0854784e960 128
andrewboyson 10:f0854784e960 129 int action = DO_NOTHING;
andrewboyson 10:f0854784e960 130 switch (type)
andrewboyson 10:f0854784e960 131 {
andrewboyson 10:f0854784e960 132 case ARP: action = ArpHandleReceivedPacket(pHeader->src, pData, &dataLength, pHeader->dst); break;
andrewboyson 10:f0854784e960 133 case IPV4: action = Ip4HandleReceivedPacket(pHeader->src, pData, &dataLength, pHeader->dst); break;
andrewboyson 10:f0854784e960 134 case IPV6: action = Ip6HandleReceivedPacket(pHeader->src, pData, &dataLength, pHeader->dst); break;
andrewboyson 10:f0854784e960 135 default:
andrewboyson 10:f0854784e960 136 char text[20];
andrewboyson 10:f0854784e960 137 LogTimeF("\r\nEthernet packet not handled\r\n");
andrewboyson 10:f0854784e960 138 NetMacToString(pHeader->dst, sizeof(text), text);
andrewboyson 10:f0854784e960 139 LogTimeF("Destination: %s\r\n", text);
andrewboyson 10:f0854784e960 140 NetMacToString(pHeader->src, sizeof(text), text);
andrewboyson 10:f0854784e960 141 LogTimeF("Source: %s\r\n", text);
andrewboyson 10:f0854784e960 142 typeToString(type, sizeof(text), text);
andrewboyson 10:f0854784e960 143 LogTimeF("EtherType: %s\r\n", text);
andrewboyson 10:f0854784e960 144 break;
andrewboyson 10:f0854784e960 145 }
andrewboyson 10:f0854784e960 146
andrewboyson 10:f0854784e960 147 finalisePacket(action, type, dataLength, pPacket, pSize);
andrewboyson 10:f0854784e960 148
andrewboyson 10:f0854784e960 149 return action;
andrewboyson 10:f0854784e960 150 }
andrewboyson 10:f0854784e960 151 int EthPollForPacketToSend(void* pPacket, int* pSize)
andrewboyson 10:f0854784e960 152 {
andrewboyson 10:f0854784e960 153 struct header * pHeader = (header*)pPacket;
andrewboyson 10:f0854784e960 154 void* pData = (char*)pPacket + HEADER_SIZE;
andrewboyson 10:f0854784e960 155
andrewboyson 10:f0854784e960 156 int dataLength = 0;
andrewboyson 10:f0854784e960 157 int type = 0;
andrewboyson 10:f0854784e960 158 int action = DO_NOTHING;
andrewboyson 10:f0854784e960 159
andrewboyson 10:f0854784e960 160 if (action == DO_NOTHING)
andrewboyson 10:f0854784e960 161 {
andrewboyson 10:f0854784e960 162 action = Ip6PollForPacketToSend(pData, &dataLength, pHeader->dst);
andrewboyson 10:f0854784e960 163 type = IPV6;
andrewboyson 10:f0854784e960 164 }
andrewboyson 10:f0854784e960 165
andrewboyson 10:f0854784e960 166 if (action == DO_NOTHING)
andrewboyson 10:f0854784e960 167 {
andrewboyson 10:f0854784e960 168 action = Ip4PollForPacketToSend(pData, &dataLength, pHeader->dst);
andrewboyson 10:f0854784e960 169 type = IPV4;
andrewboyson 10:f0854784e960 170 }
andrewboyson 10:f0854784e960 171
andrewboyson 10:f0854784e960 172 finalisePacket(action, type, dataLength, pPacket, pSize);
andrewboyson 10:f0854784e960 173
andrewboyson 10:f0854784e960 174 return action;
andrewboyson 10:f0854784e960 175 }
andrewboyson 10:f0854784e960 176