static int raw_send(struct transport *t, struct fdarray *fda, int event,
int peer, void *buf, int len, struct address *addr,
struct hw_timestamp *hwts)
{
struct raw *raw = container_of(t, struct raw, t);
ssize_t cnt;
int fd = event ? fda->fd[FD_EVENT] : fda->fd[FD_GENERAL];
unsigned char pkt[1600], *ptr = buf;
struct eth_hdr *hdr;
ptr -= sizeof(*hdr);
len += sizeof(*hdr);
if (!addr)
addr = peer ? &raw->p2p_addr : &raw->ptp_addr;
hdr = (struct eth_hdr *) ptr;
addr_to_mac(&hdr->dst, addr);
addr_to_mac(&hdr->src, &raw->src_addr);
hdr->type = htons(ETH_P_1588);
cnt = send(fd, ptr, len, 0);
if (cnt < 1) {
pr_err("send failed: %d %m", errno);
return cnt;
}
/*
* Get the time stamp right away.
*/
return event == TRANS_EVENT ? sk_receive(fd, pkt, len, NULL, hwts, MSG_ERRQUEUE) : cnt;
}
static int udp6_send(struct transport *t, struct fdarray *fda, int event, int peer,
void *buf, int len, struct hw_timestamp *hwts)
{
struct udp6 *udp6 = container_of(t, struct udp6, t);
ssize_t cnt;
int fd = event ? fda->fd[FD_EVENT] : fda->fd[FD_GENERAL];
struct sockaddr_in6 addr;
unsigned char junk[1600];
memset(&addr, 0, sizeof(addr));
addr.sin6_family = AF_INET6;
addr.sin6_addr = peer ? mc6_addr[MC_PDELAY] : mc6_addr[MC_PRIMARY];
addr.sin6_port = htons(event ? EVENT_PORT : GENERAL_PORT);
if (is_link_local(&addr.sin6_addr)) {
addr.sin6_scope_id = udp6->index;
}
len += 2; /* Extend the payload by two, for UDP checksum corrections. */
cnt = sendto(fd, buf, len, 0, (struct sockaddr *)&addr, sizeof(addr));
if (cnt < 1) {
pr_err("sendto failed: %m");
return cnt;
}
/*
* Get the time stamp right away.
*/
return event == TRANS_EVENT ? sk_receive(fd, junk, len, hwts, MSG_ERRQUEUE) : cnt;
}
static int raw_recv(struct transport *t, int fd, void *buf, int buflen,
struct address *addr, struct hw_timestamp *hwts)
{
int cnt, hlen;
unsigned char *ptr = buf;
struct eth_hdr *hdr;
struct raw *raw = container_of(t, struct raw, t);
if (raw->vlan) {
hlen = sizeof(struct vlan_hdr);
} else {
hlen = sizeof(struct eth_hdr);
}
ptr -= hlen;
buflen += hlen;
hdr = (struct eth_hdr *) ptr;
cnt = sk_receive(fd, ptr, buflen, addr, hwts, 0);
if (cnt >= 0)
cnt -= hlen;
if (cnt < 0)
return cnt;
if (raw->vlan) {
if (ETH_P_1588 == ntohs(hdr->type)) {
pr_notice("raw: disabling VLAN mode");
raw->vlan = 0;
}
} else {
if (ETH_P_8021Q == ntohs(hdr->type)) {
pr_notice("raw: switching to VLAN mode");
raw->vlan = 1;
}
}
return cnt;
}
static int udp6_recv(struct transport *t, int fd, void *buf, int buflen,
struct hw_timestamp *hwts)
{
return sk_receive(fd, buf, buflen, hwts, 0);
}
