1 /**************************************************************************/
2 /* */
3 /* Copyright (c) Microsoft Corporation. All rights reserved. */
4 /* */
5 /* This software is licensed under the Microsoft Software License */
6 /* Terms for Microsoft Azure RTOS. Full text of the license can be */
7 /* found in the LICENSE file at https://aka.ms/AzureRTOS_EULA */
8 /* and in the root directory of this software. */
9 /* */
10 /**************************************************************************/
11
12 /**************************************************************************/
13 /**************************************************************************/
14 /** */
15 /** NetX PTP Client Component */
16 /** */
17 /** Precision Time Protocol (PTP) */
18 /** */
19 /**************************************************************************/
20 /**************************************************************************/
21
22 /**************************************************************************/
23 /* */
24 /* APPLICATION INTERFACE DEFINITION RELEASE */
25 /* */
26 /* nxd_ptp_client.c PORTABLE C */
27 /* 6.1.3 */
28 /* AUTHOR */
29 /* */
30 /* Yuxin Zhou, Microsoft Corporation */
31 /* */
32 /* DESCRIPTION */
33 /* */
34 /* This file defines the NetX Precision Time Protocol (PTP) */
35 /* Client component, including all data types and external references. */
36 /* */
37 /* RELEASE HISTORY */
38 /* */
39 /* DATE NAME DESCRIPTION */
40 /* */
41 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
42 /* */
43 /**************************************************************************/
44
45
46 /* Force error checking to be disabled in this module */
47
48 #ifndef NX_DISABLE_ERROR_CHECKING
49 #define NX_DISABLE_ERROR_CHECKING
50 #endif
51
52 #define NX_PTP_SOURCE_CODE
53
54 #include "nxd_ptp_client.h"
55 #include "nx_udp.h"
56 #include "nx_ipv4.h"
57 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
58 #include "nx_ipv6.h"
59 #endif
60 #include "tx_timer.h"
61
62 /* #define NX_PTP_DEBUG */
63 /* #define NX_PTP_DEBUG_OFFSET */
64 #if defined(NX_PTP_DEBUG) || defined(NX_PTP_DEBUG_OFFSET)
65 #include <stdio.h>
66 #endif
67 #ifdef NX_PTP_DEBUG
68 #ifndef NX_PTP_DEBUG_PRINTF
69 #define NX_PTP_DEBUG_PRINTF(x) printf x
70 #endif
71 #else
72 #define NX_PTP_DEBUG_PRINTF(x)
73 #endif
74
75 /* Bring in externs for caller checking code. */
76
77 NX_CALLER_CHECKING_EXTERNS
78
79 /* number of nanoseconds per second */
80 #define NX_PTP_NANOSECONDS_PER_SEC 1000000000L
81
82 /* Define the PTP version */
83 #define NX_PTP_VERSION 2
84
85 /* Define the UDP ports */
86 #define NX_PTP_EVENT_UDP_PORT 319
87 #define NX_PTP_GENERAL_UDP_PORT 320
88
89 /* Define the TTL of PTP packets */
90 #define NX_PTP_TIME_TO_LIVE 1
91
92 /* Define the IPv4 multicast address "224.0.1.129" */
93 #define NX_PTP_IPV4_MULTICAST_ADDR IP_ADDRESS(224, 0, 1, 129)
94
95 /* Define the IPv6 multicast address "ff0e::181" */
96 #define NX_PTP_IPV6_MULTICAST_ADDR_SET(x) { \
97 (x) -> nxd_ip_version = NX_IP_VERSION_V6; \
98 (x) -> nxd_ip_address.v6[0] = 0xff0e0000UL; \
99 (x) -> nxd_ip_address.v6[1] = 0; \
100 (x) -> nxd_ip_address.v6[2] = 0; \
101 (x) -> nxd_ip_address.v6[3] = 0x181; }
102
103 /* Length of PTP message header */
104 #define NX_PTP_MSG_HDR_LEN 34
105
106 /* Length of PTP messages (without header) */
107 #define NX_PTP_MSG_ANNOUNCE_LEN 30
108 #define NX_PTP_MSG_SYNC_LEN 10
109 #define NX_PTP_MSG_FOLLOW_UP_LEN 10
110 #define NX_PTP_MSG_DELAY_RESP_LEN 20
111
112 /* Length of PTP timestamp */
113 #define NX_PTP_MSG_TIMESTAMP_LEN 10
114
115 /* Get version number */
116 #define NX_PTP_MSG_VERSION(p_) ((p_)[1] & 0xf)
117
118 /* Get domain number */
119 #define NX_PTP_MSG_DOMAIN(p_) ((p_)[4])
120
121 /* Type of messages */
122 #define NX_PTP_MSG_TYPE_SYNC 0
123 #define NX_PTP_MSG_TYPE_DELAY_REQ 1
124 #define NX_PTP_MSG_TYPE_FOLLOW_UP 8
125 #define NX_PTP_MSG_TYPE_DELAY_RESP 9
126 #define NX_PTP_MSG_TYPE_ANNOUNCE 11
127
128 /* Message flags */
129 #define NX_PTP_MSG_HDR_FLAG_LEAP61 (1 << 0)
130 #define NX_PTP_MSG_HDR_FLAG_LEAP59 (1 << 1)
131 #define NX_PTP_MSG_HDR_FLAG_UTC_REASONABLE (1 << 2)
132 #define NX_PTP_MSG_HDR_FLAG_TWO_STEP (1 << 9)
133
134 /* Common Message Header */
135 typedef struct NX_PTP_MSG_HEADER_STRUCT
136 {
137 UCHAR transportSpecific;
138 UCHAR messageType;
139 UCHAR versionPTP;
140 UCHAR domainNumber;
141 USHORT messageLength;
142 USHORT flagField;
143 ULONG cFieldHigh;
144 ULONG cFieldLow;
145 UCHAR *sourcePortIdentity;
146 USHORT sequenceId;
147 UCHAR logMessageInterval;
148 } NX_PTP_MSG_HEADER;
149
150 /* Get UTC offset from announce message */
151 #define NX_PTP_MSG_UTC_OFFSET(p_) ((SHORT)((p_[10] << 8) | p_[11]))
152
153 /* Macros for reading PTP packet fields */
154 #define NX_PTP_RD16(p_, v_) { \
155 USHORT t_; \
156 t_ = *p_++; \
157 t_ = (USHORT)(t_ << 8); \
158 v_ = (USHORT)(t_ | *p_++); }
159
160 #define NX_PTP_RD32(p_, v_) { \
161 ULONG t_; \
162 t_ = *p_++; \
163 t_ <<= 8; \
164 t_ |= *p_++; \
165 t_ <<= 8; \
166 t_ |= *p_++; \
167 t_ <<= 8; \
168 v_ = t_ |= *p_++; }
169
170
171 /**************************************************************************/
172 /* */
173 /* FUNCTION RELEASE */
174 /* */
175 /* _nx_ptp_msg_parse_timestamp PORTABLE C */
176 /* 6.1.3 */
177 /* AUTHOR */
178 /* */
179 /* Yuxin Zhou, Microsoft Corporation */
180 /* */
181 /* DESCRIPTION */
182 /* */
183 /* This function parses timestamp field of a PTP message. */
184 /* */
185 /* INPUT */
186 /* */
187 /* ptr Pointer to PTP message */
188 /* time_ptr Pointer to PTP time for output*/
189 /* */
190 /* OUTPUT */
191 /* */
192 /* None */
193 /* */
194 /* CALLS */
195 /* */
196 /* None */
197 /* */
198 /* CALLED BY */
199 /* */
200 /* _nx_ptp_client_sync_received Process Sync message */
201 /* _nx_ptp_client_delay_resp_received Process delay response */
202 /* */
203 /* RELEASE HISTORY */
204 /* */
205 /* DATE NAME DESCRIPTION */
206 /* */
207 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
208 /* */
209 /**************************************************************************/
_nx_ptp_msg_parse_timestamp(UCHAR * ptr,NX_PTP_TIME * time_ptr)210 static VOID _nx_ptp_msg_parse_timestamp(UCHAR *ptr, NX_PTP_TIME *time_ptr)
211 {
212 ULONG nanoseconds = (ULONG)time_ptr -> nanosecond;
213
214 NX_PTP_RD16(ptr, time_ptr -> second_high);
215 NX_PTP_RD32(ptr, time_ptr -> second_low);
216 NX_PTP_RD32(ptr, nanoseconds);
217 time_ptr -> nanosecond = (LONG)nanoseconds;
218 }
219
220
221 /**************************************************************************/
222 /* */
223 /* FUNCTION RELEASE */
224 /* */
225 /* _nx_ptp_msg_parse_hdr PORTABLE C */
226 /* 6.1.3 */
227 /* AUTHOR */
228 /* */
229 /* Yuxin Zhou, Microsoft Corporation */
230 /* */
231 /* DESCRIPTION */
232 /* */
233 /* This function parses the header of a PTP packet. */
234 /* */
235 /* INPUT */
236 /* */
237 /* client_ptr Pointer to PTP client */
238 /* packet_ptr Pointer to PTP packet */
239 /* hdr Parsed PTP header for output */
240 /* */
241 /* OUTPUT */
242 /* */
243 /* status Completion status */
244 /* */
245 /* CALLS */
246 /* */
247 /* nxd_udp_packet_info_extract Extract UDP packet information*/
248 /* */
249 /* CALLED BY */
250 /* */
251 /* _nx_ptp_client_process_event_packet Process PTP event packet */
252 /* _nx_ptp_client_process_general_packet Process PTP general packet */
253 /* */
254 /* RELEASE HISTORY */
255 /* */
256 /* DATE NAME DESCRIPTION */
257 /* */
258 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
259 /* */
260 /**************************************************************************/
_nx_ptp_msg_parse_hdr(NX_PTP_CLIENT * client_ptr,NX_PACKET * packet_ptr,NX_PTP_MSG_HEADER * hdr)261 static UINT _nx_ptp_msg_parse_hdr(NX_PTP_CLIENT *client_ptr, NX_PACKET *packet_ptr, NX_PTP_MSG_HEADER *hdr)
262 {
263 UCHAR b;
264 UCHAR *ptr;
265 UINT len;
266 UINT status;
267 NXD_ADDRESS src_addr;
268 UINT interface_index;
269
270 #ifndef NX_DISABLE_PACKET_CHAIN
271 if (packet_ptr -> nx_packet_next)
272 {
273
274 /* Chained packet is not supported */
275 return(NX_NOT_SUPPORTED);
276 }
277 #endif /* NX_DISABLE_PACKET_CHAIN */
278
279 /* get pointer to PTP message */
280 ptr = packet_ptr -> nx_packet_prepend_ptr;
281
282 /* get length of PTP message */
283 len = packet_ptr -> nx_packet_length;
284
285 /* check packet validity: */
286 /* - length >= PTP header length */
287 /* - PTP version */
288 /* - domain number */
289 if ((len < NX_PTP_MSG_HDR_LEN) ||
290 (NX_PTP_MSG_VERSION(ptr) != NX_PTP_VERSION) ||
291 (NX_PTP_MSG_DOMAIN(ptr) != client_ptr -> nx_ptp_client_domain))
292 {
293
294 /* discard invalid packet */
295 return(NX_INVALID_PACKET);
296 }
297
298 /* get info about sender and check packet validity: */
299 /* - network interface */
300 /* - IP version */
301 status = nxd_udp_packet_info_extract(packet_ptr, &src_addr, NX_NULL, NX_NULL, &interface_index);
302 if ((status != NX_SUCCESS) ||
303 (interface_index != client_ptr -> nx_ptp_client_interface_index)
304 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
305 || ((src_addr.nxd_ip_version == NX_IP_VERSION_V4) &&
306 !client_ptr -> nx_ptp_client_ipv4_group_joined) ||
307 ((src_addr.nxd_ip_version == NX_IP_VERSION_V6) &&
308 !client_ptr -> nx_ptp_client_ipv6_group_joined)
309 #endif
310 )
311 {
312
313 /* discard invalid packet */
314 return(NX_INVALID_PACKET);
315 }
316
317 /* Save source address in listening state */
318 if (client_ptr -> nx_ptp_client_state == NX_PTP_CLIENT_STATE_LISTENING)
319 {
320 client_ptr -> nx_ptp_client_master_addr = src_addr;
321 }
322
323 b = *ptr++;
324 hdr -> transportSpecific = b >> 4;
325 hdr -> messageType = b & 0xf;
326 b = *ptr++;
327 hdr -> versionPTP = b & 0xf;
328 NX_PTP_RD16(ptr, hdr -> messageLength);
329 hdr -> domainNumber = *ptr++;
330 ptr++; /* reserved */
331 NX_PTP_RD16(ptr, hdr -> flagField);
332 NX_PTP_RD32(ptr, hdr -> cFieldHigh);
333 NX_PTP_RD32(ptr, hdr -> cFieldLow);
334 ptr += 4; /* reserved */
335 hdr -> sourcePortIdentity = ptr;
336 ptr += NX_PTP_CLOCK_PORT_IDENTITY_SIZE;
337 NX_PTP_RD16(ptr, hdr -> sequenceId);
338 ptr++; /* controlField - ignore */
339 hdr -> logMessageInterval = *ptr;
340
341 /* adjust message length */
342 if (len > hdr -> messageLength)
343 {
344 if (hdr -> messageLength < NX_PTP_MSG_HDR_LEN)
345 {
346
347 /* invalid length */
348 return(NX_INVALID_PACKET);
349 }
350 len = hdr -> messageLength;
351 }
352
353 /* Adjust packet. */
354 packet_ptr -> nx_packet_prepend_ptr += NX_PTP_MSG_HDR_LEN;
355 packet_ptr -> nx_packet_length = len - NX_PTP_MSG_HDR_LEN;
356
357 return(NX_SUCCESS);
358 }
359
360
361 /**************************************************************************/
362 /* */
363 /* FUNCTION RELEASE */
364 /* */
365 /* _nx_ptp_msg_parse_announce PORTABLE C */
366 /* 6.1.3 */
367 /* AUTHOR */
368 /* */
369 /* Yuxin Zhou, Microsoft Corporation */
370 /* */
371 /* DESCRIPTION */
372 /* */
373 /* This function parses PTP Announce message. */
374 /* */
375 /* INPUT */
376 /* */
377 /* ptr Pointer to PTP message */
378 /* master_ptr Parsed PTP master for output */
379 /* */
380 /* OUTPUT */
381 /* */
382 /* None */
383 /* */
384 /* CALLS */
385 /* */
386 /* None */
387 /* */
388 /* CALLED BY */
389 /* */
390 /* _nx_ptp_client_announce_received Process Announce message */
391 /* */
392 /* RELEASE HISTORY */
393 /* */
394 /* DATE NAME DESCRIPTION */
395 /* */
396 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
397 /* */
398 /**************************************************************************/
_nx_ptp_msg_parse_announce(UCHAR * ptr,NX_PTP_CLIENT_MASTER * master)399 static VOID _nx_ptp_msg_parse_announce(UCHAR *ptr, NX_PTP_CLIENT_MASTER *master)
400 {
401 ptr += NX_PTP_MSG_TIMESTAMP_LEN + 2 + 1; /* originTimestamp, utcOffset, reserved */
402 master -> nx_ptp_client_master_priority1 = *ptr++;
403 master -> nx_ptp_client_master_clock_class = *ptr++;
404 master -> nx_ptp_client_master_clock_accuracy = *ptr++;
405 NX_PTP_RD16(ptr, master -> nx_ptp_client_master_offset_scaled_log_variance);
406 master -> nx_ptp_client_master_priority2 = *ptr++;
407 master -> nx_ptp_client_master_grandmaster_identity = ptr;
408 ptr += NX_PTP_CLOCK_IDENTITY_SIZE;
409 NX_PTP_RD16(ptr, master -> nx_ptp_client_master_steps_removed);
410 master -> nx_ptp_client_master_time_source = *ptr;
411 }
412
413
414 /**************************************************************************/
415 /* */
416 /* FUNCTION RELEASE */
417 /* */
418 /* _nx_ptp_client_soft_clock_adjust PORTABLE C */
419 /* 6.1.3 */
420 /* AUTHOR */
421 /* */
422 /* Yuxin Zhou, Microsoft Corporation */
423 /* */
424 /* DESCRIPTION */
425 /* */
426 /* This function adjusts the value of the soft PTP clock. */
427 /* */
428 /* INPUT */
429 /* */
430 /* ptp_instance Pointer to PTP client */
431 /* offset_ns Signed number of nanoseconds */
432 /* to add to the PTP clock */
433 /* */
434 /* OUTPUT */
435 /* */
436 /* None */
437 /* */
438 /* CALLS */
439 /* */
440 /* _nx_ptp_client_utility_inc64 Increment a 64-bit number */
441 /* _nx_ptp_client_utility_dec64 Decrement a 64-bit number */
442 /* */
443 /* CALLED BY */
444 /* */
445 /* _nx_ptp_client_soft_clock_callback Soft PTP clock */
446 /* */
447 /* RELEASE HISTORY */
448 /* */
449 /* DATE NAME DESCRIPTION */
450 /* */
451 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
452 /* */
453 /**************************************************************************/
_nx_ptp_client_soft_clock_adjust(VOID * ptp_instance,LONG offset_ns)454 static VOID _nx_ptp_client_soft_clock_adjust(VOID *ptp_instance, LONG offset_ns)
455 {
456 NX_PTP_CLIENT *client_ptr = (NX_PTP_CLIENT *)ptp_instance;
457 TX_INTERRUPT_SAVE_AREA
458
459 /* enforce min/max values of offset */
460 if (offset_ns > NX_PTP_NANOSECONDS_PER_SEC)
461 {
462 offset_ns = NX_PTP_NANOSECONDS_PER_SEC;
463 }
464 else if (offset_ns < -NX_PTP_NANOSECONDS_PER_SEC)
465 {
466 offset_ns = -NX_PTP_NANOSECONDS_PER_SEC;
467 }
468
469 /* add the number of nanosecond to the current time */
470 TX_DISABLE
471 client_ptr -> nx_ptp_client_soft_clock.nanosecond += offset_ns;
472 if (client_ptr -> nx_ptp_client_soft_clock.nanosecond >= NX_PTP_NANOSECONDS_PER_SEC)
473 {
474 client_ptr -> nx_ptp_client_soft_clock.nanosecond -= NX_PTP_NANOSECONDS_PER_SEC;
475 _nx_ptp_client_utility_inc64(&client_ptr -> nx_ptp_client_soft_clock.second_high,
476 &client_ptr -> nx_ptp_client_soft_clock.second_low);
477 }
478 else if (client_ptr -> nx_ptp_client_soft_clock.nanosecond < 0)
479 {
480 client_ptr -> nx_ptp_client_soft_clock.nanosecond += NX_PTP_NANOSECONDS_PER_SEC;
481 _nx_ptp_client_utility_dec64(&client_ptr -> nx_ptp_client_soft_clock.second_high,
482 &client_ptr -> nx_ptp_client_soft_clock.second_low);
483 }
484 TX_RESTORE
485 }
486
487
488 /**************************************************************************/
489 /* */
490 /* FUNCTION RELEASE */
491 /* */
492 /* _nx_ptp_client_timer_handler PORTABLE C */
493 /* 6.1.3 */
494 /* AUTHOR */
495 /* */
496 /* Yuxin Zhou, Microsoft Corporation */
497 /* */
498 /* DESCRIPTION */
499 /* */
500 /* This function implements the PTP client timer handler. */
501 /* */
502 /* INPUT */
503 /* */
504 /* ptp_instance Pointer to PTP client */
505 /* */
506 /* OUTPUT */
507 /* */
508 /* None */
509 /* */
510 /* CALLS */
511 /* */
512 /* tx_event_flags_set Set PTP timer event */
513 /* */
514 /* CALLED BY */
515 /* */
516 /* ThreadX Timer */
517 /* */
518 /* RELEASE HISTORY */
519 /* */
520 /* DATE NAME DESCRIPTION */
521 /* */
522 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
523 /* */
524 /**************************************************************************/
_nx_ptp_client_timer_handler(ULONG ptp_instance)525 static VOID _nx_ptp_client_timer_handler(ULONG ptp_instance)
526 {
527 NX_PTP_CLIENT *client_ptr = (NX_PTP_CLIENT *)ptp_instance;
528
529 /* Update soft timer. */
530 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_SOFT_TIMER_UPDATE,
531 &client_ptr -> nx_ptp_client_soft_clock, NX_NULL,
532 client_ptr -> nx_ptp_client_clock_callback_data);
533
534 /* set timer event */
535 tx_event_flags_set(&(client_ptr -> nx_ptp_client_events), NX_PTP_CLIENT_TIMER_EVENT, TX_OR);
536 }
537
538
539 /**************************************************************************/
540 /* */
541 /* FUNCTION RELEASE */
542 /* */
543 /* _nx_ptp_client_socket_receive_notify PORTABLE C */
544 /* 6.1.3 */
545 /* AUTHOR */
546 /* */
547 /* Yuxin Zhou, Microsoft Corporation */
548 /* */
549 /* DESCRIPTION */
550 /* */
551 /* This function is invoked when UDP packet is received. */
552 /* */
553 /* INPUT */
554 /* */
555 /* socket_ptr Pointer to general socket */
556 /* */
557 /* OUTPUT */
558 /* */
559 /* None */
560 /* */
561 /* CALLS */
562 /* */
563 /* tx_event_flags_set Set UDP receive event */
564 /* */
565 /* CALLED BY */
566 /* */
567 /* NetX UDP */
568 /* */
569 /* RELEASE HISTORY */
570 /* */
571 /* DATE NAME DESCRIPTION */
572 /* */
573 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
574 /* */
575 /**************************************************************************/
_nx_ptp_client_socket_receive_notify(NX_UDP_SOCKET * socket_ptr)576 static VOID _nx_ptp_client_socket_receive_notify(NX_UDP_SOCKET *socket_ptr)
577 {
578 NX_PTP_CLIENT *client_ptr = (NX_PTP_CLIENT *)(socket_ptr -> nx_udp_socket_reserved_ptr);
579
580 /* set timer event */
581 tx_event_flags_set(&(client_ptr -> nx_ptp_client_events), NX_PTP_CLIENT_RX_EVENT, TX_OR);
582 }
583
584
585 /**************************************************************************/
586 /* */
587 /* FUNCTION RELEASE */
588 /* */
589 /* _nx_ptp_client_clock_adjust PORTABLE C */
590 /* 6.1.3 */
591 /* AUTHOR */
592 /* */
593 /* Yuxin Zhou, Microsoft Corporation */
594 /* */
595 /* DESCRIPTION */
596 /* */
597 /* Adjust the PTP clock with the given offset */
598 /* If the offset is greater than one second, the clock is updated, */
599 /* otherwise it is adjusted with the number of nanoseconds. */
600 /* */
601 /* INPUT */
602 /* */
603 /* client_ptr Pointer to PTP client */
604 /* offset_ptr Pointer to time offset */
605 /* */
606 /* OUTPUT */
607 /* */
608 /* None */
609 /* */
610 /* CALLS */
611 /* */
612 /* _nx_ptp_client_utility_inc64 Increment a 64-bit number */
613 /* _nx_ptp_client_utility_dec64 Decrement a 64-bit number */
614 /* _nx_ptp_client_utility_add64 Add two 64-bit number */
615 /* */
616 /* CALLED BY */
617 /* */
618 /* _nx_ptp_client_delay_resp_received Process delay response */
619 /* */
620 /* RELEASE HISTORY */
621 /* */
622 /* DATE NAME DESCRIPTION */
623 /* */
624 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
625 /* */
626 /**************************************************************************/
_nx_ptp_client_clock_adjust(NX_PTP_CLIENT * client_ptr,NX_PTP_TIME * offset_ptr)627 static VOID _nx_ptp_client_clock_adjust(NX_PTP_CLIENT *client_ptr, NX_PTP_TIME *offset_ptr)
628 {
629 NX_PTP_TIME current;
630
631 if ((offset_ptr -> second_high == 0) && (offset_ptr -> second_low == 0))
632 {
633
634 /* offset less than 1s, adjust clock */
635 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_ADJUST, offset_ptr, NX_NULL,
636 client_ptr -> nx_ptp_client_clock_callback_data);
637 NX_PTP_DEBUG_PRINTF(("PTP: adjust clock %d ns\r\n", (INT)offset_ptr -> nanosecond));
638 }
639 else
640 {
641
642 /* offset greater than 1s, set new clock value */
643 /* get current clock value */
644 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_GET, ¤t, NX_NULL,
645 client_ptr -> nx_ptp_client_clock_callback_data);
646
647 /* add nanoseconds offset */
648 current.nanosecond += offset_ptr -> nanosecond;
649
650 /* ensure nanosecond field is in range 0-999999999 */
651 if (current.nanosecond < 0)
652 {
653 current.nanosecond += NX_PTP_NANOSECONDS_PER_SEC;
654 _nx_ptp_client_utility_dec64(¤t.second_high, ¤t.second_low);
655 }
656 else if (current.nanosecond >= NX_PTP_NANOSECONDS_PER_SEC)
657 {
658 current.nanosecond -= NX_PTP_NANOSECONDS_PER_SEC;
659 _nx_ptp_client_utility_inc64(¤t.second_high, ¤t.second_low);
660 }
661
662 /* add seconds offset */
663 _nx_ptp_client_utility_add64(¤t.second_high, ¤t.second_low,
664 offset_ptr -> second_high, offset_ptr -> second_low);
665
666 /* set new clock value */
667 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_SET, ¤t, NX_NULL,
668 client_ptr -> nx_ptp_client_clock_callback_data);
669
670 NX_PTP_DEBUG_PRINTF(("PTP: set clock %u.%d\r\n",
671 (UINT)current.second_low,
672 (INT)current.nanosecond));
673 }
674 }
675
676
677 /**************************************************************************/
678 /* */
679 /* FUNCTION RELEASE */
680 /* */
681 /* _nx_ptp_client_send_delay_req PORTABLE C */
682 /* 6.1.3 */
683 /* AUTHOR */
684 /* */
685 /* Yuxin Zhou, Microsoft Corporation */
686 /* */
687 /* DESCRIPTION */
688 /* */
689 /* This function sends a PTP Delay Request message. */
690 /* */
691 /* INPUT */
692 /* */
693 /* client_ptr Pointer to PTP client */
694 /* */
695 /* OUTPUT */
696 /* */
697 /* None */
698 /* */
699 /* CALLS */
700 /* */
701 /* nx_packet_allocate Allocate a packet */
702 /* nxd_udp_socket_source_send Send a UDP packet */
703 /* nx_packet_release Release a packet */
704 /* */
705 /* CALLED BY */
706 /* */
707 /* _nx_ptp_client_sync_received Process Sync message */
708 /* */
709 /* RELEASE HISTORY */
710 /* */
711 /* DATE NAME DESCRIPTION */
712 /* */
713 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
714 /* */
715 /**************************************************************************/
_nx_ptp_client_send_delay_req(NX_PTP_CLIENT * client_ptr)716 static VOID _nx_ptp_client_send_delay_req(NX_PTP_CLIENT *client_ptr)
717 {
718 NX_PACKET *packet_ptr;
719 UINT status = NX_NOT_SUCCESSFUL;
720 UCHAR *ptr;
721 NXD_ADDRESS addr;
722 UINT addr_index = 0;
723 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
724 NXD_IPV6_ADDRESS *ipv6_addr;
725 NX_IP *ip_ptr;
726 NX_INTERFACE *if_ptr;
727 #endif
728
729 NX_PTP_DEBUG_PRINTF(("PTP: send DELAY_REQ\r\n"));
730
731 /* allocate a packet from the pool */
732 status = nx_packet_allocate(client_ptr -> nx_ptp_client_packet_pool_ptr, &packet_ptr, NX_UDP_PACKET, NX_NO_WAIT);
733 if (status != NX_SUCCESS)
734 {
735 /* Failed to allocate the packet */
736 return;
737 }
738
739 /* start of message */
740 ptr = packet_ptr -> nx_packet_prepend_ptr;
741
742 #define PTP_MSG_DELAY_REQ_TOTAL_LEN (NX_PTP_MSG_HDR_LEN + NX_PTP_MSG_TIMESTAMP_LEN)
743 #define PTP_MSG_DELAY_REQ_ZERO1_LEN (1 + 2 + 8 + 4)
744
745 /* write header */
746 *ptr++ = NX_PTP_MSG_TYPE_DELAY_REQ;
747 *ptr++ = NX_PTP_VERSION;
748 *ptr++ = PTP_MSG_DELAY_REQ_TOTAL_LEN >> 8;
749 *ptr++ = (UCHAR)PTP_MSG_DELAY_REQ_TOTAL_LEN;
750 *ptr++ = (UCHAR)(client_ptr -> nx_ptp_client_transport_specific << 4) | client_ptr -> nx_ptp_client_domain;
751 memset(ptr, 0, PTP_MSG_DELAY_REQ_ZERO1_LEN); /* reserved/flags/correction/reserved */
752 ptr += PTP_MSG_DELAY_REQ_ZERO1_LEN;
753 memcpy(ptr, client_ptr -> nx_ptp_client_port_identity,
754 NX_PTP_CLOCK_PORT_IDENTITY_SIZE); /* use case of memcpy is verified. */
755 ptr += NX_PTP_CLOCK_PORT_IDENTITY_SIZE;
756 client_ptr -> nx_ptp_client_delay_req_id++;
757 *ptr++ = (UCHAR)(client_ptr -> nx_ptp_client_delay_req_id >> 8);
758 *ptr++ = (UCHAR)client_ptr -> nx_ptp_client_delay_req_id;
759 *ptr++ = 0; /* control */
760 *ptr++ = 0; /* XXX */
761
762 /* write timestamp (0) */
763 memset(ptr, 0, NX_PTP_MSG_TIMESTAMP_LEN);
764 ptr += NX_PTP_MSG_TIMESTAMP_LEN;
765
766 /* set final length of message */
767 packet_ptr -> nx_packet_length = (ULONG)(ptr - packet_ptr -> nx_packet_prepend_ptr);
768 packet_ptr -> nx_packet_append_ptr = ptr;
769
770 /* set source and destination addresses */
771 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
772 if (client_ptr -> nx_ptp_client_master_addr.nxd_ip_version == NX_IP_VERSION_V6)
773 {
774 NX_PTP_IPV6_MULTICAST_ADDR_SET(&addr);
775
776 /* Use first IPv6 address as source address. */
777 ip_ptr = client_ptr -> nx_ptp_client_ip_ptr;
778 if_ptr = &ip_ptr -> nx_ip_interface[client_ptr -> nx_ptp_client_interface_index];
779 ipv6_addr = if_ptr -> nxd_interface_ipv6_address_list_head;
780 if (ipv6_addr == NX_NULL)
781 {
782
783 /* No available IPv6 address. */
784 /* Release packet. */
785 nx_packet_release(packet_ptr);
786
787 /* Reset state. */
788 client_ptr -> nx_ptp_client_delay_state = NX_PTP_CLIENT_DELAY_IDLE;
789 client_ptr -> nx_ptp_client_delay_req_packet_ptr = NX_NULL;
790
791 return;
792 }
793 addr_index = ipv6_addr -> nxd_ipv6_address_index;
794 }
795 else
796 #endif
797 {
798 #ifndef NX_DISABLE_IPV4
799 addr.nxd_ip_version = NX_IP_VERSION_V4;
800 addr.nxd_ip_address.v4 = NX_PTP_IPV4_MULTICAST_ADDR;
801 addr_index = client_ptr -> nx_ptp_client_interface_index;
802 #endif
803 }
804
805 /* Prepare timestamp for current packet */
806 client_ptr -> nx_ptp_client_delay_state = NX_PTP_CLIENT_DELAY_WAIT_REQ_TS;
807 client_ptr -> nx_ptp_client_delay_req_packet_ptr = packet_ptr;
808 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_PACKET_TS_PREPARE,
809 &client_ptr -> nx_ptp_client_delay_ts, packet_ptr,
810 client_ptr -> nx_ptp_client_clock_callback_data);
811
812 /* Send delay request */
813 status = nxd_udp_socket_source_send((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_event_socket,
814 packet_ptr, &addr, NX_PTP_EVENT_UDP_PORT, addr_index);
815 if (status)
816 {
817
818 /* release packet in case of error */
819 nx_packet_release(packet_ptr);
820
821 /* reset state */
822 client_ptr -> nx_ptp_client_delay_state = NX_PTP_CLIENT_DELAY_IDLE;
823 client_ptr -> nx_ptp_client_delay_req_packet_ptr = NX_NULL;
824
825 return;
826 }
827
828 /* rearm delay req timer */
829 client_ptr -> nx_ptp_client_delay_req_timer = NX_PTP_CLIENT_DELAY_REQ_INTERVAL;
830 client_ptr -> nx_ptp_client_delay_req_flag = 0;
831 }
832
833
834 /**************************************************************************/
835 /* */
836 /* FUNCTION RELEASE */
837 /* */
838 /* _nx_ptp_client_sync_received PORTABLE C */
839 /* 6.1.3 */
840 /* AUTHOR */
841 /* */
842 /* Yuxin Zhou, Microsoft Corporation */
843 /* */
844 /* DESCRIPTION */
845 /* */
846 /* This function processes a received PTP Sync message. */
847 /* */
848 /* INPUT */
849 /* */
850 /* client_ptr Pointer to PTP client */
851 /* ts_ptr Pointer to the timestamp */
852 /* delivered by the Sync message*/
853 /* */
854 /* OUTPUT */
855 /* */
856 /* None */
857 /* */
858 /* CALLS */
859 /* */
860 /* _nx_ptp_msg_parse_timestamp Parse timestamp field */
861 /* _nx_ptp_client_send_delay_req Send delay request */
862 /* */
863 /* CALLED BY */
864 /* */
865 /* _nx_ptp_client_process_event_packet Process PTP event packet */
866 /* _nx_ptp_client_process_general_packet Process PTP general packet */
867 /* */
868 /* RELEASE HISTORY */
869 /* */
870 /* DATE NAME DESCRIPTION */
871 /* */
872 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
873 /* */
874 /**************************************************************************/
_nx_ptp_client_sync_received(NX_PTP_CLIENT * client_ptr,VOID * ts_ptr)875 static VOID _nx_ptp_client_sync_received(NX_PTP_CLIENT *client_ptr, VOID *ts_ptr)
876 {
877 NX_PTP_DEBUG_PRINTF(("PTP: rcv SYNC\r\n"));
878
879 /* store Sync master timestamp */
880 _nx_ptp_msg_parse_timestamp(ts_ptr, &client_ptr -> nx_ptp_client_sync);
881
882 /* delay and offset determination */
883 if (client_ptr -> nx_ptp_client_delay_req_flag)
884 {
885
886 /* send delay request message */
887 /* (delay_req_flag is cleared by this function) */
888 _nx_ptp_client_send_delay_req(client_ptr);
889 }
890
891 /* update Sync state */
892 client_ptr -> nx_ptp_client_state = NX_PTP_CLIENT_STATE_WAIT_SYNC;
893 }
894
895
896 /**************************************************************************/
897 /* */
898 /* FUNCTION RELEASE */
899 /* */
900 /* _nx_ptp_client_delay_resp_received PORTABLE C */
901 /* 6.1.5 */
902 /* AUTHOR */
903 /* */
904 /* Yuxin Zhou, Microsoft Corporation */
905 /* */
906 /* DESCRIPTION */
907 /* */
908 /* This function processes a received PTP Delay Response message. */
909 /* */
910 /* INPUT */
911 /* */
912 /* client_ptr Pointer to PTP client */
913 /* ts_ptr Pointer to the timestamp */
914 /* delivered by the Delay Resp */
915 /* message */
916 /* */
917 /* OUTPUT */
918 /* */
919 /* None */
920 /* */
921 /* CALLS */
922 /* */
923 /* _nx_ptp_client_clock_adjust Adjust PTP clock */
924 /* _nx_ptp_msg_parse_timestamp Parse timestamp field */
925 /* _nx_ptp_client_utility_time_diff Diff two PTP times */
926 /* _nx_ptp_client_utility_time_div_by_2 Divide a PTP time by 2 */
927 /* */
928 /* CALLED BY */
929 /* */
930 /* _nx_ptp_client_process_general_packet Process PTP general packet */
931 /* */
932 /* RELEASE HISTORY */
933 /* */
934 /* DATE NAME DESCRIPTION */
935 /* */
936 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
937 /* 03-02-2021 Yuxin Zhou Modified comment(s), and */
938 /* fixed compiler warnings, */
939 /* resulting in version 6.1.5 */
940 /* */
941 /**************************************************************************/
_nx_ptp_client_delay_resp_received(NX_PTP_CLIENT * client_ptr,VOID * ts_ptr)942 static VOID _nx_ptp_client_delay_resp_received(NX_PTP_CLIENT *client_ptr, VOID *ts_ptr)
943 {
944 NX_PTP_TIME t4, a, b;
945 NX_PTP_CLIENT_SYNC sync;
946
947 NX_PTP_DEBUG_PRINTF(("PTP: rcv DELAY_RESP\r\n"));
948
949 /*
950 * The following timestamps are used for delay/offset determination:
951 *
952 * t1 = nx_ptp_client_sync
953 * t2 = nx_ptp_client_sync_ts
954 * t3 = nx_ptp_client_delay_ts
955 * t4 = timestamp in Delay_Resp message (ts_ptr)
956 *
957 * A = t2 - t1
958 * B = t4 - t3
959 *
960 * delay = (A + B) / 2
961 * offset = (B - A) / 2
962 */
963
964 /* check for valid timestamp t1 */
965 if ((client_ptr -> nx_ptp_client_sync.second_low == 0) &&
966 (client_ptr -> nx_ptp_client_sync.second_high == 0))
967 {
968 return;
969 }
970
971 /* get master clock timestamp */
972 _nx_ptp_msg_parse_timestamp(ts_ptr, &t4);
973
974 /* compute A = t2 - t1 */
975 _nx_ptp_client_utility_time_diff(&client_ptr -> nx_ptp_client_sync_ts, &client_ptr -> nx_ptp_client_sync, &a);
976
977 /* compute B = t4 - t3 */
978 _nx_ptp_client_utility_time_diff(&t4, &client_ptr -> nx_ptp_client_delay_ts, &b);
979
980 /* compute offset = (B - A) / 2 */
981 _nx_ptp_client_utility_time_diff(&b, &a, &a);
982 _nx_ptp_client_utility_time_div_by_2(&a);
983
984 #if defined(NX_PTP_DEBUG) || defined(NX_PTP_DEBUG_OFFSET)
985 if ((a.second_low == 0) && (a.second_high == 0))
986 {
987 if ((a.nanosecond > -1000) && (a.nanosecond < 1000))
988 {
989 printf("PTP: offset = %ld ns\n", a.nanosecond);
990 }
991 else if ((a.nanosecond > -1000000) && (a.nanosecond < 1000000))
992 {
993 printf("PTP: offset = %ld us\n", a.nanosecond / 1000);
994 }
995 else
996 {
997 printf("PTP: offset = %ld ms\n", a.nanosecond / 1000000);
998 }
999 }
1000 else
1001 {
1002 printf("PTP: offset > 1s\n");
1003 }
1004 #endif
1005
1006 /* add the time offset the client clock */
1007 _nx_ptp_client_clock_adjust(client_ptr, &a);
1008
1009 /* set calibrated flag */
1010 if (!(client_ptr -> nx_ptp_client_sync_flags & NX_PTP_CLIENT_SYNC_CALIBRATED))
1011 {
1012
1013 client_ptr -> nx_ptp_client_sync_flags |= NX_PTP_CLIENT_SYNC_CALIBRATED;
1014
1015 /* application callback */
1016 if (client_ptr -> nx_ptp_client_event_callback)
1017 {
1018 sync.nx_ptp_client_sync_flags = client_ptr -> nx_ptp_client_sync_flags;
1019 sync.nx_ptp_client_sync_utc_offset = client_ptr -> nx_ptp_client_utc_offset;
1020 client_ptr -> nx_ptp_client_event_callback(client_ptr, NX_PTP_CLIENT_EVENT_SYNC, &sync,
1021 client_ptr -> nx_ptp_client_event_callback_data);
1022 }
1023 }
1024
1025 /* update delay req state */
1026 client_ptr -> nx_ptp_client_delay_state = NX_PTP_CLIENT_DELAY_IDLE;
1027 }
1028
1029
1030 /**************************************************************************/
1031 /* */
1032 /* FUNCTION RELEASE */
1033 /* */
1034 /* _nx_ptp_client_announce_received PORTABLE C */
1035 /* 6.1.3 */
1036 /* AUTHOR */
1037 /* */
1038 /* Yuxin Zhou, Microsoft Corporation */
1039 /* */
1040 /* DESCRIPTION */
1041 /* */
1042 /* This function processes a received PTP Announce message. */
1043 /* */
1044 /* INPUT */
1045 /* */
1046 /* client_ptr Pointer to PTP client */
1047 /* hdr Pointer to PTP header */
1048 /* ptr Pointer to PTP message */
1049 /* */
1050 /* OUTPUT */
1051 /* */
1052 /* None */
1053 /* */
1054 /* CALLS */
1055 /* */
1056 /* _nx_ptp_msg_parse_announce Parse Announce message */
1057 /* memcpy Copy memory */
1058 /* */
1059 /* CALLED BY */
1060 /* */
1061 /* _nx_ptp_client_process_general_packet Process PTP general packet */
1062 /* */
1063 /* RELEASE HISTORY */
1064 /* */
1065 /* DATE NAME DESCRIPTION */
1066 /* */
1067 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1068 /* */
1069 /**************************************************************************/
_nx_ptp_client_announce_received(NX_PTP_CLIENT * client_ptr,NX_PTP_MSG_HEADER * hdr,UCHAR * ptr)1070 static VOID _nx_ptp_client_announce_received(NX_PTP_CLIENT *client_ptr,
1071 NX_PTP_MSG_HEADER *hdr,
1072 UCHAR *ptr)
1073 {
1074 NX_PTP_CLIENT_SYNC sync;
1075 NX_PTP_CLIENT_MASTER master;
1076
1077 /* parse Sync information */
1078 sync.nx_ptp_client_sync_flags = 0;
1079 if (hdr -> flagField & NX_PTP_MSG_HDR_FLAG_UTC_REASONABLE)
1080 {
1081 sync.nx_ptp_client_sync_flags |= NX_PTP_CLIENT_SYNC_UTC_REASONABLE;
1082 }
1083 if (hdr -> flagField & NX_PTP_MSG_HDR_FLAG_LEAP59)
1084 {
1085 sync.nx_ptp_client_sync_flags |= NX_PTP_CLIENT_SYNC_LEAP59;
1086 }
1087 if (hdr -> flagField & NX_PTP_MSG_HDR_FLAG_LEAP61)
1088 {
1089 sync.nx_ptp_client_sync_flags |= NX_PTP_CLIENT_SYNC_LEAP61;
1090 }
1091 sync.nx_ptp_client_sync_utc_offset = NX_PTP_MSG_UTC_OFFSET(ptr);
1092
1093 /* check for new master */
1094 if (client_ptr -> nx_ptp_client_state == NX_PTP_CLIENT_STATE_LISTENING)
1095 {
1096
1097 /* first announce message, save master clock parameters */
1098 client_ptr -> nx_ptp_client_sync_flags = sync.nx_ptp_client_sync_flags;
1099 client_ptr -> nx_ptp_client_utc_offset = sync.nx_ptp_client_sync_utc_offset;
1100 memcpy(client_ptr -> nx_ptp_client_master_port_identity, hdr -> sourcePortIdentity,
1101 NX_PTP_CLOCK_PORT_IDENTITY_SIZE); /* use case of memcpy is verified. */
1102
1103 /* wait for Sync message */
1104 client_ptr -> nx_ptp_client_state = NX_PTP_CLIENT_STATE_WAIT_SYNC;
1105 client_ptr -> nx_ptp_client_delay_state = NX_PTP_CLIENT_DELAY_IDLE;
1106 client_ptr -> nx_ptp_client_delay_req_timer = -1;
1107 client_ptr -> nx_ptp_client_delay_req_flag = 1;
1108
1109
1110 /* call application callback */
1111 if (client_ptr -> nx_ptp_client_event_callback)
1112 {
1113
1114 /* parse annouce message */
1115 _nx_ptp_msg_parse_announce(ptr, &master);
1116 master.nx_ptp_client_master_address = &(client_ptr -> nx_ptp_client_master_addr);
1117 master.nx_ptp_client_master_port_identity = hdr -> sourcePortIdentity;
1118 client_ptr -> nx_ptp_client_event_callback(client_ptr, NX_PTP_CLIENT_EVENT_MASTER, &master,
1119 client_ptr -> nx_ptp_client_event_callback_data);
1120 }
1121 }
1122 else
1123 {
1124
1125 /* check for UTC offset update or flags changes */
1126 if (((client_ptr -> nx_ptp_client_sync_flags & ~NX_PTP_CLIENT_SYNC_CALIBRATED) !=
1127 sync.nx_ptp_client_sync_flags) ||
1128 (client_ptr -> nx_ptp_client_utc_offset != sync.nx_ptp_client_sync_utc_offset))
1129 {
1130 client_ptr -> nx_ptp_client_sync_flags =
1131 (USHORT)((client_ptr -> nx_ptp_client_sync_flags & NX_PTP_CLIENT_SYNC_CALIBRATED) |
1132 sync.nx_ptp_client_sync_flags);
1133 client_ptr -> nx_ptp_client_utc_offset = sync.nx_ptp_client_sync_utc_offset;
1134
1135 /* call application callback */
1136 if ((client_ptr -> nx_ptp_client_sync_flags & NX_PTP_CLIENT_SYNC_CALIBRATED) &&
1137 (client_ptr -> nx_ptp_client_event_callback))
1138 {
1139 client_ptr -> nx_ptp_client_event_callback(client_ptr, NX_PTP_CLIENT_EVENT_SYNC, &sync,
1140 client_ptr -> nx_ptp_client_event_callback_data);
1141 }
1142 }
1143 }
1144
1145 /* reset annouce timer */
1146 client_ptr -> nx_ptp_client_announce_timeout = NX_PTP_CLIENT_ANNOUNCE_EXPIRATION;
1147 }
1148
1149
1150 /**************************************************************************/
1151 /* */
1152 /* FUNCTION RELEASE */
1153 /* */
1154 /* _nx_ptp_client_process_event_packet PORTABLE C */
1155 /* 6.1.3 */
1156 /* AUTHOR */
1157 /* */
1158 /* Yuxin Zhou, Microsoft Corporation */
1159 /* */
1160 /* DESCRIPTION */
1161 /* */
1162 /* This function processes PTP packet received through event socket. */
1163 /* */
1164 /* INPUT */
1165 /* */
1166 /* client_ptr Pointer to PTP client */
1167 /* packet_ptr Pointer to PTP packet */
1168 /* */
1169 /* OUTPUT */
1170 /* */
1171 /* None */
1172 /* */
1173 /* CALLS */
1174 /* */
1175 /* _nx_ptp_client_sync_received Process Sync message */
1176 /* _nx_ptp_msg_parse_hdr Parse PTP header */
1177 /* memcmp Compare memory */
1178 /* */
1179 /* CALLED BY */
1180 /* */
1181 /* _nx_ptp_client_thread_entry PTP thread entry */
1182 /* */
1183 /* RELEASE HISTORY */
1184 /* */
1185 /* DATE NAME DESCRIPTION */
1186 /* */
1187 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1188 /* */
1189 /**************************************************************************/
_nx_ptp_client_process_event_packet(NX_PTP_CLIENT * client_ptr,NX_PACKET * packet_ptr)1190 static VOID _nx_ptp_client_process_event_packet(NX_PTP_CLIENT *client_ptr, NX_PACKET *packet_ptr)
1191 {
1192 NX_PTP_MSG_HEADER hdr;
1193
1194 /* parse PTP message header */
1195 if (_nx_ptp_msg_parse_hdr(client_ptr, packet_ptr, &hdr))
1196 {
1197 return;
1198 }
1199
1200 /* check origin of message */
1201 if (memcmp(&client_ptr -> nx_ptp_client_master_port_identity,
1202 hdr.sourcePortIdentity, NX_PTP_CLOCK_PORT_IDENTITY_SIZE) != 0)
1203 {
1204
1205 /* not from our master clock */
1206 return;
1207 }
1208
1209 if (hdr.messageType != NX_PTP_MSG_TYPE_SYNC)
1210 {
1211 return;
1212 }
1213
1214 if (((client_ptr -> nx_ptp_client_state != NX_PTP_CLIENT_STATE_WAIT_SYNC) &&
1215 (client_ptr -> nx_ptp_client_state != NX_PTP_CLIENT_STATE_WAIT_FOLLOW_UP)) ||
1216 (packet_ptr -> nx_packet_length < NX_PTP_MSG_SYNC_LEN))
1217 {
1218
1219 /* not waiting for Sync or invalid message */
1220 return;
1221 }
1222
1223 /* retrieve timestamp of event message */
1224 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_PACKET_TS_EXTRACT,
1225 &client_ptr -> nx_ptp_client_sync_ts, packet_ptr,
1226 client_ptr -> nx_ptp_client_clock_callback_data);
1227
1228 /* two-step message? */
1229 if (hdr.flagField & NX_PTP_MSG_HDR_FLAG_TWO_STEP)
1230 {
1231
1232 /* wait for follow up message */
1233 client_ptr -> nx_ptp_client_state = NX_PTP_CLIENT_STATE_WAIT_FOLLOW_UP;
1234 client_ptr -> nx_ptp_client_sync_id = hdr.sequenceId;
1235 }
1236 else
1237 {
1238
1239 /* process Sync event */
1240 _nx_ptp_client_sync_received(client_ptr, packet_ptr -> nx_packet_prepend_ptr);
1241 }
1242 }
1243
1244
1245 /**************************************************************************/
1246 /* */
1247 /* FUNCTION RELEASE */
1248 /* */
1249 /* _nx_ptp_client_process_general_packet PORTABLE C */
1250 /* 6.1.3 */
1251 /* AUTHOR */
1252 /* */
1253 /* Yuxin Zhou, Microsoft Corporation */
1254 /* */
1255 /* DESCRIPTION */
1256 /* */
1257 /* This function processes PTP packet received through general socket. */
1258 /* */
1259 /* INPUT */
1260 /* */
1261 /* client_ptr Pointer to PTP client */
1262 /* packet_ptr Pointer to PTP packet */
1263 /* */
1264 /* OUTPUT */
1265 /* */
1266 /* None */
1267 /* */
1268 /* CALLS */
1269 /* */
1270 /* _nx_ptp_client_sync_received Process Sync message */
1271 /* _nx_ptp_msg_parse_hdr Parse PTP header */
1272 /* _nx_ptp_client_delay_resp_received Process delay response */
1273 /* _nx_ptp_client_announce_received Process Announce message */
1274 /* memcmp Compare memory */
1275 /* */
1276 /* CALLED BY */
1277 /* */
1278 /* _nx_ptp_client_thread_entry PTP thread entry */
1279 /* */
1280 /* RELEASE HISTORY */
1281 /* */
1282 /* DATE NAME DESCRIPTION */
1283 /* */
1284 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1285 /* */
1286 /**************************************************************************/
_nx_ptp_client_process_general_packet(NX_PTP_CLIENT * client_ptr,NX_PACKET * packet_ptr)1287 static VOID _nx_ptp_client_process_general_packet(NX_PTP_CLIENT *client_ptr, NX_PACKET *packet_ptr)
1288 {
1289 NX_PTP_MSG_HEADER hdr;
1290
1291 /* parse PTP message header */
1292 if (_nx_ptp_msg_parse_hdr(client_ptr, packet_ptr, &hdr))
1293 {
1294 return;
1295 }
1296
1297 /* check origin of message */
1298 if ((client_ptr -> nx_ptp_client_state != NX_PTP_CLIENT_STATE_LISTENING) &&
1299 (memcmp(&client_ptr -> nx_ptp_client_master_port_identity,
1300 hdr.sourcePortIdentity, NX_PTP_CLOCK_PORT_IDENTITY_SIZE) != 0))
1301 {
1302
1303 /* not from our master clock */
1304 return;
1305 }
1306
1307 /* process ANNOUNCE message */
1308 if (hdr.messageType == NX_PTP_MSG_TYPE_ANNOUNCE)
1309 {
1310 if (packet_ptr -> nx_packet_length < NX_PTP_MSG_ANNOUNCE_LEN)
1311 {
1312
1313 /* invalid message */
1314 return;
1315 }
1316
1317 /* process announce message */
1318 _nx_ptp_client_announce_received(client_ptr, &hdr, packet_ptr -> nx_packet_prepend_ptr);
1319 }
1320 else if (hdr.messageType == NX_PTP_MSG_TYPE_FOLLOW_UP)
1321 {
1322 if ((client_ptr -> nx_ptp_client_state != NX_PTP_CLIENT_STATE_WAIT_FOLLOW_UP) ||
1323 (client_ptr -> nx_ptp_client_sync_id != hdr.sequenceId) ||
1324 (packet_ptr -> nx_packet_length < NX_PTP_MSG_FOLLOW_UP_LEN))
1325 {
1326
1327 /* not a follow up for a previous Sync or invalid message */
1328 return;
1329 }
1330
1331 /* process Sync message */
1332 _nx_ptp_client_sync_received(client_ptr, packet_ptr -> nx_packet_prepend_ptr);
1333 }
1334 else if (hdr.messageType == NX_PTP_MSG_TYPE_DELAY_RESP)
1335 {
1336 if ((client_ptr -> nx_ptp_client_delay_state != NX_PTP_CLIENT_DELAY_WAIT_RESP) ||
1337 (client_ptr -> nx_ptp_client_delay_req_id != hdr.sequenceId) ||
1338 (packet_ptr -> nx_packet_length < NX_PTP_MSG_DELAY_RESP_LEN) ||
1339 (memcmp(client_ptr -> nx_ptp_client_port_identity,
1340 packet_ptr -> nx_packet_prepend_ptr + NX_PTP_MSG_TIMESTAMP_LEN,
1341 NX_PTP_CLOCK_PORT_IDENTITY_SIZE) != 0))
1342 {
1343
1344 /* not a delay_resp for a previous delay_req or invalid message */
1345 return;
1346 }
1347
1348 /* process delay response message */
1349 _nx_ptp_client_delay_resp_received(client_ptr, packet_ptr -> nx_packet_prepend_ptr);
1350 }
1351 }
1352
1353
1354 /**************************************************************************/
1355 /* */
1356 /* FUNCTION RELEASE */
1357 /* */
1358 /* _nx_ptp_client_thread_entry PORTABLE C */
1359 /* 6.1.3 */
1360 /* AUTHOR */
1361 /* */
1362 /* Yuxin Zhou, Microsoft Corporation */
1363 /* */
1364 /* DESCRIPTION */
1365 /* */
1366 /* This function implements the PTP client processing thread. */
1367 /* */
1368 /* INPUT */
1369 /* */
1370 /* ptp_instance Pointer to PTP client */
1371 /* */
1372 /* OUTPUT */
1373 /* */
1374 /* None */
1375 /* */
1376 /* CALLS */
1377 /* */
1378 /* _nx_ptp_client_process_general_packet Process PTP general packet */
1379 /* _nx_ptp_client_process_event_packet Process PTP event packet */
1380 /* tx_event_flags_get Get PTP events */
1381 /* nx_udp_socket_receive Receive a UDP packet */
1382 /* nx_packet_release Release a packet */
1383 /* */
1384 /* CALLED BY */
1385 /* */
1386 /* ThreadX */
1387 /* */
1388 /* RELEASE HISTORY */
1389 /* */
1390 /* DATE NAME DESCRIPTION */
1391 /* */
1392 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1393 /* */
1394 /**************************************************************************/
_nx_ptp_client_thread_entry(ULONG ptp_instance)1395 static VOID _nx_ptp_client_thread_entry(ULONG ptp_instance)
1396 {
1397 TX_INTERRUPT_SAVE_AREA
1398 NX_PTP_CLIENT *client_ptr = (NX_PTP_CLIENT *)ptp_instance;
1399 ULONG ptp_events;
1400 UINT status;
1401 NX_PACKET *packet_ptr = NX_NULL;
1402 UCHAR packet_received;
1403
1404 /* start in listening state */
1405 client_ptr -> nx_ptp_client_state = NX_PTP_CLIENT_STATE_LISTENING;
1406 client_ptr -> nx_ptp_client_delay_req_timer = -1;
1407 client_ptr -> nx_ptp_client_announce_timeout = -1;
1408
1409 /* main loop */
1410 for (;;)
1411 {
1412
1413 /* wait for message */
1414 status = tx_event_flags_get(&(client_ptr -> nx_ptp_client_events),
1415 NX_PTP_CLIENT_ALL_EVENTS, TX_OR_CLEAR,
1416 &ptp_events, TX_WAIT_FOREVER);
1417 if (status != TX_SUCCESS)
1418 {
1419
1420 /* error while reading queue, abort thread */
1421 break;
1422 }
1423
1424 if (ptp_events & NX_PTP_CLIENT_STOP_EVENT)
1425 {
1426
1427 /* terminate thread */
1428 break;
1429 }
1430
1431 /*
1432 * PTP Message Received
1433 */
1434 if (ptp_events & NX_PTP_CLIENT_RX_EVENT)
1435 {
1436
1437 /* Loop to receive all packets. */
1438 for (;;)
1439 {
1440 packet_received = NX_FALSE;
1441 if (nx_udp_socket_receive(&(client_ptr -> nx_ptp_client_general_socket),
1442 &packet_ptr, NX_NO_WAIT) == NX_SUCCESS)
1443 {
1444 _nx_ptp_client_process_general_packet(client_ptr, packet_ptr);
1445
1446 /* Release packet. */
1447 nx_packet_release(packet_ptr);
1448 packet_received = NX_TRUE;
1449 }
1450
1451 if (nx_udp_socket_receive(&(client_ptr -> nx_ptp_client_event_socket),
1452 &packet_ptr, NX_NO_WAIT) == NX_SUCCESS)
1453 {
1454 _nx_ptp_client_process_event_packet(client_ptr, packet_ptr);
1455
1456 /* Release packet. */
1457 nx_packet_release(packet_ptr);
1458 packet_received = NX_TRUE;
1459 }
1460
1461 if (packet_received == NX_FALSE)
1462 {
1463
1464 /* No more packets available. */
1465 break;
1466 }
1467 }
1468 }
1469
1470 /*
1471 * Timer Event
1472 */
1473 if (ptp_events & NX_PTP_CLIENT_TIMER_EVENT)
1474 {
1475
1476 /* announce messages timeout */
1477 if ((client_ptr -> nx_ptp_client_announce_timeout > 0) &&
1478 (--client_ptr -> nx_ptp_client_announce_timeout == 0))
1479 {
1480
1481 /* no Annouce message received from master clock, back to listening state */
1482 client_ptr -> nx_ptp_client_state = NX_PTP_CLIENT_STATE_LISTENING;
1483 client_ptr -> nx_ptp_client_delay_req_timer = -1;
1484 client_ptr -> nx_ptp_client_announce_timeout = -1;
1485
1486 /* call handler */
1487 if (client_ptr -> nx_ptp_client_event_callback)
1488 {
1489 client_ptr -> nx_ptp_client_event_callback(client_ptr, NX_PTP_CLIENT_EVENT_TIMEOUT, NX_NULL,
1490 client_ptr -> nx_ptp_client_event_callback_data);
1491 }
1492 }
1493
1494 /* delay req interval timer */
1495 if ((client_ptr -> nx_ptp_client_delay_req_timer > 0) &&
1496 (--client_ptr -> nx_ptp_client_delay_req_timer == 0))
1497 {
1498
1499 /* set flag */
1500 client_ptr -> nx_ptp_client_delay_req_flag = 1;
1501 }
1502 }
1503 }
1504
1505 /* set stopped state */
1506 TX_DISABLE
1507 client_ptr -> nx_ptp_client_thread_state = NX_PTP_CLIENT_THREAD_STOPPED;
1508 TX_RESTORE
1509 }
1510
1511
1512 /**************************************************************************/
1513 /* */
1514 /* FUNCTION RELEASE */
1515 /* */
1516 /* _nxe_ptp_client_create PORTABLE C */
1517 /* 6.1.3 */
1518 /* AUTHOR */
1519 /* */
1520 /* Yuxin Zhou, Microsoft Corporation */
1521 /* */
1522 /* DESCRIPTION */
1523 /* */
1524 /* This function checks for errors on the PTP client create service. */
1525 /* */
1526 /* INPUT */
1527 /* */
1528 /* client_ptr Pointer to PTP client */
1529 /* ip_ptr Pointer to client IP instance */
1530 /* interface_index Index of PTP network interface*/
1531 /* packet_pool_ptr Pointer to client packet pool */
1532 /* thread_priority Priority of PTP thread */
1533 /* thread_stack Pointer to thread stack */
1534 /* stack_size Size of thread stack */
1535 /* clock_callback PTP clock callback */
1536 /* clock_callback_data Data for the clock callback */
1537 /* */
1538 /* OUTPUT */
1539 /* */
1540 /* status Completion status */
1541 /* */
1542 /* CALLS */
1543 /* */
1544 /* _nx_ptp_client_create Actual create service */
1545 /* */
1546 /* CALLED BY */
1547 /* */
1548 /* Application */
1549 /* */
1550 /* RELEASE HISTORY */
1551 /* */
1552 /* DATE NAME DESCRIPTION */
1553 /* */
1554 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1555 /* */
1556 /**************************************************************************/
_nxe_ptp_client_create(NX_PTP_CLIENT * client_ptr,NX_IP * ip_ptr,UINT interface_index,NX_PACKET_POOL * packet_pool_ptr,UINT thread_priority,UCHAR * thread_stack,UINT stack_size,NX_PTP_CLIENT_CLOCK_CALLBACK clock_callback,VOID * clock_callback_data)1557 UINT _nxe_ptp_client_create(NX_PTP_CLIENT *client_ptr, NX_IP *ip_ptr, UINT interface_index,
1558 NX_PACKET_POOL *packet_pool_ptr, UINT thread_priority, UCHAR *thread_stack, UINT stack_size,
1559 NX_PTP_CLIENT_CLOCK_CALLBACK clock_callback, VOID *clock_callback_data)
1560 {
1561
1562 /* Check input parameters. */
1563 if ((client_ptr == NX_NULL) || (ip_ptr == NX_NULL) || (packet_pool_ptr == NX_NULL) ||
1564 (thread_stack == NX_NULL) || (stack_size == 0) || (clock_callback == NX_NULL))
1565 {
1566
1567 /* Return error status. */
1568 return(NX_PTR_ERROR);
1569 }
1570
1571 /* Check for invalid network interface input. */
1572 if (interface_index >= NX_MAX_PHYSICAL_INTERFACES)
1573 {
1574 return(NX_INVALID_INTERFACE);
1575 }
1576
1577 /* Check for appropriate caller. */
1578 NX_THREADS_ONLY_CALLER_CHECKING
1579
1580 /* Call the actual function. */
1581 return _nx_ptp_client_create(client_ptr, ip_ptr, interface_index, packet_pool_ptr, thread_priority,
1582 thread_stack, stack_size, clock_callback, clock_callback_data);
1583 }
1584
1585
1586 /**************************************************************************/
1587 /* */
1588 /* FUNCTION RELEASE */
1589 /* */
1590 /* _nx_ptp_client_create PORTABLE C */
1591 /* 6.1.3 */
1592 /* AUTHOR */
1593 /* */
1594 /* Yuxin Zhou, Microsoft Corporation */
1595 /* */
1596 /* DESCRIPTION */
1597 /* */
1598 /* This function creates the PTP client. */
1599 /* */
1600 /* INPUT */
1601 /* */
1602 /* client_ptr Pointer to PTP client */
1603 /* ip_ptr Pointer to client IP instance */
1604 /* interface_index Index of PTP network interface*/
1605 /* packet_pool_ptr Pointer to client packet pool */
1606 /* thread_priority Priority of PTP thread */
1607 /* thread_stack Pointer to thread stack */
1608 /* stack_size Size of thread stack */
1609 /* clock_callback PTP clock callback */
1610 /* clock_callback_data Data for the clock callback */
1611 /* */
1612 /* OUTPUT */
1613 /* */
1614 /* status Completion status */
1615 /* */
1616 /* CALLS */
1617 /* */
1618 /* memset Reset memory */
1619 /* [clock_callback] Initialize clock */
1620 /* tx_event_flags_create Create PTP event flags */
1621 /* tx_event_flags_delete Delete PTP event flags */
1622 /* nx_udp_socket_create Create a UDP socket */
1623 /* nx_udp_socket_delete Delete a UDP socket */
1624 /* nx_udp_socket_receive_notify Set UDP receive notify */
1625 /* tx_timer_create Create a timer */
1626 /* tx_timer_delete Delete a timer */
1627 /* */
1628 /* CALLED BY */
1629 /* */
1630 /* Application */
1631 /* */
1632 /* RELEASE HISTORY */
1633 /* */
1634 /* DATE NAME DESCRIPTION */
1635 /* */
1636 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1637 /* */
1638 /**************************************************************************/
_nx_ptp_client_create(NX_PTP_CLIENT * client_ptr,NX_IP * ip_ptr,UINT interface_index,NX_PACKET_POOL * packet_pool_ptr,UINT thread_priority,UCHAR * thread_stack,UINT stack_size,NX_PTP_CLIENT_CLOCK_CALLBACK clock_callback,VOID * clock_callback_data)1639 UINT _nx_ptp_client_create(NX_PTP_CLIENT *client_ptr, NX_IP *ip_ptr, UINT interface_index,
1640 NX_PACKET_POOL *packet_pool_ptr, UINT thread_priority, UCHAR *thread_stack, UINT stack_size,
1641 NX_PTP_CLIENT_CLOCK_CALLBACK clock_callback, VOID *clock_callback_data)
1642 {
1643 UINT status;
1644
1645 /* Null the members of NX_PTP_CLIENT. */
1646 memset(client_ptr, 0, sizeof(NX_PTP_CLIENT));
1647
1648 /* Set the Client ID to indicate the PTP client thread is ready. */
1649 client_ptr -> nx_ptp_client_id = NX_PTP_CLIENT_ID;
1650
1651 /* Set the IP instance. */
1652 client_ptr -> nx_ptp_client_ip_ptr = ip_ptr;
1653
1654 /* Set the PTP network interface. */
1655 client_ptr -> nx_ptp_client_interface_index = interface_index;
1656
1657 /* Set the packet pool, check for minimal packet size requirement. */
1658 if (packet_pool_ptr -> nx_packet_pool_payload_size <
1659 (NX_UDP_PACKET + NX_PTP_CLIENT_PACKET_DATA_SIZE))
1660 {
1661 return(NX_PTP_CLIENT_INSUFFICIENT_PACKET_PAYLOAD);
1662 }
1663 client_ptr -> nx_ptp_client_packet_pool_ptr = packet_pool_ptr;
1664
1665 /* Initialize callback function. */
1666 status = clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_INIT, NX_NULL, NX_NULL, clock_callback_data);
1667 if (status)
1668 {
1669
1670 /* Fail to initialize clock in callback function. */
1671 return(NX_PTP_CLIENT_CLOCK_CALLBACK_FAILURE);
1672 }
1673
1674 /* Set the PTP clock callback */
1675 client_ptr -> nx_ptp_client_clock_callback = clock_callback;
1676 client_ptr -> nx_ptp_client_clock_callback_data = clock_callback_data;
1677
1678 /* create the internal PTP event flag object. */
1679 status = tx_event_flags_create(&(client_ptr -> nx_ptp_client_events), "NetX PTP event flag");
1680 if (status != TX_SUCCESS)
1681 {
1682 return(status);
1683 }
1684
1685 /* create the general socket */
1686 status = nx_udp_socket_create(ip_ptr, &client_ptr -> nx_ptp_client_general_socket,
1687 "NetX PTP Client general socket", NX_IP_NORMAL,
1688 NX_DONT_FRAGMENT, NX_PTP_TIME_TO_LIVE,
1689 NX_PTP_CLIENT_MAX_QUEUE_DEPTH);
1690 if (status != NX_SUCCESS)
1691 {
1692 tx_event_flags_delete(&client_ptr -> nx_ptp_client_events);
1693 return(status);
1694 }
1695 client_ptr -> nx_ptp_client_general_socket.nx_udp_socket_reserved_ptr = client_ptr;
1696 nx_udp_socket_receive_notify(&client_ptr -> nx_ptp_client_general_socket, _nx_ptp_client_socket_receive_notify);
1697
1698 /* create the event socket */
1699 status = nx_udp_socket_create(ip_ptr, &client_ptr -> nx_ptp_client_event_socket,
1700 "NetX PTP Client event socket", NX_IP_NORMAL, NX_DONT_FRAGMENT,
1701 NX_PTP_TIME_TO_LIVE,
1702 NX_PTP_CLIENT_MAX_QUEUE_DEPTH);
1703 if (status != NX_SUCCESS)
1704 {
1705 nx_udp_socket_delete(&client_ptr -> nx_ptp_client_general_socket);
1706 tx_event_flags_delete(&client_ptr -> nx_ptp_client_events);
1707 return(status);
1708 }
1709 client_ptr -> nx_ptp_client_event_socket.nx_udp_socket_reserved_ptr = client_ptr;
1710 nx_udp_socket_receive_notify(&client_ptr -> nx_ptp_client_event_socket, _nx_ptp_client_socket_receive_notify);
1711
1712 /* create the timer */
1713 status = tx_timer_create(&client_ptr -> nx_ptp_client_timer,
1714 "NetX PTP Client timer",
1715 _nx_ptp_client_timer_handler,
1716 (ULONG)client_ptr,
1717 TX_TIMER_TICKS_PER_SECOND / NX_PTP_CLIENT_TIMER_TICKS_PER_SECOND,
1718 TX_TIMER_TICKS_PER_SECOND / NX_PTP_CLIENT_TIMER_TICKS_PER_SECOND,
1719 TX_NO_ACTIVATE);
1720 if (status != TX_SUCCESS)
1721 {
1722 nx_udp_socket_delete(&client_ptr -> nx_ptp_client_general_socket);
1723 nx_udp_socket_delete(&client_ptr -> nx_ptp_client_event_socket);
1724 tx_event_flags_delete(&client_ptr -> nx_ptp_client_events);
1725 return(status);
1726 }
1727
1728 /* create the Client thread */
1729 status = tx_thread_create(&client_ptr -> nx_ptp_client_thread,
1730 "NetX PTP Client", _nx_ptp_client_thread_entry,
1731 (ULONG)client_ptr, thread_stack, stack_size,
1732 thread_priority, thread_priority,
1733 NX_PTP_CLIENT_THREAD_TIME_SLICE, TX_DONT_START);
1734 if (status != TX_SUCCESS)
1735 {
1736 nx_udp_socket_delete(&client_ptr -> nx_ptp_client_general_socket);
1737 nx_udp_socket_delete(&client_ptr -> nx_ptp_client_event_socket);
1738 tx_timer_delete(&client_ptr -> nx_ptp_client_timer);
1739 tx_event_flags_delete(&client_ptr -> nx_ptp_client_events);
1740 return(status);
1741 }
1742
1743 /* return Success */
1744 return(NX_SUCCESS);
1745 }
1746
1747 /**************************************************************************/
1748 /* */
1749 /* FUNCTION RELEASE */
1750 /* */
1751 /* _nxe_ptp_client_delete PORTABLE C */
1752 /* 6.1.3 */
1753 /* AUTHOR */
1754 /* */
1755 /* Yuxin Zhou, Microsoft Corporation */
1756 /* */
1757 /* DESCRIPTION */
1758 /* */
1759 /* This function checks for errors on the PTP client delete service. */
1760 /* */
1761 /* INPUT */
1762 /* */
1763 /* client_ptr Pointer to PTP client */
1764 /* */
1765 /* OUTPUT */
1766 /* */
1767 /* status Completion status */
1768 /* */
1769 /* CALLS */
1770 /* */
1771 /* _nx_ptp_client_delete Actual delete service */
1772 /* */
1773 /* CALLED BY */
1774 /* */
1775 /* Application */
1776 /* */
1777 /* RELEASE HISTORY */
1778 /* */
1779 /* DATE NAME DESCRIPTION */
1780 /* */
1781 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1782 /* */
1783 /**************************************************************************/
_nxe_ptp_client_delete(NX_PTP_CLIENT * client_ptr)1784 UINT _nxe_ptp_client_delete(NX_PTP_CLIENT *client_ptr)
1785 {
1786
1787 /* Check input parameters. */
1788 if (client_ptr == NX_NULL)
1789 {
1790
1791 /* Return error status. */
1792 return(NX_PTR_ERROR);
1793 }
1794
1795 /* Check for appropriate caller. */
1796 NX_THREADS_ONLY_CALLER_CHECKING
1797
1798 /* Call the actual function. */
1799 return(_nx_ptp_client_delete(client_ptr));
1800 }
1801
1802
1803 /**************************************************************************/
1804 /* */
1805 /* FUNCTION RELEASE */
1806 /* */
1807 /* _nx_ptp_client_delete PORTABLE C */
1808 /* 6.1.3 */
1809 /* AUTHOR */
1810 /* */
1811 /* Yuxin Zhou, Microsoft Corporation */
1812 /* */
1813 /* DESCRIPTION */
1814 /* */
1815 /* This function deletes the PTP client. */
1816 /* */
1817 /* INPUT */
1818 /* */
1819 /* client_ptr Pointer to PTP client */
1820 /* */
1821 /* OUTPUT */
1822 /* */
1823 /* status Completion status */
1824 /* */
1825 /* CALLS */
1826 /* */
1827 /* _nx_ptp_client_stop Stop PTP service */
1828 /* tx_thread_suspend Suspend PTP thread */
1829 /* tx_thread_terminate Terminate PTP thread */
1830 /* tx_thread_delete Delete PTP thread */
1831 /* tx_event_flags_delete Delete PTP event flags */
1832 /* nx_udp_socket_delete Delete a UDP socket */
1833 /* tx_timer_delete Delete a timer */
1834 /* */
1835 /* CALLED BY */
1836 /* */
1837 /* Application */
1838 /* */
1839 /* RELEASE HISTORY */
1840 /* */
1841 /* DATE NAME DESCRIPTION */
1842 /* */
1843 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1844 /* */
1845 /**************************************************************************/
_nx_ptp_client_delete(NX_PTP_CLIENT * client_ptr)1846 UINT _nx_ptp_client_delete(NX_PTP_CLIENT *client_ptr)
1847 {
1848
1849 /* Ensure the Client is stopped */
1850 _nx_ptp_client_stop(client_ptr);
1851
1852 /* Suspend the Client thread. */
1853 tx_thread_suspend(&client_ptr -> nx_ptp_client_thread);
1854
1855 /* Terminate Client thread. */
1856 tx_thread_terminate(&client_ptr -> nx_ptp_client_thread);
1857
1858 /* Delete Client thread. */
1859 tx_thread_delete(&client_ptr -> nx_ptp_client_thread);
1860
1861 /* Delete the timer */
1862 tx_timer_delete(&client_ptr -> nx_ptp_client_timer);
1863
1864 /* Delete the general socket */
1865 nx_udp_socket_delete((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_general_socket);
1866
1867 /* Delete the event socket */
1868 nx_udp_socket_delete((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_event_socket);
1869
1870 /* Delete the event flag */
1871 tx_event_flags_delete(&client_ptr -> nx_ptp_client_events);
1872
1873 /* return Success */
1874 return(NX_SUCCESS);
1875 }
1876
1877
1878 /**************************************************************************/
1879 /* */
1880 /* FUNCTION RELEASE */
1881 /* */
1882 /* _nxe_ptp_client_start PORTABLE C */
1883 /* 6.1.3 */
1884 /* AUTHOR */
1885 /* */
1886 /* Yuxin Zhou, Microsoft Corporation */
1887 /* */
1888 /* DESCRIPTION */
1889 /* */
1890 /* This function checks for errors on the PTP client start service. */
1891 /* */
1892 /* INPUT */
1893 /* */
1894 /* client_ptr Pointer to PTP client */
1895 /* client_port_identity_ptr Pointer to client port and */
1896 /* identity */
1897 /* client_port_identity_length Length of client port and */
1898 /* identity */
1899 /* domain PTP clock domain */
1900 /* transport_specific Transport specific */
1901 /* event_callback Event callback */
1902 /* event_callback_data Data for the event callback */
1903 /* */
1904 /* OUTPUT */
1905 /* */
1906 /* status Completion status */
1907 /* */
1908 /* CALLS */
1909 /* */
1910 /* _nx_ptp_client_start Actual start service */
1911 /* */
1912 /* CALLED BY */
1913 /* */
1914 /* Application */
1915 /* */
1916 /* RELEASE HISTORY */
1917 /* */
1918 /* DATE NAME DESCRIPTION */
1919 /* */
1920 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
1921 /* */
1922 /**************************************************************************/
_nxe_ptp_client_start(NX_PTP_CLIENT * client_ptr,UCHAR * client_port_identity_ptr,UINT client_port_identity_length,UINT domain,UINT transport_specific,NX_PTP_CLIENT_EVENT_CALLBACK event_callback,VOID * event_callback_data)1923 UINT _nxe_ptp_client_start(NX_PTP_CLIENT *client_ptr, UCHAR *client_port_identity_ptr, UINT client_port_identity_length,
1924 UINT domain, UINT transport_specific, NX_PTP_CLIENT_EVENT_CALLBACK event_callback,
1925 VOID *event_callback_data)
1926 {
1927
1928 /* Check input parameters. */
1929 if ((client_ptr == NX_NULL) ||
1930 ((client_port_identity_ptr == NX_NULL) && (client_port_identity_length != 0)) ||
1931 ((client_port_identity_ptr != NX_NULL) && (client_port_identity_length != NX_PTP_CLOCK_PORT_IDENTITY_SIZE)))
1932 {
1933
1934 /* Return error status. */
1935 return(NX_PTR_ERROR);
1936 }
1937
1938 /* Check for appropriate caller. */
1939 NX_THREADS_ONLY_CALLER_CHECKING
1940
1941 /* Call the actual function. */
1942 return(_nx_ptp_client_start(client_ptr, client_port_identity_ptr, client_port_identity_length,
1943 domain, transport_specific, event_callback, event_callback_data));
1944 }
1945
1946
1947 /**************************************************************************/
1948 /* */
1949 /* FUNCTION RELEASE */
1950 /* */
1951 /* _nx_ptp_client_start PORTABLE C */
1952 /* 6.1.3 */
1953 /* AUTHOR */
1954 /* */
1955 /* Yuxin Zhou, Microsoft Corporation */
1956 /* */
1957 /* DESCRIPTION */
1958 /* */
1959 /* This function starts the PTP client. */
1960 /* */
1961 /* INPUT */
1962 /* */
1963 /* client_ptr Pointer to PTP client */
1964 /* client_port_identity_ptr Pointer to client port and */
1965 /* identity */
1966 /* client_port_identity_length Length of client port and */
1967 /* identity */
1968 /* domain PTP clock domain */
1969 /* transport_specific Transport specific */
1970 /* event_callback Event callback */
1971 /* event_callback_data Data for the event callback */
1972 /* */
1973 /* OUTPUT */
1974 /* */
1975 /* status Completion status */
1976 /* */
1977 /* CALLS */
1978 /* */
1979 /* tx_thread_suspend Suspend PTP thread */
1980 /* tx_thread_terminate Terminate PTP thread */
1981 /* tx_thread_reset Reset PTP thread */
1982 /* tx_thread_resume Resume PTP thread */
1983 /* memcpy Copy memory */
1984 /* nx_ip_interface_physical_address_get Get physical address */
1985 /* nx_udp_socket_bind Bind UDP port */
1986 /* nx_ipv4_multicast_interface_join Join IPv4 multicast group */
1987 /* nxd_ipv6_multicast_interface_join Join IPv6 multicast group */
1988 /* tx_timer_activate Activate timer */
1989 /* _nx_ptp_client_stop Stop PTP service */
1990 /* */
1991 /* CALLED BY */
1992 /* */
1993 /* Application */
1994 /* */
1995 /* RELEASE HISTORY */
1996 /* */
1997 /* DATE NAME DESCRIPTION */
1998 /* */
1999 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2000 /* */
2001 /**************************************************************************/
_nx_ptp_client_start(NX_PTP_CLIENT * client_ptr,UCHAR * client_port_identity_ptr,UINT client_port_identity_length,UINT domain,UINT transport_specific,NX_PTP_CLIENT_EVENT_CALLBACK event_callback,VOID * event_callback_data)2002 UINT _nx_ptp_client_start(NX_PTP_CLIENT *client_ptr, UCHAR *client_port_identity_ptr, UINT client_port_identity_length,
2003 UINT domain, UINT transport_specific, NX_PTP_CLIENT_EVENT_CALLBACK event_callback,
2004 VOID *event_callback_data)
2005 {
2006 TX_INTERRUPT_SAVE_AREA
2007 UINT state;
2008 UINT status;
2009 ULONG msw, lsw;
2010 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
2011 NXD_ADDRESS maddr;
2012 #endif
2013
2014 NX_PARAMETER_NOT_USED(client_port_identity_length);
2015
2016 /* Check if Client is running */
2017 TX_DISABLE
2018 state = client_ptr -> nx_ptp_client_thread_state;
2019 if (state == NX_PTP_CLIENT_THREAD_IDLE)
2020 {
2021 client_ptr -> nx_ptp_client_thread_state = NX_PTP_CLIENT_THREAD_RUNNING;
2022 }
2023 TX_RESTORE
2024 if (state != NX_PTP_CLIENT_THREAD_IDLE)
2025 {
2026
2027 /* Client is already running */
2028 return(NX_PTP_CLIENT_ALREADY_STARTED);
2029 }
2030
2031 /* ensure the thread is terminated */
2032 tx_thread_suspend(&client_ptr -> nx_ptp_client_thread);
2033 tx_thread_terminate(&client_ptr -> nx_ptp_client_thread);
2034
2035 /* save the client parameters */
2036 client_ptr -> nx_ptp_client_domain = (UCHAR)domain;
2037 client_ptr -> nx_ptp_client_transport_specific = (UCHAR)transport_specific;
2038 client_ptr -> nx_ptp_client_event_callback = event_callback;
2039 client_ptr -> nx_ptp_client_event_callback_data = event_callback_data;
2040
2041 /* reset and resume the thread */
2042 status = tx_thread_reset(&client_ptr -> nx_ptp_client_thread);
2043 if (status != TX_SUCCESS)
2044 {
2045 TX_DISABLE
2046 client_ptr -> nx_ptp_client_thread_state = NX_PTP_CLIENT_THREAD_IDLE;
2047 TX_RESTORE
2048
2049 return(status);
2050 }
2051 tx_thread_resume(&client_ptr -> nx_ptp_client_thread);
2052
2053 /* set the client port and identity */
2054 if (client_port_identity_ptr != NX_NULL)
2055 {
2056
2057 /* copy provided identifier */
2058 memcpy(client_ptr -> nx_ptp_client_port_identity, client_port_identity_ptr,
2059 NX_PTP_CLOCK_PORT_IDENTITY_SIZE); /* use case of memcpy is verified. */
2060 }
2061 else
2062 {
2063
2064 /* get MAC address of interface */
2065 status = nx_ip_interface_physical_address_get(client_ptr -> nx_ptp_client_ip_ptr,
2066 client_ptr -> nx_ptp_client_interface_index,
2067 &msw, &lsw);
2068 if (status == TX_SUCCESS)
2069 {
2070 /* convert 48-bit MAC address to 64-bit EUI */
2071 client_ptr -> nx_ptp_client_port_identity[0] = (UCHAR)(msw >> 8);
2072 client_ptr -> nx_ptp_client_port_identity[1] = (UCHAR)msw;
2073 client_ptr -> nx_ptp_client_port_identity[2] = (UCHAR)(lsw >> 24);
2074 client_ptr -> nx_ptp_client_port_identity[3] = 0xff;
2075 client_ptr -> nx_ptp_client_port_identity[4] = 0xfe;
2076 client_ptr -> nx_ptp_client_port_identity[5] = (UCHAR)(lsw >> 16);
2077 client_ptr -> nx_ptp_client_port_identity[6] = (UCHAR)(lsw >> 8);
2078 client_ptr -> nx_ptp_client_port_identity[7] = (UCHAR)lsw;
2079
2080 /* set default port number (1) */
2081 client_ptr -> nx_ptp_client_port_identity[8] = 0;
2082 client_ptr -> nx_ptp_client_port_identity[9] = 1;
2083 }
2084 }
2085
2086 /* bind the general socket */
2087 if (status == TX_SUCCESS)
2088 {
2089 status = nx_udp_socket_bind((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_general_socket,
2090 NX_PTP_GENERAL_UDP_PORT, NX_NO_WAIT);
2091 }
2092
2093 /* bind the event socket */
2094 if (status == TX_SUCCESS)
2095 {
2096 status = nx_udp_socket_bind((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_event_socket,
2097 NX_PTP_EVENT_UDP_PORT, NX_NO_WAIT);
2098 }
2099
2100 /* join the multicast groups */
2101 if (status == TX_SUCCESS)
2102 {
2103 status = nx_ipv4_multicast_interface_join(client_ptr -> nx_ptp_client_ip_ptr,
2104 NX_PTP_IPV4_MULTICAST_ADDR,
2105 client_ptr -> nx_ptp_client_interface_index);
2106 if (status == TX_SUCCESS)
2107 {
2108 client_ptr -> nx_ptp_client_ipv4_group_joined = NX_TRUE;
2109 }
2110 }
2111 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
2112 if (status == TX_SUCCESS)
2113 {
2114 NX_PTP_IPV6_MULTICAST_ADDR_SET(&maddr);
2115 status = nxd_ipv6_multicast_interface_join(client_ptr -> nx_ptp_client_ip_ptr,
2116 &maddr, client_ptr -> nx_ptp_client_interface_index);
2117 if (status == TX_SUCCESS)
2118 {
2119 client_ptr -> nx_ptp_client_ipv6_group_joined = NX_TRUE;
2120 }
2121 else if ((status == NX_NOT_SUPPORTED) && (client_ptr -> nx_ptp_client_ipv4_group_joined))
2122 {
2123
2124 /* IPv6 not enabled, use IPv4 only */
2125 status = TX_SUCCESS;
2126 }
2127 }
2128 #endif
2129
2130 /* activate the Client timer */
2131 if (status == TX_SUCCESS)
2132 {
2133
2134 /* activate the timer */
2135 status = tx_timer_activate(&client_ptr -> nx_ptp_client_timer);
2136 }
2137
2138 /* stop the client thread in case of error */
2139 if (status != TX_SUCCESS)
2140 {
2141 _nx_ptp_client_stop(client_ptr);
2142
2143 /* return failure */
2144 return(status);
2145 }
2146
2147 /* return Success */
2148 return(NX_SUCCESS);
2149 }
2150
2151
2152 /**************************************************************************/
2153 /* */
2154 /* FUNCTION RELEASE */
2155 /* */
2156 /* _nxe_ptp_client_stop PORTABLE C */
2157 /* 6.1.3 */
2158 /* AUTHOR */
2159 /* */
2160 /* Yuxin Zhou, Microsoft Corporation */
2161 /* */
2162 /* DESCRIPTION */
2163 /* */
2164 /* This function checks for errors on the PTP client stop service. */
2165 /* */
2166 /* INPUT */
2167 /* */
2168 /* client_ptr Pointer to PTP client */
2169 /* */
2170 /* OUTPUT */
2171 /* */
2172 /* status Completion status */
2173 /* */
2174 /* CALLS */
2175 /* */
2176 /* _nx_ptp_client_stop Actual stop service */
2177 /* */
2178 /* CALLED BY */
2179 /* */
2180 /* Application */
2181 /* */
2182 /* RELEASE HISTORY */
2183 /* */
2184 /* DATE NAME DESCRIPTION */
2185 /* */
2186 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2187 /* */
2188 /**************************************************************************/
_nxe_ptp_client_stop(NX_PTP_CLIENT * client_ptr)2189 UINT _nxe_ptp_client_stop(NX_PTP_CLIENT *client_ptr)
2190 {
2191
2192 /* Check input parameters. */
2193 if (client_ptr == NX_NULL)
2194 {
2195
2196 /* Return error status. */
2197 return(NX_PTR_ERROR);
2198 }
2199
2200 /* Check for appropriate caller. */
2201 NX_THREADS_ONLY_CALLER_CHECKING
2202
2203 /* Call the actual function. */
2204 return(_nx_ptp_client_stop(client_ptr));
2205 }
2206
2207
2208 /**************************************************************************/
2209 /* */
2210 /* FUNCTION RELEASE */
2211 /* */
2212 /* _nx_ptp_client_stop PORTABLE C */
2213 /* 6.1.3 */
2214 /* AUTHOR */
2215 /* */
2216 /* Yuxin Zhou, Microsoft Corporation */
2217 /* */
2218 /* DESCRIPTION */
2219 /* */
2220 /* This function stops the PTP client. */
2221 /* */
2222 /* INPUT */
2223 /* */
2224 /* client_ptr Pointer to PTP client */
2225 /* */
2226 /* OUTPUT */
2227 /* */
2228 /* status Completion status */
2229 /* */
2230 /* CALLS */
2231 /* */
2232 /* tx_timer_deactivate Deactivate timer */
2233 /* nx_udp_socket_unbind Unbind UDP port */
2234 /* nx_ipv4_multicast_interface_leave Leave IPv4 multicast group */
2235 /* nxd_ipv6_multicast_interface_leave Leave IPv6 multicast group */
2236 /* tx_event_flags_set Set PTP timer event */
2237 /* tx_thread_sleep Thread sleep */
2238 /* */
2239 /* CALLED BY */
2240 /* */
2241 /* Application */
2242 /* */
2243 /* RELEASE HISTORY */
2244 /* */
2245 /* DATE NAME DESCRIPTION */
2246 /* */
2247 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2248 /* */
2249 /**************************************************************************/
_nx_ptp_client_stop(NX_PTP_CLIENT * client_ptr)2250 UINT _nx_ptp_client_stop(NX_PTP_CLIENT *client_ptr)
2251 {
2252 TX_INTERRUPT_SAVE_AREA
2253 UINT state;
2254 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
2255 NXD_ADDRESS maddr;
2256 #endif
2257
2258 /* Check if Client is running */
2259 TX_DISABLE
2260 state = client_ptr -> nx_ptp_client_thread_state;
2261 if (state == NX_PTP_CLIENT_THREAD_RUNNING)
2262 {
2263 client_ptr -> nx_ptp_client_thread_state = NX_PTP_CLIENT_THREAD_STOPPING;
2264 }
2265 TX_RESTORE
2266 if ((state != NX_PTP_CLIENT_THREAD_RUNNING) &&
2267 (state != NX_PTP_CLIENT_THREAD_STOPPED))
2268 {
2269
2270 /* Client is not running */
2271 return(NX_PTP_CLIENT_NOT_STARTED);
2272 }
2273
2274 /* deactivate the timer */
2275 tx_timer_deactivate(&client_ptr -> nx_ptp_client_timer);
2276
2277 /* unbind the sockets */
2278 nx_udp_socket_unbind((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_general_socket);
2279 nx_udp_socket_unbind((NX_UDP_SOCKET *)&client_ptr -> nx_ptp_client_event_socket);
2280
2281 /* leave multicast groups */
2282 if (client_ptr -> nx_ptp_client_ipv4_group_joined)
2283 {
2284 nx_ipv4_multicast_interface_leave(client_ptr -> nx_ptp_client_ip_ptr,
2285 NX_PTP_IPV4_MULTICAST_ADDR,
2286 client_ptr -> nx_ptp_client_interface_index);
2287 client_ptr -> nx_ptp_client_ipv4_group_joined = NX_FALSE;
2288 }
2289 #if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
2290 if (client_ptr -> nx_ptp_client_ipv6_group_joined)
2291 {
2292 NX_PTP_IPV6_MULTICAST_ADDR_SET(&maddr);
2293 nxd_ipv6_multicast_interface_leave(client_ptr -> nx_ptp_client_ip_ptr,
2294 &maddr, client_ptr -> nx_ptp_client_interface_index);
2295 client_ptr -> nx_ptp_client_ipv6_group_joined = NX_FALSE;
2296 }
2297 #endif
2298
2299 /* send STOP message */
2300 tx_event_flags_set(&(client_ptr -> nx_ptp_client_events), NX_PTP_CLIENT_STOP_EVENT, TX_OR);
2301
2302 /* wait for thread termination */
2303 while (state != NX_PTP_CLIENT_THREAD_STOPPED)
2304 {
2305 tx_thread_sleep(1);
2306 TX_DISABLE
2307 state = client_ptr -> nx_ptp_client_thread_state;
2308 TX_RESTORE
2309 }
2310
2311 /* set Idle state */
2312 TX_DISABLE
2313 client_ptr -> nx_ptp_client_thread_state = NX_PTP_CLIENT_THREAD_IDLE;
2314 TX_RESTORE
2315
2316 /* return Success */
2317 return(NX_SUCCESS);
2318 }
2319
2320
2321 /**************************************************************************/
2322 /* */
2323 /* FUNCTION RELEASE */
2324 /* */
2325 /* _nxe_ptp_client_time_get PORTABLE C */
2326 /* 6.1.3 */
2327 /* AUTHOR */
2328 /* */
2329 /* Yuxin Zhou, Microsoft Corporation */
2330 /* */
2331 /* DESCRIPTION */
2332 /* */
2333 /* This function checks for errors on the PTP time get service. */
2334 /* */
2335 /* INPUT */
2336 /* */
2337 /* client_ptr Pointer to PTP client */
2338 /* time_ptr Pointer to PTP time */
2339 /* */
2340 /* OUTPUT */
2341 /* */
2342 /* status Completion status */
2343 /* */
2344 /* CALLS */
2345 /* */
2346 /* _nx_ptp_client_time_get Actual time get service */
2347 /* */
2348 /* CALLED BY */
2349 /* */
2350 /* Application */
2351 /* */
2352 /* RELEASE HISTORY */
2353 /* */
2354 /* DATE NAME DESCRIPTION */
2355 /* */
2356 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2357 /* */
2358 /**************************************************************************/
_nxe_ptp_client_time_get(NX_PTP_CLIENT * client_ptr,NX_PTP_TIME * time_ptr)2359 UINT _nxe_ptp_client_time_get(NX_PTP_CLIENT *client_ptr, NX_PTP_TIME *time_ptr)
2360 {
2361
2362 /* Check input parameters. */
2363 if ((client_ptr == NX_NULL) || (time_ptr == NX_NULL))
2364 {
2365
2366 /* Return error status. */
2367 return(NX_PTR_ERROR);
2368 }
2369
2370 /* Check for appropriate caller. */
2371 NX_THREADS_ONLY_CALLER_CHECKING
2372
2373 /* Call the actual function. */
2374 return(_nx_ptp_client_time_get(client_ptr, time_ptr));
2375 }
2376
2377
2378 /**************************************************************************/
2379 /* */
2380 /* FUNCTION RELEASE */
2381 /* */
2382 /* _nx_ptp_client_time_get PORTABLE C */
2383 /* 6.1.3 */
2384 /* AUTHOR */
2385 /* */
2386 /* Yuxin Zhou, Microsoft Corporation */
2387 /* */
2388 /* DESCRIPTION */
2389 /* */
2390 /* This function gets the current value of the PTP clock. */
2391 /* */
2392 /* INPUT */
2393 /* */
2394 /* client_ptr Pointer to PTP client */
2395 /* time_ptr Pointer to PTP time */
2396 /* */
2397 /* OUTPUT */
2398 /* */
2399 /* status Completion status */
2400 /* */
2401 /* CALLS */
2402 /* */
2403 /* None */
2404 /* */
2405 /* CALLED BY */
2406 /* */
2407 /* Application */
2408 /* */
2409 /* RELEASE HISTORY */
2410 /* */
2411 /* DATE NAME DESCRIPTION */
2412 /* */
2413 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2414 /* */
2415 /**************************************************************************/
_nx_ptp_client_time_get(NX_PTP_CLIENT * client_ptr,NX_PTP_TIME * time_ptr)2416 UINT _nx_ptp_client_time_get(NX_PTP_CLIENT *client_ptr, NX_PTP_TIME *time_ptr)
2417 {
2418
2419 /* Get the current PTP clock */
2420 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_GET,
2421 time_ptr, NX_NULL,
2422 client_ptr -> nx_ptp_client_clock_callback_data);
2423
2424 /* return Success */
2425 return(NX_SUCCESS);
2426 }
2427
2428
2429 /**************************************************************************/
2430 /* */
2431 /* FUNCTION RELEASE */
2432 /* */
2433 /* _nxe_ptp_client_time_set PORTABLE C */
2434 /* 6.1.3 */
2435 /* AUTHOR */
2436 /* */
2437 /* Yuxin Zhou, Microsoft Corporation */
2438 /* */
2439 /* DESCRIPTION */
2440 /* */
2441 /* This function checks for errors on the PTP time set service. */
2442 /* */
2443 /* INPUT */
2444 /* */
2445 /* client_ptr Pointer to PTP client */
2446 /* time_ptr Pointer to PTP time */
2447 /* */
2448 /* OUTPUT */
2449 /* */
2450 /* status Completion status */
2451 /* */
2452 /* CALLS */
2453 /* */
2454 /* _nx_ptp_client_time_set Actual time set service */
2455 /* */
2456 /* CALLED BY */
2457 /* */
2458 /* Application */
2459 /* */
2460 /* RELEASE HISTORY */
2461 /* */
2462 /* DATE NAME DESCRIPTION */
2463 /* */
2464 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2465 /* */
2466 /**************************************************************************/
_nxe_ptp_client_time_set(NX_PTP_CLIENT * client_ptr,NX_PTP_TIME * time_ptr)2467 UINT _nxe_ptp_client_time_set(NX_PTP_CLIENT *client_ptr, NX_PTP_TIME *time_ptr)
2468 {
2469
2470 /* Check input parameters. */
2471 if ((client_ptr == NX_NULL) || (time_ptr == NX_NULL))
2472 {
2473
2474 /* Return error status. */
2475 return(NX_PTR_ERROR);
2476 }
2477
2478 /* Check for appropriate caller. */
2479 NX_THREADS_ONLY_CALLER_CHECKING
2480
2481 /* Call the actual function. */
2482 return(_nx_ptp_client_time_set(client_ptr, time_ptr));
2483 }
2484
2485
2486 /**************************************************************************/
2487 /* */
2488 /* FUNCTION RELEASE */
2489 /* */
2490 /* _nx_ptp_client_time_set PORTABLE C */
2491 /* 6.1.3 */
2492 /* AUTHOR */
2493 /* */
2494 /* Yuxin Zhou, Microsoft Corporation */
2495 /* */
2496 /* DESCRIPTION */
2497 /* */
2498 /* This function sets the initial time of the PTP clock. */
2499 /* */
2500 /* INPUT */
2501 /* */
2502 /* client_ptr Pointer to PTP client */
2503 /* time_ptr Pointer to PTP time */
2504 /* */
2505 /* OUTPUT */
2506 /* */
2507 /* status Completion status */
2508 /* */
2509 /* CALLS */
2510 /* */
2511 /* None */
2512 /* */
2513 /* CALLED BY */
2514 /* */
2515 /* Application */
2516 /* */
2517 /* RELEASE HISTORY */
2518 /* */
2519 /* DATE NAME DESCRIPTION */
2520 /* */
2521 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2522 /* */
2523 /**************************************************************************/
_nx_ptp_client_time_set(NX_PTP_CLIENT * client_ptr,NX_PTP_TIME * time_ptr)2524 UINT _nx_ptp_client_time_set(NX_PTP_CLIENT *client_ptr, NX_PTP_TIME *time_ptr)
2525 {
2526 TX_INTERRUPT_SAVE_AREA
2527 UINT state;
2528
2529 /* Check if Client is running */
2530 TX_DISABLE
2531 state = client_ptr -> nx_ptp_client_thread_state;
2532 TX_RESTORE
2533
2534 if (state != NX_PTP_CLIENT_THREAD_IDLE)
2535 {
2536
2537 /* Cannot set the clock when the client is running */
2538 return(NX_PTP_CLIENT_ALREADY_STARTED);
2539 }
2540
2541 /* Set the current PTP clock */
2542 client_ptr -> nx_ptp_client_clock_callback(client_ptr, NX_PTP_CLIENT_CLOCK_SET,
2543 time_ptr, NX_NULL,
2544 client_ptr -> nx_ptp_client_clock_callback_data);
2545
2546 /* return Success */
2547 return(NX_SUCCESS);
2548 }
2549
2550
2551 /**************************************************************************/
2552 /* */
2553 /* FUNCTION RELEASE */
2554 /* */
2555 /* _nxe_ptp_client_master_info_get PORTABLE C */
2556 /* 6.1.3 */
2557 /* AUTHOR */
2558 /* */
2559 /* Yuxin Zhou, Microsoft Corporation */
2560 /* */
2561 /* DESCRIPTION */
2562 /* */
2563 /* This function checks for errors on the PTP master info get service. */
2564 /* */
2565 /* INPUT */
2566 /* */
2567 /* master_ptr Pointer to PTP master clock */
2568 /* address Address of master clock */
2569 /* port_identity PTP master port and identity */
2570 /* port_identity_length Length of PTP master port and */
2571 /* identity */
2572 /* priority1 Priority1 of PTP master clock */
2573 /* priority2 Priority2 of PTP master clock */
2574 /* clock_class Class of PTP master clock */
2575 /* clock_accuracy Accuracy of PTP master clock */
2576 /* clock_variance Variance of PTP master clock */
2577 /* grandmaster_identity Identity of grandmaster clock */
2578 /* grandmaster_identity_length Length of grandmaster Identity*/
2579 /* steps_removed Steps removed from PTP header */
2580 /* time_source The source of timer used by */
2581 /* grandmaster clock */
2582 /* */
2583 /* OUTPUT */
2584 /* */
2585 /* status Completion status */
2586 /* */
2587 /* CALLS */
2588 /* */
2589 /* _nx_ptp_client_master_info_get Actual master info get service*/
2590 /* */
2591 /* CALLED BY */
2592 /* */
2593 /* Application */
2594 /* */
2595 /* RELEASE HISTORY */
2596 /* */
2597 /* DATE NAME DESCRIPTION */
2598 /* */
2599 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2600 /* */
2601 /**************************************************************************/
_nxe_ptp_client_master_info_get(NX_PTP_CLIENT_MASTER * master_ptr,NXD_ADDRESS * address,UCHAR ** port_identity,UINT * port_identity_length,UCHAR * priority1,UCHAR * priority2,UCHAR * clock_class,UCHAR * clock_accuracy,USHORT * clock_variance,UCHAR ** grandmaster_identity,UINT * grandmaster_identity_length,USHORT * steps_removed,UCHAR * time_source)2602 UINT _nxe_ptp_client_master_info_get(NX_PTP_CLIENT_MASTER *master_ptr, NXD_ADDRESS *address, UCHAR **port_identity,
2603 UINT *port_identity_length, UCHAR *priority1, UCHAR *priority2, UCHAR *clock_class,
2604 UCHAR *clock_accuracy, USHORT *clock_variance, UCHAR **grandmaster_identity,
2605 UINT *grandmaster_identity_length, USHORT *steps_removed, UCHAR *time_source)
2606 {
2607 if (master_ptr == NX_NULL)
2608 {
2609
2610 /* Return error status. */
2611 return(NX_PTR_ERROR);
2612 }
2613
2614 /* Check for appropriate caller. */
2615 NX_THREADS_ONLY_CALLER_CHECKING
2616
2617 return(_nx_ptp_client_master_info_get(master_ptr, address, port_identity, port_identity_length, priority1,
2618 priority2, clock_class, clock_accuracy, clock_variance, grandmaster_identity,
2619 grandmaster_identity_length, steps_removed, time_source));
2620 }
2621
2622
2623 /**************************************************************************/
2624 /* */
2625 /* FUNCTION RELEASE */
2626 /* */
2627 /* _nx_ptp_client_master_info_get PORTABLE C */
2628 /* 6.1.3 */
2629 /* AUTHOR */
2630 /* */
2631 /* Yuxin Zhou, Microsoft Corporation */
2632 /* */
2633 /* DESCRIPTION */
2634 /* */
2635 /* This function gets information of master clock. */
2636 /* */
2637 /* INPUT */
2638 /* */
2639 /* master_ptr Pointer to PTP master clock */
2640 /* address Address of master clock */
2641 /* port_identity PTP master port and identity */
2642 /* port_identity_length Length of PTP master port and */
2643 /* identity */
2644 /* priority1 Priority1 of PTP master clock */
2645 /* priority2 Priority2 of PTP master clock */
2646 /* clock_class Class of PTP master clock */
2647 /* clock_accuracy Accuracy of PTP master clock */
2648 /* clock_variance Variance of PTP master clock */
2649 /* grandmaster_identity Identity of grandmaster clock */
2650 /* grandmaster_identity_length Length of grandmaster Identity*/
2651 /* steps_removed Steps removed from PTP header */
2652 /* time_source The source of timer used by */
2653 /* grandmaster clock */
2654 /* */
2655 /* OUTPUT */
2656 /* */
2657 /* status Completion status */
2658 /* */
2659 /* CALLS */
2660 /* */
2661 /* None */
2662 /* */
2663 /* CALLED BY */
2664 /* */
2665 /* Application */
2666 /* */
2667 /* RELEASE HISTORY */
2668 /* */
2669 /* DATE NAME DESCRIPTION */
2670 /* */
2671 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2672 /* */
2673 /**************************************************************************/
_nx_ptp_client_master_info_get(NX_PTP_CLIENT_MASTER * master_ptr,NXD_ADDRESS * address,UCHAR ** port_identity,UINT * port_identity_length,UCHAR * priority1,UCHAR * priority2,UCHAR * clock_class,UCHAR * clock_accuracy,USHORT * clock_variance,UCHAR ** grandmaster_identity,UINT * grandmaster_identity_length,USHORT * steps_removed,UCHAR * time_source)2674 UINT _nx_ptp_client_master_info_get(NX_PTP_CLIENT_MASTER *master_ptr, NXD_ADDRESS *address, UCHAR **port_identity,
2675 UINT *port_identity_length, UCHAR *priority1, UCHAR *priority2, UCHAR *clock_class,
2676 UCHAR *clock_accuracy, USHORT *clock_variance, UCHAR **grandmaster_identity,
2677 UINT *grandmaster_identity_length, USHORT *steps_removed, UCHAR *time_source)
2678 {
2679
2680 /* Set master information when the output pointer is provided. */
2681 if (address)
2682 {
2683 *address = *(master_ptr -> nx_ptp_client_master_address);
2684 }
2685
2686 if (port_identity && port_identity_length)
2687 {
2688 *port_identity = master_ptr -> nx_ptp_client_master_port_identity;
2689 *port_identity_length = NX_PTP_CLOCK_PORT_IDENTITY_SIZE;
2690 }
2691
2692 if (priority1)
2693 {
2694 *priority1 = master_ptr -> nx_ptp_client_master_priority1;
2695 }
2696
2697 if (priority2)
2698 {
2699 *priority2 = master_ptr -> nx_ptp_client_master_priority2;
2700 }
2701
2702 if (clock_class)
2703 {
2704 *clock_class = master_ptr -> nx_ptp_client_master_clock_class;
2705 }
2706
2707 if (clock_accuracy)
2708 {
2709 *clock_accuracy = master_ptr -> nx_ptp_client_master_clock_accuracy;
2710 }
2711
2712 if (clock_variance)
2713 {
2714 *clock_variance = master_ptr -> nx_ptp_client_master_offset_scaled_log_variance;
2715 }
2716
2717 if (grandmaster_identity && grandmaster_identity_length)
2718 {
2719 *grandmaster_identity = master_ptr -> nx_ptp_client_master_grandmaster_identity;
2720 *grandmaster_identity_length = NX_PTP_CLOCK_IDENTITY_SIZE;
2721 }
2722
2723 if (steps_removed)
2724 {
2725 *steps_removed = master_ptr -> nx_ptp_client_master_steps_removed;
2726 }
2727
2728 if (time_source)
2729 {
2730 *time_source = master_ptr -> nx_ptp_client_master_time_source;
2731 }
2732
2733 return(NX_SUCCESS);
2734 }
2735
2736
2737 /**************************************************************************/
2738 /* */
2739 /* FUNCTION RELEASE */
2740 /* */
2741 /* _nxe_ptp_client_sync_info_get PORTABLE C */
2742 /* 6.1.3 */
2743 /* AUTHOR */
2744 /* */
2745 /* Yuxin Zhou, Microsoft Corporation */
2746 /* */
2747 /* DESCRIPTION */
2748 /* */
2749 /* This function checks for errors on the PTP Sync get service. */
2750 /* */
2751 /* INPUT */
2752 /* */
2753 /* client_ptr Pointer to PTP client */
2754 /* flags Flags in Sync message */
2755 /* utc_offset Offset between TAI and UTC */
2756 /* */
2757 /* OUTPUT */
2758 /* */
2759 /* status Completion status */
2760 /* */
2761 /* CALLS */
2762 /* */
2763 /* _nx_ptp_client_sync_info_get Actual Sync info get service */
2764 /* */
2765 /* CALLED BY */
2766 /* */
2767 /* Application */
2768 /* */
2769 /* RELEASE HISTORY */
2770 /* */
2771 /* DATE NAME DESCRIPTION */
2772 /* */
2773 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2774 /* */
2775 /**************************************************************************/
_nxe_ptp_client_sync_info_get(NX_PTP_CLIENT_SYNC * sync_ptr,USHORT * flags,SHORT * utc_offset)2776 UINT _nxe_ptp_client_sync_info_get(NX_PTP_CLIENT_SYNC *sync_ptr, USHORT *flags, SHORT *utc_offset)
2777 {
2778 if (sync_ptr == NX_NULL)
2779 {
2780
2781 /* Return error status. */
2782 return(NX_PTR_ERROR);
2783 }
2784
2785 /* Check for appropriate caller. */
2786 NX_THREADS_ONLY_CALLER_CHECKING
2787
2788 return(_nx_ptp_client_sync_info_get(sync_ptr, flags, utc_offset));
2789 }
2790
2791
2792 /**************************************************************************/
2793 /* */
2794 /* FUNCTION RELEASE */
2795 /* */
2796 /* _nx_ptp_client_sync_info_get PORTABLE C */
2797 /* 6.1.3 */
2798 /* AUTHOR */
2799 /* */
2800 /* Yuxin Zhou, Microsoft Corporation */
2801 /* */
2802 /* DESCRIPTION */
2803 /* */
2804 /* This function gets information of Sync message. */
2805 /* */
2806 /* INPUT */
2807 /* */
2808 /* client_ptr Pointer to PTP client */
2809 /* flags Flags in Sync message */
2810 /* utc_offset Offset between TAI and UTC */
2811 /* */
2812 /* OUTPUT */
2813 /* */
2814 /* status Completion status */
2815 /* */
2816 /* CALLS */
2817 /* */
2818 /* None */
2819 /* */
2820 /* CALLED BY */
2821 /* */
2822 /* Application */
2823 /* */
2824 /* RELEASE HISTORY */
2825 /* */
2826 /* DATE NAME DESCRIPTION */
2827 /* */
2828 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2829 /* */
2830 /**************************************************************************/
_nx_ptp_client_sync_info_get(NX_PTP_CLIENT_SYNC * sync_ptr,USHORT * flags,SHORT * utc_offset)2831 UINT _nx_ptp_client_sync_info_get(NX_PTP_CLIENT_SYNC *sync_ptr, USHORT *flags, SHORT *utc_offset)
2832 {
2833
2834 /* Set SYNC information when the output pointer is provided. */
2835 if (flags)
2836 {
2837 *flags = sync_ptr -> nx_ptp_client_sync_flags;
2838 }
2839
2840 if (utc_offset)
2841 {
2842 *utc_offset = sync_ptr -> nx_ptp_client_sync_utc_offset;
2843 }
2844
2845 return(NX_SUCCESS);
2846 }
2847
2848
2849 /**************************************************************************/
2850 /* */
2851 /* FUNCTION RELEASE */
2852 /* */
2853 /* _nx_ptp_client_packet_timestamp_notify PORTABLE C */
2854 /* 6.1.3 */
2855 /* AUTHOR */
2856 /* */
2857 /* Yuxin Zhou, Microsoft Corporation */
2858 /* */
2859 /* DESCRIPTION */
2860 /* */
2861 /* This function notifies the PTP packet is transmitted with timestamp.*/
2862 /* */
2863 /* INPUT */
2864 /* */
2865 /* client_ptr Pointer to PTP client */
2866 /* packet_ptr Pointer to PTP packet */
2867 /* timestamp_ptr Pointer to timestamp */
2868 /* */
2869 /* OUTPUT */
2870 /* */
2871 /* None */
2872 /* */
2873 /* CALLS */
2874 /* */
2875 /* None */
2876 /* */
2877 /* CALLED BY */
2878 /* */
2879 /* Application */
2880 /* */
2881 /* RELEASE HISTORY */
2882 /* */
2883 /* DATE NAME DESCRIPTION */
2884 /* */
2885 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2886 /* */
2887 /**************************************************************************/
_nx_ptp_client_packet_timestamp_notify(NX_PTP_CLIENT * client_ptr,NX_PACKET * packet_ptr,NX_PTP_TIME * timestamp_ptr)2888 VOID _nx_ptp_client_packet_timestamp_notify(NX_PTP_CLIENT *client_ptr, NX_PACKET *packet_ptr, NX_PTP_TIME *timestamp_ptr)
2889 {
2890
2891 /* get timestamp of previous delay_req message */
2892 if (client_ptr &&
2893 (client_ptr -> nx_ptp_client_delay_state == NX_PTP_CLIENT_DELAY_WAIT_REQ_TS) &&
2894 (client_ptr -> nx_ptp_client_delay_req_packet_ptr == packet_ptr))
2895 {
2896
2897 /* store timestamp */
2898 client_ptr -> nx_ptp_client_delay_ts = *timestamp_ptr;
2899
2900 /* update state */
2901 client_ptr -> nx_ptp_client_delay_state = NX_PTP_CLIENT_DELAY_WAIT_RESP;
2902 }
2903 }
2904
2905
2906 /**************************************************************************/
2907 /* */
2908 /* FUNCTION RELEASE */
2909 /* */
2910 /* _nx_ptp_client_soft_clock_callback PORTABLE C */
2911 /* 6.1.3 */
2912 /* AUTHOR */
2913 /* */
2914 /* Yuxin Zhou, Microsoft Corporation */
2915 /* */
2916 /* DESCRIPTION */
2917 /* */
2918 /* This function implements soft PTP clock. */
2919 /* */
2920 /* INPUT */
2921 /* */
2922 /* client_ptr Pointer to PTP client */
2923 /* operation PTP clock operation */
2924 /* time_ptr Pointer to timestamp */
2925 /* packet_ptr Pointer to PTP packet */
2926 /* callback_data Pointer to callback data */
2927 /* */
2928 /* OUTPUT */
2929 /* */
2930 /* status Completion status */
2931 /* */
2932 /* CALLS */
2933 /* */
2934 /* _nx_ptp_client_soft_clock_adjust Adjust soft PTP clock */
2935 /* _nx_ptp_client_utility_inc64 Increment a 64-bit number */
2936 /* */
2937 /* CALLED BY */
2938 /* */
2939 /* PTP internal */
2940 /* */
2941 /* RELEASE HISTORY */
2942 /* */
2943 /* DATE NAME DESCRIPTION */
2944 /* */
2945 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
2946 /* */
2947 /**************************************************************************/
_nx_ptp_client_soft_clock_callback(NX_PTP_CLIENT * client_ptr,UINT operation,NX_PTP_TIME * time_ptr,NX_PACKET * packet_ptr,VOID * callback_data)2948 UINT _nx_ptp_client_soft_clock_callback(NX_PTP_CLIENT *client_ptr, UINT operation,
2949 NX_PTP_TIME *time_ptr, NX_PACKET *packet_ptr,
2950 VOID *callback_data)
2951 {
2952 TX_INTERRUPT_SAVE_AREA
2953
2954 NX_PARAMETER_NOT_USED(callback_data);
2955
2956 switch (operation)
2957 {
2958
2959 /* Nothing to do for soft initialization. */
2960 case NX_PTP_CLIENT_CLOCK_INIT:
2961 break;
2962
2963 /* Set clock. */
2964 case NX_PTP_CLIENT_CLOCK_SET:
2965 TX_DISABLE
2966 client_ptr -> nx_ptp_client_soft_clock = *time_ptr;
2967 TX_RESTORE
2968 break;
2969
2970 /* Extract timestamp from packet.
2971 For soft implementation, simply fallthrough and return current timestamp. */
2972 case NX_PTP_CLIENT_CLOCK_PACKET_TS_EXTRACT:
2973
2974 /* Get clock. */
2975 case NX_PTP_CLIENT_CLOCK_GET:
2976 TX_DISABLE
2977 *time_ptr = client_ptr -> nx_ptp_client_soft_clock;
2978 TX_RESTORE
2979 break;
2980
2981 /* Adjust clock. */
2982 case NX_PTP_CLIENT_CLOCK_ADJUST:
2983 _nx_ptp_client_soft_clock_adjust(client_ptr, time_ptr -> nanosecond);
2984 break;
2985
2986 /* Prepare timestamp for current packet.
2987 For soft implementation, simply notify current timestamp. */
2988 case NX_PTP_CLIENT_CLOCK_PACKET_TS_PREPARE:
2989 _nx_ptp_client_packet_timestamp_notify(client_ptr, packet_ptr, &(client_ptr -> nx_ptp_client_soft_clock));
2990 break;
2991
2992 /* Update soft timer. */
2993 case NX_PTP_CLIENT_CLOCK_SOFT_TIMER_UPDATE:
2994 TX_DISABLE
2995
2996 /* increment the nanosecond field of the software clock */
2997 time_ptr -> nanosecond +=
2998 (LONG)(NX_PTP_NANOSECONDS_PER_SEC / NX_PTP_CLIENT_TIMER_TICKS_PER_SECOND);
2999
3000 /* update the second field */
3001 if (time_ptr -> nanosecond >= NX_PTP_NANOSECONDS_PER_SEC)
3002 {
3003 time_ptr -> nanosecond -= NX_PTP_NANOSECONDS_PER_SEC;
3004 _nx_ptp_client_utility_inc64(&(time_ptr -> second_high),
3005 &(time_ptr -> second_low));
3006 }
3007 TX_RESTORE
3008 break;
3009
3010 default:
3011 return(NX_PTP_PARAM_ERROR);
3012 }
3013
3014 return(NX_SUCCESS);
3015 }
3016
3017
3018 /**************************************************************************/
3019 /* */
3020 /* FUNCTION RELEASE */
3021 /* */
3022 /* _nxe_ptp_client_utility_time_diff PORTABLE C */
3023 /* 6.1.3 */
3024 /* AUTHOR */
3025 /* */
3026 /* Yuxin Zhou, Microsoft Corporation */
3027 /* */
3028 /* DESCRIPTION */
3029 /* */
3030 /* This function checks for errors on the PTP time difference service. */
3031 /* */
3032 /* INPUT */
3033 /* */
3034 /* time1_ptr Pointer to first PTP time */
3035 /* time2_ptr Pointer to second PTP time */
3036 /* result_ptr Pointer to result time1-time2 */
3037 /* */
3038 /* OUTPUT */
3039 /* */
3040 /* status Completion status */
3041 /* */
3042 /* CALLS */
3043 /* */
3044 /* _nx_ptp_client_utility_time_diff Actual time difference service*/
3045 /* */
3046 /* CALLED BY */
3047 /* */
3048 /* Application */
3049 /* */
3050 /* RELEASE HISTORY */
3051 /* */
3052 /* DATE NAME DESCRIPTION */
3053 /* */
3054 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3055 /* */
3056 /**************************************************************************/
_nxe_ptp_client_utility_time_diff(NX_PTP_TIME * time1_ptr,NX_PTP_TIME * time2_ptr,NX_PTP_TIME * result_ptr)3057 UINT _nxe_ptp_client_utility_time_diff(NX_PTP_TIME *time1_ptr, NX_PTP_TIME *time2_ptr, NX_PTP_TIME *result_ptr)
3058 {
3059
3060 /* Check input parameters. */
3061 if ((time1_ptr == NX_NULL) || (time2_ptr == NX_NULL) || (result_ptr == NX_NULL))
3062 {
3063
3064 /* Return error status. */
3065 return(NX_PTR_ERROR);
3066 }
3067
3068 /* Check for appropriate caller. */
3069 NX_THREADS_ONLY_CALLER_CHECKING
3070
3071 /* Call the actual function. */
3072 return(_nx_ptp_client_utility_time_diff(time1_ptr, time2_ptr, result_ptr));
3073 }
3074
3075
3076 /**************************************************************************/
3077 /* */
3078 /* FUNCTION RELEASE */
3079 /* */
3080 /* _nx_ptp_client_utility_time_diff PORTABLE C */
3081 /* 6.1.3 */
3082 /* AUTHOR */
3083 /* */
3084 /* Yuxin Zhou, Microsoft Corporation */
3085 /* */
3086 /* DESCRIPTION */
3087 /* */
3088 /* This function computes the difference between two PTP times. */
3089 /* */
3090 /* INPUT */
3091 /* */
3092 /* time1_ptr Pointer to first PTP time */
3093 /* time2_ptr Pointer to second PTP time */
3094 /* result_ptr Pointer to result time1-time2 */
3095 /* */
3096 /* OUTPUT */
3097 /* */
3098 /* status Completion status */
3099 /* */
3100 /* CALLS */
3101 /* */
3102 /* _nx_ptp_client_utility_sub64 Subtracts two 64-bit numbers */
3103 /* _nx_ptp_client_utility_dec64 Decrement a 64-bit number */
3104 /* _nx_ptp_client_utility_inc64 Increment a 64-bit number */
3105 /* */
3106 /* CALLED BY */
3107 /* */
3108 /* Application */
3109 /* _nx_ptp_client_delay_resp_received Process delay response */
3110 /* */
3111 /* RELEASE HISTORY */
3112 /* */
3113 /* DATE NAME DESCRIPTION */
3114 /* */
3115 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3116 /* */
3117 /**************************************************************************/
_nx_ptp_client_utility_time_diff(NX_PTP_TIME * time1_ptr,NX_PTP_TIME * time2_ptr,NX_PTP_TIME * result_ptr)3118 UINT _nx_ptp_client_utility_time_diff(NX_PTP_TIME *time1_ptr, NX_PTP_TIME *time2_ptr, NX_PTP_TIME *result_ptr)
3119 {
3120 LONG sec_hi;
3121 ULONG sec_lo;
3122 LONG ns;
3123
3124 /* compute difference of seconds */
3125 sec_hi = time1_ptr -> second_high;
3126 sec_lo = time1_ptr -> second_low;
3127 _nx_ptp_client_utility_sub64(&sec_hi, &sec_lo, time2_ptr -> second_high,
3128 time2_ptr -> second_low);
3129
3130 /* compute difference of nanoseconds */
3131 /* note: this cannot overflow as nanosecond field is in range +/-0-999999999 */
3132 ns = time1_ptr -> nanosecond - time2_ptr -> nanosecond;
3133
3134 /* keep nanoseconds in range +/-0-999999999 */
3135 if (ns <= -NX_PTP_NANOSECONDS_PER_SEC)
3136 {
3137 ns += NX_PTP_NANOSECONDS_PER_SEC;
3138 _nx_ptp_client_utility_dec64(&sec_hi, &sec_lo);
3139 }
3140 else if (ns >= NX_PTP_NANOSECONDS_PER_SEC)
3141 {
3142 ns -= NX_PTP_NANOSECONDS_PER_SEC;
3143 _nx_ptp_client_utility_inc64(&sec_hi, &sec_lo);
3144 }
3145
3146 /* ensure the nanoseconds field has same sign as seconds field */
3147 if ((sec_hi >= 0) && ((sec_hi != 0) || (sec_lo != 0)))
3148 {
3149 /* positive number of seconds */
3150 if (ns < 0)
3151 {
3152 ns += NX_PTP_NANOSECONDS_PER_SEC;
3153 _nx_ptp_client_utility_dec64(&sec_hi, &sec_lo);
3154 }
3155 }
3156 else if (sec_hi < 0)
3157 {
3158 /* negative number of seconds */
3159 if (ns > 0)
3160 {
3161 ns -= NX_PTP_NANOSECONDS_PER_SEC;
3162 _nx_ptp_client_utility_inc64(&sec_hi, &sec_lo);
3163 }
3164 }
3165
3166 /* return result time */
3167 result_ptr -> second_high = sec_hi;
3168 result_ptr -> second_low = sec_lo;
3169 result_ptr -> nanosecond = ns;
3170
3171 return(NX_SUCCESS);
3172 }
3173
3174
3175 /**************************************************************************/
3176 /* */
3177 /* FUNCTION RELEASE */
3178 /* */
3179 /* _nxe_ptp_client_utility_convert_time_to_date PORTABLE C */
3180 /* 6.1.3 */
3181 /* AUTHOR */
3182 /* */
3183 /* Yuxin Zhou, Microsoft Corporation */
3184 /* */
3185 /* DESCRIPTION */
3186 /* */
3187 /* This function checks for errors on the PTP time conversion service. */
3188 /* */
3189 /* INPUT */
3190 /* */
3191 /* time_ptr Pointer to PTP time */
3192 /* offset signed second offset to add */
3193 /* the PTP time */
3194 /* date_time_ptr Pointer to resulting date */
3195 /* */
3196 /* OUTPUT */
3197 /* */
3198 /* status Completion status */
3199 /* */
3200 /* CALLS */
3201 /* */
3202 /* _nx_ptp_client_utility_convert_time_to_date */
3203 /* Actual time conversion service*/
3204 /* */
3205 /* CALLED BY */
3206 /* */
3207 /* Application */
3208 /* */
3209 /* RELEASE HISTORY */
3210 /* */
3211 /* DATE NAME DESCRIPTION */
3212 /* */
3213 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3214 /* */
3215 /**************************************************************************/
_nxe_ptp_client_utility_convert_time_to_date(NX_PTP_TIME * time_ptr,LONG offset,NX_PTP_DATE_TIME * date_time_ptr)3216 UINT _nxe_ptp_client_utility_convert_time_to_date(NX_PTP_TIME *time_ptr, LONG offset, NX_PTP_DATE_TIME *date_time_ptr)
3217 {
3218
3219 /* Check input parameters. */
3220 if ((time_ptr == NX_NULL) || (date_time_ptr == NX_NULL))
3221 {
3222
3223 /* Return error status. */
3224 return(NX_PTR_ERROR);
3225 }
3226
3227 /* Check for appropriate caller. */
3228 NX_THREADS_ONLY_CALLER_CHECKING
3229
3230 /* Call the actual function. */
3231 return(_nx_ptp_client_utility_convert_time_to_date(time_ptr, offset, date_time_ptr));
3232 }
3233
3234
3235 /**************************************************************************/
3236 /* */
3237 /* FUNCTION RELEASE */
3238 /* */
3239 /* _nx_ptp_client_utility_convert_time_to_date PORTABLE C */
3240 /* 6.1.3 */
3241 /* AUTHOR */
3242 /* */
3243 /* Yuxin Zhou, Microsoft Corporation */
3244 /* */
3245 /* DESCRIPTION */
3246 /* */
3247 /* This function converts a PTP time to a UTC date and time. */
3248 /* */
3249 /* INPUT */
3250 /* */
3251 /* time_ptr Pointer to PTP time */
3252 /* offset Signed second offset to add */
3253 /* the PTP time */
3254 /* date_time_ptr Pointer to resulting date */
3255 /* */
3256 /* OUTPUT */
3257 /* */
3258 /* status Completion status */
3259 /* */
3260 /* CALLS */
3261 /* */
3262 /* _nx_ptp_client_utility_add64 Add two 64-bit number */
3263 /* */
3264 /* CALLED BY */
3265 /* */
3266 /* Application */
3267 /* */
3268 /* RELEASE HISTORY */
3269 /* */
3270 /* DATE NAME DESCRIPTION */
3271 /* */
3272 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3273 /* */
3274 /**************************************************************************/
_nx_ptp_client_utility_convert_time_to_date(NX_PTP_TIME * time_ptr,LONG offset,NX_PTP_DATE_TIME * date_time_ptr)3275 UINT _nx_ptp_client_utility_convert_time_to_date(NX_PTP_TIME *time_ptr, LONG offset, NX_PTP_DATE_TIME *date_time_ptr)
3276 {
3277 #define IS_LEAP(y) (((y) % 4) == 0 && (((y) % 100) != 0 || ((y) % 400) == 0))
3278 #define SECS_PER_MINUTE 60
3279 #define SECS_PER_HOUR (60 * SECS_PER_MINUTE)
3280 #define SECS_PER_DAY (24 * SECS_PER_HOUR)
3281 LONG secs_high;
3282 ULONG secs_low;
3283 UINT year, month, day, hour, minute;
3284 ULONG secs_per_year, secs_per_month;
3285 ULONG weekday;
3286 UINT is_leap;
3287
3288 /* get number of seconds */
3289 secs_high = time_ptr -> second_high;
3290 secs_low = time_ptr -> second_low;
3291
3292 /* add local time offset */
3293 if (offset != 0)
3294 {
3295 _nx_ptp_client_utility_add64(&secs_high, &secs_low, offset < 0 ? -1 : 0, (ULONG)offset);
3296 }
3297 if (secs_high < 0)
3298 {
3299
3300 /* invalid negative time */
3301 return(NX_PTP_PARAM_ERROR);
3302 }
3303
3304 /* determine the current year from Epoch (January 1, 1970) */
3305 year = 1970;
3306 secs_per_year = 365 * SECS_PER_DAY;
3307 is_leap = 0;
3308 weekday = 4; /* thursday */
3309 while ((secs_high > 0) || (secs_low >= secs_per_year))
3310 {
3311 if (secs_low < secs_per_year)
3312 {
3313 secs_high--;
3314 }
3315 secs_low -= secs_per_year;
3316 weekday += is_leap ? 366 : 365;
3317 year++;
3318 is_leap = IS_LEAP(year) ? 1 : 0;
3319 secs_per_year = is_leap ? 366 * SECS_PER_DAY : 365 * SECS_PER_DAY;
3320 }
3321 /* compute day of the week from remaining seconds */
3322 weekday = (weekday + secs_low / SECS_PER_DAY) % 7;
3323
3324 /* determine current month */
3325 month = 1;
3326 secs_per_month = 31 * SECS_PER_DAY;
3327 while (secs_low >= secs_per_month)
3328 {
3329 secs_low -= secs_per_month;
3330 month++;
3331 if (month == 2)
3332 {
3333
3334 /* february */
3335 secs_per_month = is_leap ? 29 * SECS_PER_DAY : 28 * SECS_PER_DAY;
3336 }
3337 else if ((month == 4) || (month == 6) || (month == 9) || (month == 11))
3338 {
3339
3340 /* april, june, september, november */
3341 secs_per_month = 30 * SECS_PER_DAY;
3342 }
3343 else
3344 {
3345
3346 /* the other months */
3347 secs_per_month = 31 * SECS_PER_DAY;
3348 }
3349 }
3350
3351 /* determine current day of the month */
3352 day = secs_low / SECS_PER_DAY;
3353 secs_low -= day * SECS_PER_DAY;
3354
3355 /* determine current hour */
3356 hour = secs_low / SECS_PER_HOUR;
3357 secs_low -= hour * SECS_PER_HOUR;
3358
3359 /* determine current minute */
3360 minute = secs_low / SECS_PER_MINUTE;
3361 secs_low -= minute * SECS_PER_MINUTE;
3362
3363 /* return date */
3364 date_time_ptr -> year = year;
3365 date_time_ptr -> month = (UCHAR)month;
3366 date_time_ptr -> day = (UCHAR)(day + 1);
3367 date_time_ptr -> hour = (UCHAR)hour;
3368 date_time_ptr -> minute = (UCHAR)minute;
3369 date_time_ptr -> second = (UCHAR)secs_low;
3370 date_time_ptr -> weekday = (UCHAR)weekday;
3371 date_time_ptr -> nanosecond = (ULONG)time_ptr -> nanosecond;
3372
3373 return(NX_SUCCESS);
3374 }
3375
3376
3377 /**************************************************************************/
3378 /* */
3379 /* FUNCTION RELEASE */
3380 /* */
3381 /* _nx_ptp_client_utility_add64 PORTABLE C */
3382 /* 6.1.3 */
3383 /* AUTHOR */
3384 /* */
3385 /* Yuxin Zhou, Microsoft Corporation */
3386 /* */
3387 /* DESCRIPTION */
3388 /* */
3389 /* This function adds two 64-bit numbers: A = A + B. */
3390 /* */
3391 /* INPUT */
3392 /* */
3393 /* a_hi Pointer to higher 32-bit of A */
3394 /* a_lo Pointer to lower 32-bit of A */
3395 /* b_hi higher 32-bit of B */
3396 /* b_lo lower 32-bit of B */
3397 /* */
3398 /* OUTPUT */
3399 /* */
3400 /* None */
3401 /* */
3402 /* CALLS */
3403 /* */
3404 /* None */
3405 /* */
3406 /* CALLED BY */
3407 /* */
3408 /* _nx_ptp_client_clock_adjust Adjust PTP clock */
3409 /* _nx_ptp_client_utility_convert_time_to_date */
3410 /* Convert time to date */
3411 /* */
3412 /* RELEASE HISTORY */
3413 /* */
3414 /* DATE NAME DESCRIPTION */
3415 /* */
3416 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3417 /* */
3418 /**************************************************************************/
_nx_ptp_client_utility_add64(LONG * a_hi,ULONG * a_lo,LONG b_hi,ULONG b_lo)3419 VOID _nx_ptp_client_utility_add64(LONG *a_hi, ULONG *a_lo, LONG b_hi, ULONG b_lo)
3420 {
3421 LONG r_hi;
3422 ULONG r_lo;
3423
3424 r_hi = *a_hi + b_hi;
3425 r_lo = *a_lo + b_lo;
3426 if (r_lo < *a_lo)
3427 {
3428 r_hi++; /* add carry */
3429 }
3430 *a_hi = r_hi;
3431 *a_lo = r_lo;
3432 }
3433
3434
3435 /**************************************************************************/
3436 /* */
3437 /* FUNCTION RELEASE */
3438 /* */
3439 /* _nx_ptp_client_utility_sub64 PORTABLE C */
3440 /* 6.1.3 */
3441 /* AUTHOR */
3442 /* */
3443 /* Yuxin Zhou, Microsoft Corporation */
3444 /* */
3445 /* DESCRIPTION */
3446 /* */
3447 /* This function substracts two 64-bit numbers: A = A - B. */
3448 /* */
3449 /* INPUT */
3450 /* */
3451 /* a_hi Pointer to higher 32-bit of A */
3452 /* a_lo Pointer to lower 32-bit of A */
3453 /* b_hi higher 32-bit of B */
3454 /* b_lo lower 32-bit of B */
3455 /* */
3456 /* OUTPUT */
3457 /* */
3458 /* None */
3459 /* */
3460 /* CALLS */
3461 /* */
3462 /* None */
3463 /* */
3464 /* CALLED BY */
3465 /* */
3466 /* _nx_ptp_client_utility_time_diff Diff two PTP times */
3467 /* */
3468 /* RELEASE HISTORY */
3469 /* */
3470 /* DATE NAME DESCRIPTION */
3471 /* */
3472 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3473 /* */
3474 /**************************************************************************/
_nx_ptp_client_utility_sub64(LONG * a_hi,ULONG * a_lo,LONG b_hi,ULONG b_lo)3475 VOID _nx_ptp_client_utility_sub64(LONG *a_hi, ULONG *a_lo, LONG b_hi, ULONG b_lo)
3476 {
3477 LONG r_hi;
3478 ULONG r_lo;
3479
3480 r_hi = *a_hi - b_hi;
3481 r_lo = *a_lo - b_lo;
3482 if (*a_lo < b_lo)
3483 {
3484 r_hi--; /* substract carry */
3485 }
3486 *a_hi = r_hi;
3487 *a_lo = r_lo;
3488 }
3489
3490
3491 /**************************************************************************/
3492 /* */
3493 /* FUNCTION RELEASE */
3494 /* */
3495 /* _nx_ptp_client_utility_inc64 PORTABLE C */
3496 /* 6.1.3 */
3497 /* AUTHOR */
3498 /* */
3499 /* Yuxin Zhou, Microsoft Corporation */
3500 /* */
3501 /* DESCRIPTION */
3502 /* */
3503 /* This function increments a 64-bit number: A = A + 1. */
3504 /* */
3505 /* INPUT */
3506 /* */
3507 /* a_hi Pointer to higher 32-bit of A */
3508 /* a_lo Pointer to lower 32-bit of A */
3509 /* */
3510 /* OUTPUT */
3511 /* */
3512 /* None */
3513 /* */
3514 /* CALLS */
3515 /* */
3516 /* None */
3517 /* */
3518 /* CALLED BY */
3519 /* */
3520 /* _nx_ptp_client_soft_clock_adjust Adjust soft PTP clock */
3521 /* _nx_ptp_client_clock_adjust Adjust PTP clock */
3522 /* _nx_ptp_client_utility_time_diff Diff two PTP times */
3523 /* _nx_ptp_client_soft_clock_callback Soft PTP clock */
3524 /* */
3525 /* RELEASE HISTORY */
3526 /* */
3527 /* DATE NAME DESCRIPTION */
3528 /* */
3529 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3530 /* */
3531 /**************************************************************************/
_nx_ptp_client_utility_inc64(LONG * a_hi,ULONG * a_lo)3532 VOID _nx_ptp_client_utility_inc64(LONG *a_hi, ULONG *a_lo)
3533 {
3534 ULONG r_lo;
3535
3536 r_lo = *a_lo + 1;
3537 if (r_lo == 0)
3538 {
3539 *a_hi = *a_hi + 1; /* add carry */
3540 }
3541 *a_lo = r_lo;
3542 }
3543
3544
3545 /**************************************************************************/
3546 /* */
3547 /* FUNCTION RELEASE */
3548 /* */
3549 /* _nx_ptp_client_utility_dec64 PORTABLE C */
3550 /* 6.1.3 */
3551 /* AUTHOR */
3552 /* */
3553 /* Yuxin Zhou, Microsoft Corporation */
3554 /* */
3555 /* DESCRIPTION */
3556 /* */
3557 /* This function decrements a 64-bit number: A = A - 1. */
3558 /* */
3559 /* INPUT */
3560 /* */
3561 /* a_hi Pointer to higher 32-bit of A */
3562 /* a_lo Pointer to lower 32-bit of A */
3563 /* */
3564 /* OUTPUT */
3565 /* */
3566 /* None */
3567 /* */
3568 /* CALLS */
3569 /* */
3570 /* None */
3571 /* */
3572 /* CALLED BY */
3573 /* */
3574 /* _nx_ptp_client_soft_clock_adjust Adjust soft PTP clock */
3575 /* _nx_ptp_client_clock_adjust Adjust PTP clock */
3576 /* _nx_ptp_client_utility_time_diff Diff two PTP times */
3577 /* */
3578 /* RELEASE HISTORY */
3579 /* */
3580 /* DATE NAME DESCRIPTION */
3581 /* */
3582 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3583 /* */
3584 /**************************************************************************/
_nx_ptp_client_utility_dec64(LONG * a_hi,ULONG * a_lo)3585 VOID _nx_ptp_client_utility_dec64(LONG *a_hi, ULONG *a_lo)
3586 {
3587 ULONG r_lo;
3588
3589 r_lo = *a_lo;
3590 if (r_lo == 0)
3591 {
3592 *a_hi = *a_hi - 1; /* substract carry */
3593 }
3594 *a_lo = r_lo - 1;
3595 }
3596
3597
3598 /**************************************************************************/
3599 /* */
3600 /* FUNCTION RELEASE */
3601 /* */
3602 /* _nx_ptp_client_utility_neg64 PORTABLE C */
3603 /* 6.1.3 */
3604 /* AUTHOR */
3605 /* */
3606 /* Yuxin Zhou, Microsoft Corporation */
3607 /* */
3608 /* DESCRIPTION */
3609 /* */
3610 /* This function changes the sign of a 64-bit number: A = -A. */
3611 /* */
3612 /* INPUT */
3613 /* */
3614 /* a_hi Pointer to higher 32-bit of A */
3615 /* a_lo Pointer to lower 32-bit of A */
3616 /* */
3617 /* OUTPUT */
3618 /* */
3619 /* None */
3620 /* */
3621 /* CALLS */
3622 /* */
3623 /* None */
3624 /* */
3625 /* CALLED BY */
3626 /* */
3627 /* _nx_ptp_client_utility_time_div_by_2 Divide a PTP time by 2 */
3628 /* */
3629 /* RELEASE HISTORY */
3630 /* */
3631 /* DATE NAME DESCRIPTION */
3632 /* */
3633 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3634 /* */
3635 /**************************************************************************/
_nx_ptp_client_utility_neg64(LONG * a_hi,ULONG * a_lo)3636 VOID _nx_ptp_client_utility_neg64(LONG *a_hi, ULONG *a_lo)
3637 {
3638 LONG r_hi;
3639
3640 r_hi = -*a_hi;
3641 if (*a_lo != 0)
3642 {
3643 r_hi--; /* substract carry */
3644 }
3645 *a_hi = r_hi;
3646 *a_lo = -*a_lo;
3647 }
3648
3649
3650 /**************************************************************************/
3651 /* */
3652 /* FUNCTION RELEASE */
3653 /* */
3654 /* _nx_ptp_client_utility_time_div_by_2 PORTABLE C */
3655 /* 6.1.3 */
3656 /* AUTHOR */
3657 /* */
3658 /* Yuxin Zhou, Microsoft Corporation */
3659 /* */
3660 /* DESCRIPTION */
3661 /* */
3662 /* This function divides a PTP time by 2. */
3663 /* */
3664 /* INPUT */
3665 /* */
3666 /* time_ptr Pointer to PTP time */
3667 /* */
3668 /* OUTPUT */
3669 /* */
3670 /* None */
3671 /* */
3672 /* CALLS */
3673 /* */
3674 /* _nx_ptp_client_utility_neg64 Change the sign of number */
3675 /* */
3676 /* CALLED BY */
3677 /* */
3678 /* _nx_ptp_client_delay_resp_received Process delay response */
3679 /* */
3680 /* RELEASE HISTORY */
3681 /* */
3682 /* DATE NAME DESCRIPTION */
3683 /* */
3684 /* 12-31-2020 Yuxin Zhou Initial Version 6.1.3 */
3685 /* */
3686 /**************************************************************************/
_nx_ptp_client_utility_time_div_by_2(NX_PTP_TIME * time_ptr)3687 VOID _nx_ptp_client_utility_time_div_by_2(NX_PTP_TIME *time_ptr)
3688 {
3689 UINT neg;
3690 LONG sec_hi;
3691 ULONG sec_lo;
3692 LONG ns;
3693
3694 /* get current time value */
3695 sec_hi = time_ptr -> second_high;
3696 sec_lo = time_ptr -> second_low;
3697 ns = time_ptr -> nanosecond;
3698
3699 /* implement division on unsigned values */
3700 if ((sec_hi < 0) || (ns < 0))
3701 {
3702 _nx_ptp_client_utility_neg64(&sec_hi, &sec_lo);
3703 ns = -ns;
3704 neg = 1;
3705 }
3706 else
3707 {
3708 neg = 0;
3709 }
3710
3711 /* divide nanoseconds by two */
3712 ns >>= 1;
3713 if (sec_lo & 1)
3714 {
3715
3716 /* add rest of seconds division */
3717 ns += 500000000L;
3718 }
3719
3720 /* divide seconds by two */
3721 sec_lo >>= 1;
3722 if (sec_hi & 1)
3723 {
3724 sec_lo |= 0x80000000UL;
3725 }
3726 sec_hi >>= 1;
3727
3728 /* restore sign */
3729 if (neg)
3730 {
3731 _nx_ptp_client_utility_neg64(&sec_hi, &sec_lo);
3732 ns = -ns;
3733 }
3734
3735 /* return result */
3736 time_ptr -> second_high = sec_hi;
3737 time_ptr -> second_low = sec_lo;
3738 time_ptr -> nanosecond = ns;
3739 }
3740
