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
udp/ntp.cpp@22:914b970356f0, 2017-07-03 (annotated)
- Committer:
- andrewboyson
- Date:
- Mon Jul 03 14:29:07 2017 +0000
- Revision:
- 22:914b970356f0
- Parent:
- 20:23f2b29b48ea
- Child:
- 35:93c39d260a83
Corrected NTP client function
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
andrewboyson | 1:5f70c361db20 | 1 | #include "mbed.h" |
andrewboyson | 4:31fa7d50722c | 2 | #include "log.h" |
andrewboyson | 1:5f70c361db20 | 3 | #include "net.h" |
andrewboyson | 7:b794780e33b4 | 4 | #include "udp.h" |
andrewboyson | 22:914b970356f0 | 5 | #include "ar.h" |
andrewboyson | 22:914b970356f0 | 6 | #include "arp.h" |
andrewboyson | 22:914b970356f0 | 7 | #include "io.h" |
andrewboyson | 22:914b970356f0 | 8 | #include "eth.h" |
andrewboyson | 22:914b970356f0 | 9 | |
andrewboyson | 22:914b970356f0 | 10 | #define DEBUG false |
andrewboyson | 0:faa09bd4e6bf | 11 | |
andrewboyson | 0:faa09bd4e6bf | 12 | #define HEADER_SIZE 48 |
andrewboyson | 0:faa09bd4e6bf | 13 | |
andrewboyson | 0:faa09bd4e6bf | 14 | #define CLIENT 3 |
andrewboyson | 0:faa09bd4e6bf | 15 | #define SERVER 4 |
andrewboyson | 0:faa09bd4e6bf | 16 | |
andrewboyson | 22:914b970356f0 | 17 | uint64_t (*NtpGetClockNowFunction) (); |
andrewboyson | 22:914b970356f0 | 18 | |
andrewboyson | 22:914b970356f0 | 19 | bool NtpServerEnable = false; |
andrewboyson | 22:914b970356f0 | 20 | uint64_t (*NtpGetClockRefFunction) (); |
andrewboyson | 22:914b970356f0 | 21 | int (*NtpGetClockStratumFunction)(); |
andrewboyson | 22:914b970356f0 | 22 | char * (*NtpGetClockIdentFunction) (); |
andrewboyson | 22:914b970356f0 | 23 | |
andrewboyson | 22:914b970356f0 | 24 | bool NtpClientRequest = false; |
andrewboyson | 22:914b970356f0 | 25 | uint32_t NtpServerIp4 = 0; |
andrewboyson | 22:914b970356f0 | 26 | void (*NtpSetClockFunction) (uint64_t ori, uint64_t rec); |
andrewboyson | 17:e475ab861365 | 27 | |
andrewboyson | 0:faa09bd4e6bf | 28 | __packed struct header { |
andrewboyson | 0:faa09bd4e6bf | 29 | unsigned Mode : 3; |
andrewboyson | 0:faa09bd4e6bf | 30 | unsigned VN : 3; |
andrewboyson | 0:faa09bd4e6bf | 31 | unsigned LI : 2; |
andrewboyson | 0:faa09bd4e6bf | 32 | uint8_t Stratum; |
andrewboyson | 0:faa09bd4e6bf | 33 | int8_t Poll; |
andrewboyson | 0:faa09bd4e6bf | 34 | int8_t Precision; |
andrewboyson | 0:faa09bd4e6bf | 35 | uint32_t RootDelay; |
andrewboyson | 0:faa09bd4e6bf | 36 | uint32_t Dispersion; |
andrewboyson | 0:faa09bd4e6bf | 37 | char RefIdentifier[4]; |
andrewboyson | 0:faa09bd4e6bf | 38 | |
andrewboyson | 0:faa09bd4e6bf | 39 | uint64_t RefTimeStamp; |
andrewboyson | 0:faa09bd4e6bf | 40 | uint64_t OriTimeStamp; |
andrewboyson | 0:faa09bd4e6bf | 41 | uint64_t RecTimeStamp; |
andrewboyson | 0:faa09bd4e6bf | 42 | uint64_t TraTimeStamp; |
andrewboyson | 0:faa09bd4e6bf | 43 | }; |
andrewboyson | 22:914b970356f0 | 44 | |
andrewboyson | 22:914b970356f0 | 45 | static int handleRequest(struct header* pHeader) |
andrewboyson | 0:faa09bd4e6bf | 46 | { |
andrewboyson | 22:914b970356f0 | 47 | if (!NtpServerEnable) return DO_NOTHING; |
andrewboyson | 17:e475ab861365 | 48 | |
andrewboyson | 17:e475ab861365 | 49 | if (!NtpGetClockRefFunction || !NtpGetClockNowFunction || !NtpGetClockStratumFunction || !NtpGetClockIdentFunction) |
andrewboyson | 17:e475ab861365 | 50 | { |
andrewboyson | 17:e475ab861365 | 51 | LogTimeF("NtpHandleRequest - NTP server is enabled but has not been plumbed into a clock\r\n"); |
andrewboyson | 17:e475ab861365 | 52 | return DO_NOTHING; |
andrewboyson | 17:e475ab861365 | 53 | } |
andrewboyson | 22:914b970356f0 | 54 | |
andrewboyson | 20:23f2b29b48ea | 55 | uint64_t refNtp = NtpGetClockRefFunction(); |
andrewboyson | 20:23f2b29b48ea | 56 | uint64_t nowNtp = NtpGetClockNowFunction(); |
andrewboyson | 20:23f2b29b48ea | 57 | int stratum = NtpGetClockStratumFunction(); |
andrewboyson | 20:23f2b29b48ea | 58 | char* ident = NtpGetClockIdentFunction(); |
andrewboyson | 22:914b970356f0 | 59 | |
andrewboyson | 22:914b970356f0 | 60 | pHeader->Mode = SERVER; |
andrewboyson | 22:914b970356f0 | 61 | pHeader->LI = 0; |
andrewboyson | 22:914b970356f0 | 62 | pHeader->Stratum = stratum; |
andrewboyson | 22:914b970356f0 | 63 | pHeader->Poll = 0; |
andrewboyson | 22:914b970356f0 | 64 | pHeader->Precision = 0; |
andrewboyson | 22:914b970356f0 | 65 | pHeader->RootDelay = 0; |
andrewboyson | 22:914b970356f0 | 66 | pHeader->Dispersion = 0; |
andrewboyson | 22:914b970356f0 | 67 | pHeader->RefIdentifier[0] = ident[0]; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string. |
andrewboyson | 22:914b970356f0 | 68 | pHeader->RefIdentifier[1] = ident[1]; |
andrewboyson | 22:914b970356f0 | 69 | pHeader->RefIdentifier[2] = ident[2]; |
andrewboyson | 22:914b970356f0 | 70 | pHeader->RefIdentifier[3] = ident[3]; |
andrewboyson | 22:914b970356f0 | 71 | |
andrewboyson | 22:914b970356f0 | 72 | pHeader->RefTimeStamp = NetToHost64(refNtp); |
andrewboyson | 22:914b970356f0 | 73 | pHeader->OriTimeStamp = pHeader->TraTimeStamp; |
andrewboyson | 22:914b970356f0 | 74 | pHeader->RecTimeStamp = NetToHost64(nowNtp); |
andrewboyson | 22:914b970356f0 | 75 | pHeader->TraTimeStamp = NetToHost64(nowNtp); |
andrewboyson | 22:914b970356f0 | 76 | return UNICAST; |
andrewboyson | 22:914b970356f0 | 77 | } |
andrewboyson | 22:914b970356f0 | 78 | static int handleReply(struct header* pHeader) |
andrewboyson | 22:914b970356f0 | 79 | { |
andrewboyson | 22:914b970356f0 | 80 | if (!NtpGetClockNowFunction || !NtpSetClockFunction) |
andrewboyson | 22:914b970356f0 | 81 | { |
andrewboyson | 22:914b970356f0 | 82 | LogTimeF("Ntp reply has been received but NTP has not been plumbed into a clock\r\n"); |
andrewboyson | 22:914b970356f0 | 83 | return DO_NOTHING; |
andrewboyson | 22:914b970356f0 | 84 | } |
andrewboyson | 22:914b970356f0 | 85 | if (pHeader->Mode != SERVER) |
andrewboyson | 22:914b970356f0 | 86 | { |
andrewboyson | 22:914b970356f0 | 87 | LogTimeF("Ntp reply has been received but mode is not SERVER it is %d\r\n", pHeader->Mode); |
andrewboyson | 22:914b970356f0 | 88 | return DO_NOTHING; |
andrewboyson | 22:914b970356f0 | 89 | } |
andrewboyson | 22:914b970356f0 | 90 | if (DEBUG) |
andrewboyson | 22:914b970356f0 | 91 | { |
andrewboyson | 22:914b970356f0 | 92 | LogTimeF("NTP received reply\r\n"); |
andrewboyson | 22:914b970356f0 | 93 | LogTimeF("REF %llu\r\n", NetToHost64(pHeader->RefTimeStamp)); |
andrewboyson | 22:914b970356f0 | 94 | LogTimeF("ORI %llu\r\n", NetToHost64(pHeader->OriTimeStamp)); |
andrewboyson | 22:914b970356f0 | 95 | LogTimeF("REC %llu\r\n", NetToHost64(pHeader->RecTimeStamp)); |
andrewboyson | 22:914b970356f0 | 96 | LogTimeF("TRA %llu\r\n", NetToHost64(pHeader->TraTimeStamp)); |
andrewboyson | 22:914b970356f0 | 97 | } |
andrewboyson | 22:914b970356f0 | 98 | uint64_t ori = NetToHost64(pHeader->OriTimeStamp); |
andrewboyson | 22:914b970356f0 | 99 | uint64_t rec = NetToHost64(pHeader->RecTimeStamp); |
andrewboyson | 22:914b970356f0 | 100 | |
andrewboyson | 22:914b970356f0 | 101 | NtpSetClockFunction(ori, rec); |
andrewboyson | 22:914b970356f0 | 102 | return DO_NOTHING; |
andrewboyson | 22:914b970356f0 | 103 | } |
andrewboyson | 22:914b970356f0 | 104 | int NtpHandlePacketReceived(int* pSize, void * pPacket) |
andrewboyson | 22:914b970356f0 | 105 | { |
andrewboyson | 22:914b970356f0 | 106 | if (*pSize != HEADER_SIZE) |
andrewboyson | 22:914b970356f0 | 107 | { |
andrewboyson | 22:914b970356f0 | 108 | LogTimeF("\r\nNTP packet wrong size %d\r\n", *pSize); |
andrewboyson | 22:914b970356f0 | 109 | return DO_NOTHING; |
andrewboyson | 22:914b970356f0 | 110 | } |
andrewboyson | 22:914b970356f0 | 111 | struct header* pHeader = (struct header*)pPacket; |
andrewboyson | 17:e475ab861365 | 112 | |
andrewboyson | 0:faa09bd4e6bf | 113 | switch (pHeader->Mode) |
andrewboyson | 0:faa09bd4e6bf | 114 | { |
andrewboyson | 22:914b970356f0 | 115 | case CLIENT: return handleRequest(pHeader); |
andrewboyson | 22:914b970356f0 | 116 | case SERVER: return handleReply(pHeader); |
andrewboyson | 0:faa09bd4e6bf | 117 | default: |
andrewboyson | 22:914b970356f0 | 118 | LogTimeF("\r\nNTP packet unknown mode %d\r\n", pHeader->Mode); |
andrewboyson | 22:914b970356f0 | 119 | return DO_NOTHING; |
andrewboyson | 0:faa09bd4e6bf | 120 | } |
andrewboyson | 22:914b970356f0 | 121 | } |
andrewboyson | 22:914b970356f0 | 122 | static int sendRequest(void* pPacket, int* pSize) |
andrewboyson | 22:914b970356f0 | 123 | { |
andrewboyson | 22:914b970356f0 | 124 | struct header* pHeader = (struct header*)pPacket; |
andrewboyson | 22:914b970356f0 | 125 | |
andrewboyson | 22:914b970356f0 | 126 | pHeader->Mode = CLIENT; |
andrewboyson | 22:914b970356f0 | 127 | pHeader->VN = 3; |
andrewboyson | 22:914b970356f0 | 128 | pHeader->LI = 0; |
andrewboyson | 22:914b970356f0 | 129 | pHeader->Stratum = 0; |
andrewboyson | 22:914b970356f0 | 130 | pHeader->Poll = 0; |
andrewboyson | 22:914b970356f0 | 131 | pHeader->Precision = 0; |
andrewboyson | 22:914b970356f0 | 132 | pHeader->RootDelay = 0; |
andrewboyson | 22:914b970356f0 | 133 | pHeader->Dispersion = 0; |
andrewboyson | 22:914b970356f0 | 134 | pHeader->RefIdentifier[0] = 0; |
andrewboyson | 22:914b970356f0 | 135 | pHeader->RefIdentifier[1] = 0; |
andrewboyson | 22:914b970356f0 | 136 | pHeader->RefIdentifier[2] = 0; |
andrewboyson | 22:914b970356f0 | 137 | pHeader->RefIdentifier[3] = 0; |
andrewboyson | 22:914b970356f0 | 138 | |
andrewboyson | 22:914b970356f0 | 139 | pHeader->RefTimeStamp = 0; |
andrewboyson | 22:914b970356f0 | 140 | pHeader->OriTimeStamp = 0; |
andrewboyson | 22:914b970356f0 | 141 | pHeader->RecTimeStamp = 0; |
andrewboyson | 22:914b970356f0 | 142 | pHeader->TraTimeStamp = NetToHost64(NtpGetClockNowFunction()); |
andrewboyson | 22:914b970356f0 | 143 | *pSize = HEADER_SIZE; |
andrewboyson | 22:914b970356f0 | 144 | |
andrewboyson | 22:914b970356f0 | 145 | return UNICAST_NTP; |
andrewboyson | 0:faa09bd4e6bf | 146 | } |
andrewboyson | 22:914b970356f0 | 147 | static bool arpRequested = true; |
andrewboyson | 22:914b970356f0 | 148 | static uint32_t elapsed = 0; |
andrewboyson | 22:914b970356f0 | 149 | static uint32_t started = 0; |
andrewboyson | 22:914b970356f0 | 150 | int NtpPollForPacketToSend(int type, void* pPacket, int* pSize) |
andrewboyson | 22:914b970356f0 | 151 | { |
andrewboyson | 22:914b970356f0 | 152 | if (!NtpClientRequest) return DO_NOTHING; //Wait until a request for time is made |
andrewboyson | 22:914b970356f0 | 153 | if (type != IPV4) return DO_NOTHING; //Only have an IP4 address at this moment |
andrewboyson | 22:914b970356f0 | 154 | |
andrewboyson | 22:914b970356f0 | 155 | if (!NtpGetClockNowFunction || !NtpSetClockFunction || !NtpServerIp4) |
andrewboyson | 22:914b970356f0 | 156 | { |
andrewboyson | 22:914b970356f0 | 157 | LogTimeF("NtpPollForRequestToSend - A request to send a client message has been made but NTP has not been plumbed into a clock\r\n"); |
andrewboyson | 22:914b970356f0 | 158 | NtpClientRequest = false; |
andrewboyson | 22:914b970356f0 | 159 | return DO_NOTHING; |
andrewboyson | 22:914b970356f0 | 160 | } |
andrewboyson | 22:914b970356f0 | 161 | |
andrewboyson | 22:914b970356f0 | 162 | //Check if have MAC |
andrewboyson | 22:914b970356f0 | 163 | char mac[6]; |
andrewboyson | 22:914b970356f0 | 164 | ArRev4(NtpServerIp4, mac); |
andrewboyson | 22:914b970356f0 | 165 | if (mac[0] || mac[1] || mac[2] || mac[3] || mac[4] || mac[5]) |
andrewboyson | 22:914b970356f0 | 166 | { |
andrewboyson | 22:914b970356f0 | 167 | //Yes - send request |
andrewboyson | 22:914b970356f0 | 168 | if (DEBUG) LogTimeF("Sending NTP request\r\n"); |
andrewboyson | 22:914b970356f0 | 169 | NtpClientRequest = false; |
andrewboyson | 22:914b970356f0 | 170 | arpRequested = false; |
andrewboyson | 22:914b970356f0 | 171 | return sendRequest(pPacket, pSize); |
andrewboyson | 22:914b970356f0 | 172 | } |
andrewboyson | 22:914b970356f0 | 173 | else |
andrewboyson | 22:914b970356f0 | 174 | { |
andrewboyson | 22:914b970356f0 | 175 | //No - send RARP |
andrewboyson | 22:914b970356f0 | 176 | if (!arpRequested) |
andrewboyson | 22:914b970356f0 | 177 | { |
andrewboyson | 22:914b970356f0 | 178 | if (DEBUG) LogTimeF("Sending NTP RARP request\r\n"); |
andrewboyson | 22:914b970356f0 | 179 | ArpAddressToResolve = NtpServerIp4; |
andrewboyson | 22:914b970356f0 | 180 | ArpResolveRequestFlag = true; |
andrewboyson | 22:914b970356f0 | 181 | arpRequested = true; |
andrewboyson | 22:914b970356f0 | 182 | started = elapsed; |
andrewboyson | 22:914b970356f0 | 183 | } |
andrewboyson | 22:914b970356f0 | 184 | else |
andrewboyson | 22:914b970356f0 | 185 | { |
andrewboyson | 22:914b970356f0 | 186 | if (elapsed - started > 2) |
andrewboyson | 22:914b970356f0 | 187 | { |
andrewboyson | 22:914b970356f0 | 188 | if (DEBUG) LogTimeF("Reaped NTP RARP request\r\n"); |
andrewboyson | 22:914b970356f0 | 189 | NtpClientRequest = false; |
andrewboyson | 22:914b970356f0 | 190 | arpRequested = false; |
andrewboyson | 22:914b970356f0 | 191 | } |
andrewboyson | 22:914b970356f0 | 192 | } |
andrewboyson | 22:914b970356f0 | 193 | return DO_NOTHING; |
andrewboyson | 22:914b970356f0 | 194 | } |
andrewboyson | 22:914b970356f0 | 195 | } |
andrewboyson | 22:914b970356f0 | 196 | |
andrewboyson | 22:914b970356f0 | 197 | void NtpTick() |
andrewboyson | 22:914b970356f0 | 198 | { |
andrewboyson | 22:914b970356f0 | 199 | elapsed++; |
andrewboyson | 22:914b970356f0 | 200 | } |