Line data    Source code

       1             : /*
       2             :  * Copyright (c) 2013 Juniper Networks, Inc. All rights reserved.
       3             :  * Copyright Contributors to the OpenSDN project
       4             :  */
       5             : 
       6             : #include <stdint.h>
       7             : #include "base/os.h"
       8             : #include "vr_defs.h"
       9             : #include "pkt/proto_handler.h"
      10             : #include "pkt/pkt_init.h"
      11             : #include "pkt/packet_buffer.h"
      12             : 
      13             : ///////////////////////////////////////////////////////////////////////////////
      14             : 
      15        3620 : ProtoHandler::ProtoHandler(Agent *agent, boost::shared_ptr<PktInfo> info,
      16        3620 :                            boost::asio::io_context &io)
      17        3620 :     : agent_(agent), pkt_info_(info), io_(io) {}
      18             : 
      19        3620 : ProtoHandler::~ProtoHandler() {
      20        3620 : }
      21             : 
      22           0 : uint32_t ProtoHandler::EncapHeaderLen() const {
      23           0 :     return agent_->pkt()->pkt_handler()->EncapHeaderLen();
      24             : }
      25             : 
      26             : // send packet to the pkt0 interface
      27        2204 : void ProtoHandler::Send(uint32_t itf, uint32_t vrf, uint16_t cmd,
      28             :                         PktHandler::PktModuleName mod) {
      29        2204 :     Send(itf, vrf, cmd, 0, 0, mod);
      30        2204 : }
      31             : 
      32        2204 : void ProtoHandler::Send(uint32_t itf, uint32_t vrf, uint16_t cmd,
      33             :                         uint32_t param1, uint32_t param2,
      34             :                         PktHandler::PktModuleName mod) {
      35             :     // If pkt_info_->pkt is non-NULL, pkt is freed in destructor of pkt_info_
      36        2204 :     if (agent_->pkt()->pkt_handler() == NULL) {
      37           0 :         return;
      38             :     }
      39             : 
      40        2204 :     AgentHdr hdr(itf, vrf, cmd, param1, param2);
      41        2204 :     agent_->pkt()->pkt_handler()->Send(hdr, pkt_info_->packet_buffer_ptr());
      42        2204 : }
      43             : 
      44        2204 : int ProtoHandler::EthHdr(char *buff, uint16_t len, const MacAddress &src,
      45             :                          const MacAddress &dest, const uint16_t proto,
      46             :                          uint16_t vlan_id) {
      47        2204 :     struct ether_header *eth = (struct ether_header *)buff;
      48        2204 :     uint16_t encap_len = sizeof(struct ether_header);
      49             : 
      50        2204 :     if (vlan_id != VmInterface::kInvalidVlanId) {
      51           0 :         encap_len += 4;
      52             :     }
      53             : 
      54        2204 :     if (len < encap_len) {
      55           0 :         return 0;
      56             :     }
      57             : 
      58        2204 :     dest.ToArray(eth->ether_dhost, sizeof(eth->ether_dhost));
      59        2204 :     src.ToArray(eth->ether_shost, sizeof(eth->ether_shost));
      60             : 
      61        2204 :     uint16_t *ptr = (uint16_t *) (buff + ETHER_ADDR_LEN * 2);
      62        2204 :     if (vlan_id != VmInterface::kInvalidVlanId) {
      63           0 :         *ptr = htons(ETHERTYPE_VLAN);
      64           0 :         ptr++;
      65           0 :         *ptr = htons(vlan_id & 0xFFF);
      66           0 :         ptr++;
      67             :     }
      68             : 
      69        2204 :     *ptr = htons(proto);
      70        2204 :     return encap_len;
      71             : }
      72             : 
      73           0 : int ProtoHandler::EthHdr(const MacAddress &src, const MacAddress &dest,
      74             :                          const uint16_t proto) {
      75           0 :     return EthHdr((char *)pkt_info_->eth, sizeof(struct ether_header), src,
      76           0 :                   dest, proto, VmInterface::kInvalidVlanId);
      77             : }
      78             : 
      79        2204 : int ProtoHandler::EthHdr(char *buff, uint16_t len, const Interface *intrface,
      80             :                          const MacAddress &src, const MacAddress &dest,
      81             :                          const uint16_t proto) {
      82        2204 :     uint16_t vlan_id = VmInterface::kInvalidVlanId;
      83        4404 :     if (intrface && intrface->type() == Interface::VM_INTERFACE &&
      84        2200 :            !(agent_->tsn_enabled())) {
      85        2200 :         vlan_id = static_cast<const VmInterface *>(intrface)->tx_vlan_id();
      86             :     }
      87             : 
      88        2204 :     return EthHdr(buff, len, src, dest, proto, vlan_id);
      89             : }
      90             : 
      91        2204 : int ProtoHandler::EthHdr(char *buff, uint16_t len, uint32_t ifindex,
      92             :                          const MacAddress &src, const MacAddress &dest,
      93             :                          const uint16_t proto) {
      94        2204 :     const Interface *intf = agent()->interface_table()->FindInterface(ifindex);
      95        2204 :     return EthHdr(buff, len, intf, src, dest, proto);
      96             : }
      97             : 
      98           0 : void ProtoHandler::VlanHdr(uint8_t *ptr, uint16_t tci) {
      99           0 :     vlanhdr *vlan = reinterpret_cast<vlanhdr *>(ptr);
     100           0 :     vlan->tpid = htons(0x8100);
     101           0 :     vlan->tci = htons(tci);
     102           0 :     vlan += 1;
     103           0 :     vlan->tpid = htons(0x800);
     104           0 :     return;
     105             : }
     106             : 
     107           0 : uint16_t ProtoHandler::IpHdr(char *buff, uint16_t buf_len, uint16_t len,
     108             :                              in_addr_t src, in_addr_t dest, uint8_t protocol,
     109             :                              uint16_t id, uint8_t ttl) {
     110           0 :     struct ip *ip = (struct ip *)buff;
     111           0 :     if (buf_len < sizeof(struct ip))
     112           0 :         return 0;
     113             : 
     114           0 :     ip->ip_hl = 5;
     115           0 :     ip->ip_v = 4;
     116           0 :     ip->ip_tos = 0;
     117           0 :     ip->ip_len = htons(len);
     118           0 :     ip->ip_id = htons(id);
     119           0 :     ip->ip_off = 0;
     120           0 :     ip->ip_ttl = ttl;
     121           0 :     ip->ip_p = protocol;
     122           0 :     ip->ip_sum = 0;
     123           0 :     ip->ip_src.s_addr = src;
     124           0 :     ip->ip_dst.s_addr = dest;
     125             : 
     126           0 :     ip->ip_sum = Csum((uint16_t *)ip, ip->ip_hl * 4, 0);
     127           0 :     return sizeof(struct ip);
     128             : }
     129             : 
     130           0 : void ProtoHandler::IpHdr(uint16_t len, in_addr_t src, in_addr_t dest,
     131             :                          uint8_t protocol, uint16_t id, uint8_t ttl) {
     132             : 
     133           0 :     IpHdr((char *)pkt_info_->ip, sizeof(struct ip), len,
     134             :           src, dest, protocol, id, ttl);
     135           0 : }
     136             : 
     137           0 : void ProtoHandler::Ip6Hdr(ip6_hdr *ip, uint16_t plen, uint8_t next_header,
     138             :                           uint8_t hlim, uint8_t *src, uint8_t *dest) {
     139           0 :     ip->ip6_flow = htonl(0x60000000); // version 6, TC and Flow set to 0
     140           0 :     ip->ip6_plen = htons(plen);
     141           0 :     ip->ip6_nxt = next_header;
     142           0 :     ip->ip6_hlim= hlim;
     143           0 :     memcpy(ip->ip6_src.s6_addr, src, 16);
     144           0 :     memcpy(ip->ip6_dst.s6_addr, dest, 16);
     145           0 : }
     146             : 
     147           0 : void ProtoHandler::FillUdpHdr(udphdr *udp, uint16_t len,
     148             :                               uint16_t src_port, uint16_t dest_port) {
     149           0 :     udp->uh_sport = htons(src_port);
     150           0 :     udp->uh_dport = htons(dest_port);
     151           0 :     udp->uh_ulen = htons(len);
     152           0 :     udp->uh_sum = 0;
     153           0 : }
     154             : 
     155           0 : uint16_t ProtoHandler::UdpHdr(udphdr *udp, uint16_t buf_len, uint16_t len,
     156             :                               in_addr_t src, uint16_t src_port, in_addr_t dest,
     157             :                               uint16_t dest_port) {
     158           0 :     if (buf_len < sizeof(udphdr))
     159           0 :         return 0;
     160             : 
     161           0 :     FillUdpHdr(udp, len, src_port, dest_port);
     162             : #ifdef VNSW_AGENT_UDP_CSUM
     163             :     udp->uh_sum = UdpCsum(src, dest, len, udp);
     164             : #endif
     165             : 
     166           0 :     return sizeof(udphdr);
     167             : }
     168             : 
     169           0 : void ProtoHandler::UdpHdr(uint16_t len, in_addr_t src, uint16_t src_port,
     170             :                           in_addr_t dest, uint16_t dest_port) {
     171           0 :     UdpHdr(pkt_info_->transp.udp, sizeof(udphdr), len, src, src_port,
     172             :            dest, dest_port);
     173           0 : }
     174             : 
     175           0 : uint16_t ProtoHandler::IcmpHdr(char *buff, uint16_t buf_len, uint8_t type,
     176             :                                uint8_t code, uint16_t word1, uint16_t word2) {
     177           0 :     struct icmp *hdr = ((struct icmp *)buff);
     178           0 :     if (buf_len < sizeof(hdr))
     179           0 :         return 0;
     180             : 
     181           0 :     memset(hdr, 0, sizeof(struct icmp));
     182             : 
     183           0 :     hdr->icmp_type = type;
     184           0 :     hdr->icmp_code = code;
     185           0 :     if(type != ICMP_UNREACH) {
     186           0 :         LOG(ERROR,
     187             :           "Error type != ICMP_UNREACH. BackTrace: " << AgentBackTrace(1));
     188           0 :         return 0;
     189             :     }
     190           0 :     hdr->icmp_nextmtu = htons(word2);
     191           0 :     hdr->icmp_cksum = 0;
     192           0 :     if (type == ICMP_UNREACH)
     193           0 :         return ICMP_UNREACH_HDR_LEN;
     194           0 :     return 0;
     195             : }
     196             : 
     197           0 : void ProtoHandler::IcmpChecksum(char *buff, uint16_t buf_len) {
     198           0 :     struct icmp *hdr = ((struct icmp *)buff);
     199           0 :     hdr->icmp_cksum = Csum((uint16_t *)buff, buf_len, 0);
     200           0 : }
     201             : 
     202           0 : void ProtoHandler::IgmpChecksum(char *buff, uint16_t buf_len) {
     203           0 :     struct igmp *hdr = ((struct igmp *)buff);
     204           0 :     hdr->igmp_cksum = Csum((uint16_t *)buff, buf_len, 0);
     205           0 : }
     206             : 
     207           0 : void ProtoHandler::UdpHdr(udphdr *udp ,uint16_t len, const uint8_t *src,
     208             :                           uint16_t src_port, const uint8_t *dest,
     209             :                           uint16_t dest_port, uint8_t next_hdr) {
     210           0 :     FillUdpHdr(udp, len, src_port, dest_port);
     211           0 :     pkt_info_->transp.udp->uh_sum = Ipv6Csum(src, dest, len, next_hdr,
     212           0 :                                             (uint16_t *)pkt_info_->transp.udp);
     213           0 : }
     214             : 
     215           0 : void ProtoHandler::UdpHdr(uint16_t len, const uint8_t *src, uint16_t src_port,
     216             :                           const uint8_t *dest, uint16_t dest_port,
     217             :                           uint8_t next_hdr) {
     218           0 :     FillUdpHdr(pkt_info_->transp.udp, len, src_port, dest_port);
     219           0 :     pkt_info_->transp.udp->uh_sum = Ipv6Csum(src, dest, len, next_hdr,
     220           0 :                                             (uint16_t *)pkt_info_->transp.udp);
     221           0 : }
     222             : 
     223           0 : uint32_t ProtoHandler::Sum(uint16_t *ptr, std::size_t len, uint32_t sum) const {
     224           0 :     while (len > 1) {
     225           0 :         sum += *ptr++;
     226           0 :         len -= 2;
     227           0 :         if (sum & 0x80000000)
     228           0 :             sum = (sum & 0xFFFF) + (sum >> 16);
     229             :     }
     230             : 
     231           0 :     if (len > 0)
     232           0 :         sum += *(uint8_t *)ptr;
     233             : 
     234           0 :     return sum;
     235             : }
     236             : 
     237           0 : uint16_t ProtoHandler::Csum(uint16_t *ptr, std::size_t len, uint32_t sum) const{
     238           0 :     sum = Sum(ptr, len, sum);
     239             : 
     240           0 :     while (sum >> 16)
     241           0 :         sum = (sum & 0xFFFF) + (sum >> 16);
     242             : 
     243           0 :     return ~sum;
     244             : }
     245             : 
     246           0 : uint16_t ProtoHandler::UdpCsum(in_addr_t src, in_addr_t dest,
     247             :                                std::size_t len, udphdr *udp) const {
     248           0 :     uint32_t sum = 0;
     249           0 :     PseudoUdpHdr phdr(src, dest, IPPROTO_UDP, htons(len));
     250           0 :     sum = Sum((uint16_t *)&phdr, sizeof(PseudoUdpHdr), sum);
     251           0 :     return Csum((uint16_t *)udp, len, sum);
     252             : }
     253             : 
     254           0 : uint16_t ProtoHandler::Ipv6Csum(const uint8_t *src, const uint8_t *dest,
     255             :                                 uint16_t plen, uint8_t next_hdr,
     256             :                                 uint16_t *hdr) const {
     257           0 :     uint32_t len = htonl((uint32_t)plen);
     258           0 :     uint32_t next = htonl((uint32_t)next_hdr);
     259             : 
     260           0 :     uint32_t pseudo = 0;
     261           0 :     pseudo = Sum((uint16_t *)src, 16, 0);
     262           0 :     pseudo = Sum((uint16_t *)dest, 16, pseudo);
     263           0 :     pseudo = Sum((uint16_t *)&len, 4, pseudo);
     264           0 :     pseudo = Sum((uint16_t *)&next, 4, pseudo);
     265           0 :     return Csum(hdr, plen, pseudo);
     266             : }
     267             : 
     268           0 : uint16_t ProtoHandler::Icmpv6Csum(const uint8_t *src, const uint8_t *dest,
     269             :                                   icmp6_hdr *icmp, uint16_t plen) const {
     270           0 :     return Ipv6Csum(src, dest, plen, IPPROTO_ICMPV6, (uint16_t *)icmp);
     271             : }
     272             : 
     273             : ///////////////////////////////////////////////////////////////////////////////