-
Notifications
You must be signed in to change notification settings - Fork 3
/
common_socket.c
113 lines (104 loc) · 3.26 KB
/
common_socket.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#ifdef WIN32
#include <windows.h>
#else
#include <sys/socket.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <sys/time.h>
#endif
#include <stdlib.h>
#include <stdio.h>
#include <memory.h>
#include "ptp_common.h"
void init_socket()
{
#ifdef WIN32
WSADATA wsa_data;
long result = WSAStartup(MAKEWORD(2, 2), &wsa_data);
#endif
}
long long test_extra_offset_ns;
void ptp_get_time(ptp_time_t* in)
{
#ifdef WIN32
// no need for absolute time
__int64 wintime;
GetSystemTimeAsFileTime((FILETIME*)&wintime);
wintime -= 116444736000000000; //1jan1601 to 1jan1970
wintime *= 100; // nanoseconds from 100-nanosecond
wintime += test_extra_offset_ns;
*in = wintime;
#else
/* check for nanosecond resolution support */
#ifndef CLOCK_REALTIME
// TODO: OSX alternative clock
struct timeval tv = {0};
gettimeofday(&tv, NULL);
*in = (tv.tv_sec*(ptp_time_t)1000000000) + tv.tv_usec*1000 + test_extra_offset_ns;
#else
struct timespec ts = {0};
clock_gettime(CLOCK_REALTIME, &ts);
*in = (tv.tv_sec*(ptp_time_t)1000000000) + tv.tv_nsec + test_extra_offset_ns;
#endif
#endif
}
void ptp_send_packet(int sock, unsigned char * data, int size, void *clientdata,int clientdatasize)
{
sendto(sock, data, size, 0, (struct sockaddr *) clientdata, clientdatasize);
}
#ifdef SO_TIMESTAMP
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)
{
// FROM: https://www.kernel.org/doc/Documentation/networking/timestamping/timestamping.c
int n;
struct timeval tv;
struct msghdr msg;
struct iovec iov;
struct cmsghdr *cmsg;
struct {
struct cmsghdr cm;
char control[512]; // maybe too much
} control;
memset(&msg, 0, sizeof(msg));
iov.iov_base = bufin;
iov.iov_len = bufin_size;
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_name = (caddr_t)from_addr;
msg.msg_namelen = *from_addr_size;
msg.msg_control = &control;
msg.msg_controllen = sizeof(control);
// mark default
*alttime = 0;
n = recvmsg(sock,&msg,flags); //|MSG_DONTWAIT|MSG_ERRQUEUE);
#if 1
// generic loopy from linux
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
switch (cmsg->cmsg_type) {
case SOL_SOCKET:
switch (cmsg->cmsg_type) {
case SO_TIMESTAMP:
{
struct timeval *stamp = (struct timeval *)CMSG_DATA(cmsg);
*alttime = (stamp->tv_sec*(ptp_time_t)1000000000) + stamp->tv_usec*1000 + test_extra_offset_ns;
return n; // no need to wait for the rest
}
break;
}
}
}
#else
// compact as in OSX ping (when we are sure of what we setted)
cmsg = (struct cmsghdr *)&control;
if (cmsg->cmsg_level == SOL_SOCKET &&
cmsg->cmsg_type == SCM_TIMESTAMP &&
cmsg->cmsg_len == CMSG_LEN(sizeof tv)) {
/* Copy to avoid alignment problems: */
memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv));
alttime[0] = tv.tv_sec;
alttime[1] = (int)(tv.tv_usec*1000 + test_extra_offset_ns);
}
#endif
return n;
}
#endif