From b0cd39d82c87a2e11b9eb6fd0b330d293552739c Mon Sep 17 00:00:00 2001
From: Nico Schottelius <nico@nico-notebook.schottelius.org>
Date: Mon, 25 Mar 2019 14:12:51 +0100
Subject: [PATCH] Reformat controller processing

---
 p4app/controller.py | 57 ++++++++++++++++++++++++++-------------------
 1 file changed, 33 insertions(+), 24 deletions(-)

diff --git a/p4app/controller.py b/p4app/controller.py
index ae23b5d..c40669d 100755
--- a/p4app/controller.py
+++ b/p4app/controller.py
@@ -431,15 +431,18 @@ class L2Controller(object):
     def recv_msg_cpu(self, pkg):
         packet = Ether(str(pkg))
 
+        cpu_header = ""
+        ether_orig = ""
+        orig_packet = ""
+
         if packet.type == 0x0800:
-            log.debug("Received raw (untagged) packet - BUG")
+            log.debug("Received raw (untagged) IPv4 packet - BUG")
             self.debug_print_pkg(pkg)
 
         elif packet.type == 0x86dd:
+            log.debug("Received raw (untagged) IPv6 packet - BUG")
             self.debug_print_pkg(pkg)
-            cpu_header = CpuHeader(packet.payload)
-            ether_orig = Ether(src=packet.src, dst=packet.dst, type=cpu_header.type)
-            orig_packet = ether_orig / IP(cpu_header.load)
+
             log.debug("cpu = {}".format(cpu_header.__repr__()))
             log.debug("v4 reassambled={}".format(orig_packet.__repr__()))
 
@@ -451,30 +454,36 @@ class L2Controller(object):
 
             ether_orig = Ether(src=packet.src, dst=packet.dst, type=cpu_header.type)
 
-            # Bug? This might be IPv4 as well later!
-            orig_packet = ether_orig / IPv6(cpu_header.load)
-
-            if ICMPv6ND_NS in orig_packet and orig_packet['IPv6'].src == '::':
-                log.debug("Neighbor solicitation for checking her own IP address")
-            elif ICMPv6MLReport2 in orig_packet and orig_packet['IPv6'].dst == 'ff02::16':
-                mc_group = orig_packet['ICMPv6MLDMultAddrRec'].dst
-                log.debug("Multicast registration for {} port {} -- should probably handle this".format(mc_group, cpu_header.ingress_port))
-            elif ICMPv6ND_RS in orig_packet and orig_packet['IPv6'].dst == 'ff02::2':
-                src = orig_packet['IPv6'].src
-                log.debug("Router solicitation from {} -- should probably handle this?".format(src))
-            elif cpu_header.task == self.task['DEBUG']:
-                log.debug("[debug] v6 reassambled={}".format(orig_packet.__repr__()))
-            elif cpu_header.task == self.task['ICMP6_NS']:
-                log.info("Doing neighbor solicitation for the switch in the controller")
-                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)
-
+            if cpu_header.type == 0x0800:
+                orig_packet = ether_orig / IP(cpu_header.load)
+            elif cpu_header.type == 0x86dd:
+                orig_packet = ether_orig / IPv6(cpu_header.load)
+            else:
+                print("Broken pkg: {}".format(pkg.__repr__()))
+                return
         else:
             print("Broken pkg: {}".format(pkg.__repr__()))
             return
 
+        # Process parsed
+        if ICMPv6ND_NS in orig_packet and orig_packet['IPv6'].src == '::':
+            log.debug("Neighbor solicitation for checking her own IP address")
+        elif ICMPv6MLReport2 in orig_packet and orig_packet['IPv6'].dst == 'ff02::16':
+            mc_group = orig_packet['ICMPv6MLDMultAddrRec'].dst
+            log.debug("Multicast registration for {} port {} -- should probably handle this".format(mc_group, cpu_header.ingress_port))
+        elif ICMPv6ND_RS in orig_packet and orig_packet['IPv6'].dst == 'ff02::2':
+            src = orig_packet['IPv6'].src
+            log.debug("Router solicitation from {} -- should probably handle this?".format(src))
+        elif cpu_header.task == self.task['ICMP6_NS']:
+            log.info("Doing neighbor solicitation for the switch in the controller")
+            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:
+            log.debug("unhandled reassambled={} from table {}".format(orig_packet.__repr__(), cpu_header.table_id))
+
+
     def run_cpu_port_loop(self):
         sniff(iface=self.intf, prn=self.recv_msg_cpu)