1 
2 #include "netx_ptp_utility.h"
3 
4 #define NANOSECONDS_PER_SEC             1000000000L
5 
6 extern VOID _nx_ram_network_driver(struct NX_IP_DRIVER_STRUCT *driver_req);
7 
8 /*
9 Precision Time Protocol (IEEE1588)
10     0000 .... = transportSpecific: 0x0
11     .... 1011 = messageId: Announce Message (0xb)
12     0000 .... = Reserved: 0
13     .... 0010 = versionPTP: 2
14     messageLength: 64
15     subdomainNumber: 0
16     Reserved: 0
17     flags: 0x0000
18     correction: 0.000000 nanoseconds
19     Reserved: 0
20     ClockIdentity: 0x46e7c8fffe7161a1
21     SourcePortID: 1
22     sequenceId: 61
23     control: Other Message (5)
24     logMessagePeriod: 1
25     originTimestamp (seconds): 0
26     originTimestamp (nanoseconds): 0
27     originCurrentUTCOffset: 0
28     priority1: 128
29     grandmasterClockClass: 127
30     grandmasterClockAccuracy: Accuracy Unknown (0xfe)
31     grandmasterClockVariance: 28768
32     priority2: 128
33     grandmasterClockIdentity: 0x46e7c8fffe7161a1
34     localStepsRemoved: 0
35     TimeSource: INTERNAL_OSCILLATOR (0xa0)
36 */
37 static UCHAR announce_data[] = \
38 "\x0b\x02\x00\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
39 "\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x3d" \
40 "\x05\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x22\x80" \
41 "\x7f\xfe\x70\x60\x80\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x00\xa0";
42 
43 /*
44 Precision Time Protocol (IEEE1588)
45     0000 .... = transportSpecific: 0x0
46     .... 0000 = messageId: Sync Message (0x0)
47     0000 .... = Reserved: 0
48     .... 0010 = versionPTP: 2
49     messageLength: 44
50     subdomainNumber: 0
51     Reserved: 0
52     flags: 0x0200
53     correction: 0.000000 nanoseconds
54     Reserved: 0
55     ClockIdentity: 0x46e7c8fffe7161a1
56     SourcePortID: 1
57     sequenceId: 123
58     control: Sync Message (0)
59     logMessagePeriod: 0
60     originTimestamp (seconds): 1603784044
61     originTimestamp (nanoseconds): 604899854
62 */
63 static UCHAR sync_data[] = \
64 "\x00\x02\x00\x2c\x00\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
65 "\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x7b" \
66 "\x00\x00\x00\x00\x5f\x97\xcd\x6c\x24\x0e\x0a\x0e";
67 
68 /*
69 Precision Time Protocol (IEEE1588)
70     0000 .... = transportSpecific: 0x0
71     .... 1000 = messageId: Follow_Up Message (0x8)
72     0000 .... = Reserved: 0
73     .... 0010 = versionPTP: 2
74     messageLength: 44
75     subdomainNumber: 0
76     Reserved: 0
77     flags: 0x0400
78     correction: 0.000000 nanoseconds
79     Reserved: 0
80     ClockIdentity: 0x46e7c8fffe7161a1
81     SourcePortID: 1
82     sequenceId: 123
83     control: Follow_Up Message (2)
84     logMessagePeriod: 0
85     preciseOriginTimestamp (seconds): 1603784044
86     preciseOriginTimestamp (nanoseconds): 604908000
87 */
88 static UCHAR follow_up_data[] = \
89 "\x08\x02\x00\x2c\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
90 "\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x7b" \
91 "\x02\x00\x00\x00\x5f\x97\xcd\x6c\x24\x0e\x29\xe0";
92 
93 /*
94 Precision Time Protocol (IEEE1588)
95     0000 .... = transportSpecific: 0x0
96     .... 1001 = messageId: Delay_Resp Message (0x9)
97     0000 .... = Reserved: 0
98     .... 0010 = versionPTP: 2
99     messageLength: 54
100     subdomainNumber: 0
101     Reserved: 0
102     flags: 0x0000
103     correction: 0.000000 nanoseconds
104     Reserved: 0
105     ClockIdentity: 0x46e7c8fffe7161a1
106     SourcePortID: 1
107     sequenceId: 1
108     control: Delay_Resp Message (3)
109     logMessagePeriod: 0
110     receiveTimestamp (seconds): 1603784044
111     receiveTimestamp (nanoseconds): 613726000
112     requestingSourcePortIdentity: 0x001122fffe334457
113     requestingSourcePortId: 1
114 */
115 static UCHAR delay_resp_data[] = \
116 "\x09\x02\x00\x36\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
117 "\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x01" \
118 "\x03\x00\x00\x00\x5f\x97\xcd\x6c\x24\x94\xb7\x30\x00\x11\x22\xff" \
119 "\xfe\x33\x44\x57\x00\x01";
120 
121 static UCHAR use_two_steps = NX_TRUE;
122 static NX_PTP_CLIENT *ptp_client_ptr = NX_NULL;
123 static NX_PTP_TIME ptp_timestamp = {0};
124 static UCHAR use_ipv4 = NX_TRUE;
125 
send_udp_data(NX_UDP_SOCKET * socket_ptr,NX_PACKET_POOL * pool_ptr,UINT port,UCHAR * data,UINT size)126 static VOID send_udp_data(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr,
127                           UINT port, UCHAR *data, UINT size)
128 {
129 NX_PACKET *packet_ptr;
130 UINT status;
131 #ifndef NX_DISABLE_IPV6
132 NXD_ADDRESS ipv6_addr;
133 #endif
134 
135     /* Allocate a packet.  */
136     status =  nx_packet_allocate(pool_ptr, &packet_ptr, NX_UDP_PACKET, NX_NO_WAIT);
137     ASSERT_SUCCESS(status);
138 
139     /* Append announce data.  */
140     status = nx_packet_data_append(packet_ptr, data, size, pool_ptr, NX_NO_WAIT);
141     ASSERT_SUCCESS(status);
142 
143     /* Send out the packet.  */
144     if (use_ipv4)
145     {
146         status = nx_udp_socket_send(socket_ptr, packet_ptr, PTP_IPV4_MULTICAST_ADDR, port);
147     }
148 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
149     else
150     {
151         PTP_IPV6_MULTICAST_ADDR_SET(&ipv6_addr);
152         status = nxd_udp_socket_send(socket_ptr, packet_ptr, &ipv6_addr, port);
153     }
154 #endif
155 
156     ASSERT_SUCCESS(status);
157 }
158 
fill_timestamp(UCHAR * buffer,NX_PTP_TIME * ts)159 static VOID fill_timestamp(UCHAR *buffer, NX_PTP_TIME *ts)
160 {
161     buffer[0] = (ts -> second_high >> 8) & 0xFF;
162     buffer[1] = ts -> second_high & 0xFF;
163     buffer[2] = (ts -> second_low >> 24) & 0xFF;
164     buffer[3] = (ts -> second_low >> 16) & 0xFF;
165     buffer[4] = (ts -> second_low >> 8) & 0xFF;
166     buffer[5] = ts -> second_low & 0xFF;
167     buffer[6] = (ts -> nanosecond >> 24) & 0xFF;
168     buffer[7] = (ts -> nanosecond >> 16) & 0xFF;
169     buffer[8] = (ts -> nanosecond >> 8) & 0xFF;
170     buffer[9] = ts -> nanosecond & 0xFF;
171 }
172 
create_socket(NX_IP * ip_ptr,NX_UDP_SOCKET * socket_ptr,UINT port)173 VOID create_socket(NX_IP *ip_ptr, NX_UDP_SOCKET *socket_ptr, UINT port)
174 {
175 UINT status;
176 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
177 NXD_ADDRESS ipv6_addr;
178 #endif
179 
180     /* Create a UDP socket.  */
181     status = nx_udp_socket_create(ip_ptr, socket_ptr, "UDP Socket", NX_IP_NORMAL,
182                                   NX_FRAGMENT_OKAY, 0x80, 5);
183     ASSERT_SUCCESS(status);
184 
185     /* Bind the UDP socket to the IP port.  */
186     status = nx_udp_socket_bind(socket_ptr, port, NX_NO_WAIT);
187     ASSERT_SUCCESS(status);
188 
189     /* Join multicast group.  */
190     status = nx_ipv4_multicast_interface_join(ip_ptr, PTP_IPV4_MULTICAST_ADDR, 0);
191     ASSERT_SUCCESS(status);
192 
193 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
194     PTP_IPV6_MULTICAST_ADDR_SET(&ipv6_addr);
195     status = nxd_ipv6_multicast_interface_join(ip_ptr, &ipv6_addr, 0);
196     ASSERT_SUCCESS(status);
197 #endif
198 }
199 
send_announce(NX_UDP_SOCKET * socket_ptr,NX_PACKET_POOL * pool_ptr,USHORT utc_offset)200 VOID send_announce(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr, USHORT utc_offset)
201 {
202 
203     /* Adjust utc offset.  */
204     NX_CHANGE_USHORT_ENDIAN(utc_offset);
205     memcpy(&announce_data[44], &utc_offset, sizeof(utc_offset));
206 
207     /* Send out announce packet.  */
208     send_udp_data(socket_ptr, pool_ptr, PTP_GENERAL_UDP_PORT,
209                   announce_data, sizeof(announce_data));
210 }
211 
send_sync(NX_UDP_SOCKET * socket_ptr,NX_PACKET_POOL * pool_ptr,NX_PTP_TIME * ts)212 VOID send_sync(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr, NX_PTP_TIME *ts)
213 {
214     if (use_two_steps)
215     {
216         sync_data[6] |= 0x2;
217     }
218     else
219     {
220         sync_data[6] &= 0xFD;
221     }
222 
223 
224     /* Adjust timestamp. */
225     fill_timestamp(&sync_data[34], ts);
226 
227     /* Send out sync packet.  */
228     send_udp_data(socket_ptr, pool_ptr, PTP_EVENT_UDP_PORT,
229                   sync_data, sizeof(sync_data));
230 }
231 
send_follow_up(NX_UDP_SOCKET * socket_ptr,NX_PACKET_POOL * pool_ptr,NX_PTP_TIME * ts)232 VOID send_follow_up(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr, NX_PTP_TIME *ts)
233 {
234 
235     /* Adjust timestamp. */
236     fill_timestamp(&follow_up_data[34], ts);
237 
238     /* Send out follow up packet.  */
239     send_udp_data(socket_ptr, pool_ptr, PTP_GENERAL_UDP_PORT,
240                   follow_up_data, sizeof(follow_up_data));
241 }
242 
send_delay_response(NX_UDP_SOCKET * socket_ptr,NX_PACKET_POOL * pool_ptr,DELAY_REQUEST_CONTEXT * context,NX_PTP_TIME * ts)243 VOID send_delay_response(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr,
244                          DELAY_REQUEST_CONTEXT *context, NX_PTP_TIME *ts)
245 {
246 
247     /* Adjust timestamp. */
248     fill_timestamp(&delay_resp_data[34], ts);
249 
250     /* Adjust clock ID and sequence ID.  */
251     memcpy(&delay_resp_data[30], context -> sequence_id, 2);
252 
253     memcpy(&delay_resp_data[sizeof(delay_resp_data) - 11], context -> clock_id,
254            NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
255 
256     /* Send out delay resp packet.  */
257     send_udp_data(socket_ptr, pool_ptr, PTP_GENERAL_UDP_PORT,
258                   delay_resp_data, sizeof(delay_resp_data));
259 }
260 
receive_delay_request(NX_UDP_SOCKET * socket_ptr,DELAY_REQUEST_CONTEXT * context,UINT wait_option)261 UINT receive_delay_request(NX_UDP_SOCKET *socket_ptr, DELAY_REQUEST_CONTEXT *context,
262                            UINT wait_option)
263 {
264 NX_PACKET *packet_ptr;
265 NXD_ADDRESS ip_address;
266 ULONG bytes_copied;
267 UINT port;
268 UINT status;
269 
270 
271     status = nx_udp_socket_receive(socket_ptr, &packet_ptr, wait_option);
272     if (status)
273     {
274         return(status);
275     }
276 
277     status = nxd_udp_source_extract(packet_ptr, &ip_address, &port);
278     ASSERT_SUCCESS(status);
279     ASSERT_TRUE(port == PTP_EVENT_UDP_PORT);
280 
281     status = nx_packet_data_extract_offset(packet_ptr, 20, context -> clock_id,
282                                            NX_PTP_CLOCK_PORT_IDENTITY_SIZE,
283                                            &bytes_copied);
284     ASSERT_SUCCESS(status);
285     ASSERT_TRUE(bytes_copied == NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
286 
287     status = nx_packet_data_extract_offset(packet_ptr, 30, context -> sequence_id,
288                                            2, &bytes_copied);
289     ASSERT_SUCCESS(status);
290     ASSERT_TRUE(bytes_copied == 2);
291 
292     return(NX_SUCCESS);
293 }
294 
295 
set_two_steps(UCHAR two_steps)296 VOID set_two_steps(UCHAR two_steps)
297 {
298     use_two_steps = two_steps;
299 }
300 
301 
set_clock_id(UCHAR * clock_id)302 VOID set_clock_id(UCHAR *clock_id)
303 {
304 
305     /* Adjust clock ID.  */
306     memcpy(&announce_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
307     memcpy(&sync_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
308     memcpy(&follow_up_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
309     memcpy(&delay_resp_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
310 }
311 
312 
set_ip_version(UCHAR ip_version)313 VOID set_ip_version(UCHAR ip_version)
314 {
315     if (ip_version == NX_IP_VERSION_V4)
316     {
317         use_ipv4 = NX_TRUE;
318     }
319 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
320     else
321     {
322         use_ipv4 = NX_FALSE;
323     }
324 #endif
325 }
326 
327 
calibrate_timestamp(TEST_TIMESTAMP * test_ts,NX_PTP_TIME * ts)328 VOID calibrate_timestamp(TEST_TIMESTAMP *test_ts, NX_PTP_TIME *ts)
329 {
330 ULONG64 second_low;
331 ULONG64 carry;
332 long long nanosecond;
333 NX_PTP_TIME *t1;
334 
335     /* Means of following symbols,
336          t1 is timestamp of Sync if two step is disabled,
337             or else, it is timestamp of FollowUp.
338          t2 is timestamp of Sync received.
339          t3 is timestamp of DelayRequest sent. Also equals client_clock in the test
340          t4 is timestamp of DelayRequest in DelayResponse packet.
341 
342        meanPathDelay between client and server is,
343          [(t2-t1)+(t4-t3)]/2
344 
345        offsetFromMaster = client_clock - master_clock
346                         = t2 - t1 - meanPathDelay - correctionField
347 
348         master_clock = [(t4+t3)-(t2-t1)]/2
349 
350         Note, The recommendation that the timestamps themselves be
351         the best possible estimate of the time enables simple devices
352         that only need approximate time to ignore the correctionField.
353     */
354 
355     if ((test_ts -> follow_up.second_high == 0) &&
356         (test_ts -> follow_up.second_low == 0) &&
357         (test_ts -> follow_up.nanosecond == 0))
358     {
359 
360         /* No FollowUp timestamp. Use Sync timestamp.  */
361         t1 = &test_ts -> sync;
362     }
363     else
364     {
365 
366         /* Use FollowUp timestamp.  */
367         t1 = &test_ts -> follow_up;
368     }
369 
370     ts -> second_high = (test_ts -> delay_response.second_high + test_ts -> delay_request.second_high) -
371                         (test_ts -> sync_received.second_high - t1 -> second_high);
372     if (ts -> second_high & 1)
373     {
374         carry = 0x100000000;
375     }
376     else
377     {
378         carry = 0;
379     }
380     ts -> second_high /= 2;
381 
382     second_low = carry + ((ULONG64)test_ts -> delay_response.second_low + (ULONG64)test_ts -> delay_request.second_low) -
383                  ((ULONG64)test_ts -> sync_received.second_low - (ULONG64)t1 -> second_low);
384     if (second_low & 1)
385     {
386         carry = NANOSECONDS_PER_SEC;
387     }
388     else
389     {
390         carry = 0;
391     }
392     second_low /= 2;
393 
394     nanosecond = (carry + ((long long)test_ts -> delay_response.nanosecond + (long long)test_ts -> delay_request.nanosecond) -
395                   ((long long)test_ts -> sync_received.nanosecond - (long long)t1 -> nanosecond)) / 2;
396 
397     /* Adjust according to carry. */
398     while (nanosecond >= NANOSECONDS_PER_SEC)
399     {
400         nanosecond -= NANOSECONDS_PER_SEC;
401         second_low++;
402     }
403 
404     if (nanosecond < 0)
405     {
406         nanosecond += NANOSECONDS_PER_SEC;
407         if (second_low == 0)
408         {
409             ts -> second_high--;
410             second_low = 0xFFFFFFFF;
411         }
412         else
413         {
414             second_low--;
415         }
416 
417     }
418 
419     while (second_low > 0xFFFFFFFF)
420     {
421         second_low -= 0xFFFFFFFF;
422         ts -> second_high++;
423     }
424 
425     ts -> second_low = (ULONG)second_low;
426     ts -> nanosecond = (LONG)nanosecond;
427 }
428 
_netx_ptp_network_driver(struct NX_IP_DRIVER_STRUCT * driver_req)429 VOID _netx_ptp_network_driver(struct NX_IP_DRIVER_STRUCT *driver_req)
430 {
431 NX_PACKET *packet_ptr;
432 
433 #ifdef NX_ENABLE_INTERFACE_CAPABILITY
434     if (driver_req -> nx_ip_driver_command == NX_LINK_PACKET_SEND)
435     {
436         packet_ptr = driver_req -> nx_ip_driver_packet;
437         if (packet_ptr -> nx_packet_interface_capability_flag & NX_INTERFACE_CAPABILITY_PTP_TIMESTAMP)
438         {
439 
440             /* call notification callback */
441             nx_ptp_client_packet_timestamp_notify(ptp_client_ptr, packet_ptr, &ptp_timestamp);
442         }
443     }
444 #endif
445 
446     _nx_ram_network_driver(driver_req);
447 }
448 
449 #ifdef NX_ENABLE_INTERFACE_CAPABILITY
_netx_ptp_clock_callback(NX_PTP_CLIENT * client_ptr,UINT operation,NX_PTP_TIME * time_ptr,NX_PACKET * packet_ptr,VOID * callback_data)450 UINT _netx_ptp_clock_callback(NX_PTP_CLIENT *client_ptr, UINT operation,
451                               NX_PTP_TIME *time_ptr, NX_PACKET *packet_ptr,
452                               VOID *callback_data)
453 {
454 
455     NX_PARAMETER_NOT_USED(callback_data);
456 
457     switch (operation)
458     {
459 
460     /* Save pointer to PTP client.  */
461     case NX_PTP_CLIENT_CLOCK_INIT:
462         ptp_client_ptr = client_ptr;
463         break;
464 
465     /* Set clock.  */
466     case NX_PTP_CLIENT_CLOCK_SET:
467         ptp_timestamp = *time_ptr;
468         break;
469 
470     /* Extract timestamp from packet.  */
471     case NX_PTP_CLIENT_CLOCK_PACKET_TS_EXTRACT:
472         *time_ptr = ptp_timestamp;
473         break;
474 
475     /* Get clock.  */
476     case NX_PTP_CLIENT_CLOCK_GET:
477         *time_ptr = ptp_timestamp;
478         break;
479 
480     /* Adjust clock.  */
481     case NX_PTP_CLIENT_CLOCK_ADJUST:
482         ptp_timestamp.nanosecond += time_ptr -> nanosecond;
483         if (ptp_timestamp.nanosecond >= NANOSECONDS_PER_SEC)
484         {
485             ptp_timestamp.nanosecond -= NANOSECONDS_PER_SEC;
486             if (ptp_timestamp.second_low == 0xFFFFFFFF)
487             {
488                 ptp_timestamp.second_high++;
489                 ptp_timestamp.second_low = 0;
490             }
491             else
492             {
493                 ptp_timestamp.second_low++;
494             }
495         }
496         else if (ptp_timestamp.nanosecond < 0)
497         {
498             ptp_timestamp.nanosecond += NANOSECONDS_PER_SEC;
499             if (ptp_timestamp.second_low == 0)
500             {
501                 ptp_timestamp.second_high--;
502                 ptp_timestamp.second_low = 0xFFFFFFFF;
503             }
504             else
505             {
506                 ptp_timestamp.second_low--;
507             }
508         }
509         break;
510 
511     /* Prepare timestamp for current packet. */
512     case NX_PTP_CLIENT_CLOCK_PACKET_TS_PREPARE:
513         packet_ptr -> nx_packet_interface_capability_flag |= NX_INTERFACE_CAPABILITY_PTP_TIMESTAMP;
514         break;
515 
516     /* Update soft timer.  */
517     case NX_PTP_CLIENT_CLOCK_SOFT_TIMER_UPDATE:
518         break;
519 
520     default:
521         return(NX_PTP_PARAM_ERROR);
522     }
523 
524     return(NX_SUCCESS);
525 }
526 #endif