├── dockerme.sh ├── CMakeLists.txt ├── wiced.txt ├── ptp_common.h ├── README.md ├── slave.c ├── slave_socket.c ├── master_socket.c ├── common_socket.c └── master.c /dockerme.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | docker run -v ~/Dropbox/repos/ptpraw:/ptp -p 1320:1320/udp -ti ramcip 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | if(WIN32) 3 | link_libraries(ws2_32) 4 | endif() 5 | 6 | add_executable(master_socket master_socket.c master.c common_socket.c) 7 | add_executable(slave_socket slave_socket.c slave.c common_socket.c) -------------------------------------------------------------------------------- /wiced.txt: -------------------------------------------------------------------------------- 1 | wiced_result_t wiced_time_get_utc_time_ms( wiced_utc_time_ms_t* utc_time_ms ) 2 | { 3 | wiced_time_t temp_wiced_time; 4 | uint32_t time_since_last_reference; 5 | 6 | /* Update the UTC time by the time difference between now and the last update */ 7 | wiced_time_get_time( &temp_wiced_time ); 8 | time_since_last_reference = ( temp_wiced_time - last_utc_time_wiced_reference ); 9 | 10 | if ( time_since_last_reference != 0 ) 11 | { 12 | current_utc_time += time_since_last_reference; 13 | last_utc_time_wiced_reference = temp_wiced_time; 14 | } 15 | 16 | *utc_time_ms = current_utc_time; 17 | return WICED_SUCCESS; 18 | } 19 | 20 | // ThreadX 21 | wiced_result_t wiced_time_get_time( wiced_time_t* time_ptr ) 22 | { 23 | *time_ptr = (wiced_time_t) ( tx_time_get( ) * ms_to_tick_ratio ); 24 | return WICED_SUCCESS; 25 | } 26 | 27 | // FreeRTOS 28 | wiced_result_t wiced_time_get_time(wiced_time_t* time_ptr) 29 | { 30 | *time_ptr = (wiced_time_t) ( xTaskGetTickCount( ) * ms_to_tick_ratio ) + wiced_time_offset; 31 | return WICED_SUCCESS; 32 | } 33 | 34 | // HAL Drivers of STM32 F4: 1ms controlled by SysTick 35 | HAL_GetTick() 36 | -------------------------------------------------------------------------------- /ptp_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | typedef long long ptp_time_t; // ns since epoch 4 | 5 | // external events to the state machines: zero both and send EVENT_RESET to both 6 | enum Event { EVENT_NONE, EVENT_RESET, EVENT_NEWPACKET }; 7 | 8 | //packet exchanged 9 | enum Packet { PTPM_START, PTPM_SYNC, PTPM_TIMES, PTPM_DELAY, PTPM_NEXT, PTPS_HELLO,PTPX_MAX }; 10 | 11 | struct master_data 12 | { 13 | // results below 14 | long long largest_offset ; 15 | long long smallest_offset ; 16 | long long sum_offset ; 17 | long long smallest_delay ; 18 | long long largest_delay ; 19 | long long sum_delay ; 20 | 21 | // state 22 | int steps; // effective 23 | int bigstate; // state 24 | 25 | // params here 26 | int nsteps ; // planned 27 | void * clientdata; 28 | int clientdatasize; 29 | 30 | // internal data 31 | int i; // counter 32 | long long ms_diff,sm_diff; 33 | long long offset,delay; 34 | 35 | ptp_time_t alttime; 36 | 37 | // networking 38 | int sock; 39 | }; 40 | 41 | struct slave_data 42 | { 43 | int bigstate; 44 | int sock; 45 | void * clientdata; 46 | int clientdatasize; 47 | ptp_time_t alttime; 48 | }; 49 | 50 | // master state machine 51 | int master_sm(struct master_data * md, enum Event e, unsigned char * data, int n); 52 | 53 | // state machine of the slave 54 | int slave_sm(struct slave_data * md, enum Event e, unsigned char * data, int n); 55 | 56 | /// returns time in nanoseconds with in[0] containing the high-part (seconds) and in[1] the nanoseconds 57 | void ptp_get_time(ptp_time_t *in); 58 | 59 | /// sends a packet via sock with given size and client data 60 | void ptp_send_packet(int sock, unsigned char * , int n, void *clientdata,int clientdatasize); 61 | 62 | //#define TO_NSEC(t) t (((long long)t[0] * 1000000000L) + t[1]) 63 | #define TO_NSEC(t) t 64 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | Minimal PTP 3 | =========== 4 | 5 | Simple implementation of master-slave PTP with a loop made of sync & delay exchanges. 6 | 7 | The files are master.c and slave.c with slave being very minimal. For embedding it is needed to implement ptp_get_time and ptp_send_packet in both 8 | 9 | The provided example uses UDP sockets for computing, and allows for an extra argument in slave_socket application for introducing a time offset for local machine testing: test_extra_offset_ns. 10 | 11 | For FULL ptp look at ptpd that works also on Windows 12 | 13 | Limitations 14 | =========== 15 | - not the real protocol 16 | - sync is measuring time at packet preparation 17 | - sync should be sent regularly multiple times 18 | - delay should be initiated by slave (here by master) 19 | - reliability of UDP is not addressed 20 | - OSX uses gettime but we could use another timer 21 | - Windows tests needed 22 | 23 | Ideas and Improvements 24 | ====================== 25 | 1) In our target embedding (STM32F4) we are not using a very good timer (nanoseconds) so we could take into account the computation the granularity of the clock (in milliseconds) to have an error model of the result 26 | 27 | 2) We could use the Kernel timestamping in Linux for computing the precise timestamps in WRITING and RECEIVING: 28 | - SO_TIMESTAMP reports usec in msg structure of recvmsg 29 | - SO_TIMESTAMPNS same as nsec 30 | - SO_TIMESTAMPING returns the time of writing message (sendmsg) 31 | 32 | This means we need SYNCREP to which slave should answer with exact time of SYNC and the SYNCREP 33 | 34 | For Windows you need to use libpcap that it is supported by the ptpd daemon 35 | 36 | See: https://www.kernel.org/doc/Documentation/networking/timestamping.txt 37 | See for OSX: http://opensource.apple.com//source/network_cmds/network_cmds-329.2/ping.tproj/ping.c 38 | See ptpd: https://github.com/ptpd/ptpd/blob/5c7c22b4194d5f9acf1b60ac598d05796a031979/src/dep/net.c 39 | 40 | Thanks 41 | ====== 42 | The packet exchange and number crunching started from https://github.com/bestvibes/IEEE1588-PTP . I have made it state machine and created custom packets to reduce the state needed. 43 | 44 | -------------------------------------------------------------------------------- /slave.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "ptp_common.h" 6 | 7 | // https://github.com/bestvibes/IEEE1588-PTP/blob/dev/slave/slave.c 8 | static const char * names[] = {"PTPM_START", "PTPM_SYNC", "PTPM_TIMES", "PTPM_DELAY", "PTPM_NEXT", "PPTS_HELLO"}; 9 | 10 | static void ptp_send_packet_ins(struct slave_data * md, int what, ptp_time_t t_send) 11 | { 12 | unsigned char bufout[4+8+8]; // PTP# timein timeout 13 | memcpy(bufout,"PTP",3); 14 | bufout[3] = what; 15 | memset(bufout+4,0,8); 16 | memcpy(bufout+12,&t_send,8); 17 | ptp_send_packet(md->sock,bufout,sizeof(bufout),md->clientdata,md->clientdatasize); 18 | } 19 | 20 | // slave it is stateless 21 | int slave_sm(struct slave_data * md, enum Event e, unsigned char * data, int n) 22 | { 23 | if(e == EVENT_RESET) 24 | { 25 | ptp_time_t now; 26 | ptp_get_time(&now); 27 | ptp_send_packet_ins(md,PTPS_HELLO,now); 28 | printf("slave at RESET at t=%lld\n",now); 29 | } 30 | if(e == EVENT_NEWPACKET) 31 | { 32 | if(n == 20 && strncmp((char *)data,"PTP",3) == 0) 33 | { 34 | int ptype = -1; 35 | ptp_time_t treceived,treceived2; 36 | ptp_time_t now; 37 | ptp_get_time(&now); // TODO: replace it with the SOCKET timestamp if SO_TIMESTAMP available 38 | if(md->alttime != 0) 39 | { 40 | long long a = TO_NSEC(md->alttime); 41 | long long b = TO_NSEC(now); 42 | if(a != b) 43 | { 44 | printf("delta %lld\n",b-a); 45 | now = md->alttime; 46 | } 47 | else 48 | printf("nodelta\n"); 49 | } 50 | ptype = (int)data[3]; 51 | treceived = *(ptp_time_t*)(data+4); 52 | // mark time back 53 | *((ptp_time_t*)(data+12)) = now; 54 | printf("slave received %s(%d) at t=%lld => t2=%lld\n",ptype >= 0 && ptype < PTPX_MAX ? names[ptype] : "unknown",ptype,treceived,now); 55 | ptp_send_packet(md->sock,data,n,md->clientdata,md->clientdatasize); 56 | } 57 | else 58 | { 59 | printf("wrong packet data: length %d %s\n",n,data); 60 | } 61 | } 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /slave_socket.c: -------------------------------------------------------------------------------- 1 | 2 | #ifdef WIN32 3 | #include 4 | #else 5 | #include 6 | #include 7 | #endif 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "ptp_common.h" 14 | 15 | void init_socket(); 16 | extern long long test_extra_offset_ns; 17 | 18 | // only used if SO_TIMESTAMP 19 | extern int recv_with_timestamp(int sock, char * bufin, int bufin_size, int flags, struct sockaddr_in * from_addr, int *from_addr_size, ptp_time_t *aa); 20 | 21 | 22 | int main(int argc, char const *argv[]) 23 | { 24 | struct slave_data md; 25 | 26 | if(argc ==2 && strcmp(argv[1],"-h") == 0) 27 | { 28 | printf("tinyptp slave socket by ER@SSSA 2016-2017\nSyntax: [offset_ns=0]\n"); 29 | return 0; 30 | } 31 | 32 | init_socket(); 33 | int myport = 1320; 34 | int outport = 1319; 35 | const char * myaddress = "0.0.0.0"; // NOTE that it can be multicast 36 | if(argc > 1) 37 | test_extra_offset_ns = strtol(argv[1],0,10); 38 | int sock = socket(AF_INET, SOCK_DGRAM, 0); 39 | if(sock == 0) 40 | { 41 | perror("cannot socket"); 42 | return -1; 43 | } 44 | 45 | struct sockaddr_in my_addr,out_addr; 46 | memset(&my_addr,0, sizeof(my_addr)); 47 | memset(&out_addr,0, sizeof(out_addr)); 48 | 49 | my_addr.sin_family = AF_INET; 50 | my_addr.sin_addr.s_addr = inet_addr(myaddress); /* send to server address */ 51 | my_addr.sin_port = htons(myport); 52 | 53 | 54 | md.sock = sock; 55 | md.clientdata = &out_addr; 56 | md.clientdatasize = sizeof(out_addr); 57 | int enable = 1; 58 | if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int))) 59 | { 60 | perror("cannot reuseaddr opt\n"); 61 | return -1; 62 | } 63 | if(bind(sock,(struct sockadrr*)&my_addr,sizeof(my_addr))) 64 | { 65 | perror("cannot bind slave "); 66 | printf("to %s:%d\n",myaddress,myport); 67 | return -1; 68 | } 69 | 70 | int hastimestamp = 0; 71 | #ifdef SO_TIMESTAMP 72 | { 73 | if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &hastimestamp, sizeof(hastimestamp)) < 0) 74 | { 75 | perror("setsockopt SO_TIMESTAMP"); 76 | } 77 | else 78 | hastimestamp = 1; 79 | } 80 | #endif 81 | 82 | printf("Listening to %s:%d %s\n",myaddress,myport,hastimestamp ? "with timestamp" : ""); 83 | slave_sm(&md,EVENT_RESET,0,0); // initial step 84 | while(1) 85 | { 86 | unsigned char bufin[128]; 87 | // automatic reply to the receiver 88 | int rf = sizeof(out_addr); 89 | int n; 90 | #ifdef SO_TIMESTAMP 91 | if(hastimestamp) 92 | n = recv_with_timestamp(md.sock,bufin,sizeof(bufin),0, &out_addr,&rf,&md.alttime); 93 | else 94 | #endif 95 | n = recvfrom(md.sock,bufin,sizeof(bufin),0,&out_addr,&rf); 96 | slave_sm(&md,EVENT_NEWPACKET,bufin,n); 97 | } 98 | 99 | return 0; 100 | } -------------------------------------------------------------------------------- /master_socket.c: -------------------------------------------------------------------------------- 1 | 2 | #ifdef WIN32 3 | #include 4 | #else 5 | #include 6 | #include 7 | #include 8 | #endif 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "ptp_common.h" 15 | 16 | void init_socket(); 17 | 18 | 19 | // only used if SO_TIMESTAMP 20 | extern int recv_with_timestamp(int sock, char * bufin, int bufin_size,int flags, struct sockaddr_in * from_addr, int*from_addr_size, ptp_time_t* alttime); 21 | 22 | int main(int argc, char const *argv[]) 23 | { 24 | struct master_data md; 25 | 26 | if(argc ==2 && strcmp(argv[1],"-h") == 0) 27 | { 28 | printf("tinyptp master socket by ER@SSSA 2016-2017\nSyntax: [outaddress=0.0.0.0 [repeats=10]]\n"); 29 | return 0; 30 | } 31 | 32 | init_socket(); // for windows 33 | 34 | int myport = 1319; 35 | int outport = 1320; 36 | const char * myaddress = "0.0.0.0"; // NOTE that it can be multicast 37 | const char * outaddress = argc == 1 ? "0.0.0.0" : argv[1]; 38 | int repeats = argc > 2 ? strtol(argv[2],0,10): 10; 39 | 40 | int sock = socket(AF_INET, SOCK_DGRAM, 0); 41 | if(sock == 0) 42 | { 43 | perror("cannot create socket"); 44 | return -1; 45 | } 46 | 47 | struct sockaddr_in my_addr,out_addr; 48 | memset(&my_addr,0, sizeof(my_addr)); 49 | memset(&out_addr,0, sizeof(out_addr)); 50 | 51 | my_addr.sin_family = AF_INET; 52 | my_addr.sin_addr.s_addr = inet_addr(myaddress); /* send to server address */ 53 | my_addr.sin_port = htons(myport); 54 | 55 | out_addr.sin_family = AF_INET; 56 | out_addr.sin_addr.s_addr = inet_addr(outaddress); /* send to server address */ 57 | out_addr.sin_port = htons(outport); 58 | 59 | memset(&md,0,sizeof(md)); 60 | md.sock = sock; 61 | md.nsteps = repeats; 62 | md.clientdata = &out_addr; 63 | md.clientdatasize = sizeof(out_addr); 64 | #ifdef WIN32 65 | int enable = 1; 66 | if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (const char*)&enable, sizeof(int))) 67 | #else 68 | int enable = 1; 69 | if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int))) 70 | #endif 71 | { 72 | perror("cannot reuseaddr opt\n"); 73 | return -1; 74 | } 75 | if(bind(sock,(const struct sockaddr*)&my_addr,sizeof(my_addr))) 76 | { 77 | perror("cannot bind master\n"); 78 | return -1; 79 | } 80 | 81 | int hastimestamp = 0; 82 | #ifdef SO_TIMESTAMP 83 | { 84 | if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &hastimestamp, sizeof(hastimestamp)) < 0) 85 | { 86 | perror("setsockopt SO_TIMESTAMP"); 87 | } 88 | else 89 | hastimestamp = 1; 90 | } 91 | #endif 92 | printf("Listening to %s:%d and sending to %s:%d repeats:%d%s\n",myaddress,myport,outaddress,outport,repeats,hastimestamp? "has timestamp" : ""); 93 | master_sm(&md,EVENT_RESET,0,0); // initial step 94 | 95 | 96 | while(1) 97 | { 98 | char bufin[128]; 99 | int n; 100 | int rf = sizeof(out_addr); 101 | #ifdef SO_TIMESTAMP 102 | if(hastimestamp) 103 | n = recv_with_timestamp(md.sock,bufin,sizeof(bufin),0,&out_addr,&rf,&md.alttime); 104 | else 105 | #endif 106 | n = recv(md.sock,bufin,sizeof(bufin),0); 107 | if(master_sm(&md,EVENT_NEWPACKET,bufin,n)) 108 | { 109 | printf("Average Offset = %lldns\n", md.sum_offset/md.steps); 110 | printf("Average Delay = %lldns\n", md.sum_delay/md.steps); 111 | 112 | printf("Smallest Offset = %lldns\n", md.smallest_offset); 113 | printf("Smallest Delay = %lldns\n", md.smallest_delay); 114 | 115 | printf("Largest Offset = %lldns\n", md.largest_offset); 116 | printf("Largest Delay = %lldns\n", md.largest_delay); 117 | break; 118 | } 119 | } 120 | 121 | return 0; 122 | } -------------------------------------------------------------------------------- /common_socket.c: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifdef WIN32 4 | #include 5 | #else 6 | #include 7 | #include 8 | #include 9 | #include 10 | #endif 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "ptp_common.h" 17 | 18 | void init_socket() 19 | { 20 | #ifdef WIN32 21 | WSADATA wsa_data; 22 | long result = WSAStartup(MAKEWORD(2, 2), &wsa_data); 23 | #endif 24 | } 25 | 26 | long long test_extra_offset_ns; 27 | 28 | void ptp_get_time(ptp_time_t* in) 29 | { 30 | #ifdef WIN32 31 | // no need for absolute time 32 | __int64 wintime; 33 | GetSystemTimeAsFileTime((FILETIME*)&wintime); 34 | wintime -= 116444736000000000; //1jan1601 to 1jan1970 35 | wintime *= 100; // nanoseconds from 100-nanosecond 36 | wintime += test_extra_offset_ns; 37 | *in = wintime; 38 | #else 39 | /* check for nanosecond resolution support */ 40 | #ifndef CLOCK_REALTIME 41 | // TODO: OSX alternative clock 42 | struct timeval tv = {0}; 43 | gettimeofday(&tv, NULL); 44 | *in = (tv.tv_sec*(ptp_time_t)1000000000) + tv.tv_usec*1000 + test_extra_offset_ns; 45 | #else 46 | struct timespec ts = {0}; 47 | clock_gettime(CLOCK_REALTIME, &ts); 48 | *in = (tv.tv_sec*(ptp_time_t)1000000000) + tv.tv_nsec + test_extra_offset_ns; 49 | #endif 50 | #endif 51 | } 52 | 53 | void ptp_send_packet(int sock, unsigned char * data, int size, void *clientdata,int clientdatasize) 54 | { 55 | sendto(sock, data, size, 0, (struct sockaddr *) clientdata, clientdatasize); 56 | } 57 | 58 | #ifdef SO_TIMESTAMP 59 | int recv_with_timestamp(int sock, char * bufin, int bufin_size, int flags, struct sockaddr_in * from_addr, int* from_addr_size, ptp_time_t* alttime) 60 | { 61 | // FROM: https://www.kernel.org/doc/Documentation/networking/timestamping/timestamping.c 62 | int n; 63 | struct timeval tv; 64 | struct msghdr msg; 65 | struct iovec iov; 66 | struct cmsghdr *cmsg; 67 | struct { 68 | struct cmsghdr cm; 69 | char control[512]; // maybe too much 70 | } control; 71 | memset(&msg, 0, sizeof(msg)); 72 | iov.iov_base = bufin; 73 | iov.iov_len = bufin_size; 74 | msg.msg_iov = &iov; 75 | msg.msg_iovlen = 1; 76 | msg.msg_name = (caddr_t)from_addr; 77 | msg.msg_namelen = *from_addr_size; 78 | msg.msg_control = &control; 79 | msg.msg_controllen = sizeof(control); 80 | // mark default 81 | *alttime = 0; 82 | n = recvmsg(sock,&msg,flags); //|MSG_DONTWAIT|MSG_ERRQUEUE); 83 | #if 1 84 | // generic loopy from linux 85 | for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { 86 | switch (cmsg->cmsg_type) { 87 | case SOL_SOCKET: 88 | switch (cmsg->cmsg_type) { 89 | case SO_TIMESTAMP: 90 | { 91 | struct timeval *stamp = (struct timeval *)CMSG_DATA(cmsg); 92 | *alttime = (stamp->tv_sec*(ptp_time_t)1000000000) + stamp->tv_usec*1000 + test_extra_offset_ns; 93 | return n; // no need to wait for the rest 94 | } 95 | break; 96 | } 97 | } 98 | } 99 | #else 100 | // compact as in OSX ping (when we are sure of what we setted) 101 | cmsg = (struct cmsghdr *)&control; 102 | if (cmsg->cmsg_level == SOL_SOCKET && 103 | cmsg->cmsg_type == SCM_TIMESTAMP && 104 | cmsg->cmsg_len == CMSG_LEN(sizeof tv)) { 105 | /* Copy to avoid alignment problems: */ 106 | memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); 107 | alttime[0] = tv.tv_sec; 108 | alttime[1] = (int)(tv.tv_usec*1000 + test_extra_offset_ns); 109 | } 110 | #endif 111 | return n; 112 | } 113 | #endif -------------------------------------------------------------------------------- /master.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ptp_common.h" 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | /* 9 | SLAVE 10 | loop 11 | if receive PTPM_START 12 | send PTPM_START 13 | receive PTP_TIMES => times 14 | clocksync(times): 15 | sync: 16 | receive * (should be PTPM_SYNC else abort) 17 | now=t2 18 | send (PTPM_SYNC,t2) 19 | delay: 20 | receive * (should be PPTM_DELAY else abort) 21 | now=t3 22 | send (PPTM_DELAY,t3) 23 | receive_packet * (should be PPTM_NEXT else abort) 24 | 25 | else: 26 | send PPTS_HELLO 27 | 28 | */ 29 | 30 | 31 | /* 32 | MASTER 33 | send PTPM_START 34 | wait receive PTPM_START 35 | send PTPM_TIMES # 36 | wait receive PTPM_TIMES 37 | clock: 38 | loop times # 39 | sync 40 | send PTPM_SYNC 41 | now=t1 42 | receive_packet (PTPM_SYNC,othertime) 43 | now=t2 44 | return t2-t1 45 | delay_packet 46 | send PTPM_DELAY 47 | receive PTPM_DELAY othertime=t3 48 | now=t4 49 | return t4-t3 50 | send PTPM_NEXT 51 | */ 52 | 53 | static void ptp_send_packet_in(struct master_data * md, int what, ptp_time_t t_send) 54 | { 55 | unsigned char bufout[4+8+8]; // PTP# timein timeout 56 | memcpy(bufout,"PTP",3); 57 | bufout[3] = what; 58 | memcpy(bufout+4,&t_send,8); 59 | memset(bufout+12,0,8); 60 | ptp_send_packet(md->sock,bufout,sizeof(bufout),md->clientdata,md->clientdatasize); 61 | } 62 | 63 | enum MasterState { MASTER_START, MASTER_WAIT0,MASTER_TIMES, MASTER_SYNC, MASTER_SYNCANS, MASTER_DELAY, MASTER_DELAYANS,MASTER_LOOPEND, MASTER_DONE}; 64 | 65 | static const char * names[] = {"PTPM_START", "PTPM_SYNC", "PTPM_TIMES", "PTPM_DELAY", "PTPM_NEXT", "PPTS_HELLO"}; 66 | 67 | int master_sm(struct master_data * md, enum Event e, unsigned char * data, int n) 68 | { 69 | int ptype = -1; 70 | ptp_time_t treceived; 71 | ptp_time_t treceived2; 72 | 73 | // multiple steps 74 | if(e == EVENT_RESET) 75 | { 76 | md->bigstate = MASTER_START; 77 | } 78 | else if(e == EVENT_NEWPACKET) 79 | { 80 | if(n == 20 && strncmp((char*)data,"PTP",3) == 0) 81 | { 82 | // extract for the state machine and dump 83 | ptype = (int)data[3]; 84 | treceived = *(ptp_time_t*)(data+4); 85 | treceived2 = *(ptp_time_t*)(data+12); 86 | printf("master received %s(%d) with t=%lld t2=%lld\n",ptype >= 0 && ptype < PTPX_MAX ? names[ptype] : "unknown",ptype,treceived,treceived2); 87 | } 88 | else 89 | { 90 | printf("wrong packet data: length %d %s\n",n,data); 91 | } 92 | } 93 | 94 | while(1) 95 | switch(md->bigstate) 96 | { 97 | case MASTER_START: 98 | md-> largest_offset = LONG_MIN; 99 | md-> smallest_offset = LONG_MAX; 100 | md-> sum_offset = 0; 101 | md-> smallest_delay = LONG_MAX; 102 | md-> largest_delay = LONG_MIN; 103 | md-> sum_delay = 0; 104 | md-> i = 0; 105 | md-> bigstate = MASTER_WAIT0; 106 | ptp_send_packet_in(md,PTPM_START,0); 107 | return 0; // wait 108 | case MASTER_WAIT0: 109 | if(e == EVENT_NEWPACKET) 110 | { 111 | if(ptype != PTPM_START) 112 | { 113 | printf("expected START got %d\n",ptype); 114 | md->bigstate = MASTER_START; 115 | break; // retry 116 | } 117 | else 118 | { 119 | // check PTPM_START otherwise go MASTER_START 120 | ptp_send_packet_in(md,PTPM_TIMES,md->nsteps); 121 | md->bigstate = MASTER_TIMES; 122 | return 0; // wait 123 | } 124 | } 125 | return 0; // ignore 126 | case MASTER_TIMES: 127 | if(e == EVENT_NEWPACKET) 128 | { 129 | if(ptype != PTPM_TIMES) 130 | { 131 | printf("expected TIMES\n"); 132 | md->bigstate = MASTER_START; 133 | break; // retry 134 | } 135 | else 136 | { 137 | md->bigstate = MASTER_SYNC; 138 | break; // next state 139 | } 140 | } 141 | return 0; // ignore 142 | case MASTER_SYNC: 143 | { 144 | // IMPORTANT: in real PTP we use the clock at the moment of the NIC exit 145 | // NOTE: under Linux with SO_TIMESTAMPING it is possible to know when the packet left the NIC so we'll use SYNCDETAIL 146 | ptp_time_t t1; 147 | // sync is: get time, send, get answer, measure offset as difference between answer and original 148 | ptp_get_time(&t1); 149 | ptp_send_packet_in(md,PTPM_SYNC,t1); 150 | md->bigstate = MASTER_SYNCANS; 151 | } 152 | return 0; // wait 153 | case MASTER_SYNCANS: 154 | if(e == EVENT_NEWPACKET) 155 | { 156 | if(ptype != PTPM_SYNC) 157 | { 158 | printf("expected SYNC\n"); 159 | md->bigstate = MASTER_START; 160 | break; // restart 161 | } 162 | md->ms_diff = (TO_NSEC(treceived2) - TO_NSEC(treceived)); // t2-t1 163 | md->bigstate = MASTER_DELAY; 164 | break; // next state 165 | } 166 | else 167 | return 0; // ignore 168 | case MASTER_DELAY: 169 | { 170 | // this is a REQUEST for DELAY we don't care about master time here 171 | ptp_time_t tmp; 172 | ptp_get_time(&tmp); 173 | ptp_send_packet_in(md,PTPM_DELAY,tmp); // NOT used in protocol 174 | md->bigstate = MASTER_DELAYANS; 175 | } 176 | return 0; // wait 177 | case MASTER_DELAYANS: 178 | if(e == EVENT_NEWPACKET) 179 | { 180 | if(ptype != PTPM_DELAY) 181 | { 182 | printf("expected delay!"); 183 | md->bigstate = MASTER_START; 184 | break; // restart 185 | } 186 | else 187 | { 188 | // TODO: replace it with the SOCKET timestamp if SO_TIMESTAMP available 189 | ptp_time_t t4; 190 | ptp_get_time(&t4); // == t4 191 | if(md->alttime != 0) 192 | { 193 | ptp_time_t a = TO_NSEC(md->alttime); 194 | ptp_time_t b = TO_NSEC(t4); 195 | if(a != b) 196 | { 197 | printf("delta %lld\n",b-a); 198 | t4 = md->alttime; 199 | } 200 | else 201 | printf("nodelta\n"); 202 | } 203 | md->sm_diff = (TO_NSEC(t4) - TO_NSEC(treceived2)); // t4-t3 204 | md->bigstate = MASTER_LOOPEND; 205 | break; // next state 206 | } 207 | } 208 | return 0; // ignore 209 | case MASTER_LOOPEND: 210 | { 211 | long long offset = (md->ms_diff - md->sm_diff)/2; 212 | long long delay = (md->ms_diff + md->sm_diff)/2; 213 | /* calculate averages, min, max */ 214 | md->sum_offset += offset; 215 | if (md->largest_offset < offset) { 216 | md->largest_offset = offset; 217 | } 218 | if (md->smallest_offset > offset) { 219 | md->smallest_offset = offset; 220 | } 221 | 222 | md->sum_delay += delay; 223 | if (md->largest_delay < delay) { 224 | md->largest_delay = delay; 225 | } 226 | if (md->smallest_delay > delay) { 227 | md->smallest_delay = delay; 228 | } 229 | if(++md->i >= md->nsteps) 230 | { 231 | md->bigstate = MASTER_DONE; 232 | md->steps = md->i; 233 | return 1; // completion 234 | } 235 | else 236 | { 237 | ptp_send_packet_in(md, PTPM_NEXT, 0); 238 | md->bigstate = MASTER_SYNC; 239 | return 0; // wait 240 | } 241 | } 242 | default: 243 | return 0; // ignore 244 | } 245 | } 246 | 247 | --------------------------------------------------------------------------------