master-thesis/p4app/controller.py

from __future__ import unicode_literals

import nnpy
import struct

from p4utils.utils.topology import Topology
from p4utils.utils.sswitch_API import SimpleSwitchAPI

from scapy.all import sniff, get_if_list, Ether, get_if_hwaddr, sendp
from scapy.all import IP, Raw, IPv6, TCP, TCP_client, Ether
from scapy.all import sniff
from scapy.all import Packet, BitField, IntEnumField, ShortField, XShortEnumField, ShortEnumField
from scapy.all import ICMPv6ND_NS, ICMPv6ND_RS, ICMPv6NDOptSrcLLAddr, ICMPv6ND_NS, ICMPv6ND_NA, ICMPv6NDOptDstLLAddr
from scapy.all import ICMPv6EchoRequest, ICMPv6EchoReply
from scapy.data import ETHER_TYPES

import sys
import re

import logging
import argparse
import subprocess

import ipaddress

logging.basicConfig()
log = logging.getLogger("main")


cpu_fields = {
    1: 'ICMP6_NS',
    2: 'ICMP6_GENERAL',
    3: 'DEBUG'
}


class CpuHeader(Packet):
    name = 'CpuPacket'
    fields_desc = [
        ShortEnumField('task', 1, cpu_fields ),
        ShortField('ingress_port', 0),
        XShortEnumField("type", 0x9000, ETHER_TYPES)
    ]


class L2Controller(object):
    def __init__(self, sw_name):
        # Command line mapping
        self.modes = ['base', 'router', "range_router" ]

        # Reverse maps the cpu header
        self.task = dict(reversed(item) for item in cpu_fields.items())

        self.info={}
        self.info['ndp_multicast'] = ipaddress.ip_network("ff02::1:ff00:0/104")
        self.info['mac_address'] = "00:00:0a:00:00:42"

        self.info['v6_mask'] = 64
        self.info['v6_base'] = ipaddress.ip_network("2001:db8::/32")
        self.info['v6_gen'] = self.info['v6_base'].subnets(new_prefix=self.info['v6_mask'])

        self.info['v4_mask'] = 24
        self.info['v4_base'] = ipaddress.ip_network("10.0.0.0/8")
        self.info['v4_gen'] = self.info['v4_base'].subnets(new_prefix=self.info['v4_mask'])

        self.info['v4_nat64_base'] = ipaddress.ip_network("10.1.0.0/16")
        self.info['v4_nat64_map'] = self.info['v4_nat64_base'].subnets(new_prefix=self.info['v4_mask'])

        self.info['switch_suffix'] = 0x42
        self.info['nat64_prefix'] = ipaddress.ip_network("64:ff9b::/96")

        self.v6_routes = {}
        self.v6_routes[None] = []
        self.v6_routes['base'] = []

        for port in range(1,3):
            net = self.info['v6_gen'].next()
            self.v6_routes['base'].append({
                "net": net,
                "port": port}
            )

        self.v6_routes['router'] = self.v6_routes['base']
        self.v6_routes['range_router'] = self.v6_routes['base']

        self.v4_routes = {}
        self.v4_routes[None] = []
        self.v4_routes['base'] = []
        for port in range(3,5):
            net = self.info['v4_gen'].next()
            self.v4_routes['base'].append({
                "net": net,
                "port": port}
            )
        self.v4_routes['router'] = self.v4_routes['base']
        self.v4_routes['range_router'] = self.v4_routes['base']

        self.v6_addresses = {}
        self.v6_addresses[None] = []
        for mode in self.modes:
            self.v6_addresses[mode] =  [ net['net'][self.info['switch_suffix']] for net in self.v6_routes[mode] ]

        self.v4_addresses = {}
        self.v4_addresses[None] = []
        for mode in self.modes:
            self.v4_addresses[mode] = [ net['net'][self.info['switch_suffix']] for net in self.v4_routes[mode] ]

        self.nat64_map = {}
        # init default
        for mode in self.modes:
            self.nat64_map[mode] = []

        # specific settings -- only need the address (=offset), no mask
        for mode in ["range_router"]:
            for net in self.v6_routes[mode]:
                v6_net = net['net']
                v4_net = self.info['v4_nat64_map'].next()

                self.nat64_map[mode].append({
                    "v6_network": v6_net,
                    "v4_network": v4_net,
                    "nat64_prefix": self.info['nat64_prefix']
                })

        self.init_boilerplate(sw_name)

    def gen_ndp_multicast_addr(self, addr):
        """ append the 24 bit of the address to the multicast address"""

        last_24 = int(addr) & 0xffffff
        addr = self.info['ndp_multicast'][last_24]

        return addr

    def init_ndp(self):
        """ initialise neighbor discovery protocol"""

        # https://en.wikipedia.org/wiki/Solicited-node_multicast_address
        ndp_prefix = "ff02::1:ff00:0/104"

        all_ports = range(1,5)
        # create multicast nodes
        for rid in range(1,5):
            ports = [ x for x in all_ports if not x == rid ]
            n_handle = self.controller.mc_node_create(rid, ports)
            log.debug("Creating MC node rid={} ports={} handle={}".format(rid, ports, n_handle))

            g_handle = self.controller.mc_mgrp_create(rid)
            log.debug("Creating MC group mgrp={} handle={} && associating afterwards".format(rid, g_handle))

            self.controller.mc_node_associate(g_handle, n_handle)


        self.controller.table_clear("ndp")
        for port in all_ports:
            self.controller.table_add("ndp", "multicast_pkg", [ndp_prefix, str(port)], [str(port)])


        # Special rule for switch entries
        self.controller.table_add("ndp_answer", "icmp6_neighbor_solicitation", ["ff02::1:ff00:42", "135"], ["2001:db8:61::42"])

    def init_boilerplate(self, sw_name):
        self.topo = Topology(db="topology.db")
        self.sw_name = sw_name
        self.thrift_port = self.topo.get_thrift_port(sw_name)
        self.cpu_port =  self.topo.get_cpu_port_index(self.sw_name)
        self.controller = SimpleSwitchAPI(self.thrift_port)
        self.intf = str(self.topo.get_cpu_port_intf(self.sw_name).replace("eth0", "eth1"))
        self.controller.reset_state()

        if self.cpu_port:
            self.controller.mirroring_add(100, self.cpu_port)

