static int
ssdp_ip6_msearch(interface *i,const uint128_t saddr) {
const unsigned char hw[ETH_ALEN] = { 0x33, 0x33, 0x00, 0x00, 0x00, 0xc };
uint128_t net = { htonl(0xff020000ul), htonl(0x0ul),
htonl(0x0ul), htonl(0xcul)
};
struct tpacket_hdr *thdr;
struct udphdr *udp;
struct ip6_hdr *ip;
size_t flen,tlen;
void *frame;
int r;
if((frame = get_tx_frame(i,&flen)) == NULL) {
return -1;
}
thdr = frame;
tlen = thdr->tp_mac;
if((r = prep_eth_header((char *)frame + tlen,flen - tlen,i,
hw,ETH_P_IPV6)) < 0) {
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
ip = (struct ip6_hdr *)((char *)frame + tlen);
if((r = prep_ipv6_header(ip,flen - tlen,saddr,net,IPPROTO_UDP)) < 0) {
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
if(flen - tlen < sizeof(*udp)) {
abort_tx_frame(i,frame);
return -1;
}
udp = (struct udphdr *)((char *)frame + tlen);
udp->source = htons(SSDP_UDP_PORT);
udp->dest = htons(SSDP_UDP_PORT);
tlen += sizeof(*udp);
r = setup_ssdp_query((char *)frame + tlen,flen - tlen);
if(r < 0) {
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
thdr->tp_len = tlen;
ip->ip6_ctlun.ip6_un1.ip6_un1_plen = htons(thdr->tp_len -
((const char *)udp - (const char *)frame));
udp->len = ip->ip6_ctlun.ip6_un1.ip6_un1_plen;
udp->check = udp6_csum(ip);
thdr->tp_len -= thdr->tp_mac;
return send_tx_frame(i,frame);
}
static int
ssdp_ip4_msearch(interface *i,const uint32_t *saddr) {
const unsigned char hw[ETH_ALEN] = { 0x01, 0x00, 0x5e, 0x00, 0x00, 0xc };
uint32_t net = htonl(SSDP_NET4);
struct tpacket_hdr *thdr;
struct udphdr *udp;
struct iphdr *ip;
size_t flen,tlen;
void *frame;
int r;
if((frame = get_tx_frame(i,&flen)) == NULL) {
return -1;
}
thdr = frame;
tlen = thdr->tp_mac;
if((r = prep_eth_header((char *)frame + tlen,flen - tlen,i,
hw,ETH_P_IP)) < 0) {
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
ip = (struct iphdr *)((char *)frame + tlen);
if((r = prep_ipv4_header(ip,flen - tlen,*saddr,net,IPPROTO_UDP)) < 0) {
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
if(flen - tlen < sizeof(*udp)) {
abort_tx_frame(i,frame);
return -1;
}
udp = (struct udphdr *)((char *)frame + tlen);
udp->source = htons(SSDP_UDP_PORT);
udp->dest = htons(SSDP_UDP_PORT);
tlen += sizeof(*udp);
r = setup_ssdp_query((char *)frame + tlen,flen - tlen);
if(r < 0) {
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
ip->tot_len = htons(tlen - ((const char *)ip - (const char *)frame));
ip->check = ipv4_csum(ip);
udp->len = htons(ntohs(ip->tot_len) - ip->ihl * 4u);
udp->check = udp4_csum(ip);
thdr->tp_len = tlen - thdr->tp_mac;
return send_tx_frame(i,frame);
}
// Always goes to ff02::2 (ALL-HOSTS), from each source address.
int tx_ipv6_bcast_pings(interface *i,const uint128_t saddr){
const unsigned char hw[ETH_ALEN] = { 0x33, 0x33, 0x00, 0x00, 0x00, 0x01 };
uint128_t net = { htonl(0xff020000ul), htonl(0x0ul),
htonl(0x0ul), htonl(0x1ul) };
struct tpacket_hdr *thdr;
struct icmp6_hdr *icmp;
struct ip6_hdr *ip;
size_t flen,tlen;
void *frame;
int r;
if((frame = get_tx_frame(i,&flen)) == NULL){
return -1;
}
thdr = frame;
tlen = thdr->tp_mac;
if((r = prep_eth_header((char *)frame + tlen,flen - tlen,i,
hw,ETH_P_IPV6)) < 0){
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
ip = (struct ip6_hdr *)((char *)frame + tlen);
if((r = prep_ipv6_header(ip,flen - tlen,saddr,net,IPPROTO_ICMP6)) < 0){
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
if(flen - tlen < sizeof(*icmp)){
abort_tx_frame(i,frame);
return -1;
}
icmp = (struct icmp6_hdr *)((char *)frame + tlen);
icmp->icmp6_type = ICMP6_ECHO_REQUEST;
icmp->icmp6_code = 0;
tlen += sizeof(*icmp);
thdr->tp_len = tlen;
ip->ip6_ctlun.ip6_un1.ip6_un1_plen = htons(thdr->tp_len -
((const char *)icmp - (const char *)frame));
icmp->icmp6_cksum = icmp6_csum(ip);
return send_tx_frame(i,frame);
}
// Always goes to ff02::2 (ALL-HOSTS), from each source address.
int tx_ipv4_bcast_pings(interface *i,const uint32_t *saddr){
struct tpacket_hdr *thdr;
struct icmphdr *icmp;
struct iphdr *ip;
size_t flen,tlen;
void *frame;
int r;
if((frame = get_tx_frame(i,&flen)) == NULL){
return -1;
}
thdr = frame;
tlen = thdr->tp_mac;
if((r = prep_eth_bcast((char *)frame + tlen,flen - tlen,i,ETH_P_IP)) < 0){
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
ip = (struct iphdr *)((char *)frame + tlen);
// FIXME get bcast address appropriate for route
if((r = prep_ipv4_bcast(ip,flen - tlen,*saddr,IPPROTO_ICMP)) < 0){
abort_tx_frame(i,frame);
return -1;
}
tlen += r;
if(flen - tlen < sizeof(*icmp) + PING4_PAYLOAD_LEN){
abort_tx_frame(i,frame);
return -1;
}
icmp = (struct icmphdr *)((char *)frame + tlen);
icmp->type = ICMP_ECHO;
icmp->code = 0;
tlen += sizeof(*icmp) + PING4_PAYLOAD_LEN;
thdr->tp_len = tlen;
ip->tot_len = htons((const char *)icmp - (const char *)ip + sizeof(*icmp) + PING4_PAYLOAD_LEN);
icmp->checksum = 0;
icmp->checksum = icmp4_csum(icmp,sizeof(*icmp));
ip->check = ipv4_csum(ip);
return send_tx_frame(i,frame);
}
// Mark a frame as ready-to-send. Must have come from get_tx_frame() using this
// same interface. Yes, we will see packets we generate on the RX ring.
int send_tx_frame(interface *i,void *frame){
const omphalos_ctx *octx = get_octx();
struct tpacket_hdr *thdr = frame;
int ret = 0;
assert(thdr->tp_status == TP_STATUS_PREPARING);
if(octx->mode != OMPHALOS_MODE_SILENT){
int self,out;
categorize_tx(i,(const char *)frame + thdr->tp_mac,&self,&out);
if(self){
int r;
r = send_to_self(i,frame);
if(r < 0){
++i->txerrors;
}else{
i->txbytes += r;
++i->txframes;
}
ret |= r < 0 ? -1 : 0;
}
if(out){
uint32_t tplen = thdr->tp_len;
int r;
//thdr->tp_status = TP_STATUS_SEND_REQUEST;
//r = send(i->fd,NULL,0,0);
r = send(i->fd,(const char *)frame + thdr->tp_mac,tplen,0);
if(r == 0){
r = tplen;
}
//diagnostic("Transmitted %d on %s",ret,i->name);
if(r < 0){
diagnostic("Error out-TXing %u on %s (%s)",tplen,i->name,strerror(errno));
++i->txerrors;
}else{
i->txbytes += r;
++i->txframes;
}
ret |= r < 0 ? -1 : 0;
}
thdr->tp_status = TP_STATUS_AVAILABLE;
}else{
abort_tx_frame(i,frame);
ret = 0;
}
return ret;
}