#        self.init_ndp()

    def config(self):
        self.fill_tables()
        self.config_hosts()

    def listen_to_icmp6_multicast(self):
        """Only needed for debugging"""

        net = self.info['ndp_multicast']
        self.controller.table_add("v6_networks", "controller_debug", [str(net)])

    def fill_tables(self):
        self.controller.table_clear("v6_networks")
        for v6route in self.v6_routes[self.mode]:
            self.controller.table_add("v6_networks", "set_egress_port", [str(v6route['net'])], [str(v6route['port'])])

        if self.args.multicast_to_controller:
            self.listen_to_icmp6_multicast()

        self.controller.table_clear("v4_routing")
        for v4route in self.v4_routes[self.mode]:
            self.controller.table_add("v4_networks", "set_egress_port", [str(v4route['net'])], [str(v4route['port'])])

        self.controller.table_clear("v6_addresses")
        for v6addr in self.v6_addresses[self.mode]:
            log.debug("Adding v6 address: {}".format(v6addr))

            icmp6_addr = self.gen_ndp_multicast_addr(v6addr)

            another_addr = v6addr +1
            another_addr_ns = self.gen_ndp_multicast_addr(another_addr)

            self.controller.table_add("v6_addresses", "controller_reply", [str(v6addr)], [str(self.task['ICMP6_GENERAL'])])
            self.controller.table_add("v6_addresses", "controller_reply", [str(icmp6_addr)], [str(self.task['ICMP6_NS'])])

            # Experimental: controller does NDP, switch does ICMP6 echo reply
            self.controller.table_add("v6_addresses", "controller_reply", [str(another_addr_ns)], [str(self.task['ICMP6_NS'])])
            self.controller.table_add("v6_addresses", "icmp6_echo_reply", [str(another_addr)])

        for nat64map in self.nat64_map[self.mode]:
            self.static_nat64_mapping(**nat64map)

    def static_nat64_mapping(self, nat64_prefix, v6_network, v4_network):
        log.info("NAT64 map: {} -> {} -> {}".format(nat64_prefix, v6_network, v4_network))

        self.controller.table_add("v6_networks", "nat64_static", [str(nat64_prefix)],
                                  [str(v6_network.network_address),
                                   str(v4_network.network_address),
                                   str(nat64_prefix.network_address)]
        )

        self.controller.table_add("v4_networks", "nat46_static", [str(v4_network)],
                                  [str(v6_network.network_address),
                                   str(v4_network.network_address),
                                   str(nat64_prefix.network_address)]
        )


    def config_hosts(self):
        """ Assumptions:
        - all routes are networks (no /128 v6 or /32 v4
        - hosts get the first ip address in the network
        """

        for v6route in self.v6_routes[self.mode]:
            host   = "h{}".format(v6route['port'])
            dev    = "{}-eth0".format(host)
            net    = v6route['net']
            ipaddr = "{}/{}".format(net[1],net.prefixlen)
            router = str(net[self.info['switch_suffix']])

            self.config_v6_host(host, str(net), str(ipaddr), dev, router)

        for v4route in self.v4_routes[self.mode]:
            host   = "h{}".format(v4route['port'])
            dev    = "{}-eth0".format(host)
            net    = v4route['net']
            ipaddr = "{}/{}".format(net[1],net.prefixlen)
            router = str(net[self.info['switch_suffix']])

            self.config_v4_host(host, str(net), str(ipaddr), dev, router)

    @staticmethod
    def config_v6_host(host, net, ipaddr, dev, router=None):
        log.debug("Config v6 host: {} {}->{} on {}".format(host, net, ipaddr, dev))

        subprocess.call(["mx", host, "ip", "addr", "flush", "dev", dev])
        for v6dev in [ "lo", "default", "all", dev ]:
            subprocess.call(["mx", host, "sysctl", "net.ipv6.conf.{}.disable_ipv6=0".format(v6dev)])

        # Set down & up to regain link local address
        subprocess.call(["mx", host, "ip", "link", "set", dev, "down"])
        subprocess.call(["mx", host, "ip", "link", "set", dev, "up"])

        subprocess.call(["mx", host, "ip", "addr", "add", ipaddr, "dev", dev])

        if router:
            subprocess.call(["mx", host, "ip", "route", "add", "default", "via", router])

    @staticmethod
    def config_v4_host(host, net, ipaddr, dev, router=None):
        log.debug("Config v4 host: {} {}->{} on {}".format(host, net, ipaddr, dev))

        subprocess.call(["mx", host, "ip", "addr", "flush", "dev", dev])
        subprocess.call(["mx", host, "ip", "addr", "add", ipaddr, "dev", dev])

        if router:
            subprocess.call(["mx", host, "ip", "route", "add", "default", "via", router])

    def debug_print_pkg(self, pkg, msg="INCOMING"):
        log.debug("{}: {}".format(msg, pkg.__repr__()))

    def debug_format_pkg(self, pkg):
        packet = Ether(str(pkg))

        if packet.type == 0x800:
            ip = pkg.getlayer(IP)
        elif packet.type == 0x86dd:
            ip = pkg.getlayer(IPv6)

        # tcp = pkg.getlayer(TCP)

        # raw = pkg.getlayer(Raw)

        # return "{}:{} => {}:{}: flags={} seq={} ack={} raw={}".format(
        #     ip.src, tcp.sport,
        #     ip.dst, tcp.dport,
        #     tcp.flags,
        #     tcp.seq,
        #     tcp.ack,
        #     raw)

    def handle_icmp6_echo_request(self, pkg):
        """
        Sample from the wire:

        DEBUG:main:reassambled=<Ether  dst=00:00:0a:00:00:42 src=00:00:0a:00:00:01 type=0x86dd |<IPv6  version=6 tc=0 fl=474570 plen=64 nh=ICMPv6 hlim=64 src=2001:db8::1 dst=2001:db8::42 |<ICMPv6EchoRequest  type=Echo Request code=0 cksum=0xb76d id=0x16cd seq=0x1 data='\x1f\xe2~\\\x00\x00\x00\x00\xf8\x82\x00\x00\x00\x00\x00\x00\x10\x11\x12\x13\x14\x15\x16\x17\x18\x19\x1a\x1b\x1c\x1d\x1e\x1f !"#$%&\'()*+,-./01234567' |>>>
        """

        log.info("Replying to ICMP packet")
        dst_mac = pkg[Ether].src
        src_mac = pkg[Ether].dst

        dst_addr = pkg[IPv6].src
        src_addr = pkg[IPv6].dst

        e = Ether(src=src_mac, dst=dst_mac)
        i = IPv6(src=src_addr, dst=dst_addr)
        i2 = ICMPv6EchoReply(id=pkg[ICMPv6EchoRequest].id,
                             seq=pkg[ICMPv6EchoRequest].seq,
                             data=pkg[ICMPv6EchoRequest].data)
        i2.cksum = None
        answer = e / i / i2

        self.send_pkg(answer)


    def handle_icmp6_ns(self, pkg):
        """ Solicitated NA"""

        # Both ways should work
        dst_mac = pkg[Ether].src
        dst_mac = pkg[ICMPv6NDOptSrcLLAddr].lladdr
        src_mac = self.info['mac_address']

        dst_addr = pkg[IPv6].src
        src_addr = pkg[ICMPv6ND_NS].tgt

        e = Ether(src=src_mac, dst=dst_mac)
        i = IPv6(src=src_addr, dst=dst_addr)

        # S=1 -> solicitated
        i2 = ICMPv6ND_NA(S=1, R=0, tgt=src_addr)
        # try5: cksum not chksum !
        i2.cksum = None

        i3 = ICMPv6NDOptDstLLAddr(lladdr=src_mac)

        answer = e / i / i2 / i3

        # try 4
        # for l in [Ether, IPv6, ICMPv6ND_NA, ICMPv6NDOptDstLLAddr]:
        #     try:
        #         del answer[l].chksum
        #     except AttributeError:
        #         pass

        # Let scapy recalc checksum (try3)
        # answer = answer.__class__(str(answer))

        self.send_pkg(answer)

    def send_pkg(self, pkg):
        self.debug_print_pkg(pkg, "OUTGOING")
        sendp(pkg, iface=self.intf, verbose=False)

    def recv_msg_cpu(self, pkg):
        packet = Ether(str(pkg))

        self.debug_print_pkg(pkg)

        if packet.type == 0x0800:
            pass
        elif packet.type == 0x86dd:
            pass
        elif packet.type == 0x4242:
            cpu_header = CpuHeader(packet.payload)
            log.debug("cpu = {}".format(cpu_header.__repr__()))

            ether_orig = Ether(src=packet.src, dst=packet.dst, type=cpu_header.type)
            orig_packet = ether_orig / IPv6(cpu_header.load)
            log.debug("reassambled={}".format(orig_packet.__repr__()))

            if cpu_header.task == self.task['DEBUG']:
                log.debug("Debug purpose only")
            elif cpu_header.task == self.task['ICMP6_NS']:
                log.info("Doing neighbor solicitation")
                self.handle_icmp6_ns(orig_packet)
            elif cpu_header.task == self.task['ICMP6_GENERAL']:
                if ICMPv6EchoRequest in orig_packet:
                    self.handle_icmp6_echo_request(orig_packet)

        else:
            print("Broken pkg: {}".format(pkg.__repr__()))
            return

    def run_cpu_port_loop(self):
        sniff(iface=self.intf, prn=self.recv_msg_cpu)

    def commandline(self):
        parser = argparse.ArgumentParser(description='controller++')
        parser.add_argument('--mode', help='Select mode / settings to use', choices=self.modes)
        parser.add_argument('--debug', help='Enable debug logging', action='store_true')
        parser.add_argument('--verbose', help='Enable verbose logging', action='store_true')
        parser.add_argument('--multicast-to-controller', help='Send debug multicast to controller', action='store_true')

        self.args = parser.parse_args()
        self.mode = self.args.mode
        self.debug = self.args.debug

if __name__ == "__main__":
    import sys
    import os

    if "DEBUG" in os.environ:
        log.setLevel(logging.DEBUG)
    else:
        log.setLevel(logging.INFO)

    log.info("Booting...")
    log.debug("Debug enabled.")

    sw_name = "s1"
    controller = L2Controller(sw_name)

    controller.commandline()
    if controller.args.debug:
        log.setLevel(logging.DEBUG)
    elif controller.args.verbose:
        log.setLevel(logging.INFO)
    else:
        log.setLevel(logging.WARNING)

    controller.config()
    controller.run_cpu_port_loop()